simgear/math/SGMathTest.cxx

   1 // Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Library General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Library General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU Library General Public
  14 // License along with this library; if not, write to the
  15 // Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  16 // Boston, MA  02111-1307, USA.
  17 //
  18
  19 #ifdef HAVE_CONFIG_H
  20 #  include <simgear_config.h>
  21 #endif
  22
  23 #include <cstdlib>
  24 #include <iostream>
  25
  26 #include <plib/sg.h>
  27
  28 #include "SGMath.hxx"
  29
  30 template<typename T>
  31 bool
  32 Vec3Test(void)
  33 {
  34   SGVec3<T> v1, v2, v3;
  35
  36   // Check if the equivalent function works
  37   v1 = SGVec3<T>(1, 2, 3);
  38   v2 = SGVec3<T>(3, 2, 1);
  39   if (equivalent(v1, v2))
  40     return false;
  41
  42   // Check the unary minus operator
  43   v3 = SGVec3<T>(-1, -2, -3);
  44   if (!equivalent(-v1, v3))
  45     return false;
  46
  47   // Check the unary plus operator
  48   v3 = SGVec3<T>(1, 2, 3);
  49   if (!equivalent(+v1, v3))
  50     return false;
  51
  52   // Check the addition operator
  53   v3 = SGVec3<T>(4, 4, 4);
  54   if (!equivalent(v1 + v2, v3))
  55     return false;
  56
  57   // Check the subtraction operator
  58   v3 = SGVec3<T>(-2, 0, 2);
  59   if (!equivalent(v1 - v2, v3))
  60     return false;
  61
  62   // Check the scaler multiplication operator
  63   v3 = SGVec3<T>(2, 4, 6);
  64   if (!equivalent(2*v1, v3))
  65     return false;
  66
  67   // Check the dot product
  68   if (fabs(dot(v1, v2) - 10) > 10*SGLimits<T>::epsilon())
  69     return false;
  70
  71   // Check the cross product
  72   v3 = SGVec3<T>(-4, 8, -4);
  73   if (!equivalent(cross(v1, v2), v3))
  74     return false;
  75
  76   // Check the euclidean length
  77   if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits<T>::epsilon())
  78     return false;
  79
  80   return true;
  81 }
  82
  83 template<typename T>
  84 bool
  85 QuatTest(void)
  86 {
  87   const SGVec3<T> e1(1, 0, 0);
  88   const SGVec3<T> e2(0, 1, 0);
  89   const SGVec3<T> e3(0, 0, 1);
  90   SGVec3<T> v1, v2;
  91   SGQuat<T> q1, q2, q3, q4;
  92   // Check a rotation around the x axis
  93   q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
  94   v1 = SGVec3<T>(1, 2, 3);
  95   v2 = SGVec3<T>(1, -2, -3);
  96   if (!equivalent(q1.transform(v1), v2))
  97     return false;
  98
  99   // Check a rotation around the x axis
 100   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
 101   v2 = SGVec3<T>(1, 3, -2);
 102   if (!equivalent(q1.transform(v1), v2))
 103     return false;
 104
 105   // Check a rotation around the y axis
 106   q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
 107   v2 = SGVec3<T>(-1, 2, -3);
 108   if (!equivalent(q1.transform(v1), v2))
 109     return false;
 110
 111   // Check a rotation around the y axis
 112   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
 113   v2 = SGVec3<T>(-3, 2, 1);
 114   if (!equivalent(q1.transform(v1), v2))
 115     return false;
 116
 117   // Check a rotation around the z axis
 118   q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
 119   v2 = SGVec3<T>(-1, -2, 3);
 120   if (!equivalent(q1.transform(v1), v2))
 121     return false;
 122
 123   // Check a rotation around the z axis
 124   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
 125   v2 = SGVec3<T>(2, -1, 3);
 126   if (!equivalent(q1.transform(v1), v2))
 127     return false;
 128
 129   // Now check some successive transforms
 130   // We can reuse the prevously tested stuff
 131   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
 132   q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
 133   q3 = q1*q2;
 134   v2 = q2.transform(q1.transform(v1));
 135   if (!equivalent(q3.transform(v1), v2))
 136     return false;
 137
 138   /// Test from Euler angles
 139   float x = 0.2*SGMisc<T>::pi();
 140   float y = 0.3*SGMisc<T>::pi();
 141   float z = 0.4*SGMisc<T>::pi();
 142   q1 = SGQuat<T>::fromAngleAxis(z, e3);
 143   q2 = SGQuat<T>::fromAngleAxis(y, e2);
 144   q3 = SGQuat<T>::fromAngleAxis(x, e1);
 145   v2 = q3.transform(q2.transform(q1.transform(v1)));
 146   q4 = SGQuat<T>::fromEulerRad(z, y, x);
 147   if (!equivalent(q4.transform(v1), v2))
 148     return false;
 149
 150   /// Test angle axis forward and back transform
 151   q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
 152   q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
 153   q3 = q1*q2;
 154   SGVec3<T> angleAxis;
 155   q1.getAngleAxis(angleAxis);
 156   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
 157   if (!equivalent(q1, q4))
 158     return false;
 159   q2.getAngleAxis(angleAxis);
 160   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
 161   if (!equivalent(q2, q4))
 162     return false;
 163   q3.getAngleAxis(angleAxis);
 164   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
 165   if (!equivalent(q3, q4))
 166     return false;
 167
 168   return true;
 169 }
 170
 171 template<typename T>
 172 bool
 173 MatrixTest(void)
 174 {
 175   // Create some test matrix
 176   SGVec3<T> v0(2, 7, 17);
 177   SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
 178   SGMatrix<T> m0(q0, v0);
 179
 180   // Check the tqo forms of the inverse for that kind of special matrix
 181   SGMatrix<T> m1, m2;
 182   invert(m1, m0);
 183   m2 = transNeg(m0);
 184   if (!equivalent(m1, m2))
 185     return false;
 186
 187   // Check matrix multiplication and inversion
 188   if (!equivalent(m0*m1, SGMatrix<T>::unit()))
 189     return false;
 190   if (!equivalent(m1*m0, SGMatrix<T>::unit()))
 191     return false;
 192   if (!equivalent(m0*m2, SGMatrix<T>::unit()))
 193     return false;
 194   if (!equivalent(m2*m0, SGMatrix<T>::unit()))
 195     return false;
 196
 197   return true;
 198 }
 199
 200 bool
 201 GeodesyTest(void)
 202 {
 203   // We know that the values are on the order of 1
 204   double epsDeg = 10*SGLimits<double>::epsilon();
 205   // For the altitude values we need to tolerate relative errors in the order
 206   // of the radius
 207   double epsM = 1e6*SGLimits<double>::epsilon();
 208
 209   SGVec3<double> cart0, cart1;
 210   SGGeod geod0, geod1;
 211   SGGeoc geoc0;
 212
 213   // create some geodetic position
 214   geod0 = SGGeod::fromDegM(30, 20, 17);
 215
 216   // Test the conversion routines to cartesian coordinates
 217   cart0 = geod0;
 218   geod1 = cart0;
 219   if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
 220       epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
 221       epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
 222     return false;
 223
 224   // Test the conversion routines to radial coordinates
 225   geoc0 = cart0;
 226   cart1 = geoc0;
 227   if (!equivalent(cart0, cart1))
 228     return false;
 229
 230   return true;
 231 }
 232
 233
 234 bool
 235 sgInterfaceTest(void)
 236 {
 237   SGVec3f v3f = SGVec3f::e2();
 238   SGVec4f v4f = SGVec4f::e2();
 239   SGQuatf qf = SGQuatf::fromEulerRad(1.2, 1.3, -0.4);
 240   SGMatrixf mf(qf, v3f);
 241
 242   // Copy to and from plibs types check if result is equal,
 243   // test for exact equality
 244   SGVec3f tv3f;
 245   sgVec3 sv3f;
 246   sgCopyVec3(sv3f, v3f.sg());
 247   sgCopyVec3(tv3f.sg(), sv3f);
 248   if (tv3f != v3f)
 249     return false;
 250
 251   // Copy to and from plibs types check if result is equal,
 252   // test for exact equality
 253   SGVec4f tv4f;
 254   sgVec4 sv4f;
 255   sgCopyVec4(sv4f, v4f.sg());
 256   sgCopyVec4(tv4f.sg(), sv4f);
 257   if (tv4f != v4f)
 258     return false;
 259
 260   // Copy to and from plibs types check if result is equal,
 261   // test for exact equality
 262   SGQuatf tqf;
 263   sgQuat sqf;
 264   sgCopyQuat(sqf, qf.sg());
 265   sgCopyQuat(tqf.sg(), sqf);
 266   if (tqf != qf)
 267     return false;
 268
 269   // Copy to and from plibs types check if result is equal,
 270   // test for exact equality
 271   SGMatrixf tmf;
 272   sgMat4 smf;
 273   sgCopyMat4(smf, mf.sg());
 274   sgCopyMat4(tmf.sg(), smf);
 275   if (tmf != mf)
 276     return false;
 277
 278   return true;
 279 }
 280
 281 bool
 282 sgdInterfaceTest(void)
 283 {
 284   SGVec3d v3d = SGVec3d::e2();
 285   SGVec4d v4d = SGVec4d::e2();
 286   SGQuatd qd = SGQuatd::fromEulerRad(1.2, 1.3, -0.4);
 287   SGMatrixd md(qd, v3d);
 288
 289   // Copy to and from plibs types check if result is equal,
 290   // test for exact equality
 291   SGVec3d tv3d;
 292   sgdVec3 sv3d;
 293   sgdCopyVec3(sv3d, v3d.sg());
 294   sgdCopyVec3(tv3d.sg(), sv3d);
 295   if (tv3d != v3d)
 296     return false;
 297
 298   // Copy to and from plibs types check if result is equal,
 299   // test for exact equality
 300   SGVec4d tv4d;
 301   sgdVec4 sv4d;
 302   sgdCopyVec4(sv4d, v4d.sg());
 303   sgdCopyVec4(tv4d.sg(), sv4d);
 304   if (tv4d != v4d)
 305     return false;
 306
 307   // Copy to and from plibs types check if result is equal,
 308   // test for exact equality
 309   SGQuatd tqd;
 310   sgdQuat sqd;
 311   sgdCopyQuat(sqd, qd.sg());
 312   sgdCopyQuat(tqd.sg(), sqd);
 313   if (tqd != qd)
 314     return false;
 315
 316   // Copy to and from plibs types check if result is equal,
 317   // test for exact equality
 318   SGMatrixd tmd;
 319   sgdMat4 smd;
 320   sgdCopyMat4(smd, md.sg());
 321   sgdCopyMat4(tmd.sg(), smd);
 322   if (tmd != md)
 323     return false;
 324
 325   return true;
 326 }
 327
 328 int
 329 main(void)
 330 {
 331   // Do vector tests
 332   if (!Vec3Test<float>())
 333     return EXIT_FAILURE;
 334   if (!Vec3Test<double>())
 335     return EXIT_FAILURE;
 336
 337   // Do quaternion tests
 338   if (!QuatTest<float>())
 339     return EXIT_FAILURE;
 340   if (!QuatTest<double>())
 341     return EXIT_FAILURE;
 342
 343   // Do matrix tests
 344   if (!MatrixTest<float>())
 345     return EXIT_FAILURE;
 346   if (!MatrixTest<double>())
 347     return EXIT_FAILURE;
 348
 349   // Check geodetic/geocentric/cartesian conversions
 350 //   if (!GeodesyTest())
 351 //     return EXIT_FAILURE;
 352
 353   // Check interaction with sg*/sgd*
 354   if (!sgInterfaceTest())
 355     return EXIT_FAILURE;
 356   if (!sgdInterfaceTest())
 357     return EXIT_FAILURE;
 358
 359   std::cout << "Successfully passed all tests!" << std::endl;
 360   return EXIT_SUCCESS;
 361 }