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