quaternion.h

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