X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2FSGMathTest.cxx;h=ef1f50747953e244c3454a5e0731c3422a7887ce;hb=b99f53fda3b06378668bdccd4a9d07161f263366;hp=acbdcb52ae0a609a84738c0de043c90ec2ca1b88;hpb=de020ee69524393daf11200aa0a46bfd5aa2409a;p=simgear.git

diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx
index acbdcb52..ef1f5074 100644
--- a/simgear/math/SGMathTest.cxx
+++ b/simgear/math/SGMathTest.cxx
@@ -22,9 +22,8 @@
 #include <cstdlib>
 #include <iostream>
 
-#include <plib/sg.h>
-
 #include "SGMath.hxx"
+#include "sg_random.h"
 
 template<typename T>
 bool
@@ -79,6 +78,22 @@ Vec3Test(void)
   return true;
 }
 
+template<typename T>
+bool
+isSameRotation(const SGQuat<T>& q1, const SGQuat<T>& q2)
+{
+  const SGVec3<T> e1(1, 0, 0);
+  const SGVec3<T> e2(0, 1, 0);
+  const SGVec3<T> e3(0, 0, 1);
+  if (!equivalent(q1.transform(e1), q2.transform(e1)))
+    return false;
+  if (!equivalent(q1.transform(e2), q2.transform(e2)))
+    return false;
+  if (!equivalent(q1.transform(e3), q2.transform(e3)))
+    return false;
+  return true;
+}
+
 template<typename T>
 bool
 QuatTest(void)
@@ -94,7 +109,7 @@ QuatTest(void)
   v2 = SGVec3<T>(1, -2, -3);
   if (!equivalent(q1.transform(v1), v2))
     return false;
-  
+
   // Check a rotation around the x axis
   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e1);
   v2 = SGVec3<T>(1, 3, -2);
@@ -106,7 +121,7 @@ QuatTest(void)
   v2 = SGVec3<T>(-1, 2, -3);
   if (!equivalent(q1.transform(v1), v2))
     return false;
-  
+
   // Check a rotation around the y axis
   q1 = SGQuat<T>::fromAngleAxis(0.5*SGMisc<T>::pi(), e2);
   v2 = SGVec3<T>(-3, 2, 1);
@@ -153,17 +168,62 @@ QuatTest(void)
   SGVec3<T> angleAxis;
   q1.getAngleAxis(angleAxis);
   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
-  if (!equivalent(q1, q4))
+  if (!isSameRotation(q1, q4))
     return false;
   q2.getAngleAxis(angleAxis);
   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
-  if (!equivalent(q2, q4))
+  if (!isSameRotation(q2, q4))
     return false;
   q3.getAngleAxis(angleAxis);
   q4 = SGQuat<T>::fromAngleAxis(angleAxis);
-  if (!equivalent(q3, q4))
+  if (!isSameRotation(q3, q4))
+    return false;
+
+  /// Test angle axis forward and back transform
+  q1 = SGQuat<T>::fromAngleAxis(0.2*SGMisc<T>::pi(), e1);
+  q2 = SGQuat<T>::fromAngleAxis(1.7*SGMisc<T>::pi(), e2);
+  q3 = q1*q2;
+  SGVec3<T> positiveAngleAxis = q1.getPositiveRealImag();
+  q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
+  if (!isSameRotation(q1, q4))
+    return false;
+  positiveAngleAxis = q2.getPositiveRealImag();
+  q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
+  if (!isSameRotation(q2, q4))
     return false;
+  positiveAngleAxis = q3.getPositiveRealImag();
+  q4 = SGQuat<T>::fromPositiveRealImag(positiveAngleAxis);
+  if (!isSameRotation(q3, q4))
+    return false;
+
+  return true;
+}
 
+template<typename T>
+bool
+QuatDerivativeTest(void)
+{
+  for (unsigned i = 0; i < 100; ++i) {
+    // Generate the test case:
+    // Give a lower bound to the distance, so avoid testing cancelation
+    T dt = T(0.01) + sg_random();
+    // Start with orientation o0, angular velocity av and a random stepsize
+    SGQuat<T> o0 = SGQuat<T>::fromEulerDeg(T(360)*sg_random(), T(360)*sg_random(), T(360)*sg_random());
+    SGVec3<T> av(sg_random(), sg_random(), sg_random());
+    // Do one euler step and renormalize
+    SGQuat<T> o1 = normalize(o0 + dt*o0.derivative(av));
+
+    // Check if we can restore the angular velocity
+    SGVec3<T> av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
+    if (!equivalent(av, av2))
+      return false;
+
+    // Test with the equivalent orientation
+    o1 = -o1;
+    av2 = SGQuat<T>::forwardDifferenceVelocity(o0, o1, dt);
+    if (!equivalent(av, av2))
+      return false;
+  }
   return true;
 }
 
