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 QuatTest(void)
  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   SGVec3<T> v1, v2;
  90   SGQuat<T> q1, q2, q3, q4;
  91   // Check a rotation around the x axis
  92   q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e1);
  93   v1 = SGVec3<T>(1, 2, 3);
  94   v2 = SGVec3<T>(1, -2, -3);
  95   if (!equivalent(q1.transform(v1), v2))
  96     return false;
  97
  98   // Check a rotation around the x axis
  99   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
 100   v2 = SGVec3<T>(1, 3, -2);
 101   if (!equivalent(q1.transform(v1), v2))
 102     return false;
 103
 104   // Check a rotation around the y axis
 105   q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e2);
 106   v2 = SGVec3<T>(-1, 2, -3);
 107   if (!equivalent(q1.transform(v1), v2))
 108     return false;
 109
 110   // Check a rotation around the y axis
 111   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
 112   v2 = SGVec3<T>(-3, 2, 1);
 113   if (!equivalent(q1.transform(v1), v2))
 114     return false;
 115
 116   // Check a rotation around the z axis
 117   q1 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), e3);
 118   v2 = SGVec3<T>(-1, -2, 3);
 119   if (!equivalent(q1.transform(v1), v2))
 120     return false;
 121
 122   // Check a rotation around the z axis
 123   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e3);
 124   v2 = SGVec3<T>(2, -1, 3);
 125   if (!equivalent(q1.transform(v1), v2))
 126     return false;
 127
 128   // Now check some successive transforms
 129   // We can reuse the prevously tested stuff
 130   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
 131   q2 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
 132   q3 = q1*q2;
 133   v2 = q2.transform(q1.transform(v1));
 134   if (!equivalent(q3.transform(v1), v2))
 135     return false;
 136
 137   /// Test from Euler angles
 138   float x = 0.2*SGMisc<T>::pi();
 139   float y = 0.3*SGMisc<T>::pi();
 140   float z = 0.4*SGMisc<T>::pi();
 141   q1 = SGQuat<T>::fromAngleAxis(z, e3);
 142   q2 = SGQuat<T>::fromAngleAxis(y, e2);
 143   q3 = SGQuat<T>::fromAngleAxis(x, e1);
 144   v2 = q3.transform(q2.transform(q1.transform(v1)));
 145   q4 = SGQuat<T>::fromEulerRad(z, y, x);
 146   if (!equivalent(q4.transform(v1), v2))
 147     return false;
 148
 149   /// Test angle axis forward and back transform
 150   q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
 151   q2 = SGQuat<T>::fromAngleAxis(0.7*SGMisc<T>::pi(), e2);
 152   q3 = q1*q2;
 153   SGVec3<T> angleAxis;
 154   q1.getAngleAxis(angleAxis);
 155   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
 156   if (!equivalent(q1, q4))
 157     return false;
 158   q2.getAngleAxis(angleAxis);
 159   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
 160   if (!equivalent(q2, q4))
 161     return false;
 162   q3.getAngleAxis(angleAxis);
 163   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
 164   if (!equivalent(q3, q4))
 165     return false;
 166
 167   return true;
 168 }
 169
 170 template<typename T>
 171 bool
 172 MatrixTest(void)
 173 {
 174   // Create some test matrix
 175   SGVec3<T> v0(2, 7, 17);
 176   SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
 177   SGMatrix<T> m0 = SGMatrix<T>::unit();
 178   m0.postMultTranslate(v0);
 179   m0.postMultRotate(q0);
 180
 181   // Check the three forms of the inverse for that kind of special matrix
 182   SGMatrix<T> m1 = SGMatrix<T>::unit();
 183   m1.preMultTranslate(-v0);
 184   m1.preMultRotate(inverse(q0));
 185
 186   SGMatrix<T> m2, m3;
 187   invert(m2, m0);
 188   m3 = transNeg(m0);
 189   if (!equivalent(m1, m2))
 190     return false;
 191   if (!equivalent(m2, m3))
 192     return false;
 193
 194   // Check matrix multiplication and inversion
 195   if (!equivalent(m0*m1, SGMatrix<T>::unit()))
 196     return false;
 197   if (!equivalent(m1*m0, SGMatrix<T>::unit()))
 198     return false;
 199   if (!equivalent(m0*m2, SGMatrix<T>::unit()))
 200     return false;
 201   if (!equivalent(m2*m0, SGMatrix<T>::unit()))
 202     return false;
 203   if (!equivalent(m0*m3, SGMatrix<T>::unit()))
 204     return false;
 205   if (!equivalent(m3*m0, SGMatrix<T>::unit()))
 206     return false;
 207
 208   return true;
 209 }
 210
 211 bool
 212 GeodesyTest(void)
 213 {
 214   // We know that the values are on the order of 1
 215   double epsDeg = 10*360*SGLimits<double>::epsilon();
 216   // For the altitude values we need to tolerate relative errors in the order
 217   // of the radius
 218   double epsM = 10*6e6*SGLimits<double>::epsilon();
 219
 220   SGVec3<double> cart0, cart1;
 221   SGGeod geod0, geod1;
 222   SGGeoc geoc0;
 223
 224   // create some geodetic position
 225   geod0 = SGGeod::fromDegM(30, 20, 17);
 226
 227   // Test the conversion routines to cartesian coordinates
 228   cart0 = SGVec3<double>::fromGeod(geod0);
 229   geod1 = SGGeod::fromCart(cart0);
 230   if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
 231       epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
 232       epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
 233     return false;
 234
 235   // Test the conversion routines to radial coordinates
 236   geoc0 = SGGeoc::fromCart(cart0);
 237   cart1 = SGVec3<double>::fromGeoc(geoc0);
 238   if (!equivalent(cart0, cart1))
 239     return false;
 240
 241   return true;
 242 }
 243
 244
 245 bool
 246 sgInterfaceTest(void)
 247 {
 248   SGVec3f v3f = SGVec3f::e2();
 249   SGVec4f v4f = SGVec4f::e2();
 250   SGQuatf qf = SGQuatf::fromEulerRad(1.2, 1.3, -0.4);
 251   SGMatrixf mf;
 252   mf.postMultTranslate(v3f);
 253   mf.postMultRotate(qf);
 254
 255   // Copy to and from plibs types check if result is equal,
 256   // test for exact equality
 257   SGVec3f tv3f;
 258   sgVec3 sv3f;
 259   sgCopyVec3(sv3f, v3f.sg());
 260   sgCopyVec3(tv3f.sg(), sv3f);
 261   if (tv3f != v3f)
 262     return false;
 263
 264   // Copy to and from plibs types check if result is equal,
 265   // test for exact equality
 266   SGVec4f tv4f;
 267   sgVec4 sv4f;
 268   sgCopyVec4(sv4f, v4f.sg());
 269   sgCopyVec4(tv4f.sg(), sv4f);
 270   if (tv4f != v4f)
 271     return false;
 272
 273   // Copy to and from plibs types check if result is equal,
 274   // test for exact equality
 275   SGQuatf tqf;
 276   sgQuat sqf;
 277   sgCopyQuat(sqf, qf.sg());
 278   sgCopyQuat(tqf.sg(), sqf);
 279   if (tqf != qf)
 280     return false;
 281
 282   // Copy to and from plibs types check if result is equal,
 283   // test for exact equality
 284   SGMatrixf tmf;
 285   sgMat4 smf;
 286   sgCopyMat4(smf, mf.sg());
 287   sgCopyMat4(tmf.sg(), smf);
 288   if (tmf != mf)
 289     return false;
 290
 291   return true;
 292 }
 293
 294 bool
 295 sgdInterfaceTest(void)
 296 {
 297   SGVec3d v3d = SGVec3d::e2();
 298   SGVec4d v4d = SGVec4d::e2();
 299   SGQuatd qd = SGQuatd::fromEulerRad(1.2, 1.3, -0.4);
 300   SGMatrixd md;
 301   md.postMultTranslate(v3d);
 302   md.postMultRotate(qd);
 303
 304   // Copy to and from plibs types check if result is equal,
 305   // test for exact equality
 306   SGVec3d tv3d;
 307   sgdVec3 sv3d;
 308   sgdCopyVec3(sv3d, v3d.sg());
 309   sgdCopyVec3(tv3d.sg(), sv3d);
 310   if (tv3d != v3d)
 311     return false;
 312
 313   // Copy to and from plibs types check if result is equal,
 314   // test for exact equality
 315   SGVec4d tv4d;
 316   sgdVec4 sv4d;
 317   sgdCopyVec4(sv4d, v4d.sg());
 318   sgdCopyVec4(tv4d.sg(), sv4d);
 319   if (tv4d != v4d)
 320     return false;
 321
 322   // Copy to and from plibs types check if result is equal,
 323   // test for exact equality
 324   SGQuatd tqd;
 325   sgdQuat sqd;
 326   sgdCopyQuat(sqd, qd.sg());
 327   sgdCopyQuat(tqd.sg(), sqd);
 328   if (tqd != qd)
 329     return false;
 330
 331   // Copy to and from plibs types check if result is equal,
 332   // test for exact equality
 333   SGMatrixd tmd;
 334   sgdMat4 smd;
 335   sgdCopyMat4(smd, md.sg());
 336   sgdCopyMat4(tmd.sg(), smd);
 337   if (tmd != md)
 338     return false;
 339
 340   return true;
 341 }
 342
 343 int
 344 main(void)
 345 {
 346   // Do vector tests
 347   if (!Vec3Test<float>())
 348     return EXIT_FAILURE;
 349   if (!Vec3Test<double>())
 350     return EXIT_FAILURE;
 351
 352   // Do quaternion tests
 353   if (!QuatTest<float>())
 354     return EXIT_FAILURE;
 355   if (!QuatTest<double>())
 356     return EXIT_FAILURE;
 357
 358   // Do matrix tests
 359   if (!MatrixTest<float>())
 360     return EXIT_FAILURE;
 361   if (!MatrixTest<double>())
 362     return EXIT_FAILURE;
 363
 364   // Check geodetic/geocentric/cartesian conversions
 365   if (!GeodesyTest())
 366     return EXIT_FAILURE;
 367
 368   // Check interaction with sg*/sgd*
 369   if (!sgInterfaceTest())
 370     return EXIT_FAILURE;
 371   if (!sgdInterfaceTest())
 372     return EXIT_FAILURE;
 373
 374   std::cout << "Successfully passed all tests!" << std::endl;
 375   return EXIT_SUCCESS;
 376 }