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 General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  16 //
  17
  18 #ifdef HAVE_CONFIG_H
  19 #  include <simgear_config.h>
  20 #endif
  21
  22 #include <simgear/misc/test_macros.hxx>
  23
  24 #include <cstdlib>
  25 #include <iostream>
  26
  27 #include "SGMath.hxx"
  28 #include "SGRect.hxx"
  29 #include "sg_random.h"
  30
  31 template<typename T>
  32 bool
  33 Vec3Test(void)
  34 {
  35   SGVec3<T> v1, v2, v3;
  36
  37   // Check if the equivalent function works
  38   v1 = SGVec3<T>(1, 2, 3);
  39   v2 = SGVec3<T>(3, 2, 1);
  40   if (equivalent(v1, v2))
  41     return false;
  42
  43   // Check the unary minus operator
  44   v3 = SGVec3<T>(-1, -2, -3);
  45   if (!equivalent(-v1, v3))
  46     return false;
  47
  48   // Check the unary plus operator
  49   v3 = SGVec3<T>(1, 2, 3);
  50   if (!equivalent(+v1, v3))
  51     return false;
  52
  53   // Check the addition operator
  54   v3 = SGVec3<T>(4, 4, 4);
  55   if (!equivalent(v1 + v2, v3))
  56     return false;
  57
  58   // Check the subtraction operator
  59   v3 = SGVec3<T>(-2, 0, 2);
  60   if (!equivalent(v1 - v2, v3))
  61     return false;
  62
  63   // Check the scaler multiplication operator
  64   v3 = SGVec3<T>(2, 4, 6);
  65   if (!equivalent(2*v1, v3))
  66     return false;
  67
  68   // Check the dot product
  69   if (fabs(dot(v1, v2) - 10) > 10*SGLimits<T>::epsilon())
  70     return false;
  71
  72   // Check the cross product
  73   v3 = SGVec3<T>(-4, 8, -4);
  74   if (!equivalent(cross(v1, v2), v3))
  75     return false;
  76
  77   // Check the euclidean length
  78   if (fabs(14 - length(v1)*length(v1)) > 14*SGLimits<T>::epsilon())
  79     return false;
  80
  81   return true;
  82 }
  83
  84 template<typename T>
  85 bool
  86 isSameRotation(const SGQuat<T>& q1, const SGQuat<T>& q2)
  87 {
  88   const SGVec3<T> e1(1, 0, 0);
  89   const SGVec3<T> e2(0, 1, 0);
  90   const SGVec3<T> e3(0, 0, 1);
  91   if (!equivalent(q1.transform(e1), q2.transform(e1)))
  92     return false;
  93   if (!equivalent(q1.transform(e2), q2.transform(e2)))
  94     return false;
  95   if (!equivalent(q1.transform(e3), q2.transform(e3)))
  96     return false;
  97   return true;
  98 }
  99
 100 template<typename T>
 101 bool
 102 QuatTest(void)
 103 {
 104   const SGVec3<T> e1(1, 0, 0);
 105   const SGVec3<T> e2(0, 1, 0);
 106   const SGVec3<T> e3(0, 0, 1);
 107   SGVec3<T> v1, v2;
 108   SGQuat<T> q1, q2, q3, q4;
 109   // Check a rotation around the x axis
 110   q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
 111   v1 = SGVec3<T>(1, 2, 3);
 112   v2 = SGVec3<T>(1, -2, -3);
 113   if (!equivalent(q1.transform(v1), v2))
 114     return false;
 115
 116   // Check a rotation around the x axis
 117   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
 118   v2 = SGVec3<T>(1, 3, -2);
 119   if (!equivalent(q1.transform(v1), v2))
 120     return false;
 121
 122   // Check a rotation around the y axis
 123   q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
 124   v2 = SGVec3<T>(-1, 2, -3);
 125   if (!equivalent(q1.transform(v1), v2))
 126     return false;
 127
 128   // Check a rotation around the y axis
 129   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
 130   v2 = SGVec3<T>(-3, 2, 1);
 131   if (!equivalent(q1.transform(v1), v2))
 132     return false;
 133
 134   // Check a rotation around the z axis
 135   q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
 136   v2 = SGVec3<T>(-1, -2, 3);
 137   if (!equivalent(q1.transform(v1), v2))
 138     return false;
 139
 140   // Check a rotation around the z axis
 141   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
 142   v2 = SGVec3<T>(2, -1, 3);
 143   if (!equivalent(q1.transform(v1), v2))
 144     return false;
 145
 146   // Now check some successive transforms
 147   // We can reuse the prevously tested stuff
 148   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
 149   q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
 150   q3 = q1*q2;
 151   v2 = q2.transform(q1.transform(v1));
 152   if (!equivalent(q3.transform(v1), v2))
 153     return false;
 154
 155   /// Test from Euler angles
 156   float x = 0.2*SGMisc<T>::pi();
 157   float y = 0.3*SGMisc<T>::pi();
 158   float z = 0.4*SGMisc<T>::pi();
 159   q1 = SGQuat<T>::fromAngleAxis(z, e3);
 160   q2 = SGQuat<T>::fromAngleAxis(y, e2);
 161   q3 = SGQuat<T>::fromAngleAxis(x, e1);
 162   v2 = q3.transform(q2.transform(q1.transform(v1)));
 163   q4 = SGQuat<T>::fromEulerRad(z, y, x);
 164   if (!equivalent(q4.transform(v1), v2))
 165     return false;
 166
 167   /// Test angle axis forward and back transform
 168   q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
 169   q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
 170   q3 = q1*q2;
 171   SGVec3<T> angleAxis;
 172   q1.getAngleAxis(angleAxis);
 173   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
 174   if (!isSameRotation(q1, q4))
 175     return false;
 176   q2.getAngleAxis(angleAxis);
 177   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
 178   if (!isSameRotation(q2, q4))
 179     return false;
 180   q3.getAngleAxis(angleAxis);
 181   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
 182   if (!isSameRotation(q3, q4))
 183     return false;
 184
 185   /// Test angle axis forward and back transform
 186   q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
 187   q2 = SGQuat<T>::fromAngleAxis(1.7*SGMisc<T>::pi(), e2);
 188   q3 = q1*q2;
 189   SGVec3<T> positiveAngleAxis = q1.getPositiveRealImag();
 190   q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
 191   if (!isSameRotation(q1, q4))
 192     return false;
 193   positiveAngleAxis = q2.getPositiveRealImag();
 194   q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
 195   if (!isSameRotation(q2, q4))
 196     return false;
 197   positiveAngleAxis = q3.getPositiveRealImag();
 198   q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
 199   if (!isSameRotation(q3, q4))
 200     return false;
 201
 202   return true;
 203 }
 204
 205 template<typename T>
 206 bool
 207 QuatDerivativeTest(void)
 208 {
 209   for (unsigned i = 0; i < 100; ++i) {
 210     // Generate the test case:
 211     // Give a lower bound to the distance, so avoid testing cancelation
 212     T dt = T(0.01) + sg_random();
 213     // Start with orientation o0, angular velocity av and a random stepsize
 214     SGQuat<T> o0 = SGQuat<T>::fromEulerDeg(T(360)*sg_random(), T(360)*sg_random(), T(360)*sg_random());
 215     SGVec3<T> av(sg_random(), sg_random(), sg_random());
 216     // Do one euler step and renormalize
 217     SGQuat<T> o1 = normalize(o0 + dt*o0.derivative(av));
 218
 219     // Check if we can restore the angular velocity
 220     SGVec3<T> av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
 221     if (!equivalent(av, av2))
 222       return false;
 223
 224     // Test with the equivalent orientation
 225     o1 = -o1;
 226     av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
 227     if (!equivalent(av, av2))
 228       return false;
 229   }
 230   return true;
 231 }
 232
 233 template<typename T>
 234 bool
 235 MatrixTest(void)
 236 {
 237   // Create some test matrix
 238   SGVec3<T> v0(2, 7, 17);
 239   SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
 240   SGMatrix<T> m0 = SGMatrix<T>::unit();
 241   m0.postMultTranslate(v0);
 242   m0.postMultRotate(q0);
 243
 244   // Check the three forms of the inverse for that kind of special matrix
 245   SGMatrix<T> m1 = SGMatrix<T>::unit();
 246   m1.preMultTranslate(-v0);
 247   m1.preMultRotate(inverse(q0));
 248
 249   SGMatrix<T> m2, m3;
 250   invert(m2, m0);
 251   m3 = transNeg(m0);
 252   if (!equivalent(m1, m2))
 253     return false;
 254   if (!equivalent(m2, m3))
 255     return false;
 256
 257   // Check matrix multiplication and inversion
 258   if (!equivalent(m0*m1, SGMatrix<T>::unit()))
 259     return false;
 260   if (!equivalent(m1*m0, SGMatrix<T>::unit()))
 261     return false;
 262   if (!equivalent(m0*m2, SGMatrix<T>::unit()))
 263     return false;
 264   if (!equivalent(m2*m0, SGMatrix<T>::unit()))
 265     return false;
 266   if (!equivalent(m0*m3, SGMatrix<T>::unit()))
 267     return false;
 268   if (!equivalent(m3*m0, SGMatrix<T>::unit()))
 269     return false;
 270
 271   return true;
 272 }
 273
 274 template<typename T>
 275 void doRectTest()
 276 {
 277   SGRect<T> rect(10, 15, 20, 25);
 278
 279   COMPARE(rect.x(), 10)
 280   COMPARE(rect.y(), 15)
 281   COMPARE(rect.width(), 20)
 282   COMPARE(rect.height(), 25)
 283
 284   COMPARE(rect.pos(), SGVec2<T>(10, 15))
 285   COMPARE(rect.size(), SGVec2<T>(20, 25))
 286
 287   COMPARE(rect.l(), 10)
 288   COMPARE(rect.t(), 15)
 289   COMPARE(rect.r(), 30)
 290   COMPARE(rect.b(), 40)
 291
 292   VERIFY(rect == rect)
 293   VERIFY(rect == SGRect<T>(10, 15, 20, 25))
 294   VERIFY(rect != SGRect<T>(11, 15, 20, 25))
 295
 296   VERIFY(rect.contains(10, 15))
 297   VERIFY(!rect.contains(9, 15))
 298   VERIFY(rect.contains(9, 15, 1))
 299 }
 300
 301 bool
 302 GeodesyTest(void)
 303 {
 304   // We know that the values are on the order of 1
 305   double epsDeg = 10*360*SGLimits<double>::epsilon();
 306   // For the altitude values we need to tolerate relative errors in the order
 307   // of the radius
 308   double epsM = 10*6e6*SGLimits<double>::epsilon();
 309
 310   SGVec3<double> cart0, cart1;
 311   SGGeod geod0, geod1;
 312   SGGeoc geoc0;
 313
 314   // create some geodetic position
 315   geod0 = SGGeod::fromDegM(30, 20, 17);
 316
 317   // Test the conversion routines to cartesian coordinates
 318   cart0 = SGVec3<double>::fromGeod(geod0);
 319   geod1 = SGGeod::fromCart(cart0);
 320   if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
 321       epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
 322       epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
 323     return false;
 324
 325   // Test the conversion routines to radial coordinates
 326   geoc0 = SGGeoc::fromCart(cart0);
 327   cart1 = SGVec3<double>::fromGeoc(geoc0);
 328   if (!equivalent(cart0, cart1))
 329     return false;
 330
 331   // test course / advance routines
 332   // uses examples from Williams aviation formulary
 333   SGGeoc lax = SGGeoc::fromRadM(-2.066470, 0.592539, 10.0);
 334   SGGeoc jfk = SGGeoc::fromRadM(-1.287762, 0.709186, 10.0);
 335
 336   double distNm = SGGeodesy::distanceRad(lax, jfk) * SG_RAD_TO_NM;
 337   std::cout << "distance is " << distNm << std::endl;
 338   if (0.5 < fabs(distNm - 2144)) // 2144 nm
 339         return false;
 340
 341   double crsDeg = SGGeodesy::courseRad(lax, jfk) * SG_RADIANS_TO_DEGREES;
 342   std::cout << "course is " << crsDeg << std::endl;
 343   if (0.5 < fabs(crsDeg - 66)) // 66 degrees
 344         return false;
 345
 346   SGGeoc adv;
 347   SGGeodesy::advanceRadM(lax, crsDeg * SG_DEGREES_TO_RADIANS, 100 * SG_NM_TO_METER, adv);
 348   std::cout << "lon:" << adv.getLongitudeRad() << ", lat:" << adv.getLatitudeRad() << std::endl;
 349
 350   if (0.01 < fabs(adv.getLongitudeRad() - (-2.034206)) ||
 351           0.01 < fabs(adv.getLatitudeRad() - 0.604180))
 352         return false;
 353
 354   return true;
 355 }
 356
 357 int
 358 main(void)
 359 {
 360   sg_srandom(17);
 361
 362   // Do vector tests
 363   if (!Vec3Test<float>())
 364     return EXIT_FAILURE;
 365   if (!Vec3Test<double>())
 366     return EXIT_FAILURE;
 367
 368   // Do quaternion tests
 369   if (!QuatTest<float>())
 370     return EXIT_FAILURE;
 371   if (!QuatTest<double>())
 372     return EXIT_FAILURE;
 373   if (!QuatDerivativeTest<float>())
 374     return EXIT_FAILURE;
 375   if (!QuatDerivativeTest<double>())
 376     return EXIT_FAILURE;
 377
 378   // Do matrix tests
 379   if (!MatrixTest<float>())
 380     return EXIT_FAILURE;
 381   if (!MatrixTest<double>())
 382     return EXIT_FAILURE;
 383
 384   // Do rect tests
 385   doRectTest<int>();
 386   doRectTest<double>();
 387
 388   // Check geodetic/geocentric/cartesian conversions
 389   if (!GeodesyTest())
 390     return EXIT_FAILURE;
 391
 392   std::cout << "Successfully passed all tests!" << std::endl;
 393   return EXIT_SUCCESS;
 394 }