simgear/math/SGMathTest.cxx

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