Guest User

Quaternion.cpp

a guest
Mar 1st, 2012
328
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 19.22 KB | None | 0 0
  1. #include "cbase.h"
  2. #include <math.h>
  3. //#include "vector.h"
  4. #include "Quaternion.h"
  5.  
  6. void Quat::GetRotationTo(const Vector& start, const Vector& dest)
  7. {
  8.     // Based on Stan Melax's article in Game Programming Gems
  9.     Quat q;
  10.     // Copy, since cannot modify local
  11.     Vector v0 = start;
  12.     Vector v1 = dest;
  13.     v0.NormalizeInPlace();
  14.     v1.NormalizeInPlace();
  15.  
  16.     float d = v0.Dot(v1);
  17.     // If dot == 1, vectors are the same
  18.     if (d >= 1.0f)
  19.     {
  20.         q = Quat(1,0,0,0);
  21.         *this = q;
  22.         return;
  23.     }
  24.     if (d < (1e-6f - 1.0f))
  25.     {
  26.         // Generate an axis
  27.         Vector axis = Vector(1,0,0).Cross(start);
  28.         if (axis.IsZero()) // pick another if colinear
  29.             axis = Vector(0,1,0).Cross(start);
  30.         axis.NormalizeInPlace();
  31.         q.AxisAngle(axis, 180);
  32.     }
  33.     else
  34.     {
  35.         float s = sqrt( (1+d)*2 );
  36.         float invs = 1 / s;
  37.  
  38.         Vector c = v0.Cross(v1);
  39.  
  40.         q.x = c.x * invs;
  41.         q.y = c.y * invs;
  42.         q.z = c.z * invs;
  43.         q.w = s * 0.5;
  44.         //q.NormalizeInPlace();
  45.     }
  46.     *this = q;
  47. }
  48.  
  49. void Quat::GetRotationTo( const Vector& cross, float angle )
  50. {
  51.      // Based on Stan Melax's article in Game Programming Gems
  52.     Quat q;
  53.     // Copy, since cannot modify local
  54.    
  55.     float d = angle;//cos(angle);//v0.Dot(v1);
  56.     // If dot == 1, vectors are the same
  57.     if (d >= 1.0f)
  58.     {
  59.         q = Quat(1,0,0,0);
  60.         *this = q;
  61.         return;
  62.     }
  63.  
  64.     float s = sqrt( (1+d)*2 );
  65.     float invs = 1 / s;
  66.  
  67.     Vector c = cross;//v0.Cross(v1);
  68.  
  69.     q.x = c.x * invs;
  70.     q.y = c.y * invs;
  71.     q.z = c.z * invs;
  72.     q.w = s * 0.5;
  73.    
  74.     *this = q;
  75. }
  76.  
  77. void Quat::FromRotationMatrix (const matrix3x4_t& kRot)
  78. {
  79.     // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
  80.     // article "Quat Calculus and Fast Animation".
  81.  
  82.     float fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
  83.     float fRoot;
  84.  
  85.     if ( fTrace > 0.0 )
  86.     {
  87.         // |w| > 1/2, may as well choose w > 1/2
  88.         fRoot = sqrt(fTrace + 1.0);  // 2w
  89.         w = 0.5*fRoot;
  90.         fRoot = 0.5/fRoot;  // 1/(4w)
  91.         x = (kRot[2][1]-kRot[1][2])*fRoot;
  92.         y = (kRot[0][2]-kRot[2][0])*fRoot;
  93.         z = (kRot[1][0]-kRot[0][1])*fRoot;
  94.     }
  95.     else
  96.     {
  97.         // |w| <= 1/2
  98.         static size_t s_iNext[3] = { 1, 2, 0 };
  99.         size_t i = 0;
  100.         if ( kRot[1][1] > kRot[0][0] )
  101.             i = 1;
  102.         if ( kRot[2][2] > kRot[i][i] )
  103.             i = 2;
  104.         size_t j = s_iNext[i];
  105.         size_t k = s_iNext[j];
  106.  
  107.         fRoot = sqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0);
  108.         float* apkQuat[3] = { &x, &y, &z };
  109.         *apkQuat[i] = 0.5*fRoot;
  110.         fRoot = 0.5/fRoot;
  111.         w = (kRot[k][j]-kRot[j][k])*fRoot;
  112.         *apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
  113.         *apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
  114.     }
  115. }
  116.  
  117.  void Quat::ToRotationMatrix (matrix3x4_t& kRot) const
  118. {
  119.     float fTx  = 2.0*x;
  120.     float fTy  = 2.0*y;
  121.     float fTz  = 2.0*z;
  122.     float fTwx = fTx*w;
  123.     float fTwy = fTy*w;
  124.     float fTwz = fTz*w;
  125.     float fTxx = fTx*x;
  126.     float fTxy = fTy*x;
  127.     float fTxz = fTz*x;
  128.     float fTyy = fTy*y;
  129.     float fTyz = fTz*y;
  130.     float fTzz = fTz*z;
  131.  
  132.     kRot[0][0] = 1.0-(fTyy+fTzz);
  133.     kRot[0][1] = fTxy-fTwz;
  134.     kRot[0][2] = fTxz+fTwy;
  135.     kRot[1][0] = fTxy+fTwz;
  136.     kRot[1][1] = 1.0-(fTxx+fTzz);
  137.     kRot[1][2] = fTyz-fTwx;
  138.     kRot[2][0] = fTxz-fTwy;
  139.     kRot[2][1] = fTyz+fTwx;
  140.     kRot[2][2] = 1.0-(fTxx+fTyy);
  141. }
  142. /*
  143. void Quat::GetAngles( RadEuler &angles )
  144. {
  145.     // FIXME: doing it this way calculates too much data, needs to do an optimized version...
  146.     matrix3x4_t matrix = GetMatrix();//QuatMatrix( *this, matrix );
  147.     MatrixAngles( matrix, angles );
  148.  
  149.     Assert( angles.IsValid() );
  150. }
  151. */
  152. void Quat::GetAngles( QAngle &angles )
  153. {
  154.     Assert( IsValid() );
  155.  
  156. #if 1
  157.     // FIXME: doing it this way calculates too much data, needs to do an optimized version...
  158.     matrix3x4_t matrix = GetMatrix();
  159.     MatrixAngles( matrix, angles );;
  160.     //Assert( angles.IsValid() );
  161. #else
  162.     /*
  163.     float m11, m12, m13, m23, m33;
  164.     Quat q = *this;
  165.     m11 = ( 2.0f * q.w * q.w ) + ( 2.0f * q.x * q.x ) - 1.0f;
  166.     m12 = ( 2.0f * q.x * q.y ) + ( 2.0f * q.w * q.z );
  167.     m13 = ( 2.0f * q.x * q.z ) - ( 2.0f * q.w * q.y );
  168.     m23 = ( 2.0f * q.y * q.z ) + ( 2.0f * q.w * q.x );
  169.     m33 = ( 2.0f * q.w * q.w ) + ( 2.0f * q.z * q.z ) - 1.0f;
  170.  
  171.     // FIXME: this code has a singularity near PITCH +-90
  172.     angles[YAW] = RAD2DEG( atan2(m12, m11) );
  173.     angles[PITCH] = RAD2DEG( asin(-m13) );
  174.     angles[ROLL] = RAD2DEG( atan2(m23, m33) );
  175.     */
  176.     Quat q1 = *this;
  177.     //q1.NormalizeInPlace();
  178.     float sqw = q1.w*q1.w;
  179.     float sqx = q1.x*q1.x;
  180.     float sqy = q1.y*q1.y;
  181.     float sqz = q1.z*q1.z;
  182.     float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
  183.     float test = q1.x*q1.y + q1.z*q1.w;
  184.     float hpi = M_PI/2.0f;
  185.     if (test > 0.5*unit) { // singularity at north pole
  186.         angles.x = RAD2DEG(2 * atan2(q1.x,q1.w));
  187.         angles.y = RAD2DEG(hpi);
  188.         angles.z = 0;
  189.         return;
  190.     }
  191.     if (test < -0.5*unit) { // singularity at south pole
  192.         angles.x = RAD2DEG(-2 * atan2(q1.x,q1.w));
  193.         angles.y = RAD2DEG(-hpi);
  194.         angles.z = 0;
  195.         return;
  196.     }
  197.     angles.x = RAD2DEG(atan2(2*q1.y*q1.w-2*q1.x*q1.z , sqx - sqy - sqz + sqw));
  198.     angles.y = RAD2DEG(asin(2*test/unit));
  199.     angles.z = RAD2DEG(atan2(2*q1.x*q1.w-2*q1.y*q1.z , -sqx + sqy - sqz + sqw));
  200. #endif
  201.  
  202.  
  203.     Assert( angles.IsValid() );
  204. }
  205.  
  206. matrix3x4_t Quat::GetMatrix()
  207. {
  208.     Quat q = *this;
  209.     Assert( IsValid() );
  210.  
  211.     matrix3x4_t matrix;
  212. #ifdef _VPROF_MATHLIB
  213.     VPROF_BUDGET( "QuatMatrix", "Mathlib" );
  214. #endif
  215.  
  216. // Original code
  217. // This should produce the same code as below with optimization, but looking at the assmebly,
  218. // it doesn't.  There are 7 extra multiplies in the release build of this, go figure.
  219. #if 0
  220.     matrix[0][0] = 1.0 - 2.0 * q.y * q.y - 2.0 * q.z * q.z;
  221.     matrix[1][0] = 2.0 * q.x * q.y + 2.0 * q.w * q.z;
  222.     matrix[2][0] = 2.0 * q.x * q.z - 2.0 * q.w * q.y;
  223.  
  224.     matrix[0][1] = 2.0f * q.x * q.y - 2.0f * q.w * q.z;
  225.     matrix[1][1] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.z * q.z;
  226.     matrix[2][1] = 2.0f * q.y * q.z + 2.0f * q.w * q.x;
  227.  
  228.     matrix[0][2] = 2.0f * q.x * q.z + 2.0f * q.w * q.y;
  229.     matrix[1][2] = 2.0f * q.y * q.z - 2.0f * q.w * q.x;
  230.     matrix[2][2] = 1.0f - 2.0f * q.x * q.x - 2.0f * q.y * q.y;
  231. #else
  232.    float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
  233.  
  234.     // precalculate common multiplitcations
  235.     x2 = q.x + q.x;
  236.     y2 = q.y + q.y;
  237.     z2 = q.z + q.z;
  238.     xx = q.x * x2;
  239.     xy = q.x * y2;
  240.     xz = q.x * z2;
  241.     yy = q.y * y2;
  242.     yz = q.y * z2;
  243.     zz = q.z * z2;
  244.     wx = q.w * x2;
  245.     wy = q.w * y2;
  246.     wz = q.w * z2;
  247.  
  248.     matrix[0][0] = 1.0 - (yy + zz);
  249.     matrix[0][1] = xy - wz;
  250.     matrix[0][2] = xz + wy;
  251.                            
  252.     matrix[1][0] = xy + wz;
  253.     matrix[1][1] = 1.0 - (xx + zz);
  254.     matrix[1][2] = yz - wx;
  255.  
  256.     matrix[2][0] = xz - wy;
  257.     matrix[2][1] = yz + wx;
  258.     matrix[2][2] = 1.0 - (xx + yy);
  259. #endif
  260.     return matrix;
  261. }
  262. //-----------------------------------------------------------------------------
  263. // Array access
  264. //-----------------------------------------------------------------------------
  265. inline vec_t& Quat::operator[](int i)
  266. {
  267.     Assert( (i >= 0) && (i < 4) );
  268.     return ((vec_t*)this)[i];
  269. }
  270.  
  271. inline vec_t Quat::operator[](int i) const
  272. {
  273.     Assert( (i >= 0) && (i < 4) );
  274.     return ((vec_t*)this)[i];
  275. }
  276.  
  277.  
  278. //-----------------------------------------------------------------------------
  279. // Equality test
  280. //-----------------------------------------------------------------------------
  281. inline bool Quat::operator==( const Quat &src ) const
  282. {
  283.     return ( x == src.x ) && ( y == src.y ) && ( z == src.z ) && ( w == src.w );
  284. }
  285.  
  286. inline bool Quat::operator!=( const Quat &src ) const
  287. {
  288.     return !operator==( src );
  289. }
  290.  
  291. extern void AngleQuat( RadEuler const &angles, Quat &qt );
  292. extern void QuatAngles( Quat const &q, RadEuler &angles );
  293. inline Quat::Quat(RadEuler const &angle)
  294. {
  295.     AngleQuat( angle, *this );
  296. }
  297.  
  298. //-----------------------------------------------------------------------------
  299. // Array access
  300. //-----------------------------------------------------------------------------
  301. inline vec_t& RadEuler::operator[](int i)
  302. {
  303.     Assert( (i >= 0) && (i < 3) );
  304.     return ((vec_t*)this)[i];
  305. }
  306.  
  307. inline vec_t RadEuler::operator[](int i) const
  308. {
  309.     Assert( (i >= 0) && (i < 3) );
  310.     return ((vec_t*)this)[i];
  311. }
  312.  
  313. inline Quat Quat::operator*(const Quat& q) const   
  314. {
  315.     Quat qt;
  316.     //QuatMult( this, q, res );
  317.  
  318.     Assert( IsValid() );
  319.     Assert( q.IsValid() );
  320.  
  321.     if (&*this == &qt)
  322.     {
  323.         Quat p2 = *this;
  324.         //QuatMult( p2, q, qt );
  325.         return (p2 * q);
  326.     }
  327.  
  328.     // decide if one of the Quats is backwards
  329.     Quat q2;
  330.     q2.Align( *this, q );
  331.  
  332.     qt.x =  x * q2.w + y * q2.z - z * q2.y + w * q2.x;
  333.     qt.y = -x * q2.z + y * q2.w + z * q2.x + w * q2.y;
  334.     qt.z =  x * q2.y - y * q2.x + z * q2.w + w * q2.z;
  335.     qt.w = -x * q2.x - y * q2.y - z * q2.z + w * q2.w;
  336.  
  337.     return qt;
  338. }
  339.  
  340. Vector Quat::operator* (const Vector& vec) const
  341. {
  342.     // nVidia SDK implementation
  343.     Vector uv, uuv;
  344.     Vector qvec(x, y, z);
  345.     uv = qvec.Cross(vec);
  346.     uuv = qvec.Cross(uv);
  347.     uv *= (2.0f * w);
  348.     uuv *= 2.0f;
  349.  
  350.     return vec + uv + uuv;
  351.  
  352. }
  353.  
  354. void Quat::Slerp( Quat &from, Quat &to, float t )
  355. {
  356.     Quat q2, qt;
  357.     // 0.0 returns p, 1.0 return q.
  358.  
  359.     // decide if one of the Quats is backwards
  360.     q2.Align( from, to );
  361.  
  362.     SlerpNoAlign( from, q2, t );
  363. }
  364.  
  365. void Quat::SlerpNoAlign( const Quat &p, const Quat &q, float t )
  366. {
  367.     Quat qt;
  368.     float omega, cosom, sinom, sclp, sclq;
  369.     int i;
  370.  
  371.     // 0.0 returns p, 1.0 return q.
  372.  
  373.     cosom = p[0]*q[0] + p[1]*q[1] + p[2]*q[2] + p[3]*q[3];
  374.  
  375.     if ((1.0f + cosom) > 0.000001f) {
  376.         if ((1.0f - cosom) > 0.000001f) {
  377.             omega = acos( cosom );
  378.             sinom = sin( omega );
  379.             sclp = sin( (1.0f - t)*omega) / sinom;
  380.             sclq = sin( t*omega ) / sinom;
  381.         }
  382.         else {
  383.             // TODO: add short circuit for cosom == 1.0f?
  384.             sclp = 1.0f - t;
  385.             sclq = t;
  386.         }
  387.         for (i = 0; i < 4; i++) {
  388.             qt[i] = sclp * p[i] + sclq * q[i];
  389.         }
  390.     }
  391.     else {
  392.         Assert( &qt != &q );
  393.  
  394.         qt[0] = -q[1];
  395.         qt[1] = q[0];
  396.         qt[2] = -q[3];
  397.         qt[3] = q[2];
  398.         sclp = sin( (1.0f - t) * (0.5f * M_PI));
  399.         sclq = sin( t * (0.5f * M_PI));
  400.         for (i = 0; i < 3; i++) {
  401.             qt[i] = sclp * p[i] + sclq * qt[i];
  402.         }
  403.     }
  404.  
  405.     Assert( qt.IsValid() );
  406.     *this = qt;
  407. }
  408.  
  409. inline float Quat::NormalizeInPlace()
  410. {
  411.     Quat q = *this;
  412.     float radius, iradius;
  413.  
  414.     Assert( q.IsValid() );
  415.  
  416.     radius = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
  417.  
  418.     if ( radius ) // > FLT_EPSILON && ((radius < 1.0f - 4*FLT_EPSILON) || (radius > 1.0f + 4*FLT_EPSILON))
  419.     {
  420.         radius = sqrt(radius);
  421.         iradius = 1.0f/radius;
  422.         q[3] *= iradius;
  423.         q[2] *= iradius;
  424.         q[1] *= iradius;
  425.         q[0] *= iradius;
  426.     }
  427.     return radius;
  428. }
  429.  
  430. void Quat::CreateFromYawPitchRoll(Vector up, float yaw, Vector right, float pitch, Vector forward, float roll)
  431. {
  432.     // Create a Quat for each rotation, and multiply them
  433.     // together.  We normalize them to avoid using the conjugate
  434.     Quat qyaw;
  435.     qyaw.AxisAngle( up, yaw );
  436.     qyaw.NormalizeInPlace();
  437.  
  438.     Quat qtilt;
  439.     qtilt.AxisAngle( right, pitch );
  440.     qtilt.NormalizeInPlace();
  441.  
  442.     Quat qroll;
  443.     qroll.AxisAngle( forward, roll );
  444.     qroll.NormalizeInPlace();
  445.  
  446.     Quat yawpitch = qyaw * qtilt * qroll;
  447.     yawpitch.NormalizeInPlace();
  448.  
  449.     *this = yawpitch;
  450. }
  451.  
  452. void Quat::Align( const Quat &p, const Quat &q )
  453. {
  454.     Quat qt;
  455.     int i;
  456.     // decide if one of the Quats is backwards
  457.     float a = 0;
  458.     float b = 0;
  459.     for (i = 0; i < 4; i++)
  460.     {
  461.         a += (p[i]-q[i])*(p[i]-q[i]);
  462.         b += (p[i]+q[i])*(p[i]+q[i]);
  463.     }
  464.     if (a > b)
  465.     {
  466.         for (i = 0; i < 4; i++)
  467.         {
  468.             qt[i] = -q[i];
  469.         }
  470.     }
  471.     else if (&qt != &q)
  472.     {
  473.         for (i = 0; i < 4; i++)
  474.         {
  475.             qt[i] = q[i];
  476.         }
  477.     }
  478.     *this = qt;
  479. }
  480.  
  481. void Quat::TransformVector( Vector &vec )
  482. {
  483.     Quat qConj, qVec, qResult;
  484.     qVec.x = vec.x;
  485.     qVec.y = vec.y;
  486.     qVec.z = vec.z;
  487.     qVec.w = 0;
  488.  
  489.     qConj.x = -x;
  490.     qConj.y = -y;
  491.     qConj.z = -z;
  492.     qConj.w = w;
  493.  
  494.     qResult = *this * qVec * qConj;
  495.     vec = Vector( qVec.x, qVec.y, qVec.z );
  496. }
  497.  
  498. #ifndef M_PI
  499.     #define M_PI        3.14159265358979323846  // matches value in gcc v2 math.h
  500. #endif
  501. //#define M_PI_F        ((float)(M_PI)) // Shouldn't collide with anything.
  502. #ifndef DEG2RAD
  503.     #define DEG2RAD( x )  ( x * (M_PI / 180.f) )
  504. #endif
  505.  
  506. void Quat::AxisAngle( const Vector &axis, float angles )
  507. {
  508.     float sa, ca;
  509.    
  510.     SinCos( DEG2RAD( angles/2 ), &sa, &ca );
  511.  
  512.     x = axis.x * sa;
  513.     y = axis.y * sa;
  514.     z = axis.z * sa;
  515.     w = ca;
  516. }
  517.  
  518. void Quat::ToAxisAngle( Vector &axis, float &angle )//ToAxisAngle( Vector &axis, float &angle ) const
  519. {
  520.     Quat q = *this;
  521.     if (q.w > 1)
  522.         q.NormalizeInPlace(); // if w>1 acos and sqrt will produce errors, this cant happen if Quat is normalised
  523.    
  524.     angle = 2 * acos(q.w);
  525.     double s = sqrt(1-q.w*q.w); // assuming Quat normalised then w is less than 1, so term always positive.
  526.     if (s < 0.001)
  527.     { // test to avoid divide by zero, s is always positive due to sqrt
  528.         // if s close to zero then direction of axis not important
  529.         axis.x = q.x; // if it is important that axis is normalised then replace with x=1; y=z=0;
  530.         axis.y = q.y;
  531.         axis.z = q.z;
  532.     }
  533.     else
  534.     {
  535.         axis.x = q.x / s; // normalise axis
  536.         axis.y = q.y / s;
  537.         axis.z = q.z / s;
  538.     }
  539.     /*
  540.     const float squareLength = x*x + y*y + z*z;
  541.     const float epsilon = 0.00001f;    ///< floating point epsilon for single precision. todo: verify epsilon value and usage
  542.     const float epsilonSquared = epsilon * epsilon;
  543.  
  544.     if (squareLength>epsilonSquared)
  545.     {
  546.         angle = 2.0f * (float) acos(w);
  547.         const float inverseLength = 1.0f / (float) pow(squareLength, 0.5f);
  548.         axis.x = x * inverseLength;
  549.         axis.y = y * inverseLength;
  550.         axis.z = z * inverseLength;
  551.     }
  552.     else
  553.     {
  554.         angle = 0.0f;
  555.         axis.x = 1.0f;
  556.         axis.y = 0.0f;
  557.         axis.z = 0.0f;
  558.     }
  559.     */
  560. }
  561.  
  562. void Quat::ToAxes (Vector* akAxis) const
  563. {
  564.     matrix3x4_t kRot;
  565.  
  566.     ToRotationMatrix(kRot);
  567.  
  568.     for (size_t iCol = 0; iCol < 3; iCol++)
  569.     {
  570.         akAxis[iCol].x = kRot[0][iCol];
  571.         akAxis[iCol].y = kRot[1][iCol];
  572.         akAxis[iCol].z = kRot[2][iCol];
  573.     }
  574. }
  575.  
  576. Quat Quat::operator/(const Quat & q)const
  577. //! @brief Overload / operator.
  578. {
  579. //   Quat q = rhs;
  580. //   q = *this * q.conjugate();
  581. //   q = *this * q.i();
  582.  
  583. //   return q;
  584.  
  585.     return *this * q.i();
  586. }
  587.  
  588. Quat Quat::operator*(const float c)const
  589. /*!
  590.   @brief Overload * operator, multiplication by a scalar.
  591.  
  592.   \f$q = [s, v]\f$ and let \f$r \in R\f$. Then
  593.   \f$rq = qr = [r, 0][s, v] = [rs, rv]\f$
  594.  
  595.   The result is not necessarily a unit Quat even if \f$q\f$
  596.   is a unit Quats.
  597. */
  598. {
  599.    Quat q;
  600.    q.w = w * c;
  601.    q.x = x * c;
  602.    q.y = y * c;
  603.    q.z = z * c;
  604.  
  605.    return q;
  606. }
  607.  
  608.  
  609. Quat Slerp2(const Quat & q0, const Quat & q1, const float t)
  610. /*!
  611.   @brief Spherical Linear Interpolation.
  612.  
  613.   Cite_:Dam
  614.  
  615.   The Quat \f$q(t)\f$ interpolate the Quats \f$q_0\f$
  616.   and \f$q_1\f$ given the parameter \f$t\f$ along the Quat sphere.
  617.   \f[
  618.    q(t) = c_0(t)q_0 + c_1(t)q_1
  619.   \f]
  620.   where \f$c_0\f$ and \f$c_1\f$ are real functions with \f$0\leq t \leq 1\f$.
  621.   As \f$t\f$ varies between 0 and 1. the values \f$q(t)\f$ varies uniformly
  622.   along the circular arc from \f$q_0\f$ and \f$q_1\f$. The angle between
  623.   \f$q(t)\f$ and \f$q_0\f$ is \f$\cos(t\theta)\f$ and the angle between
  624.   \f$q(t)\f$ and \f$q_1\f$ is \f$\cos((1-t)\theta)\f$. Taking the dot product
  625.   of \f$q(t)\f$ and \f$q_0\f$ yields
  626.  
  627.    \cos(t\theta) = c_0(t) + \cos(\theta)c_1(t)
  628.  
  629.   and taking the dot product of \f$q(t)\f$ and \f$q_1\f$ yields
  630.  
  631.    \cos((1-t)\theta) = \cos(\theta)c_0(t) + c_1(t)
  632.  
  633.   These are two equations with \f$c_0\f$ and \f$c_1\f$. The solution is
  634.  
  635.    c_0 = \frac{\sin((1-t)\theta)}{\sin(\theta)}
  636.  
  637.  
  638.    c_1 = \frac{\sin(t\theta)}{sin(\theta)}
  639.  
  640.   The interpolation is then
  641.  
  642.    Slerp(q_0, q_1, t) = \frac{q_0\sin((1-t)\theta)+q_1\sin(t\theta)}{\sin(\theta)}
  643.  
  644.   If \f$q_0\f$ and \f$q_1\f$ are unit Quats the \f$q(t)\f$ is also a unit
  645.   Quats. For unit Quats we have
  646.  
  647.    Slerp(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t
  648.  
  649.   For t = 0 and t = 1 we have
  650.  
  651.    q_0 = Slerp(q_0, q_1, 0)
  652.  
  653.    q_1 = Slerp(q_0, q_1, 1)
  654.  
  655.   It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle
  656.   between q0 ang Gq1 is acute). This choice avoids extra spinning caused
  657.   by the interpolated rotations.
  658. */
  659. {
  660.    if( (t < 0) || (t > 1) )
  661.       Msg( "Slerp(q0, q1, t): t < 0 or t > 1. t is set to 0.\n" );
  662.  
  663.    float dotprod = q0.dot_prod(q1);
  664.  
  665.    Quat result;
  666.    if(dotprod >= 0)
  667.        result = q0.i() * q1;
  668.    else
  669.    {
  670.        result = q0.i() * -1;
  671.        result = result * q1;
  672.    }
  673.  
  674.    result = result.power(t);
  675.    result = q0 * result;
  676.  
  677.    return result;
  678. }
  679.  
  680. Quat Slerp_Prime(const Quat & q0, const Quat & q1,
  681.                        const float t)
  682. /*!
  683.   @brief Spherical Linear Interpolation derivative.
  684.  
  685.   Cite_: Dam
  686.  
  687.   The derivative of the function \f$q^t\f$ where \f$q\f$ is a constant
  688.   unit Quat is
  689.  
  690.    \frac{d}{dt}q^t = q^t log(q)
  691.  
  692.   Using the preceding equation the Slerp derivative is then
  693.  
  694.    Slerp'(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t log(q_0^{-1}q_1)
  695.  
  696.  
  697.   It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle
  698.   between q0 ang Gq1 is acute). This choice avoids extra spinning caused
  699.   by the interpolated rotations.
  700.   The result is not necessary a unit Quat.
  701. */
  702.  
  703. {
  704.    if( (t < 0) || (t > 1) )
  705.       Msg( "Slerp_prime(q0, q1, t): t < 0 or t > 1. t is set to 0.\n" );
  706.  
  707.    Quat result;
  708.    float dotprod = q0.dot_prod(q1);
  709.  
  710.    if(dotprod >= 0)
  711.    {
  712.       result = q0.i() * q1;
  713.    }
  714.    else
  715.    {
  716.        result = q0.i() * -1;
  717.        result = result * q1;
  718.    }
  719.    result = result.Logr();
  720.    result = Slerp2(q0, q1, t) * result;
  721.  
  722.    return result;
  723. }
  724.  
  725. Quat Quat::i()const
  726. /*!
  727.   @brief Quat inverse.
  728.   \f[
  729.     q^{-1} = \frac{q^{*}}{N(q)}
  730.   \f]
  731.   where \f$q^{*}\f$ and \f$N(q)\f$ are the Quat
  732.   conjugate and the Quat norm respectively.
  733. */
  734. {
  735.     Quat norm(x,y,z,w);
  736.     Quat conj(-x,-y,-z,w);
  737.     norm.NormalizeInPlace();
  738.    
  739.     return conj/norm;
  740. }
  741.  
  742. Quat Quat::expon() const
  743. /*!
  744.   @brief Exponential of a Quat.
  745.  
  746.   Let a Quat of the form \f$q = [0, \theta v]\f$, q is not
  747.   necessarily a unit Quat. Then the exponential function
  748.   is defined by \f$q = [\cos(\theta),v \sin(\theta)]\f$.
  749. */
  750. {
  751.    Quat q;
  752.    float theta = sqrt(1.0f);
  753.    float sin_theta = sin(theta);
  754.  
  755.    q.w = cos(theta);
  756.    if ( fabs(sin_theta) > 0.0001f)
  757.       {
  758.        sin_theta /= theta;
  759.        q.x = x / sin_theta;
  760.        q.y = y / sin_theta;
  761.        q.z = z / sin_theta;
  762.      
  763.    }
  764.    else
  765.    {
  766.        q.x = x;
  767.        q.y = y;
  768.        q.z = z;
  769.    }
  770.  
  771.    return q;
  772. }
  773.  
  774. Quat Quat::power(const float t) const
  775. {
  776.    Quat q;// = (Logr()*t).expon();
  777.  
  778.    q = Logr();
  779.    q = q * t;
  780.    q = q.expon();
  781.    return q;
  782. }
  783.  
  784. Quat Quat::Logr()const
  785. /*!
  786.   @brief Logarithm of a unit Quat.
  787.  
  788.   The logarithm function of a unit Quat
  789.   \f$q = [\cos(\theta), v \sin(\theta)]\f$ is defined as
  790.   \f$log(q) = [0, v\theta]\f$. The result is not necessary
  791.   a unit Quat.
  792. */
  793. {
  794.    Quat q;
  795.    q.w = 0;
  796.    float theta = acos(w);
  797.    float sin_theta = sin(theta);
  798.  
  799.    if ( fabs(sin_theta) > 0.0001f )
  800.    {
  801.        sin_theta *= theta;
  802.        q.x = x / sin_theta;
  803.        q.y = y / sin_theta;
  804.        q.z = z / sin_theta;
  805.      
  806.    }
  807.    else
  808.    {
  809.        q.x = x;
  810.        q.y = y;
  811.        q.z = z;
  812.    }
  813.  
  814.    return q;
  815. }
  816.  
  817. float Quat::dot_prod(const Quat & q)const
  818. /*!
  819.   @brief Quat dot product.
  820. */
  821. {
  822.     Quat temp = *this;
  823.  
  824.    return (temp.w*q.w + temp.x*q.x + temp.y*q.y + temp.z*q.z);//v_(1)*q.v_(1) + v_(2)*q.v_(2) + v_(3)*q.v_(3));
  825. }
Advertisement
Add Comment
Please, Sign In to add comment