// 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 Library General Public
-// License along with this library; if not, write to the
-// Free Software Foundation, Inc., 59 Temple Place - Suite 330,
-// Boston, MA 02111-1307, USA.
+// 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 <simgear/misc/test_macros.hxx>
+
#include <cstdlib>
#include <iostream>
-#include <plib/sg.h>
-
#include "SGMath.hxx"
+#include "SGRect.hxx"
+#include "sg_random.h"
template<typename T>
bool
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);
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;
+}
+
template<typename T>
bool
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()))
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;
}
+template<typename T>
+void doRectTest()
+{
+ SGRect<T> rect(10, 15, 20, 25);
+
+ COMPARE(rect.x(), 10)
+ COMPARE(rect.y(), 15)
+ COMPARE(rect.width(), 20)
+ COMPARE(rect.height(), 25)
+
+ COMPARE(rect.pos(), SGVec2<T>(10, 15))
+ COMPARE(rect.size(), SGVec2<T>(20, 25))
+
+ COMPARE(rect.l(), 10)
+ COMPARE(rect.t(), 15)
+ COMPARE(rect.r(), 30)
+ COMPARE(rect.b(), 40)
+
+ VERIFY(rect == rect)
+ VERIFY(rect == SGRect<T>(10, 15, 20, 25))
+ VERIFY(rect != SGRect<T>(11, 15, 20, 25))
+
+ VERIFY(rect.contains(10, 15))
+ VERIFY(!rect.contains(9, 15))
+ VERIFY(rect.contains(9, 15, 1))
+}
+
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;
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;
-}
+ // 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(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;
-
- // 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(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;
+ 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;
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>())
if (!MatrixTest<double>())
return EXIT_FAILURE;
- // Check geodetic/geocentric/cartesian conversions
-// if (!GeodesyTest())
-// return EXIT_FAILURE;
+ // Do rect tests
+ doRectTest<int>();
+ doRectTest<double>();
- // Check interaction with sg*/sgd*
- if (!sgInterfaceTest())
- return EXIT_FAILURE;
- if (!sgdInterfaceTest())
+ // Check geodetic/geocentric/cartesian conversions
+ if (!GeodesyTest())
return EXIT_FAILURE;
-
+
std::cout << "Successfully passed all tests!" << std::endl;
return EXIT_SUCCESS;
}
projects / simgear.git / blobdiff