Refactor Canvas and add some helpers.

[simgear.git] / simgear / math / SGMathTest.cxx

diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx
index 010f9995dfddf87df7a8d5174dbb8def8aa132cb..ef1f50747953e244c3454a5e0731c3422a7887ce 100644 (file)
--- 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
@@ -200,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)
@@ -271,12 +298,37 @@ GeodesyTest(void)
   if (!equivalent(cart0, cart1))
     return false;
 
+  // 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;
+
+  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;
+
+  if (0.01 < fabs(adv.getLongitudeRad() - (-2.034206)) ||
+         0.01 < fabs(adv.getLatitudeRad() - 0.604180))
+       return false;
+
   return true;
 }
 
 int
 main(void)
 {
+  sg_srandom(17);
+
   // Do vector tests
   if (!Vec3Test<float>())
     return EXIT_FAILURE;
@@ -288,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>())