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