quaternion.h

00001 #ifndef QUATERNION_H
00002 #define QUATERNION_H
00003 
00004 #include <cmath>
00005 #include <mbed.h>
00006 #include "tmatrix.h"
00007 const double M_PI = 3.14159265358979323846;
00008 class Quaternion
00009 {
00010 public:
00011     typedef float FloatType;
00012 
00013 private:
00014     FloatType mData[4];
00015 
00016 public:
00017 
00018     Quaternion() {
00019         mData[0] = mData[1] = mData[2] = 0;
00020         mData[3] = 1;
00021     }
00022 
00023     Quaternion(const TVector3 & v, FloatType w) {
00024         mData[0] = v.element(0,0);
00025         mData[1] = v.element(1,0);
00026         mData[2] = v.element(2,0);
00027         mData[3] = w;
00028     }
00029 
00030     Quaternion(const TVector4& v) {
00031         mData[0] = v.element(0,0);
00032         mData[1] = v.element(1,0);
00033         mData[2] = v.element(2,0);
00034         mData[3] = v.element(3,0);
00035     }
00036 
00037     Quaternion(const FloatType* array) {
00038         MBED_ASSERT(array != NULL);
00039         for (uint32_t i = 0; i < 4; i++) {
00040             mData[i] = array[i];
00041         }
00042     }
00043 
00044     Quaternion(FloatType x, FloatType y, FloatType z, FloatType w) {
00045         mData[0] = x;
00046         mData[1] = y;
00047         mData[2] = z;
00048         mData[3] = w;
00049     }
00050 
00051     FloatType x() const { return mData[0]; }
00052     FloatType y() const { return mData[1]; }
00053     FloatType z() const { return mData[2]; }
00054     FloatType w() const { return real(); }
00055 
00056     TVector3  complex() const { return TVector3 (mData); }
00057     void complex(const TVector3 & c) { mData[0] = c[0]; mData[1] = c[1];  mData[2] = c[2]; }
00058 
00059     FloatType real() const { return mData[3]; }
00060     void real(FloatType r) { mData[3] = r; }
00061 
00062     Quaternion conjugate(void) const {
00063         return Quaternion(-complex(), real());
00064     }
00065 
00066     /**
00067      * @brief Computes the inverse of this quaternion.
00068      *
00069      * @note This is a general inverse.  If you know a priori
00070      * that you're using a unit quaternion (i.e., norm() == 1),
00071      * it will be significantly faster to use conjugate() instead.
00072      *
00073      * @return The quaternion q such that q * (*this) == (*this) * q
00074      * == [ 0 0 0 1 ]<sup>T</sup>.
00075      */
00076     Quaternion inverse(void) const {
00077         return conjugate() / norm();
00078     }
00079 
00080 
00081     /**
00082      * @brief Computes the product of this quaternion with the
00083      * quaternion 'rhs'.
00084      *
00085      * @param rhs The right-hand-side of the product operation.
00086      *
00087      * @return The quaternion product (*this) x @p rhs.
00088      */
00089     Quaternion product(const Quaternion& rhs) const {
00090         return Quaternion(y()*rhs.z() - z()*rhs.y() + x()*rhs.w() + w()*rhs.x(),
00091                           z()*rhs.x() - x()*rhs.z() + y()*rhs.w() + w()*rhs.y(),
00092                           x()*rhs.y() - y()*rhs.x() + z()*rhs.w() + w()*rhs.z(),
00093                           w()*rhs.w() - x()*rhs.x() - y()*rhs.y() - z()*rhs.z());
00094     }
00095 
00096     /**
00097      * @brief Quaternion product operator.
00098      *
00099      * The result is a quaternion such that:
00100      *
00101      * result.real() = (*this).real() * rhs.real() -
00102      * (*this).complex().dot(rhs.complex());
00103      *
00104      * and:
00105      *
00106      * result.complex() = rhs.complex() * (*this).real
00107      * + (*this).complex() * rhs.real()
00108      * - (*this).complex().cross(rhs.complex());
00109      *
00110      * @return The quaternion product (*this) x rhs.
00111      */
00112     Quaternion operator*(const Quaternion& rhs) const {
00113         return product(rhs);
00114     }
00115 
00116     /**
00117      * @brief Quaternion scalar product operator.
00118      * @param s A scalar by which to multiply all components
00119      * of this quaternion.
00120      * @return The quaternion (*this) * s.
00121      */
00122     Quaternion operator*(FloatType s) const {
00123         return Quaternion(complex()*s, real()*s);
00124     }
00125 
00126     /**
00127      * @brief Produces the sum of this quaternion and rhs.
00128      */
00129     Quaternion operator+(const Quaternion& rhs) const {
00130         return Quaternion(x()+rhs.x(), y()+rhs.y(), z()+rhs.z(), w()+rhs.w());
00131     }
00132 
00133     /**
00134      * @brief Produces the difference of this quaternion and rhs.
00135      */
00136     Quaternion operator-(const Quaternion& rhs) const {
00137         return Quaternion(x()-rhs.x(), y()-rhs.y(), z()-rhs.z(), w()-rhs.w());
00138     }
00139 
00140     /**
00141      * @brief Unary negation.
00142      */
00143     Quaternion operator-() const {
00144         return Quaternion(-x(), -y(), -z(), -w());
00145     }
00146 
00147     /**
00148      * @brief Quaternion scalar division operator.
00149      * @param s A scalar by which to divide all components
00150      * of this quaternion.
00151      * @return The quaternion (*this) / s.
00152      */
00153     Quaternion operator/(FloatType s) const {
00154         MBED_ASSERT(s != 0);
00155         return Quaternion(complex()/s, real()/s);
00156     }
00157 
00158     /**
00159      * @brief Returns a matrix representation of this
00160      * quaternion.
00161      *
00162      * Specifically this is the matrix such that:
00163      *
00164      * this->matrix() * q.vector() = (*this) * q for any quaternion q.
00165      *
00166      * Note that this is @e NOT the rotation matrix that may be
00167      * represented by a unit quaternion.
00168      */
00169     TMatrix4 matrix() const {
00170         FloatType m[16] = {
00171                 w(), -z(),  y(), x(),
00172                 z(),  w(), -x(), y(),
00173                 -y(),  x(),  w(), z(),
00174                 -x(), -y(), -z(), w()
00175         };
00176         return TMatrix4(m);
00177     }
00178 
00179     /**
00180      * @brief Returns a matrix representation of this
00181      * quaternion for right multiplication.
00182      *
00183      * Specifically this is the matrix such that:
00184      *
00185      * q.vector().transpose() * this->matrix() = (q *
00186      * (*this)).vector().transpose() for any quaternion q.
00187      *
00188      * Note that this is @e NOT the rotation matrix that may be
00189      * represented by a unit quaternion.
00190      */
00191     TMatrix4 rightMatrix() const {
00192         FloatType m[16] = {
00193                 +w(), -z(),  y(), -x(),
00194                 +z(),  w(), -x(), -y(),
00195                 -y(),  x(),  w(), -z(),
00196                 +x(),  y(),  z(),  w()
00197         };
00198         return TMatrix4(m);
00199     }
00200 
00201     /**
00202      * @brief Returns this quaternion as a 4-vector.
00203      *
00204      * This is simply the vector [x y z w]<sup>T</sup>
00205      */
00206     TVector4 vector() const { return TVector4(mData); }
00207 
00208     /**
00209      * @brief Returns the norm ("magnitude") of the quaternion.
00210      * @return The 2-norm of [ w(), x(), y(), z() ]<sup>T</sup>.
00211      */
00212     FloatType norm() const { return sqrt(mData[0]*mData[0]+mData[1]*mData[1]+
00213                                       mData[2]*mData[2]+mData[3]*mData[3]); }
00214 
00215     /**
00216      * @brief Computes the rotation matrix represented by a unit
00217      * quaternion.
00218      *
00219      * @note This does not check that this quaternion is normalized.
00220      * It formulaically returns the matrix, which will not be a
00221      * rotation if the quaternion is non-unit.
00222      */
00223     TMatrix3 rotationMatrix() const {
00224         FloatType m[9] = {
00225                 1-2*y()*y()-2*z()*z(), 2*x()*y() - 2*z()*w(), 2*x()*z() + 2*y()*w(),
00226                 2*x()*y() + 2*z()*w(), 1-2*x()*x()-2*z()*z(), 2*y()*z() - 2*x()*w(),
00227                 2*x()*z() - 2*y()*w(), 2*y()*z() + 2*x()*w(), 1-2*x()*x()-2*y()*y()
00228         };
00229         return TMatrix3(m);
00230     }
00231     /**
00232      * @brief Sets quaternion to be same as rotation by scaled axis w.
00233      */
00234     void scaledAxis(const TVector3 & w) {
00235         FloatType theta = w.norm();
00236         if (theta > 0.0001) {
00237             FloatType s = sin(theta / 2.0);
00238             TVector3  W(w / theta * s);
00239             mData[0] = W[0];
00240             mData[1] = W[1];
00241             mData[2] = W[2];
00242             mData[3] = cos(theta / 2.0);
00243         } else {
00244             mData[0]=mData[1]=mData[2]=0;
00245             mData[3]=1.0;
00246         }
00247     }
00248 
00249     /**
00250      * @brief Returns a vector rotated by this quaternion.
00251      *
00252      * Functionally equivalent to:  (rotationMatrix() * v)
00253      * or (q * Quaternion(0, v) * q.inverse()).
00254      *
00255      * @warning conjugate() is used instead of inverse() for better
00256      * performance, when this quaternion must be normalized.
00257      */
00258     TVector3  rotatedVector(const TVector3 & v) const {
00259         return (((*this) * Quaternion(v, 0)) * conjugate()).complex();
00260     }
00261 
00262 
00263 
00264     /**
00265      * @brief Computes the quaternion that is equivalent to a given
00266      * euler angle rotation.
00267      * @param euler A 3-vector in order:  roll-pitch-yaw.
00268      */
00269     void euler(const TVector3 & euler) {
00270         FloatType c1 = cos(euler[2] * 0.5f);
00271         FloatType c2 = cos(euler[1] * 0.5f);
00272         FloatType c3 = cos(euler[0] * 0.5f);
00273         FloatType s1 = sin(euler[2] * 0.5f);
00274         FloatType s2 = sin(euler[1] * 0.5f);
00275         FloatType s3 = sin(euler[0] * 0.5f);
00276 
00277         mData[0] = c1*c2*s3 - s1*s2*c3;
00278         mData[1] = c1*s2*c3 + s1*c2*s3;
00279         mData[2] = s1*c2*c3 - c1*s2*s3;
00280         mData[3] = c1*c2*c3 + s1*s2*s3;
00281     }
00282 
00283     /** @brief Returns an equivalent euler angle representation of
00284      * this quaternion.
00285      * @return Euler angles in roll-pitch-yaw order.
00286      */
00287     TVector3  euler(void) const {
00288         TVector3  euler;
00289         const static FloatType PI_OVER_2 = M_PI * 0.5;
00290         const static FloatType EPSILON = 1e-10;
00291         FloatType sqw, sqx, sqy, sqz;
00292 
00293         // quick conversion to Euler angles to give tilt to user
00294         sqw = mData[3]*mData[3];
00295         sqx = mData[0]*mData[0];
00296         sqy = mData[1]*mData[1];
00297         sqz = mData[2]*mData[2];
00298 
00299         euler[1] = asin(2.0f * (mData[3]*mData[1] - mData[0]*mData[2]));
00300         if (PI_OVER_2 - fabs(euler[1]) > EPSILON) {
00301             euler[2] = atan2(2.0f * (mData[0]*mData[1] + mData[3]*mData[2]),
00302                              sqx - sqy - sqz + sqw);
00303             euler[0] = atan2(2.0f * (mData[3]*mData[0] + mData[1]*mData[2]),
00304                              sqw - sqx - sqy + sqz);
00305         } else {
00306             // compute heading from local 'down' vector
00307             euler[2] = atan2(2*mData[1]*mData[2] - 2*mData[0]*mData[3],
00308                              2*mData[0]*mData[2] + 2*mData[1]*mData[3]);
00309             euler[0] = 0.0;
00310 
00311             // If facing down, reverse yaw
00312             if (euler[1] < 0)
00313                 euler[2] = M_PI - euler[2];
00314         }
00315         return euler;
00316     }
00317 
00318     /**
00319      * @brief Returns pointer to the internal array.
00320      *
00321      * Array is in order x,y,z,w.
00322      */
00323     FloatType* row(uint32_t i) { return mData + i; }
00324     // Const version of the above.
00325     const FloatType* row(uint32_t i) const { return mData + i; }
00326 };
00327 
00328 /**
00329  * @brief Global operator allowing left-multiply by scalar.
00330  */
00331 Quaternion operator*(Quaternion::FloatType s, const Quaternion& q);
00332 
00333 
00334 #endif /* QUATERNION_H */