// 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;
SGVec3f v3f = SGVec3f::e2();
SGVec4f v4f = SGVec4f::e2();
SGQuatf qf = SGQuatf::fromEulerRad(1.2, 1.3, -0.4);
- SGMatrixf mf(qf, v3f);
+ SGMatrixf mf;
+ mf.postMultTranslate(v3f);
+ mf.postMultRotate(qf);
// Copy to and from plibs types check if result is equal,
// test for exact equality
SGVec3d v3d = SGVec3d::e2();
SGVec4d v4d = SGVec4d::e2();
SGQuatd qd = SGQuatd::fromEulerRad(1.2, 1.3, -0.4);
- SGMatrixd md(qd, v3d);
+ SGMatrixd md;
+ md.postMultTranslate(v3d);
+ md.postMultRotate(qd);
// Copy to and from plibs types check if result is equal,
// test for exact equality
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())