From: frohlich Date: Wed, 1 Nov 2006 21:25:21 +0000 (+0000) Subject: Modified Files: X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=5127e2f89c915f3441c4ba1a777d94c344f33e7e;p=simgear.git Modified Files: Makefile.am SGMath.hxx SGMathFwd.hxx SGMatrix.hxx SGQuat.hxx Added Files: SGVec2.hxx Improove the matrix functions. Improove the quaterion functions. Add the 2d vector. --- diff --git a/simgear/math/Makefile.am b/simgear/math/Makefile.am index e20fbc57..016620ee 100644 --- a/simgear/math/Makefile.am +++ b/simgear/math/Makefile.am @@ -30,7 +30,8 @@ include_HEADERS = \ SGMisc.hxx \ SGQuat.hxx \ SGVec4.hxx \ - SGVec3.hxx + SGVec3.hxx \ + SGVec2.hxx libsgmath_a_SOURCES = \ diff --git a/simgear/math/SGMath.hxx b/simgear/math/SGMath.hxx index 1bd807dd..e33fe3ac 100644 --- a/simgear/math/SGMath.hxx +++ b/simgear/math/SGMath.hxx @@ -28,6 +28,7 @@ #include "SGLimits.hxx" #include "SGMisc.hxx" #include "SGGeodesy.hxx" +#include "SGVec2.hxx" #include "SGVec3.hxx" #include "SGVec4.hxx" #include "SGGeoc.hxx" diff --git a/simgear/math/SGMathFwd.hxx b/simgear/math/SGMathFwd.hxx index 0bbee1f8..5fe19954 100644 --- a/simgear/math/SGMathFwd.hxx +++ b/simgear/math/SGMathFwd.hxx @@ -32,6 +32,8 @@ class SGMisc; template class SGQuat; template +class SGVec2; +template class SGVec3; template class SGVec4; @@ -44,6 +46,8 @@ typedef SGMisc SGMiscf; typedef SGMisc SGMiscd; typedef SGQuat SGQuatf; typedef SGQuat SGQuatd; +typedef SGVec2 SGVec2f; +typedef SGVec2 SGVec2d; typedef SGVec3 SGVec3f; typedef SGVec3 SGVec3d; typedef SGVec4 SGVec4f; diff --git a/simgear/math/SGMatrix.hxx b/simgear/math/SGMatrix.hxx index ed146038..e91140d2 100644 --- a/simgear/math/SGMatrix.hxx +++ b/simgear/math/SGMatrix.hxx @@ -63,14 +63,13 @@ public: } /// Constructor, build up a SGMatrix from a translation - SGMatrix(const SGVec3& trans) + template + SGMatrix(const SGVec3& trans) { set(trans); } /// Constructor, build up a SGMatrix from a rotation and a translation - SGMatrix(const SGQuat& quat, const SGVec3& trans) - { set(quat, trans); } - /// Constructor, build up a SGMatrix from a rotation and a translation - SGMatrix(const SGQuat& quat) + template + SGMatrix(const SGQuat& quat) { set(quat); } /// Copy constructor for a transposed negated matrix @@ -78,39 +77,22 @@ public: { set(tm); } /// Set from a tranlation - void set(const SGVec3& trans) + template + void set(const SGVec3& trans) { _data.flat[0] = 1; _data.flat[4] = 0; - _data.flat[8] = 0; _data.flat[12] = -trans(0); + _data.flat[8] = 0; _data.flat[12] = T(trans(0)); _data.flat[1] = 0; _data.flat[5] = 1; - _data.flat[9] = 0; _data.flat[13] = -trans(1); + _data.flat[9] = 0; _data.flat[13] = T(trans(1)); _data.flat[2] = 0; _data.flat[6] = 0; - _data.flat[10] = 1; _data.flat[14] = -trans(2); + _data.flat[10] = 1; _data.flat[14] = T(trans(2)); _data.flat[3] = 0; _data.flat[7] = 0; _data.flat[11] = 0; _data.flat[15] = 1; } /// Set from a scale/rotation and tranlation - void set(const SGQuat& quat, const SGVec3& trans) - { - T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z(); - T xx = x*x; T yy = y*y; T zz = z*z; - T wx = w*x; T wy = w*y; T wz = w*z; - T xy = x*y; T xz = x*z; T yz = y*z; - _data.flat[0] = 1-2*(yy+zz); _data.flat[1] = 2*(xy-wz); - _data.flat[2] = 2*(xz+wy); _data.flat[3] = 0; - _data.flat[4] = 2*(xy+wz); _data.flat[5] = 1-2*(xx+zz); - _data.flat[6] = 2*(yz-wx); _data.flat[7] = 0; - _data.flat[8] = 2*(xz-wy); _data.flat[9] = 2*(yz+wx); - _data.flat[10] = 1-2*(xx+yy); _data.flat[11] = 0; - // Well, this one is ugly here, as that xform method on the current - // object needs the above data to be already set ... - SGVec3 t = xformVec(trans); - _data.flat[12] = -t(0); _data.flat[13] = -t(1); - _data.flat[14] = -t(2); _data.flat[15] = 1; - } - /// Set from a scale/rotation and tranlation - void set(const SGQuat& quat) + template + void set(const SGQuat& quat) { T w = quat.w(); T x = quat.x(); T y = quat.y(); T z = quat.z(); T xx = x*x; T yy = y*y; T zz = z*z; @@ -199,6 +181,45 @@ public: /// Inplace matrix multiplication, post multiply SGMatrix& operator*=(const SGMatrix& m2); + template + SGMatrix& preMultTranslate(const SGVec3& t) + { + for (unsigned i = 0; i < SGMatrix::nCols-1; ++i) + (*this)(i,3) += T(t(i)); + return *this; + } + template + SGMatrix& postMultTranslate(const SGVec3& t) + { + SGVec4 col3((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3)); + for (unsigned i = 0; i < SGMatrix::nCols-1; ++i) { + SGVec4 tmp((*this)(0,3), (*this)(1,3), (*this)(2,3), (*this)(3,3)); + col3 += T(t(i))*tmp; + } + (*this)(0,3) = col3(0); (*this)(1,3) = col3(1); + (*this)(2,3) = col3(2); (*this)(3,3) = col3(3); + return *this; + } + + SGMatrix& preMultRotate(const SGQuat& r) + { + for (unsigned i = 0; i < SGMatrix::nCols; ++i) { + SGVec3 col((*this)(0,i), (*this)(1,i), (*this)(2,i)); + col = r.transform(col); + (*this)(0,i) = col(0); (*this)(1,i) = col(1); (*this)(2,i) = col(2); + } + return *this; + } + SGMatrix& postMultRotate(const SGQuat& r) + { + for (unsigned i = 0; i < SGMatrix::nCols; ++i) { + SGVec3 col((*this)(i,0), (*this)(i,1), (*this)(i,2)); + col = r.backTransform(col); + (*this)(i,0) = col(0); (*this)(i,1) = col(1); (*this)(i,2) = col(2); + } + return *this; + } + SGVec3 xformPt(const SGVec3& pt) const { SGVec3 tpt; diff --git a/simgear/math/SGQuat.hxx b/simgear/math/SGQuat.hxx index 82a46173..b65de8dc 100644 --- a/simgear/math/SGQuat.hxx +++ b/simgear/math/SGQuat.hxx @@ -180,14 +180,100 @@ public: 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 perpandicular 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; } @@ -198,36 +284,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; } } @@ -341,18 +427,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); } @@ -376,6 +462,67 @@ public: return deriv; } + +private: + + // 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 ... diff --git a/simgear/math/SGVec2.hxx b/simgear/math/SGVec2.hxx new file mode 100644 index 00000000..2b539b84 --- /dev/null +++ b/simgear/math/SGVec2.hxx @@ -0,0 +1,322 @@ +// Copyright (C) 2006 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 +// License as published by the Free Software Foundation; either +// version 2 of the License, or (at your option) any later version. +// +// This library is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// 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 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 SGVec2_H +#define SGVec2_H + +#include +#include + +template +struct SGVec2Storage { + /// Readonly raw storage interface + const T (&data(void) const)[2] + { return _data; } + /// Readonly raw storage interface + T (&data(void))[2] + { return _data; } + + void osg() const + { } + +private: + T _data[2]; +}; + +template<> +struct SGVec2Storage : public osg::Vec2f { + /// Access raw data by index, the index is unchecked + const float (&data(void) const)[2] + { return osg::Vec2f::_v; } + /// Access raw data by index, the index is unchecked + float (&data(void))[2] + { return osg::Vec2f::_v; } + + const osg::Vec2f& osg() const + { return *this; } + osg::Vec2f& osg() + { return *this; } +}; + +template<> +struct SGVec2Storage : public osg::Vec2d { + /// Access raw data by index, the index is unchecked + const double (&data(void) const)[2] + { return osg::Vec2d::_v; } + /// Access raw data by index, the index is unchecked + double (&data(void))[2] + { return osg::Vec2d::_v; } + + const osg::Vec2d& osg() const + { return *this; } + osg::Vec2d& osg() + { return *this; } +}; + +/// 2D Vector Class +template +class SGVec2 : protected SGVec2Storage { +public: + typedef T value_type; + + /// Default constructor. Does not initialize at all. + /// If you need them zero initialized, use SGVec2::zeros() + SGVec2(void) + { + /// Initialize with nans in the debug build, that will guarantee to have + /// a fast uninitialized default constructor in the release but shows up + /// uninitialized values in the debug build very fast ... +#ifndef NDEBUG + for (unsigned i = 0; i < 2; ++i) + data()[i] = SGLimits::quiet_NaN(); +#endif + } + /// Constructor. Initialize by the given values + SGVec2(T x, T y) + { data()[0] = x; data()[1] = y; } + /// Constructor. Initialize by the content of a plain array, + /// make sure it has at least 2 elements + explicit SGVec2(const T* d) + { data()[0] = d[0]; data()[1] = d[1]; } + explicit SGVec2(const osg::Vec2f& d) + { data()[0] = d[0]; data()[1] = d[1]; } + explicit SGVec2(const osg::Vec2d& d) + { data()[0] = d[0]; data()[1] = d[1]; } + + /// Access by index, the index is unchecked + const T& operator()(unsigned i) const + { return data()[i]; } + /// Access by index, the index is unchecked + T& operator()(unsigned i) + { return data()[i]; } + + /// Access raw data by index, the index is unchecked + const T& operator[](unsigned i) const + { return data()[i]; } + /// Access raw data by index, the index is unchecked + T& operator[](unsigned i) + { return data()[i]; } + + /// Access the x component + const T& x(void) const + { return data()[0]; } + /// Access the x component + T& x(void) + { return data()[0]; } + /// Access the y component + const T& y(void) const + { return data()[1]; } + /// Access the y component + T& y(void) + { return data()[1]; } + + /// Get the data pointer + using SGVec2Storage::data; + + /// Readonly interface function to ssg's sgVec2/sgdVec2 + const T (&sg(void) const)[2] + { return data(); } + /// Interface function to ssg's sgVec2/sgdVec2 + T (&sg(void))[2] + { return data(); } + + /// Interface function to osg's Vec2* + using SGVec2Storage::osg; + + /// Inplace addition + SGVec2& operator+=(const SGVec2& v) + { data()[0] += v(0); data()[1] += v(1); return *this; } + /// Inplace subtraction + SGVec2& operator-=(const SGVec2& v) + { data()[0] -= v(0); data()[1] -= v(1); return *this; } + /// Inplace scalar multiplication + template + SGVec2& operator*=(S s) + { data()[0] *= s; data()[1] *= s; return *this; } + /// Inplace scalar multiplication by 1/s + template + SGVec2& operator/=(S s) + { return operator*=(1/T(s)); } + + /// Return an all zero vector + static SGVec2 zeros(void) + { return SGVec2(0, 0); } + /// Return unit vectors + static SGVec2 e1(void) + { return SGVec2(1, 0); } + static SGVec2 e2(void) + { return SGVec2(0, 1); } +}; + +/// Unary +, do nothing ... +template +inline +const SGVec2& +operator+(const SGVec2& v) +{ return v; } + +/// Unary -, do nearly nothing +template +inline +SGVec2 +operator-(const SGVec2& v) +{ return SGVec2(-v(0), -v(1)); } + +/// Binary + +template +inline +SGVec2 +operator+(const SGVec2& v1, const SGVec2& v2) +{ return SGVec2(v1(0)+v2(0), v1(1)+v2(1)); } + +/// Binary - +template +inline +SGVec2 +operator-(const SGVec2& v1, const SGVec2& v2) +{ return SGVec2(v1(0)-v2(0), v1(1)-v2(1)); } + +/// Scalar multiplication +template +inline +SGVec2 +operator*(S s, const SGVec2& v) +{ return SGVec2(s*v(0), s*v(1)); } + +/// Scalar multiplication +template +inline +SGVec2 +operator*(const SGVec2& v, S s) +{ return SGVec2(s*v(0), s*v(1)); } + +/// Scalar dot product +template +inline +T +dot(const SGVec2& v1, const SGVec2& v2) +{ return v1(0)*v2(0) + v1(1)*v2(1); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +T +norm(const SGVec2& v) +{ return sqrt(dot(v, v)); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +T +length(const SGVec2& v) +{ return sqrt(dot(v, v)); } + +/// The 1-norm of the vector, this one is the fastest length function we +/// can implement on modern cpu's +template +inline +T +norm1(const SGVec2& v) +{ return fabs(v(0)) + fabs(v(1)); } + +/// The euclidean norm of the vector, that is what most people call length +template +inline +SGVec2 +normalize(const SGVec2& v) +{ return (1/norm(v))*v; } + +/// Return true if exactly the same +template +inline +bool +operator==(const SGVec2& v1, const SGVec2& v2) +{ return v1(0) == v2(0) && v1(1) == v2(1); } + +/// Return true if not exactly the same +template +inline +bool +operator!=(const SGVec2& v1, const SGVec2& v2) +{ return ! (v1 == v2); } + +/// Return true if equal to the relative tolerance tol +template +inline +bool +equivalent(const SGVec2& v1, const SGVec2& v2, T rtol, T atol) +{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)) + atol; } + +/// Return true if equal to the relative tolerance tol +template +inline +bool +equivalent(const SGVec2& v1, const SGVec2& v2, T rtol) +{ return norm1(v1 - v2) < rtol*(norm1(v1) + norm1(v2)); } + +/// Return true if about equal to roundoff of the underlying type +template +inline +bool +equivalent(const SGVec2& v1, const SGVec2& v2) +{ + T tol = 100*SGLimits::epsilon(); + return equivalent(v1, v2, tol, tol); +} + +/// The euclidean distance of the two vectors +template +inline +T +dist(const SGVec2& v1, const SGVec2& v2) +{ return norm(v1 - v2); } + +/// The squared euclidean distance of the two vectors +template +inline +T +distSqr(const SGVec2& v1, const SGVec2& v2) +{ SGVec2 tmp = v1 - v2; return dot(tmp, tmp); } + +#ifndef NDEBUG +template +inline +bool +isNaN(const SGVec2& v) +{ + return SGMisc::isNaN(v(0)) || SGMisc::isNaN(v(1)); +} +#endif + +/// Output to an ostream +template +inline +std::basic_ostream& +operator<<(std::basic_ostream& s, const SGVec2& v) +{ return s << "[ " << v(0) << ", " << v(1) << " ]"; } + +inline +SGVec2f +toVec2f(const SGVec2d& v) +{ return SGVec2f((float)v(0), (float)v(1)); } + +inline +SGVec2d +toVec2d(const SGVec2f& v) +{ return SGVec2d(v(0), v(1)); } + +#endif