Allow geocentric distance computations to return radians.

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

diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx
index 4ab0763bbda8bb29830d063b3bf01ce566e75b99..aeedc7a9629fafe5c3ccc44e4222825c3fa430eb 100644 (file)
--- a/simgear/math/SGMathTest.cxx
+++ b/simgear/math/SGMathTest.cxx
@@ -1,3 +1,24 @@
+// Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
+//
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Library General Public
+// License as published by the Free Software Foundation; either
+// version 2 of the License, or (at your option) any later version.
+//
+// This library is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// Library General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+//
+
+#ifdef HAVE_CONFIG_H
+#  include <simgear_config.h>
+#endif
+
 #include <cstdlib>
 #include <iostream>
 
@@ -121,7 +142,7 @@ QuatTest(void)
   q2 = SGQuat<T>::fromAngleAxis(y, e2);
   q3 = SGQuat<T>::fromAngleAxis(x, e1);
   v2 = q3.transform(q2.transform(q1.transform(v1)));
-  q4 = SGQuat<T>::fromEuler(z, y, x);
+  q4 = SGQuat<T>::fromEulerRad(z, y, x);
   if (!equivalent(q4.transform(v1), v2))
     return false;
 
@@ -153,14 +174,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(q0, v0);
-
-  // Check the tqo forms of the inverse for that kind of special matrix
-  SGMatrix<T> m1, m2;
-  invert(m1, m0);
-  m2 = transNeg(m0);
+  SGMatrix<T> m0 = SGMatrix<T>::unit();
+  m0.postMultTranslate(v0);
+  m0.postMultRotate(q0);
+
+  // 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()))
@@ -171,6 +200,10 @@ 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;
 }
@@ -179,10 +212,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;
@@ -192,16 +225,16 @@ GeodesyTest(void)
   geod0 = SGGeod::fromDegM(30, 20, 17);
 
   // Test the conversion routines to cartesian coordinates
-  cart0 = geod0;
-  geod1 = cart0;
+  cart0 = SGVec3<double>::fromGeod(geod0);
+  geod1 = SGGeod::fromCart(cart0);
   if (epsDeg < fabs(geod0.getLongitudeDeg() - geod1.getLongitudeDeg()) ||
       epsDeg < fabs(geod0.getLatitudeDeg() - geod1.getLatitudeDeg()) ||
       epsM < fabs(geod0.getElevationM() - geod1.getElevationM()))
     return false;
 
   // Test the conversion routines to radial coordinates
-  geoc0 = cart0;
-  cart1 = geoc0;
+  geoc0 = SGGeoc::fromCart(cart0);
+  cart1 = SGVec3<double>::fromGeoc(geoc0);
   if (!equivalent(cart0, cart1))
     return false;
 
@@ -214,8 +247,10 @@ sgInterfaceTest(void)
 {
   SGVec3f v3f = SGVec3f::e2();
   SGVec4f v4f = SGVec4f::e2();
-  SGQuatf qf = SGQuatf::fromEuler(1.2, 1.3, -0.4);
-  SGMatrixf mf(qf, v3f);
+  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
@@ -261,8 +296,10 @@ sgdInterfaceTest(void)
 {
   SGVec3d v3d = SGVec3d::e2();
   SGVec4d v4d = SGVec4d::e2();
-  SGQuatd qd = SGQuatd::fromEuler(1.2, 1.3, -0.4);
-  SGMatrixd md(qd, v3d);
+  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
@@ -325,8 +362,8 @@ main(void)
     return EXIT_FAILURE;
 
   // Check geodetic/geocentric/cartesian conversions
-//   if (!GeodesyTest())
-//     return EXIT_FAILURE;
+  if (!GeodesyTest())
+    return EXIT_FAILURE;
 
   // Check interaction with sg*/sgd*
   if (!sgInterfaceTest())