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