From 594095a3634202f625d264e590448fcadc9d2a49 Mon Sep 17 00:00:00 2001 From: Mathias Froehlich Date: Wed, 31 Aug 2011 23:44:06 +0200 Subject: [PATCH] Introduce quaternion finite difference method. This implements a function for the quaternion implementation that computes the angular velocity that matches an explicit euler step that propagates from a starting quaternion orientation to a destination quaternion orientation. --- simgear/math/SGMathTest.cxx | 41 +++++++++++++++++++++++++++--- simgear/math/SGQuat.hxx | 50 +++++++++++++++++++++++++++++++------ 2 files changed, 81 insertions(+), 10 deletions(-) diff --git a/simgear/math/SGMathTest.cxx b/simgear/math/SGMathTest.cxx index 415a4d59..ef1f5074 100644 --- a/simgear/math/SGMathTest.cxx +++ b/simgear/math/SGMathTest.cxx @@ -23,6 +23,7 @@ #include #include "SGMath.hxx" +#include "sg_random.h" template bool @@ -198,6 +199,34 @@ QuatTest(void) return true; } +template +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 o0 = SGQuat::fromEulerDeg(T(360)*sg_random(), T(360)*sg_random(), T(360)*sg_random()); + SGVec3 av(sg_random(), sg_random(), sg_random()); + // Do one euler step and renormalize + SGQuat o1 = normalize(o0 + dt*o0.derivative(av)); + + // Check if we can restore the angular velocity + SGVec3 av2 = SGQuat::forwardDifferenceVelocity(o0, o1, dt); + if (!equivalent(av, av2)) + return false; + + // Test with the equivalent orientation + o1 = -o1; + av2 = SGQuat::forwardDifferenceVelocity(o0, o1, dt); + if (!equivalent(av, av2)) + return false; + } + return true; +} + template bool MatrixTest(void) @@ -273,17 +302,17 @@ GeodesyTest(void) // 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; @@ -298,6 +327,8 @@ GeodesyTest(void) int main(void) { + sg_srandom(17); + // Do vector tests if (!Vec3Test()) return EXIT_FAILURE; @@ -309,6 +340,10 @@ main(void) return EXIT_FAILURE; if (!QuatTest()) return EXIT_FAILURE; + if (!QuatDerivativeTest()) + return EXIT_FAILURE; + if (!QuatDerivativeTest()) + return EXIT_FAILURE; // Do matrix tests if (!MatrixTest()) diff --git a/simgear/math/SGQuat.hxx b/simgear/math/SGQuat.hxx index 256ddb28..9449c914 100644 --- a/simgear/math/SGQuat.hxx +++ b/simgear/math/SGQuat.hxx @@ -230,7 +230,7 @@ public: T absv1 = fabs(v(0)); T absv2 = fabs(v(1)); T absv3 = fabs(v(2)); - + SGVec3 axis; if (absv2 < absv1 && absv3 < absv1) { T quot = v(1)/v(0); @@ -279,7 +279,7 @@ public: xRad = 0; else xRad = atan2(num, den); - + T tmp = 2*(x()*z() - w()*y()); if (tmp <= -1) yRad = T(0.5)*SGMisc::pi(); @@ -287,8 +287,8 @@ public: yRad = -T(0.5)*SGMisc::pi(); else yRad = -asin(tmp); - - num = 2*(x()*y() + w()*z()); + + num = 2*(x()*y() + w()*z()); den = sqrQW + sqrQX - sqrQY - sqrQZ; if (fabs(den) <= SGLimits::min() && fabs(num) <= SGLimits::min()) @@ -323,7 +323,7 @@ public: T sAng = sin(angle); if (fabs(sAng) <= SGLimits::min()) axis = SGVec3(1, 0, 0); - else + else axis = (rNrm/sAng)*imag(*this); angle *= 2; } @@ -449,10 +449,46 @@ public: deriv.x() = T(0.5)*( w()*angVel(0) - z()*angVel(1) + y()*angVel(2)); deriv.y() = T(0.5)*( z()*angVel(0) + w()*angVel(1) - x()*angVel(2)); deriv.z() = T(0.5)*(-y()*angVel(0) + x()*angVel(1) + w()*angVel(2)); - + return deriv; } + /// Return the angular velocity w that makes q0 translate to q1 using + /// an explicit euler step with stepsize h. + /// That is, look for an w where + /// q1 = normalize(q0 + h*q0.derivative(w)) + static SGVec3 + forwardDifferenceVelocity(const SGQuat& q0, const SGQuat& q1, const T& h) + { + // Let D_q0*w = q0.derivative(w), D_q0 the above 4x3 matrix. + // Then D_q0^t*D_q0 = 0.25*Id and D_q0*q0 = 0. + // Let lambda be a nonzero normailzation factor, then + // q1 = normalize(q0 + h*q0.derivative(w)) + // can be rewritten + // lambda*q1 = q0 + h*D_q0*w. + // Multiply left by the transpose D_q0^t and reorder gives + // 4*lambda/h*D_q0^t*q1 = w. + // Now compute lambda by substitution of w into the original + // equation + // lambda*q1 = q0 + 4*lambda*D_q0*D_q0^t*q1, + // multiply by q1^t from the left + // lambda* = + 4*lambda* + // and solving for lambda gives + // lambda = /(1 - 4*). + + // The transpose of the derivative matrix + // the 0.5 factor is handled below + // also note that the initializer uses x, y, z, w instead of w, x, y, z + SGQuat d0(q0.w(), q0.z(), -q0.y(), -q0.x()); + SGQuat d1(-q0.z(), q0.w(), q0.x(), -q0.y()); + SGQuat d2(q0.y(), -q0.x(), q0.w(), -q0.z()); + // 2*D_q0^t*q1 + SGVec3 Dq(dot(d0, q1), dot(d1, q1), dot(d2, q1)); + // Like above, but take into account that Dq = 2*D_q0^t*q1 + T lambda = dot(q0, q1)/(T(1) - dot(Dq, Dq)); + return (2*lambda/h)*Dq; + } + private: // Private because it assumes normalized inputs. @@ -478,7 +514,7 @@ private: // Now our assumption of angles <= 90 deg comes in play. // For that reason, we know that cos05ang is not zero. - // It is even more, we can see from the above formula that + // It is even more, we can see from the above formula that // sqrt(0.5) < cos05ang. -- 2.39.5