simgear/math/SGMathTest.cxx

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