Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

fmatSpatial.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_fmatSpatial_h_
00003 #define INCLUDED_fmatSpatial_h_
00004 
00005 #include "fmatCore.h"
00006 
00007 namespace fmat {
00008   
00009   template<typename R> class TransformT;
00010   
00011   // ************************* rotation matrices ******************** //
00012   
00013   //! returns 2x2 rotation matrix (i.e. implied rotation about Z), for angle @a rad in radians
00014   template<typename R>
00015   inline Matrix<2,2,R> rotation2DT(R rad) {
00016     R s = std::sin(rad), c = std::cos(rad);
00017     R dat[4] = { c, s, -s, c };
00018     return Matrix<2,2,R>(dat);
00019   }
00020   //! returns 2x2 rotation matrix for angle @a rad (in radians) about Z axis
00021   inline Matrix<2,2,fmatReal> rotation2D(fmatReal rad) { return rotation2DT<fmatReal>(rad); }
00022   
00023   //! returns NxN rotation matrix for angle @a rad (in radians) about X axis (only uses upper 3x3)
00024   template<size_t N, typename R>
00025   inline Matrix<N,N,R> rotationXN(R rad) {
00026     (void)sizeof(fmat_internal::CompileTimeAssert<Rotation_requires_larger_dimensions,N>=3>);
00027     Matrix<N,N,R> ans = Matrix<N,N,R>::identity();
00028     ans(1,1) = ans(2,2) = std::cos(rad);
00029     ans(1,2) = -(ans(2,1) = std::sin(rad));
00030     return ans;
00031   }
00032   //! returns 3x3 rotation matrix for angle @a rad (in radians) about X axis
00033   inline Matrix<3,3,fmatReal> rotationX(fmatReal rad) { return rotationXN<3,fmatReal>(rad); }
00034 
00035   //! returns NxN rotation matrix for angle @a rad (in radians) about Y axis (only uses upper 3x3)
00036   template<size_t N, typename R>
00037   inline Matrix<N,N,R> rotationYN(R rad) {
00038     (void)sizeof(fmat_internal::CompileTimeAssert<Rotation_requires_larger_dimensions,N>=3>);
00039     Matrix<N,N,R> ans = Matrix<N,N,R>::identity();
00040     ans(0,0) = ans(2,2) = std::cos(rad);
00041     ans(2,0) = -(ans(0,2) = std::sin(rad));
00042     return ans;
00043   }
00044   //! returns 3x3 rotation matrix for angle @a rad (in radians) about Z axis
00045   inline Matrix<3,3,fmatReal> rotationY(fmatReal rad) { return rotationYN<3,fmatReal>(rad); }
00046   
00047   //! returns NxN rotation matrix for angle @a rad (in radians) about Z axis (only uses upper 2x2)
00048   template<size_t N, typename R>
00049   inline Matrix<N,N,R> rotationZN(R rad) {
00050     (void)sizeof(fmat_internal::CompileTimeAssert<Rotation_requires_larger_dimensions,N>=2>);
00051     if(N==2) {
00052       R s = std::sin(rad), c = std::cos(rad);
00053       R dat[4] = { c, s, -s, c };
00054       return Matrix<N,N,R>(dat);
00055     } else {
00056       Matrix<N,N,R> ans = Matrix<N,N,R>::identity();
00057       ans(0,0) = ans(1,1) = std::cos(rad);
00058       ans(0,1) = -(ans(1,0) = std::sin(rad));
00059       return ans;
00060     }
00061   }
00062   //! returns 3x3 rotation matrix for angle @a rad (in radians) about Z axis
00063   inline Matrix<3,3,fmatReal> rotationZ(fmatReal rad) { return rotationZN<3,fmatReal>(rad); }
00064   
00065   
00066   
00067   // ************************* Quaternion ******************** //
00068   
00069   //! Quaternions can be more efficient and more stable for a series of rotations than a corresponding 3x3 matrix, also more compact storage
00070   template<class R=fmatReal>
00071   class QuaternionT {
00072   public:
00073     //! Default constructor, initialize to identity (0 rotation)
00074     QuaternionT() : w(1), x(0), y(0), z(0) {}
00075     
00076     //! Explicit construction from elements (careful, does not check normalization!)
00077     QuaternionT(R w_, R x_, R y_, R z_) : w(w_), x(x_), y(y_), z(z_) {}
00078     
00079     //! copies from another representation, assuming operator[] is supported and w is index 0
00080     template<typename T> static QuaternionT from(const T& q) { return QuaternionT(q[0],q[1],q[2],q[3]); }
00081     //! copies from another representation, assuming operator[] is supported and w is index 0
00082     template<typename T> QuaternionT& importFrom(const T& q) { w=q[0]; x=q[1]; y=q[2]; z=q[3]; return *this; }
00083     //! copies into another representation, assuming operator[] is supported and w is index 0
00084     template<typename T> T exportTo() const { T q; q[0]=w; q[1]=x; q[2]=y; q[3]=z; return q; }
00085     //! copies into another representation, assuming operator[] is supported and w is index 0
00086     template<typename T> T& exportTo(T& q) const { q[0]=w; q[1]=x; q[2]=y; q[3]=z; return q; }
00087     //! unpacks into specified storage
00088     template<typename T> void exportTo(T& w_, T& x_, T& y_, T& z_) const { w_=w; x_=x; y_=y; z_=z; }
00089     //! unpacks into specified storage, skipping #w which can be reconstitued from √(1-x^2-y^2-z^2), see fromAxis()
00090     template<typename T> void exportTo(T& x_, T& y_, T& z_) const { if(w<0) { x_=-x; y_=-y; z_=-z; } else { x_=x; y_=y; z_=z; } }
00091     
00092     R getW() const { return w; } //!< returns #w
00093     R getX() const { return x; } //!< returns #x
00094     R getY() const { return y; } //!< returns #y
00095     R getZ() const { return z; } //!< returns #z
00096     
00097     bool operator==(const QuaternionT<R> &other) const { return w==other.w && x==other.x && y==other.y && z==other.z; }
00098 
00099     //! returns a no-op quaternion (0 rotation), the same as the default constructor, but can re-use this instance instead of creating new ones all the time
00100     static const QuaternionT& identity() { return IDENTITY; }
00101     static const QuaternionT IDENTITY; //!< identity instance
00102     
00103     //! generate quaternion representing rotation of @a rad radians about X axis
00104     static QuaternionT<R> aboutX(R rad) { return fmat::QuaternionT<R>(std::cos(rad/2),std::sin(rad/2),0,0); }
00105     //! generate quaternion representing rotation of @a rad radians about Y axis
00106     static QuaternionT<R> aboutY(R rad) { return fmat::QuaternionT<R>(std::cos(rad/2),0,std::sin(rad/2),0); }
00107     //! generate quaternion representing rotation of @a rad radians about Z axis
00108     static QuaternionT<R> aboutZ(R rad) { return fmat::QuaternionT<R>(std::cos(rad/2),0,0,std::sin(rad/2)); }
00109     
00110     //! Generate quaternion from axis-angle representation (@a angle in radians, @a axis will be re-normalized if non-zero @a angle)
00111     /*! If axis is 0 length, initializes to identity (0 rotation) */
00112     template<class T>
00113     static QuaternionT fromAxisAngle(const T& axis, R angle) {
00114       if(std::abs(angle) <= std::numeric_limits<R>::epsilon())
00115         return QuaternionT(); // 0 rotation, axis does not matter
00116       R n = axis.sumSq();
00117       if(n <= std::numeric_limits<R>::epsilon())
00118         return QuaternionT(); // invalid axis, just use identity
00119       R s = std::sin(angle/2)/std::sqrt(n);
00120       return QuaternionT(std::cos(angle/2), axis[0]*s, axis[1]*s, axis[2]*s);
00121     }
00122     
00123     //! Generate quaternion from just the axis component of another quaternion (i.e. @a axis magnitude should be less than 1; 0 magnitude means no rotation)
00124     /*! This method requires only basic operations on the input type, so you can pass a raw array or plist::Point... */
00125     template<class T>
00126     static QuaternionT fromAxis(const T& axis) {
00127       R n2 = static_cast<R>(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);
00128       if(n2>1) {
00129         if(n2>1+std::numeric_limits<R>::epsilon()*10) {
00130           R n = std::sqrt(n2);
00131           std::cerr << "Warning: fmat::Quaternion axis constructor passed non-normalized |" << axis[0] << ',' << axis[1] << ',' << axis[2] << "| = " << n << " (err " << (n-1) << ')' << std::endl;
00132           R s = 1/n;
00133           return QuaternionT(0, axis[0]*s, axis[1]*s, axis[2]*s);
00134         } else {
00135           // minor rounding error
00136           R s = 1/n2; // not bothering with sqrt, ensures it goes below 1
00137           return QuaternionT(0, axis[0]*s, axis[1]*s, axis[2]*s);
00138         }
00139       } else {
00140         return QuaternionT(std::sqrt(1-n2),axis[0],axis[1],axis[2]);
00141       }
00142     }
00143     
00144     //! Generate quaternion from rotation matrix, this assumes the rotation matrix is well-formed
00145     /*! This implementation is based on cross pollination between "Angel"'s code on this page:
00146      *    http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
00147      *  and the Ogre3D Quaternion implementation, which itself references:
00148      *    Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
00149      *    article "Quaternion Calculus and Fast Animation". */
00150     template<class T>
00151     static QuaternionT fromMatrix(const T& rot) {
00152       QuaternionT quat = fmat_internal::NoInit();
00153       R tracem1 = rot(0,0) + rot(1,1) + rot(2,2);
00154       if( tracem1 >= 0 ) {
00155         const R r = std::sqrt(tracem1 + 1);
00156         quat.w = (R)0.5 * r;
00157         const R s = (R)0.5 / r;
00158         quat.x = ( rot(2,1) - rot(1,2) ) * s;
00159         quat.y = ( rot(0,2) - rot(2,0) ) * s;
00160         quat.z = ( rot(1,0) - rot(0,1) ) * s;
00161       } else {
00162         if ( rot(0,0) > rot(1,1) && rot(0,0) > rot(2,2) ) {
00163           const R r = std::sqrt( 1.0f + rot(0,0) - rot(1,1) - rot(2,2));
00164           quat.x = (R)0.5 * r;
00165           const R s = (R)0.5 / r;
00166           quat.w = (rot(2,1) - rot(1,2) ) * s;
00167           quat.y = (rot(0,1) + rot(1,0) ) * s;
00168           quat.z = (rot(0,2) + rot(2,0) ) * s;
00169         } else if (rot(1,1) > rot(2,2)) {
00170           const R r = std::sqrt( 1.0f + rot(1,1) - rot(0,0) - rot(2,2));
00171           quat.y = (R)0.5 * r;
00172           const R s = (R)0.5 / r;
00173           quat.w = (rot(0,2) - rot(2,0) ) * s;
00174           quat.x = (rot(0,1) + rot(1,0) ) * s;
00175           quat.z = (rot(1,2) + rot(2,1) ) * s;
00176         } else {
00177           const R r = std::sqrt( 1.0f + rot(2,2) - rot(0,0) - rot(1,1) );
00178           quat.z = (R)0.5 * r;
00179           const R s = (R)0.5 / r;
00180           quat.w = (rot(1,0) - rot(0,1) ) * s;
00181           quat.x = (rot(0,2) + rot(2,0) ) * s;
00182           quat.y = (rot(1,2) + rot(2,1) ) * s;
00183         }
00184       }
00185       return quat;
00186     }
00187     
00188     //! returns a 3x3 rotation matrix representation
00189     /*! implemented by simplified version of operator*(*this, Matrix<3,3>::identity()) */
00190     Matrix<3,3,R> toMatrix() const {
00191       const R t2 =   w*x;
00192       const R t3 =   w*y;
00193       const R t4 =   w*z;
00194       const R t5 =  -x*x;
00195       const R t6 =   x*y;
00196       const R t7 =   x*z;
00197       const R t8 =  -y*y;
00198       const R t9 =   y*z;
00199       const R t10 = -z*z;
00200       const R values[9] = {
00201         2*(t8 + t10) + 1,
00202         2*(t4 +  t6),
00203         2*(t7 -  t3),
00204         2*(t6 -  t4),
00205         2*(t5 + t10) + 1,
00206         2*(t2 +  t9),
00207         2*(t3 + t7),
00208         2*(t9 - t2),
00209         2*(t5 + t8) + 1
00210       };
00211       return Matrix<3,3,R>(values);
00212     }
00213     //! allows conversion/assignment to Matrix<3,3>
00214     operator Matrix<3,3,R>() const { return toMatrix(); }
00215     
00216     //! return axis of rotation represented by quaternion @a q (i.e. the axis component of axis-angle representation)
00217     Column<3,R> axis() const {
00218       R n2 = (x*x + y*y + z*z);
00219       if(n2 < std::numeric_limits<R>::epsilon())
00220         return fmat::packT<R>(0,0,1); // no rotation, unknown axis
00221       R sc = 1/std::sqrt(n2);
00222       return fmat::packT<R>(x*sc, y*sc, z*sc);
00223     }
00224     
00225     //! Return angle of rotation (range ±π radians) about the quaternion's native axis (i.e. the angle component for axis-angle representation)
00226     /*! Handles some slight denormalization gracefully. */
00227     R angle() const {
00228       R n = std::sqrt(x*x + y*y + z*z);
00229       R ang = 2*std::atan2(n,w); // the atan2 result is always positive because 'n' is always positive
00230       if(ang>static_cast<R>(M_PI)) // so ang is always positive, don't need a '< M_PI' case
00231         ang-=2*static_cast<R>(M_PI);
00232       return ang;
00233       // alternative... faster/better?
00234       /*if(w>1) return 2*std::acos(2-w);
00235        else if(w<-1) return 2*std::acos(2+w);
00236        else return 2*std::acos(w);*/
00237     }
00238     
00239     //! Return angle of rotation represented by the quaternion about an arbitrary axis (assumed to already be normalized)
00240     template<class T>
00241     R axisComponent(const T& v) const {
00242       /*R ang_2 = std::acos(w);
00243        R sc = std::sin(ang_2);*/
00244       R n = std::sqrt(x*x + y*y + z*z);
00245       if(n < std::numeric_limits<R>::epsilon())
00246         return 0;
00247       R ang_2 = std::atan2(n,w);
00248       return 2*ang_2*(x*v[0] + y*v[1] + z*v[2])/n;
00249     }
00250     
00251     //! Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order)
00252     /*! From the "driver's seat" positive heading is turning to the left (z is up, using right hand rule, not compass heading),
00253      *  positive pitch is looking down, and positive roll is spinning clockwise.  Within this frame-oriented view, we apply
00254      *  rotation axes in the 'reverse' order: first z, then y, then x.
00255      *
00256      *  You can reconstruct a Quaternion from these values by: q · v = aboutZ(yaw) * aboutY(pitch) * aboutX(roll) · v
00257      *  Note we right-multiply v to apply the rotation, so x is applied to an incoming vector first, then y, then z.
00258      *
00259      *  With thanks to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
00260      *  (note that page uses different axis mapping however!) */
00261     fmat::Column<3,R> ypr() const {
00262       const R sqw=w*w, sqx=x*x, sqy=y*y, sqz=z*z;
00263       const R unit = sqw + sqx + sqy + sqz; // if normalised is one, otherwise is correction factor
00264       const R test = y*w - x*z;
00265       if (test > 0.4999*unit) { // singularity at north pole
00266         return packT<R>(-2 * std::atan2(x,w), (R)M_PI_2, 0);
00267       }
00268       if (test < -0.4999*unit) { // singularity at south pole
00269         return packT<R>(2 * std::atan2(x,w), -(R)M_PI_2, 0);
00270       }
00271       return packT<R>(
00272         std::atan2(2*z*w+2*x*y , sqx - sqz - sqy + sqw),
00273         std::asin(2*test/unit),
00274         std::atan2(2*x*w+2*z*y , -sqx + sqz - sqy + sqw)
00275       );
00276     }
00277     
00278     //! Negates just the quaternion axis
00279     QuaternionT inverse() const ATTR_must_check { return QuaternionT(w,-x,-y,-z); }
00280     
00281     // ! Negates the entire quaternion (not the same as the inverse)
00282     /* ! Although I wonder if it should be the same as inverse(), this is what other quaternion implementations do */
00283     // Since it's not clear what this should do, it's left undefined
00284     //QuaternionT operator-() const ATTR_must_check { return QuaternionT(-w,-x,-y,-z); }
00285     
00286     //! multiply quaternions
00287     template<typename Rb>
00288     QuaternionT<typename fmat_internal::promotion_trait<R,Rb>::type>
00289     operator*(const QuaternionT<Rb>& q) const {
00290       return QuaternionT<typename fmat_internal::promotion_trait<R,Rb>::type>(
00291         w*q.w - x*q.x - y*q.y - z*q.z,
00292         w*q.x + x*q.w + y*q.z - z*q.y,
00293         w*q.y - x*q.z + y*q.w + z*q.x,
00294         w*q.z + x*q.y - y*q.x + z*q.w
00295       );
00296     }
00297     
00298     //! multiply quaternions
00299     QuaternionT operator*=(const QuaternionT& q) { return this->operator=(operator*(q)); }
00300     
00301     //! multiply 3-row matrix by quaternion
00302     template<template<size_t H, size_t W, typename Rt> class T, size_t W>
00303     Matrix<3,W,R> operator*(const T<3,W,R>& m) const {
00304       const R t2 =   w*x;
00305       const R t3 =   w*y;
00306       const R t4 =   w*z;
00307       const R t5 =  -x*x;
00308       const R t6 =   x*y;
00309       const R t7 =   x*z;
00310       const R t8 =  -y*y;
00311       const R t9 =   y*z;
00312       const R t10 = -z*z;
00313       Matrix<3,W,R> o=fmat_internal::NoInit();
00314       for(size_t c = 0; c<W; ++c) {
00315         o(0,c) = 2*( (t8 + t10)*m(0,c) + (t6 -  t4)*m(1,c) + (t3 + t7)*m(2,c) ) + m(0,c);
00316         o(1,c) = 2*( (t4 +  t6)*m(0,c) + (t5 + t10)*m(1,c) + (t9 - t2)*m(2,c) ) + m(1,c);
00317         o(2,c) = 2*( (t7 -  t3)*m(0,c) + (t2 +  t9)*m(1,c) + (t5 + t8)*m(2,c) ) + m(2,c);
00318       }
00319       return o;
00320     }
00321     
00322     //! multiply Transform by quaternion
00323     TransformT<R> operator*(const TransformT<R>& m) const {
00324       const R t2 =   w*x;
00325       const R t3 =   w*y;
00326       const R t4 =   w*z;
00327       const R t5 =  -x*x;
00328       const R t6 =   x*y;
00329       const R t7 =   x*z;
00330       const R t8 =  -y*y;
00331       const R t9 =   y*z;
00332       const R t10 = -z*z;
00333       TransformT<R> o=fmat_internal::NoInit();
00334       for(size_t c = 0; c<4; ++c) {
00335         o(0,c) = 2*( (t8 + t10)*m(0,c) + (t6 -  t4)*m(1,c) + (t3 + t7)*m(2,c) ) + m(0,c);
00336         o(1,c) = 2*( (t4 +  t6)*m(0,c) + (t5 + t10)*m(1,c) + (t9 - t2)*m(2,c) ) + m(1,c);
00337         o(2,c) = 2*( (t7 -  t3)*m(0,c) + (t2 +  t9)*m(1,c) + (t5 + t8)*m(2,c) ) + m(2,c);
00338       }
00339       return o;
00340     }
00341     
00342     //! multiply 3-element vector by quaternion
00343     Column<3,R> operator*(const Column<3,R> v) const {
00344       const R t2 =   w*x;
00345       const R t3 =   w*y;
00346       const R t4 =   w*z;
00347       const R t5 =  -x*x;
00348       const R t6 =   x*y;
00349       const R t7 =   x*z;
00350       const R t8 =  -y*y;
00351       const R t9 =   y*z;
00352       const R t10 = -z*z;
00353       Column<3,R> o=fmat_internal::NoInit();
00354       o[0] = 2*( (t8 + t10)*v[0] + (t6 -  t4)*v[1] + (t3 + t7)*v[2] ) + v[0];
00355       o[1] = 2*( (t4 +  t6)*v[0] + (t5 + t10)*v[1] + (t9 - t2)*v[2] ) + v[1];
00356       o[2] = 2*( (t7 -  t3)*v[0] + (t2 +  t9)*v[1] + (t5 + t8)*v[2] ) + v[2];
00357       return o;
00358     }
00359     
00360     //! returns sum of squares of components; should be "close" to 1, otherwise call normalize()
00361     R sumSq() const { return w*w + x*x + y*y + z*z; }
00362     
00363     //! returns magnitude of quaternion; should be "close" to 1, otherwise call normalize()
00364     R norm() const { return std::sqrt(w*w + x*x + y*y + z*z); }
00365     
00366     //! Re-normalize the quaternion magnitude to 1 (or 0 if norm is already invalid) and positive W, returns the previous magnitude
00367     R normalize() {
00368       R n = sumSq();
00369       if(n < std::numeric_limits<R>::epsilon()) {
00370         w=x=y=z=0; // can't normalize a zero vector, just clean it up
00371         return 0;
00372       }
00373       if(n<=1 && 1-n < std::numeric_limits<R>::epsilon()) {
00374         // already normalized, just fix w if needed
00375         if(w<0) {
00376           w=-w;
00377           x=-x;
00378           y=-y;
00379           z=-z;
00380         }
00381         return 1;
00382       }
00383       R sc = (w<0?-1:1) / std::sqrt(n);
00384       w*=sc;
00385       x*=sc;
00386       y*=sc;
00387       z*=sc;
00388       return n;
00389     }
00390     
00391     friend inline std::ostream& operator<<(std::ostream& os, const QuaternionT<R>& q) { return os << fmat::SubVector<4,const R>((const R*)&q.w); }
00392     
00393   protected:
00394     R w, x, y, z;
00395     
00396     //! no-op constructor for functions which fill in results with additional computation
00397     QuaternionT(const fmat_internal::NoInit&) : w(), x(), y(), z() {}
00398   };
00399   typedef QuaternionT<> Quaternion;
00400   
00401   //! returns the rotation needed to transform @a p into @a q (this is the core of a 'slerp' implementation)
00402   template<class Ra, class Rb>
00403   QuaternionT<typename fmat_internal::promotion_trait<Ra,Rb>::type>
00404   crossProduct(const QuaternionT<Ra>& p, const QuaternionT<Rb>& q) { return q * p.inverse(); }
00405   
00406   // forward to the specialized Quaternion invert
00407   template<typename R> QuaternionT<R> invert(const QuaternionT<R>& q) { return q.inverse(); }
00408   
00409   
00410   
00411   // ************************* Transform ******************** //
00412   
00413   namespace fmat_internal {
00414     //! Expanded calculations to facilitate optimized compilation of these operations for TransformT
00415     template<typename R1, typename R2>
00416     struct transformOps {
00417       //! The output type for these operations, e.g. float * double -> double
00418       typedef typename fmat_internal::promotion_trait<R1,R2>::type out_t;
00419       //! Transform a length-3 column
00420       template<typename V>
00421       inline static Column<3,out_t> multiply3(const TransformT<R1>& tr, const V& x) {
00422         Column<3,out_t> ans=fmat_internal::NoInit();
00423         const R1 *a = &tr(0,0);
00424         const R2 *b = &x[0];
00425         ans[0] = a[0+3*0]*b[0] + a[0+3*1]*b[1] + a[0+3*2]*b[2] + a[0+3*3];
00426         ans[1] = a[1+3*0]*b[0] + a[1+3*1]*b[1] + a[1+3*2]*b[2] + a[1+3*3];
00427         ans[2] = a[2+3*0]*b[0] + a[2+3*1]*b[1] + a[2+3*2]*b[2] + a[2+3*3];
00428         return ans;
00429       }
00430       //! Transform a length-4 column (homogenous coordinate)
00431       template<typename V>
00432       inline static Column<4,out_t> multiply4(const TransformT<R1>& tr, const V& x) {
00433         Column<4,out_t> ans=fmat_internal::NoInit();
00434         const R1 *a = &tr(0,0);
00435         const R2 *b = &x[0];
00436         ans[0] = a[0+3*0]*b[0] + a[0+3*1]*b[1] + a[0+3*2]*b[2] + a[0+3*3]*b[3];
00437         ans[1] = a[1+3*0]*b[0] + a[1+3*1]*b[1] + a[1+3*2]*b[2] + a[1+3*3]*b[3];
00438         ans[2] = a[2+3*0]*b[0] + a[2+3*1]*b[1] + a[2+3*2]*b[2] + a[2+3*3]*b[3];
00439         ans[3] = b[3];
00440         return ans;
00441       }
00442       //! Concatenate transforms
00443       inline static void multiplyT(const R1* a, const R2* b, out_t* ans) {
00444         ans[0+3*0] = a[0+3*0]*b[0+3*0] + a[0+3*1]*b[1+3*0] + a[0+3*2]*b[2+3*0];
00445         ans[1+3*0] = a[1+3*0]*b[0+3*0] + a[1+3*1]*b[1+3*0] + a[1+3*2]*b[2+3*0];
00446         ans[2+3*0] = a[2+3*0]*b[0+3*0] + a[2+3*1]*b[1+3*0] + a[2+3*2]*b[2+3*0];
00447         ans[0+3*1] = a[0+3*0]*b[0+3*1] + a[0+3*1]*b[1+3*1] + a[0+3*2]*b[2+3*1];
00448         ans[1+3*1] = a[1+3*0]*b[0+3*1] + a[1+3*1]*b[1+3*1] + a[1+3*2]*b[2+3*1];
00449         ans[2+3*1] = a[2+3*0]*b[0+3*1] + a[2+3*1]*b[1+3*1] + a[2+3*2]*b[2+3*1];
00450         ans[0+3*2] = a[0+3*0]*b[0+3*2] + a[0+3*1]*b[1+3*2] + a[0+3*2]*b[2+3*2];
00451         ans[1+3*2] = a[1+3*0]*b[0+3*2] + a[1+3*1]*b[1+3*2] + a[1+3*2]*b[2+3*2];
00452         ans[2+3*2] = a[2+3*0]*b[0+3*2] + a[2+3*1]*b[1+3*2] + a[2+3*2]*b[2+3*2];
00453         ans[0+3*3] = a[0+3*0]*b[0+3*3] + a[0+3*1]*b[1+3*3] + a[0+3*2]*b[2+3*3] + a[0+3*3];
00454         ans[1+3*3] = a[1+3*0]*b[0+3*3] + a[1+3*1]*b[1+3*3] + a[1+3*2]*b[2+3*3] + a[1+3*3];
00455         ans[2+3*3] = a[2+3*0]*b[0+3*3] + a[2+3*1]*b[1+3*3] + a[2+3*2]*b[2+3*3] + a[2+3*3];
00456       }
00457     };
00458   }
00459   
00460   
00461   //! Efficient computation of affine transform operations
00462   template<typename R=fmatReal>
00463   class TransformT : public Matrix<3,4,R> {
00464   public:
00465     enum {
00466       HEIGHT=3, H=3,
00467       WIDTH=4, W=4,
00468       CAP=12
00469     };
00470     typedef R storage_t;
00471     typedef typename fmat_internal::unconst<R>::type out_t;
00472     
00473     TransformT(const fmat_internal::NoInit& noinit) : Matrix<H,W,R>(noinit) {}
00474     
00475     TransformT() : Matrix<H,W,R>(Matrix<H,W,R>::identity()) { }
00476     explicit TransformT(const R* x, size_t colStride=H) : Matrix<H,W,R>(x,colStride) {}
00477     TransformT(const SubMatrix<H,W,R>& x) : Matrix<H,W,R>(x) {}
00478     TransformT(const Matrix<H,W,R>& x) : Matrix<H,W,R>(x) {}
00479     template<typename Rot, typename Pt> TransformT(const Rot& r, const Pt& p) : Matrix<H,W,R>(fmat_internal::NoInit()) { rotation()=r; translation()=p; }
00480     template<size_t SH, size_t SW> explicit TransformT(const SubMatrix<SH,SW,R>& x) : Matrix<H,W,R>(x) {}
00481     template<size_t SH, size_t SW> explicit TransformT(const SubMatrix<SH,SW,R>& x, size_t rowOff, size_t colOff) : Matrix<H,W,R>(x,rowOff,colOff) {}
00482     template<size_t SH, size_t SW> explicit TransformT(const Matrix<SH,SW,R>& x) : Matrix<H,W,R>(x) {}
00483     template<size_t SH, size_t SW> explicit TransformT(const Matrix<SH,SW,R>& x, size_t rowOff, size_t colOff) : Matrix<H,W,R>(x,rowOff,colOff) {}
00484     
00485     SubMatrix<3,3,R> rotation() { return SubMatrix<3,3,R>(data); }
00486     SubMatrix<3,3,const R> rotation() const { return SubMatrix<3,3,const R>(data); }
00487     SubVector<3,R> translation() { return Matrix<H,W,R>::column(3); }
00488     SubVector<3,const R> translation() const { return Matrix<H,W,R>::column(3); }
00489     
00490     //! generate transform representing rotation of @a rad radians about X axis
00491     static TransformT<R> aboutX(R rad) {
00492       TransformT<R> ans;
00493       ans(1,1) = ans(2,2) = std::cos(rad);
00494       ans(1,2) = -(ans(2,1) = std::sin(rad));
00495       return ans;
00496     }
00497     //! generate transform representing rotation of @a rad radians about Y axis
00498     static TransformT<R> aboutY(R rad) {
00499       TransformT<R> ans;
00500       ans(0,0) = ans(2,2) = std::cos(rad);
00501       ans(2,0) = -(ans(0,2) = std::sin(rad));
00502       return ans;
00503     }
00504     //! generate transform representing rotation of @a rad radians about Z axis
00505     static TransformT<R> aboutZ(R rad) {
00506       TransformT<R> ans;
00507       ans(0,0) = ans(1,1) = std::cos(rad);
00508       ans(0,1) = -(ans(1,0) = std::sin(rad));
00509       return ans;
00510     }
00511     //! generate transform representing a translation of @a x
00512     template<class V>
00513     static TransformT<R> offset(const V& x) { TransformT<R> ans; ans.translation() = x; return ans; }
00514         
00515     static const TransformT& identity() { return IDENTITY; }
00516     static const TransformT IDENTITY; //!< identity instance
00517     
00518     //! returns the inverse transform, allowing for affine transformations (however, cannot invert 0 or inf scaling!)
00519     inline TransformT inverse() const ATTR_must_check {
00520       TransformT ans=fmat_internal::NoInit();
00521       ans.rotation() = fmat::invert(rotation());
00522       ans.translation() = -ans.rotation() * translation();
00523       return ans;
00524     }
00525 
00526     //! returns the inverse transform, assuming a rigid transform (zero scale and skew) for faster computation
00527     inline TransformT rigidInverse() const ATTR_must_check {
00528       TransformT ans=fmat_internal::NoInit();
00529       R * t2dat = &ans(0,0);
00530       t2dat[0] = data[0];
00531       t2dat[1] = data[3];
00532       t2dat[2] = data[6];
00533       t2dat[3] = data[1];
00534       t2dat[4] = data[4];
00535       t2dat[5] = data[7];
00536       t2dat[6] = data[2];
00537       t2dat[7] = data[5];
00538       t2dat[8] = data[8];
00539       t2dat[9]  = -(data[0]*data[9] + data[1]*data[10] + data[2]*data[11]);
00540       t2dat[10] = -(data[3]*data[9] + data[4]*data[10] + data[5]*data[11]);
00541       t2dat[11] = -(data[6]*data[9] + data[7]*data[10] + data[8]*data[11]);
00542       return ans;
00543     }
00544     
00545     template<typename R2>
00546     inline Column<3,typename fmat_internal::promotion_trait<R,R2>::type>
00547     operator*(const Column<3,R2>& x) const {
00548       return fmat_internal::transformOps<R,R2>::multiply3(*this,x);
00549     }
00550     
00551     template<typename R2>
00552     inline Column<3,typename fmat_internal::promotion_trait<R,R2>::type>
00553     operator*(const SubVector<3,R2>& x) const {
00554       return fmat_internal::transformOps<R,R2>::multiply3(*this,x);
00555     }
00556     
00557     template<size_t N, typename R2>
00558     inline Matrix<3,N,typename fmat_internal::promotion_trait<R,R2>::type>
00559     operator*(const Matrix<3,N,R2>& x) const {
00560       Matrix<3,N,typename fmat_internal::promotion_trait<R,R2>::type> ans = fmat_internal::NoInit();
00561       for(size_t i=0; i<N; ++i)
00562         ans.column(i) = fmat_internal::transformOps<R,R2>::multiply3(*this,x.column(i));
00563       return ans;
00564     }
00565     
00566     template<typename R2>
00567     inline Column<4,typename fmat_internal::promotion_trait<R,R2>::type>
00568     operator*(const Column<4,R2>& x) const {
00569       return fmat_internal::transformOps<R,R2>::multiply4(*this,x);
00570     }
00571     
00572     template<typename R2>
00573     inline Column<4,typename fmat_internal::promotion_trait<R,R2>::type>
00574     operator*(const SubVector<4,R2>& x) const {
00575       return fmat_internal::transformOps<R,R2>::multiply4(*this,x);
00576     }
00577     
00578     template<size_t N, typename R2>
00579     inline Matrix<4,N,typename fmat_internal::promotion_trait<R,R2>::type>
00580     operator*(const Matrix<4,N,R2>& x) const {
00581       Matrix<4,N,typename fmat_internal::promotion_trait<R,R2>::type> ans = fmat_internal::NoInit();
00582       for(size_t i=0; i<N; ++i)
00583         ans.column(i) = fmat_internal::transformOps<R,R2>::multiply4(*this,x.column(i));
00584       return ans;
00585     }
00586     
00587     template<typename R2>
00588     inline TransformT<typename fmat_internal::promotion_trait<R,R2>::type>
00589     operator*(const TransformT<R2>& tb) const {
00590       TransformT<typename fmat_internal::promotion_trait<R,R2>::type> mans=fmat_internal::NoInit();
00591       fmat_internal::transformOps<R,R2>::multiplyT(data,&tb(0,0),&mans(0,0));
00592       return mans;
00593     }
00594     
00595     template<typename R2>
00596     TransformT&
00597     operator*=(const TransformT<R2>& t) {
00598       fmat_internal::transformOps<R,R2>::multiplyT(&TransformT(*this)(0,0),&t(0,0),data);
00599       return *this;
00600     }
00601     using Matrix<H,W,R>::operator=;
00602     using Matrix<H,W,R>::operator*=;
00603     using Matrix<H,W,R>::operator/=;
00604     
00605   protected:
00606     using Matrix<H,W,R>::data;
00607   };
00608   typedef TransformT<> Transform;
00609   
00610   //! invert the matrix, taking advantage of known structure:
00611   template<typename R>
00612   inline Matrix<4,4,R> invertTransform(const Matrix<4,4,R>& t) {
00613     fmat::Column<4> p = -t.column(3);
00614     const R * tdat = &t(0,0);
00615     /*R t2dat[16] = {
00616      tdat[0], tdat[4], tdat[8], 0,
00617      tdat[1], tdat[5], tdat[9], 0,
00618      tdat[2], tdat[6], tdat[10], 0,
00619      fmat::dotProduct(t.column(0),p), fmat::dotProduct(t.column(1),p), fmat::dotProduct(t.column(2),p), tdat[15]
00620      };
00621      return Matrix<4,4,R>(t2dat);*/
00622     Matrix<4,4,R> t2(t);
00623     R * t2dat = &t2(0,0);
00624     t2dat[1] = tdat[4];
00625     t2dat[2] = tdat[8];
00626     t2dat[4] = tdat[1];
00627     t2dat[6] = tdat[9];
00628     t2dat[8] = tdat[2];
00629     t2dat[9] = tdat[6];
00630     t2dat[12] = fmat::dotProduct(t.column(0),p);
00631     t2dat[13] = fmat::dotProduct(t.column(1),p);
00632     t2dat[14] = fmat::dotProduct(t.column(2),p);
00633     return t2;
00634   }
00635   
00636   // forward to the specialized Transform invert
00637   template<typename R>
00638   TransformT<R>
00639   invert(const TransformT<R>& m) { return m.inverse(); }
00640   
00641   //! Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order)
00642   /*! From the "driver's seat" positive heading is turning to the left (z is up, using right hand rule, not compass heading),
00643    *  positive pitch is looking down, and positive roll is spinning clockwise.  Within this frame-oriented view, we apply
00644    *  rotation axes in the 'reverse' order: first z, then y, then x.
00645    *
00646    *  You can reconstruct a Quaternion from these values by: q · v = aboutZ(yaw) * aboutY(pitch) * aboutX(roll) · v
00647    *  Note we right-multiply v to apply the rotation, so x is applied to an incoming vector first, then y, then z.
00648    *
00649    *  With thanks to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
00650    *  (note that page uses different axis mapping however!) */
00651   template<typename R>
00652   Column<3,R>
00653   ypr(const QuaternionT<R>& q) { return q.ypr(); }
00654   
00655   //! Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order)
00656   /*! From the "driver's seat" positive heading is turning to the left (z is up, using right hand rule, not compass heading),
00657    *  positive pitch is looking down, and positive roll is spinning clockwise.  Within this frame-oriented view, we apply
00658    *  rotation axes in the 'reverse' order: first z, then y, then x.
00659    *
00660    *  You can reconstruct a rotation from these values by: q · v = rotationZ(yaw) * rotationY(pitch) * rotationX(roll) · v
00661    *  Note we right-multiply v to apply the rotation, so x is applied to an incoming vector first, then y, then z. */
00662   template<typename R>
00663   Column<3,R>
00664   ypr(const TransformT<R>& m) { return ypr(m.rotation()); }
00665   
00666   //! Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order)
00667   /*! From the "driver's seat" positive heading is turning to the left (z is up, using right hand rule, not compass heading),
00668    *  positive pitch is looking down, and positive roll is spinning clockwise.  Within this frame-oriented view, we apply
00669    *  rotation axes in the 'reverse' order: first z, then y, then x.
00670    *
00671    *  You can reconstruct a rotation from these values by: q · v = rotationZ(yaw) * rotationY(pitch) * rotationX(roll) · v
00672    *  Note we right-multiply v to apply the rotation, so x is applied to an incoming vector first, then y, then z. */
00673   template<template<size_t,size_t,typename R> class M, typename R>
00674   Column<3,typename fmat_internal::unconst<R>::type>
00675   ypr(const M<3,3,R>& m) {
00676     typedef typename fmat_internal::unconst<R>::type out_t;
00677     if (m(2,0) > 0.9998) { // singularity at north pole
00678       return packT<out_t>(std::atan2(-m(0,1),m(1,1)), -M_PI_2, 0);
00679     }
00680     if (m(2,0) < -0.9998) { // singularity at south pole
00681       return packT<out_t>(std::atan2(-m(0,1),m(1,1)), M_PI_2, 0);
00682     }
00683     return packT<out_t>(
00684       std::atan2(m(1,0),m(0,0)),
00685       std::asin(-m(2,0)),
00686       std::atan2(m(2,1),m(2,2))
00687     );
00688   }
00689   
00690   //! Returns the scaling factors for d1 and d2 through p1 and p2 respectively to reach common intersection
00691   /*! May return infinity if d1 and d2 are parallel (including collinear).  */
00692   template<typename R>
00693   Column<2,R>
00694   segmentIntersection(const Column<2,R>& p1, const Column<2,R>& d1, const Column<2,R>& p2, const Column<2,R>& d2) {
00695     fmat::Matrix<2,2,R> dm = fmat_internal::NoInit();
00696     dm.column(0) = d1;
00697     dm.column(1) = -d2;
00698     try {
00699       dm = invert(dm);
00700     } catch(...) {
00701       // no solution, parallel lines, return infinity
00702       return Column<2,R>(std::numeric_limits<fmat::fmatReal>::infinity());
00703     }
00704     Column<2,R> o = p2-p1;
00705     return dm * o;
00706   }
00707   
00708   //! Returns the scaling factor of d1 from p1 to reach intersection of d2 through p2
00709   /*! May return infinity if d1 and d2 are parallel (including collinear).  */
00710   template<typename R>
00711   R
00712   rayIntersection(const Column<2,R>& p1, const Column<2,R>& d1, const Column<2,R>& p2, const Column<2,R>& d2) {
00713     return segmentIntersection(p1, d1, p2, d2)[0];
00714   }
00715   
00716   //! Returns the point of intersection of d1 through p1 and d2 through p2
00717   /*! May return point at infinity if d1 and d2 are parallel (including collinear),
00718     *  such a point will be in the correct quadrant. */
00719   template<typename R>
00720   Column<2,R>
00721   lineIntersection(const Column<2,R>& p1, const Column<2,R>& d1, const Column<2,R>& p2, const Column<2,R>& d2) {
00722     return d1 * rayIntersection(p1,d1,p2,d2) + p1;
00723   }
00724   
00725   //! Returns the orthogonal left vector (rotate by 90°)
00726   template<template<size_t,typename R> class V, typename R>
00727   Column<2,typename V<2,R>::out_t>
00728   normalLeft(const V<2,R>& v) { return fmat::pack(-v[1],v[0]); }
00729 
00730   //! Returns the orthogonal right vector (rotate by -90°)
00731   template<template<size_t,typename R> class V, typename R>
00732   Column<2,typename V<2,R>::out_t>
00733   normalRight(const V<2,R>& v) { return fmat::pack(v[1],-v[0]); }
00734 }
00735 
00736 /*! @file
00737  * @brief Provides data structures and algorithms for spatial operations
00738  * @author Ethan Tira-Thompson (ejt) (Creator)
00739  */
00740 
00741 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:40 2016 by Doxygen 1.6.3