X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2FSGQuat.hxx;h=a48cbf1ab08a17c2deb8a0c35316c271248c2c00;hb=578af00b0d48100c540154f54293a1b77a0655fe;hp=b65de8dc6dc790acef8c9d2c86841f15580e795d;hpb=e0b26872315ab2036beb4332fdfa61f244a47956;p=simgear.git diff --git a/simgear/math/SGQuat.hxx b/simgear/math/SGQuat.hxx index b65de8dc..a48cbf1a 100644 --- a/simgear/math/SGQuat.hxx +++ b/simgear/math/SGQuat.hxx @@ -1,4 +1,4 @@ -// Copyright (C) 2006 Mathias Froehlich - Mathias.Froehlich@web.de +// Copyright (C) 2006-2009 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 @@ -26,42 +26,13 @@ #undef max #endif +#ifndef NO_OPENSCENEGRAPH_INTERFACE #include +#endif +/// Quaternion Class template -struct SGQuatStorage { - /// Readonly raw storage interface - const T (&data(void) const)[4] - { return _data; } - /// Readonly raw storage interface - T (&data(void))[4] - { return _data; } - - void osg() const - { } - -private: - T _data[4]; -}; - -template<> -struct SGQuatStorage : public osg::Quat { - /// Access raw data by index, the index is unchecked - const double (&data(void) const)[4] - { return osg::Quat::_v; } - /// Access raw data by index, the index is unchecked - double (&data(void))[4] - { return osg::Quat::_v; } - - const osg::Quat& osg() const - { return *this; } - osg::Quat& osg() - { return *this; } -}; - -/// 3D Vector Class -template -class SGQuat : protected SGQuatStorage { +class SGQuat { public: typedef T value_type; @@ -84,8 +55,6 @@ public: /// make sure it has at least 4 elements explicit SGQuat(const T* d) { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; } - explicit SGQuat(const osg::Quat& d) - { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; } /// Return a unit quaternion static SGQuat unit(void) @@ -130,13 +99,17 @@ public: static SGQuat fromHeadAttBankDeg(T h, T a, T b) { return fromEulerDeg(h, a, b); } - /// Return a quaternion rotation the the horizontal local frame from given - /// longitude and latitude + /// Return a quaternion rotation from the earth centered to the + /// simulation usual horizontal local frame from given + /// longitude and latitude. + /// The horizontal local frame used in simulations is the frame with x-axis + /// pointing north, the y-axis pointing eastwards and the z axis + /// pointing downwards. static SGQuat fromLonLatRad(T lon, T lat) { SGQuat q; T zd2 = T(0.5)*lon; - T yd2 = T(-0.25)*SGMisc::pi() - T(0.5)*lat; + T yd2 = T(-0.25)*SGMisc::pi() - T(0.5)*lat; T Szd2 = sin(zd2); T Syd2 = sin(yd2); T Czd2 = cos(zd2); @@ -147,21 +120,18 @@ public: q.z() = Szd2*Cyd2; return q; } - - /// Return a quaternion rotation the the horizontal local frame from given - /// longitude and latitude + /// Like the above provided for convenience static SGQuat fromLonLatDeg(T lon, T lat) { return fromLonLatRad(SGMisc::deg2rad(lon), SGMisc::deg2rad(lat)); } - - /// Return a quaternion rotation the the horizontal local frame from given - /// longitude and latitude + /// Like the above provided for convenience static SGQuat fromLonLat(const SGGeod& geod) { return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); } + /// Create a quaternion from the angle axis representation static SGQuat fromAngleAxis(T angle, const SGVec3& axis) { - T angle2 = 0.5*angle; + T angle2 = T(0.5)*angle; return fromRealImag(cos(angle2), T(sin(angle2))*axis); } @@ -175,34 +145,36 @@ public: { T nAxis = norm(axis); if (nAxis <= SGLimits::min()) - return SGQuat(1, 0, 0, 0); - T angle2 = 0.5*nAxis; + return SGQuat::unit(); + T angle2 = T(0.5)*nAxis; return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis); } + /// Return a quaternion that rotates the from vector onto the to vector. static SGQuat fromRotateTo(const SGVec3& from, const SGVec3& to) { T nfrom = norm(from); T nto = norm(to); - if (nfrom < SGLimits::min() || nto < SGLimits::min()) + if (nfrom <= SGLimits::min() || nto <= SGLimits::min()) return SGQuat::unit(); return SGQuat::fromRotateToNorm((1/nfrom)*from, (1/nto)*to); } - // FIXME more finegrained error behavour. + /// Return a quaternion that rotates v1 onto the i1-th unit vector + /// and v2 into a plane that is spanned by the i2-th and i1-th unit vector. static SGQuat fromRotateTo(const SGVec3& v1, unsigned i1, const SGVec3& v2, unsigned i2) { T nrmv1 = norm(v1); T nrmv2 = norm(v2); - if (nrmv1 < SGLimits::min() || nrmv2 < SGLimits::min()) + if (nrmv1 <= SGLimits::min() || nrmv2 <= SGLimits::min()) return SGQuat::unit(); SGVec3 nv1 = (1/nrmv1)*v1; SGVec3 nv2 = (1/nrmv2)*v2; T dv1v2 = dot(nv1, nv2); - if (fabs(fabs(dv1v2)-1) < SGLimits::epsilon()) + if (fabs(fabs(dv1v2)-1) <= SGLimits::epsilon()) return SGQuat::unit(); // The target vector for the first rotation @@ -223,12 +195,12 @@ public: SGVec3 tnv2 = q.transform(nv2); T cosang = dot(nto2, tnv2); - T cos05ang = T(0.5+0.5*cosang); + T cos05ang = T(0.5)+T(0.5)*cosang; if (cos05ang <= 0) - cosang = T(0); + cosang = 0; cos05ang = sqrt(cos05ang); T sig = dot(nto1, cross(nto2, tnv2)); - T sin05ang = T(0.5-0.5*cosang); + T sin05ang = T(0.5)-T(0.5)*cosang; if (sin05ang <= 0) sin05ang = 0; sin05ang = copysign(sqrt(sin05ang), sig); @@ -243,7 +215,7 @@ public: static SGQuat fromChangeSign(const SGVec3& v) { // The vector from points to the oposite direction than to. - // Find a vector perpandicular to the vector to. + // Find a vector perpendicular to the vector to. T absv1 = fabs(v(0)); T absv2 = fabs(v(1)); T absv3 = fabs(v(2)); @@ -291,24 +263,24 @@ public: T num = 2*(y()*z() + w()*x()); T den = sqrQW - sqrQX - sqrQY + sqrQZ; - if (fabs(den) < SGLimits::min() && - fabs(num) < SGLimits::min()) + if (fabs(den) <= SGLimits::min() && + fabs(num) <= SGLimits::min()) xRad = 0; else xRad = atan2(num, den); T tmp = 2*(x()*z() - w()*y()); - if (tmp < -1) - yRad = 0.5*SGMisc::pi(); - else if (1 < tmp) - yRad = -0.5*SGMisc::pi(); + if (tmp <= -1) + yRad = T(0.5)*SGMisc::pi(); + else if (1 <= tmp) + yRad = -T(0.5)*SGMisc::pi(); else yRad = -asin(tmp); num = 2*(x()*y() + w()*z()); den = sqrQW + sqrQX - sqrQY - sqrQZ; - if (fabs(den) < SGLimits::min() && - fabs(num) < SGLimits::min()) + if (fabs(den) <= SGLimits::min() && + fabs(num) <= SGLimits::min()) zRad = 0; else { T psi = atan2(num, den); @@ -331,14 +303,14 @@ public: void getAngleAxis(T& angle, SGVec3& axis) const { T nrm = norm(*this); - if (nrm < SGLimits::min()) { + if (nrm <= SGLimits::min()) { angle = 0; axis = SGVec3(0, 0, 0); } else { T rNrm = 1/nrm; angle = acos(SGMisc::max(-1, SGMisc::min(1, rNrm*w()))); T sAng = sin(angle); - if (fabs(sAng) < SGLimits::min()) + if (fabs(sAng) <= SGLimits::min()) axis = SGVec3(1, 0, 0); else axis = (rNrm/sAng)*imag(*this); @@ -394,17 +366,11 @@ public: { return data()[3]; } /// Get the data pointer - using SGQuatStorage::data; - - /// Readonly interface function to ssg's sgQuat/sgdQuat - const T (&sg(void) const)[4] - { return data(); } - /// Interface function to ssg's sgQuat/sgdQuat - T (&sg(void))[4] - { return data(); } - - /// Interface function to osg's Quat* - using SGQuatStorage::osg; + const T (&data(void) const)[4] + { return _data; } + /// Get the data pointer + T (&data(void))[4] + { return _data; } /// Inplace addition SGQuat& operator+=(const SGQuat& v) @@ -451,14 +417,14 @@ public: /// Return the time derivative of the quaternion given the angular velocity SGQuat - derivative(const SGVec3& angVel) + derivative(const SGVec3& angVel) const { SGQuat deriv; - deriv.w() = 0.5*(-x()*angVel(0) - y()*angVel(1) - z()*angVel(2)); - deriv.x() = 0.5*( w()*angVel(0) - z()*angVel(1) + y()*angVel(2)); - deriv.y() = 0.5*( z()*angVel(0) + w()*angVel(1) - x()*angVel(2)); - deriv.z() = 0.5*(-y()*angVel(0) + x()*angVel(1) + w()*angVel(2)); + deriv.w() = T(0.5)*(-x()*angVel(0) - y()*angVel(1) - z()*angVel(2)); + 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; } @@ -483,7 +449,7 @@ private: // in the interval [-pi,pi]. That means that 0.5*angle is in the interval // [-pi/2,pi/2]. But in that range the cosine is allways >= 0. // So we do not need to care for egative roots in the following equation: - T cos05ang = sqrt(0.5+0.5*cosang); + T cos05ang = sqrt(T(0.5)+T(0.5)*cosang); // Now our assumption of angles <= 90 deg comes in play. @@ -523,6 +489,8 @@ private: SGQuat q2 = SGQuat::fromRotateToSmaller90Deg(-cosang, -from, to); return q1*q2; } + + T _data[4]; }; /// Unary +, do nothing ... @@ -721,7 +689,7 @@ interpolate(T t, const SGQuat& src, const SGQuat& dst) // need the scales now, if the angle is very small, do linear interpolation // to avoid instabilities T scale0, scale1; - if (fabs(o) < SGLimits::epsilon()) { + if (fabs(o) <= SGLimits::epsilon()) { scale0 = 1 - t; scale1 = t; } else { @@ -752,4 +720,16 @@ SGQuatd toQuatd(const SGQuatf& v) { return SGQuatd(v(0), v(1), v(2), v(3)); } +#ifndef NO_OPENSCENEGRAPH_INTERFACE +inline +SGQuatd +toSG(const osg::Quat& q) +{ return SGQuatd(q[0], q[1], q[2], q[3]); } + +inline +osg::Quat +toOsg(const SGQuatd& q) +{ return osg::Quat(q[0], q[1], q[2], q[3]); } +#endif + #endif