X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=simgear%2Fmath%2FSGQuat.hxx;h=1f6aaef004a24d3800bb267793f50615e24e518d;hb=8e5e65734478a5bd4058f3d9828edd371309570e;hp=3dc0889402e20979e912f8037061d3d1cbc64fb2;hpb=9be14a63b1b593b02b5cc3e82edb7a266ae3cbe7;p=simgear.git diff --git a/simgear/math/SGQuat.hxx b/simgear/math/SGQuat.hxx index 3dc08894..1f6aaef0 100644 --- a/simgear/math/SGQuat.hxx +++ b/simgear/math/SGQuat.hxx @@ -10,18 +10,58 @@ // 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. // #ifndef SGQuat_H #define SGQuat_H +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif + +#include + +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 { +class SGQuat : protected SGQuatStorage { public: typedef T value_type; @@ -34,7 +74,7 @@ public: /// uninitialized values in the debug build very fast ... #ifndef NDEBUG for (unsigned i = 0; i < 4; ++i) - _data[i] = SGLimits::quiet_NaN(); + data()[i] = SGLimits::quiet_NaN(); #endif } /// Constructor. Initialize by the given values @@ -43,11 +83,13 @@ public: /// Constructor. Initialize by the content of a plain array, /// 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]; } + { 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) - { return fromRealImag(1, SGVec3(0)); } + { return fromRealImag(1, SGVec3(0, 0, 0)); } /// Return a quaternion from euler angles static SGQuat fromEulerRad(T z, T y, T x) @@ -88,13 +130,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); @@ -105,11 +151,50 @@ 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)); } + /// Like the above provided for convenience + static SGQuat fromLonLat(const SGGeod& geod) + { return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); } + + /// Return a quaternion rotation from the earth centered to the + /// OpenGL/viewer horizontal local frame from given longitude and latitude. + /// This frame matches the usual OpenGL axis directions. That is the target + /// frame has an x-axis pointing eastwards, y-axis pointing up and y z-axis + /// pointing south. + static SGQuat viewHLRad(T lon, T lat) + { + // That bails down to a 3-2-1 euler sequence lon+pi/2, 0, -lat-pi + // what is here is again the hand optimized version ... + SGQuat q; + T xd2 = -T(0.5)*lat - T(0.5)*SGMisc::pi(); + T zd2 = T(0.5)*lon + T(0.25)*SGMisc::pi(); + T Szd2 = sin(zd2); + T Sxd2 = sin(xd2); + T Czd2 = cos(zd2); + T Cxd2 = cos(xd2); + q.w() = Cxd2*Czd2; + q.x() = Sxd2*Czd2; + q.y() = Sxd2*Szd2; + q.z() = Cxd2*Szd2; + return q; + } + /// Like the above provided for convenience + static SGQuat viewHLDeg(T lon, T lat) + { return viewHLRad(SGMisc::deg2rad(lon), SGMisc::deg2rad(lat)); } + /// Like the above provided for convenience + static SGQuat viewHL(const SGGeod& geod) + { return viewHLRad(geod.getLongitudeRad(), geod.getLatitudeRad()); } + + /// Convert a quaternion rotation from the simulation frame + /// to the view (OpenGL) frame. That is it just swaps the axis part of + /// this current quaternion. + /// That proves useful when you want to use the euler 3-2-1 sequence + /// for the usual heading/pitch/roll sequence within the context of + /// OpenGL/viewer frames. + static SGQuat simToView(const SGQuat& q) + { return SGQuat(q.y(), -q.z(), -q.x(), q.w()); } /// Create a quaternion from the angle axis representation static SGQuat fromAngleAxis(T angle, const SGVec3& axis) @@ -128,19 +213,105 @@ public: { T nAxis = norm(axis); if (nAxis <= SGLimits::min()) - return SGQuat(1, 0, 0, 0); + return SGQuat::unit(); T angle2 = 0.5*nAxis; return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis); } + static SGQuat fromRotateTo(const SGVec3& from, const SGVec3& to) + { + T nfrom = norm(from); + T nto = norm(to); + if (nfrom < SGLimits::min() || nto < SGLimits::min()) + return SGQuat::unit(); + + return SGQuat::fromRotateToNorm((1/nfrom)*from, (1/nto)*to); + } + + // FIXME more finegrained error behavour. + 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()) + 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()) + return SGQuat::unit(); + + // The target vector for the first rotation + SGVec3 nto1 = SGVec3::zeros(); + SGVec3 nto2 = SGVec3::zeros(); + nto1[i1] = 1; + nto2[i2] = 1; + + // The first rotation can be done with the usual routine. + SGQuat q = SGQuat::fromRotateToNorm(nv1, nto1); + + // The rotation axis for the second rotation is the + // target for the first one, so the rotation axis is nto1 + // We need to get the angle. + + // Make nv2 exactly orthogonal to nv1. + nv2 = normalize(nv2 - dv1v2*nv1); + + SGVec3 tnv2 = q.transform(nv2); + T cosang = dot(nto2, tnv2); + T cos05ang = T(0.5+0.5*cosang); + if (cos05ang <= 0) + cosang = T(0); + cos05ang = sqrt(cos05ang); + T sig = dot(nto1, cross(nto2, tnv2)); + T sin05ang = T(0.5-0.5*cosang); + if (sin05ang <= 0) + sin05ang = 0; + sin05ang = copysign(sqrt(sin05ang), sig); + q *= SGQuat::fromRealImag(cos05ang, sin05ang*nto1); + + return q; + } + + + // Return a quaternion which rotates the vector given by v + // to the vector -v. Other directions are *not* preserved. + static SGQuat fromChangeSign(const SGVec3& v) + { + // The vector from points to the oposite direction than to. + // Find a vector perpendicular to the vector to. + 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); + axis = (1/sqrt(1+quot*quot))*SGVec3(quot, -1, 0); + } else if (absv1 < absv2 && absv3 < absv2) { + T quot = v(2)/v(1); + axis = (1/sqrt(1+quot*quot))*SGVec3(0, quot, -1); + } else if (absv1 < absv3 && absv2 < absv3) { + T quot = v(0)/v(2); + axis = (1/sqrt(1+quot*quot))*SGVec3(-1, 0, quot); + } else { + // The all zero case. + return SGQuat::unit(); + } + + return SGQuat::fromRealImag(0, axis); + } + /// Return a quaternion from real and imaginary part static SGQuat fromRealImag(T r, const SGVec3& i) { SGQuat q; q.w() = r; - q.x() = i(0); - q.y() = i(1); - q.z() = i(2); + q.x() = i.x(); + q.y() = i.y(); + q.z() = i.z(); return q; } @@ -151,36 +322,36 @@ public: /// write the euler angles into the references void getEulerRad(T& zRad, T& yRad, T& xRad) const { - value_type sqrQW = w()*w(); - value_type sqrQX = x()*x(); - value_type sqrQY = y()*y(); - value_type sqrQZ = z()*z(); - - value_type num = 2*(y()*z() + w()*x()); - value_type den = sqrQW - sqrQX - sqrQY + sqrQZ; - if (fabs(den) < SGLimits::min() && - fabs(num) < SGLimits::min()) + T sqrQW = w()*w(); + T sqrQX = x()*x(); + T sqrQY = y()*y(); + T sqrQZ = z()*z(); + + T num = 2*(y()*z() + w()*x()); + T den = sqrQW - sqrQX - sqrQY + sqrQZ; + if (fabs(den) < SGLimits::min() && + fabs(num) < SGLimits::min()) xRad = 0; else xRad = atan2(num, den); - value_type tmp = 2*(x()*z() - w()*y()); + T tmp = 2*(x()*z() - w()*y()); if (tmp < -1) - yRad = 0.5*SGMisc::pi(); + yRad = 0.5*SGMisc::pi(); else if (1 < tmp) - yRad = -0.5*SGMisc::pi(); + yRad = -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 { - value_type psi = atan2(num, den); + T psi = atan2(num, den); if (psi < 0) - psi += 2*SGMisc::pi(); + psi += 2*SGMisc::pi(); zRad = psi; } } @@ -223,67 +394,66 @@ public: /// Access by index, the index is unchecked const T& operator()(unsigned i) const - { return _data[i]; } + { return data()[i]; } /// Access by index, the index is unchecked T& operator()(unsigned i) - { return _data[i]; } + { return data()[i]; } /// Access raw data by index, the index is unchecked const T& operator[](unsigned i) const - { return _data[i]; } + { return data()[i]; } /// Access raw data by index, the index is unchecked T& operator[](unsigned i) - { return _data[i]; } + { return data()[i]; } /// Access the x component const T& x(void) const - { return _data[0]; } + { return data()[0]; } /// Access the x component T& x(void) - { return _data[0]; } + { return data()[0]; } /// Access the y component const T& y(void) const - { return _data[1]; } + { return data()[1]; } /// Access the y component T& y(void) - { return _data[1]; } + { return data()[1]; } /// Access the z component const T& z(void) const - { return _data[2]; } + { return data()[2]; } /// Access the z component T& z(void) - { return _data[2]; } + { return data()[2]; } /// Access the w component const T& w(void) const - { return _data[3]; } + { return data()[3]; } /// Access the w component T& w(void) - { return _data[3]; } + { return data()[3]; } - /// Get the data pointer, usefull for interfacing with plib's sg*Vec - const T* data(void) const - { return _data; } - /// Get the data pointer, usefull for interfacing with plib's sg*Vec - T* data(void) - { return _data; } + /// Get the data pointer + using SGQuatStorage::data; /// Readonly interface function to ssg's sgQuat/sgdQuat const T (&sg(void) const)[4] - { return _data; } + { return data(); } /// Interface function to ssg's sgQuat/sgdQuat T (&sg(void))[4] - { return _data; } + { return data(); } + + /// Interface function to osg's Quat* + using SGQuatStorage::osg; /// Inplace addition SGQuat& operator+=(const SGQuat& v) - { _data[0]+=v(0);_data[1]+=v(1);_data[2]+=v(2);_data[3]+=v(3);return *this; } + { data()[0]+=v(0);data()[1]+=v(1);data()[2]+=v(2);data()[3]+=v(3);return *this; } /// Inplace subtraction SGQuat& operator-=(const SGQuat& v) - { _data[0]-=v(0);_data[1]-=v(1);_data[2]-=v(2);_data[3]-=v(3);return *this; } + { data()[0]-=v(0);data()[1]-=v(1);data()[2]-=v(2);data()[3]-=v(3);return *this; } /// Inplace scalar multiplication template SGQuat& operator*=(S s) - { _data[0] *= s; _data[1] *= s; _data[2] *= s; _data[3] *= s; return *this; } + { data()[0] *= s; data()[1] *= s; data()[2] *= s; data()[3] *= s; return *this; } /// Inplace scalar multiplication by 1/s template SGQuat& operator/=(S s) @@ -295,18 +465,18 @@ public: /// frame rotated with the quaternion SGVec3 transform(const SGVec3& v) const { - value_type r = 2/dot(*this, *this); + T r = 2/dot(*this, *this); SGVec3 qimag = imag(*this); - value_type qr = real(*this); + T qr = real(*this); return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag - (r*qr)*cross(qimag, v); } /// Transform a vector from the coordinate frame rotated with the quaternion /// to the current coordinate frame SGVec3 backTransform(const SGVec3& v) const { - value_type r = 2/dot(*this, *this); + T r = 2/dot(*this, *this); SGVec3 qimag = imag(*this); - value_type qr = real(*this); + T qr = real(*this); return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag + (r*qr)*cross(qimag, v); } @@ -319,7 +489,7 @@ public: /// Return the time derivative of the quaternion given the angular velocity SGQuat - derivative(const SGVec3& angVel) + derivative(const SGVec3& angVel) const { SGQuat deriv; @@ -332,8 +502,65 @@ public: } private: - /// The actual data - T _data[4]; + + // Private because it assumes normalized inputs. + static SGQuat + fromRotateToSmaller90Deg(T cosang, + const SGVec3& from, const SGVec3& to) + { + // In this function we assume that the angle required to rotate from + // the vector from to the vector to is <= 90 deg. + // That is done so because of possible instabilities when we rotate more + // then 90deg. + + // Note that the next comment does actually cover a *more* *general* case + // than we need in this function. That shows that this formula is even + // valid for rotations up to 180deg. + + // Because of the signs in the axis, it is sufficient to care for angles + // 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); + + + // 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 + // sqrt(0.5) < cos05ang. + + + // Compute the rotation axis, that is + // sin(angle)*normalized rotation axis + SGVec3 axis = cross(to, from); + + // We need sin(0.5*angle)*normalized rotation axis. + // So rescale with sin(0.5*x)/sin(x). + // To do that we use the equation: + // sin(x) = 2*sin(0.5*x)*cos(0.5*x) + return SGQuat::fromRealImag( cos05ang, (1/(2*cos05ang))*axis); + } + + // Private because it assumes normalized inputs. + static SGQuat + fromRotateToNorm(const SGVec3& from, const SGVec3& to) + { + // To avoid instabilities with roundoff, we distinguish between rotations + // with more then 90deg and rotations with less than 90deg. + + // Compute the cosine of the angle. + T cosang = dot(from, to); + + // For the small ones do direct computation + if (T(-0.5) < cosang) + return SGQuat::fromRotateToSmaller90Deg(cosang, from, to); + + // For larger rotations. first rotate from to -from. + // Past that we will have a smaller angle again. + SGQuat q1 = SGQuat::fromChangeSign(from); + SGQuat q2 = SGQuat::fromRotateToSmaller90Deg(-cosang, -from, to); + return q1*q2; + } }; /// Unary +, do nothing ... @@ -553,10 +780,6 @@ std::basic_ostream& operator<<(std::basic_ostream& s, const SGQuat& v) { return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; } -/// Two classes doing actually the same on different types -typedef SGQuat SGQuatf; -typedef SGQuat SGQuatd; - inline SGQuatf toQuatf(const SGQuatd& v)