@@ -174,16 +234,22 @@ MatrixTest(void)
   // Create some test matrix
   SGVec3<T> v0(2, 7, 17);
   SGQuat<T> q0 = SGQuat<T>::fromAngleAxis(SGMisc<T>::pi(), normalize(v0));
-  SGMatrix<T> m0;
+  SGMatrix<T> m0 = SGMatrix<T>::unit();
   m0.postMultTranslate(v0);
   m0.postMultRotate(q0);
 
-  // Check the tqo forms of the inverse for that kind of special matrix
-  SGMatrix<T> m1, m2;
-  invert(m1, m0);
-  m2 = transNeg(m0);
+  // Check the three forms of the inverse for that kind of special matrix
+  SGMatrix<T> m1 = SGMatrix<T>::unit();
+  m1.preMultTranslate(-v0);
+  m1.preMultRotate(inverse(q0));
+
+  SGMatrix<T> m2, m3;
+  invert(m2, m0);
+  m3 = transNeg(m0);
   if (!equivalent(m1, m2))
     return false;
+  if (!equivalent(m2, m3))
+    return false;
 
   // Check matrix multiplication and inversion
   if (!equivalent(m0*m1, SGMatrix<T>::unit()))
@@ -194,7 +260,11 @@ MatrixTest(void)
     return false;
   if (!equivalent(m2*m0, SGMatrix<T>::unit()))
     return false;
-  
+  if (!equivalent(m0*m3, SGMatrix<T>::unit()))
+    return false;
+  if (!equivalent(m3*m0, SGMatrix<T>::unit()))
+    return false;
+
   return true;
 }
 
@@ -202,10 +272,10 @@ bool
 GeodesyTest(void)
 {
   // We know that the values are on the order of 1
-  double epsDeg = 10*SGLimits<double>::epsilon();
+  double epsDeg = 10*360*SGLimits<double>::epsilon();
   // For the altitude values we need to tolerate relative errors in the order
   // of the radius
-  double epsM = 1e6*SGLimits<double>::epsilon();
+  double epsM = 10*6e6*SGLimits<double>::epsilon();
 
   SGVec3<double> cart0, cart1;
   SGGeod geod0, geod1;
@@ -228,104 +298,28 @@ GeodesyTest(void)
   if (!equivalent(cart0, cart1))
     return false;
 
-  return true;
-}
+  // test course / advance routines
+  // uses examples from Williams aviation formulary
+  SGGeoc lax = SGGeoc::fromRadM(-2.066470, 0.592539, 10.0);
+  SGGeoc jfk = SGGeoc::fromRadM(-1.287762, 0.709186, 10.0);
 
+  double distNm = SGGeodesy::distanceRad(lax, jfk) * SG_RAD_TO_NM;
+  std::cout << "distance is " << distNm << std::endl;
+  if (0.5 < fabs(distNm - 2144)) // 2144 nm
+	return false;
 
