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

diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx
index 415a4d59..ef1f5074 100644
--- a/simgear/math/SGMathTest.cxx
+++ b/simgear/math/SGMathTest.cxx
@@ -23,6 +23,7 @@
 #include <iostream>
 
 #include "SGMath.hxx"
+#include "sg_random.h"
 
 template<typename T>
 bool
@@ -198,6 +199,34 @@ QuatTest(void)
   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;
+}
+
 template<typename T>
 bool
 MatrixTest(void)
@@ -273,17 +302,17 @@ GeodesyTest(void)
   // 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;
-	
+
   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;
-	
+
   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;
@@ -298,6 +327,8 @@ GeodesyTest(void)
 int
 main(void)
 {
+  sg_srandom(17);
+
   // Do vector tests
   if (!Vec3Test<float>())
     return EXIT_FAILURE;
@@ -309,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>())