jot: quat.C Source File

00001 /*!
00002  *  \file Quat.C
00003  *  \brief Contains the implementation of the non-inline functions of the Quat
00004  *  class.
00005  *  \ingroup group_MLIB
00006  *
00007  *  This code comes in part from Ken Shoemake's article on quaternions
00008  *  (available at ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z).
00009  *
00010  */
00011 #include "mlib/quat.H"
00012 
00013 /* Constructors */
00014 
00015 template <class QUAT,class M,class P,class V,class L>
00016 MLIB_INLINE
00017 mlib::Quat<QUAT,M,P,V,L>::Quat(const M& t)
00018 {
00019    double s;
00020    double tr = t(0,0) + t(1,1) + t(2,2);
00021 
00022    if (tr >= 0.0) {
00023       s  = sqrt(tr + t(3,3));
00024       _w = s * .5;
00025       s  = 0.5 / s;
00026       _v[0] = (t(2,1) - t(1,2)) * s;
00027       _v[1] = (t(0,2) - t(2,0)) * s;
00028       _v[2] = (t(1,0) - t(0,1)) * s;
00029    } else {
00030       int h = 0; //X
00031       if (t(1,1) > t(0,0))      h = 1; //Y
00032       if (t(2,2) > t(h,h))      h = 2; //Z
00033       switch (h) {
00034       #define caseMacro(i,j,k,I,J,K) \
00035       case I:\
00036              s = sqrt( (t(I,I) - (t(J,J)+t(K,K))) + t(3,3) );\
00037                 _v[i] = s*0.5;\
00038                 s = 0.5 / s;\
00039                 _v[j] = (t(I,J) + t(J,I)) * s;\
00040                 _v[k] = (t(K,I) + t(I,K)) * s;\
00041                 _v[3] = (t(K,J) + t(J,K)) * s;\
00042                 break
00043                 caseMacro(0,1,2,0,1,2);
00044              caseMacro(1,2,0,1,2,0);
00045              caseMacro(2,0,1,2,0,1);
00046       #undef caseMacro
00047       }
00048    }
00049    if (t(3,3) != 1.0) {
00050       s = 1.0/sqrt(t(3,3));
00051       _v *= s;
00052    }
00053 
00054 }
00055 
00056 
00057 template <class QUAT,class M,class P,class V,class L>
00058 MLIB_INLINE
00059 mlib::Quat<QUAT,M,P,V,L>::Quat(const V& v1, const V& v2)
00060 {
00061    V n = cross(v1,v2).normalized();
00062    if (n.is_null()) {
00063       _v = V();
00064       _w = 1;
00065    } else {
00066       double theta = v1.angle(v2);
00067       _v = n*sin(theta/2);
00068       _w = cos(theta/2);
00069    }
00070 }
00071 
00072 /* Two Quaternion Operations */
00073 
00074 /*
00075  *  From SIGGRAPH '85 Shoemake
00076  */
00077 template <class QUAT,class M,class P,class V,class L>
00078 MLIB_INLINE
00079 QUAT
00080 mlib::Quat<QUAT,M,P,V,L>::slerp(const QUAT& q1, const QUAT& q2, double u)
00081 {
00082    double theta = Acos((q1*q2).w());
00083    return ( sin((1-u)*theta) * q1 + sin(u*theta) * q2 ) / sin(theta);
00084 }