+// 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>
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)
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);
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);
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;
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;
// 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()))
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;
}
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;
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;
- return true;
-}
-
-
-bool
-sgInterfaceTest(void)
-{
- SGVec3f v3f = SGVec3f::e2();
- SGVec4f v4f = SGVec4f::e2();
- SGQuatf qf = SGQuatf::fromEuler(1.2, 1.3, -0.4);
- SGMatrixf mf(qf, v3f);
-
- // 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;
+ // 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);
- // 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;
-
- return true;
-}
-
-bool
-sgdInterfaceTest(void)
-{
- SGVec3d v3d = SGVec3d::e2();
- SGVec4d v4d = SGVec4d::e2();
- SGQuatd qd = SGQuatd::fromEuler(1.2, 1.3, -0.4);
- SGMatrixd md(qd, v3d);
-
- // 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;
+ 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;
}
return EXIT_FAILURE;
// Check geodetic/geocentric/cartesian conversions
-// if (!GeodesyTest())
-// return EXIT_FAILURE;
-
- // Check interaction with sg*/sgd*
- if (!sgInterfaceTest())
+ if (!GeodesyTest())
return EXIT_FAILURE;
- if (!sgdInterfaceTest())
- return EXIT_FAILURE;
-
+
std::cout << "Successfully passed all tests!" << std::endl;
return EXIT_SUCCESS;
}