-bool
-sgInterfaceTest(void)
-{
-  SGVec3f v3f = SGVec3f::e2();
-  SGVec4f v4f = SGVec4f::e2();
-  SGQuatf qf = SGQuatf::fromEulerRad(1.2, 1.3, -0.4);
-  SGMatrixf mf;
-  mf.postMultTranslate(v3f);
-  mf.postMultRotate(qf);
-
-  // Copy to and from plibs types check if result is equal,
-  // test for exact equality
-  SGVec3f tv3f;
-  sgVec3 sv3f;
-  sgCopyVec3(sv3f, v3f.sg());
-  sgCopyVec3(tv3f.sg(), sv3f);
-  if (tv3f != v3f)
-    return false;
-  
-  // Copy to and from plibs types check if result is equal,
-  // test for exact equality
-  SGVec4f tv4f;
-  sgVec4 sv4f;
-  sgCopyVec4(sv4f, v4f.sg());
-  sgCopyVec4(tv4f.sg(), sv4f);
-  if (tv4f != v4f)
-    return false;
-
-  // Copy to and from plibs types check if result is equal,
-  // test for exact equality
-  SGQuatf tqf;
-  sgQuat sqf;
-  sgCopyQuat(sqf, qf.sg());
-  sgCopyQuat(tqf.sg(), sqf);
-  if (tqf != qf)
-    return false;
-
-  // Copy to and from plibs types check if result is equal,
-  // test for exact equality
-  SGMatrixf tmf;
-  sgMat4 smf;
-  sgCopyMat4(smf, mf.sg());
-  sgCopyMat4(tmf.sg(), smf);
-  if (tmf != mf)
-    return false;
+  double crsDeg = SGGeodesy::courseRad(lax, jfk) * SG_RADIANS_TO_DEGREES;
+  std::cout << "course is " << crsDeg << std::endl;
+  if (0.5 < fabs(crsDeg - 66)) // 66 degrees
+	return false;
 
-  return true;
-}
+  SGGeoc adv;
+  SGGeodesy::advanceRadM(lax, crsDeg * SG_DEGREES_TO_RADIANS, 100 * SG_NM_TO_METER, adv);
+  std::cout << "lon:" << adv.getLongitudeRad() << ", lat:" << adv.getLatitudeRad() << std::endl;
 
-bool
-sgdInterfaceTest(void)
-{
-  SGVec3d v3d = SGVec3d::e2();
-  SGVec4d v4d = SGVec4d::e2();
-  SGQuatd qd = SGQuatd::fromEulerRad(1.2, 1.3, -0.4);
-  SGMatrixd md;
-  md.postMultTranslate(v3d);
-  md.postMultRotate(qd);
-
-  // Copy to and from plibs types check if result is equal,
-  // test for exact equality
-  SGVec3d tv3d;
-  sgdVec3 sv3d;
-  sgdCopyVec3(sv3d, v3d.sg());
-  sgdCopyVec3(tv3d.sg(), sv3d);
-  if (tv3d != v3d)
-    return false;
-  
-  // Copy to and from plibs types check if result is equal,
-  // test for exact equality
-  SGVec4d tv4d;
-  sgdVec4 sv4d;
-  sgdCopyVec4(sv4d, v4d.sg());
-  sgdCopyVec4(tv4d.sg(), sv4d);
-  if (tv4d != v4d)
-    return false;
-
-  // Copy to and from plibs types check if result is equal,
-  // test for exact equality
-  SGQuatd tqd;
-  sgdQuat sqd;
-  sgdCopyQuat(sqd, qd.sg());
-  sgdCopyQuat(tqd.sg(), sqd);
-  if (tqd != qd)
-    return false;
-
-  // Copy to and from plibs types check if result is equal,
-  // test for exact equality
-  SGMatrixd tmd;
-  sgdMat4 smd;
-  sgdCopyMat4(smd, md.sg());
-  sgdCopyMat4(tmd.sg(), smd);
-  if (tmd != md)
-    return false;
+  if (0.01 < fabs(adv.getLongitudeRad() - (-2.034206)) ||
+	  0.01 < fabs(adv.getLatitudeRad() - 0.604180))
+	return false;
 
   return true;
 }
@@ -333,6 +327,8 @@ sgdInterfaceTest(void)
 int
 main(void)
 {
+  sg_srandom(17);
+
   // Do vector tests
   if (!Vec3Test<float>())
     return EXIT_FAILURE;
@@ -344,6 +340,10 @@ main(void)
     return EXIT_FAILURE;
   if (!QuatTest<double>())
     return EXIT_FAILURE;
+  if (!QuatDerivativeTest<float>())
+    return EXIT_FAILURE;
+  if (!QuatDerivativeTest<double>())
+    return EXIT_FAILURE;
 
   // Do matrix tests
   if (!MatrixTest<float>())
@@ -355,12 +355,6 @@ main(void)
   if (!GeodesyTest())
     return EXIT_FAILURE;
 
-  // Check interaction with sg*/sgd*
-  if (!sgInterfaceTest())
-    return EXIT_FAILURE;
-  if (!sgdInterfaceTest())
-    return EXIT_FAILURE;
-  
   std::cout << "Successfully passed all tests!" << std::endl;
   return EXIT_SUCCESS;
 }