Biomechanical Joint Model  Author: Anderson Maciel

---

quat.cpp

Go to the documentation of this file.

#include "quat.h"

namespace LinAlg{



QUAT::QUAT(const HMAT &M) : VEC(4)
{
   REAL ww, xx, yy, four_w, two_x;   
   ww = 0.25 * (1.0 + M(0,0) + M(1,1) + M(2,2));
  
   if (ww > epsilon) 
   {
      v_[QUAT_W] = sqrt(ww);
      four_w  = 4.0 * v_[QUAT_W];
      v_[QUAT_X] = (M(2,1) - M(1,2)) / four_w;
      v_[QUAT_Y] = (M(0,2) - M(2,0)) / four_w;
      v_[QUAT_Z] = (M(1,0) - M(0,1)) / four_w;
   } 
   else 
   {
      v_[QUAT_W] = 0.0;
      xx = -0.5 * (M(1,1) + M(2,2));
     
      if (xx > epsilon) 
      {
         v_[QUAT_X] = sqrt(xx);
         two_x = v_[QUAT_X] * 2.0;
         v_[QUAT_Y] = M(1,0) / two_x;
         v_[QUAT_Z] = M(2,0) / two_x;
      } 
      else 
      {
         v_[QUAT_X] = 0.0;
         yy = 0.5 * (1.0 - M(2,2));
        
         if (yy > epsilon) 
         {
            v_[QUAT_Y] = sqrt(yy);
            v_[QUAT_Z] = M(2,1) / (2.0 * v_[QUAT_Y]);
         } 
         else 
         {
            v_[QUAT_Y] = 0.0;
            v_[QUAT_Z] = 1.0;
         }
      }
   }
}

QUAT::operator HMAT() const
{
   REAL xx, yy, zz, xy, xz, yz, wx, wy, wz;
   HMAT result;

   REAL qnorm2 = norm2();
  
   xx = 2.0 * v_[QUAT_X] * v_[QUAT_X] / qnorm2;
   yy = 2.0 * v_[QUAT_Y] * v_[QUAT_Y] / qnorm2;
   zz = 2.0 * v_[QUAT_Z] * v_[QUAT_Z] / qnorm2;
   xy = 2.0 * v_[QUAT_X] * v_[QUAT_Y] / qnorm2;
   xz = 2.0 * v_[QUAT_X] * v_[QUAT_Z] / qnorm2;
   yz = 2.0 * v_[QUAT_Y] * v_[QUAT_Z] / qnorm2;
   wx = 2.0 * v_[QUAT_W] * v_[QUAT_X] / qnorm2;
   wy = 2.0 * v_[QUAT_W] * v_[QUAT_Y] / qnorm2;
   wz = 2.0 * v_[QUAT_W] * v_[QUAT_Z] / qnorm2;
  
   result(0,0)= 1.0-yy-zz; 
   result(1,0)= xy+wz;     
   result(2,0)= xz-wy;     
   result(0,1)= xy-wz;     
   result(1,1)= 1.0-xx-zz; 
   result(2,1)= yz+wx;     
   result(0,2)= xz+wy;     
   result(1,2)= yz-wx;     
   result(2,2)= 1.0-xx-yy; 

   return result;
}

QUAT 
operator*(const QUAT &left, const QUAT &right)
{
   QUAT result;

   result[QUAT_W] = 
      left[QUAT_W]*right[QUAT_W] - 
      left[QUAT_X]*right[QUAT_X] - 
      left[QUAT_Y]*right[QUAT_Y] - 
      left[QUAT_Z]*right[QUAT_Z];

   result[QUAT_X] = 
      left[QUAT_W]*right[QUAT_X] + 
      left[QUAT_X]*right[QUAT_W] + 
      left[QUAT_Y]*right[QUAT_Z] - 
      left[QUAT_Z]*right[QUAT_Y];

   result[QUAT_Y] = 
      left[QUAT_W]*right[QUAT_Y] + 
      left[QUAT_Y]*right[QUAT_W] + 
      left[QUAT_Z]*right[QUAT_X] - 
      left[QUAT_X]*right[QUAT_Z];

   result[QUAT_Z] = 
      left[QUAT_W]*right[QUAT_Z] + 
      left[QUAT_Z]*right[QUAT_W] + 
      left[QUAT_X]*right[QUAT_Y] - 
      left[QUAT_Y]*right[QUAT_X];

   return result;
}

void 
QUAT::slerp(const QUAT &left, const QUAT &right, REAL u, QUAT &result)
{
   QUAT Q2;
   REAL cs, sx, theta, scale0, scale1;

   cs = dot_prod(left, right);
   /* if quaternions are opposed, negate the second one */
   /* this ensures the shortest path is taken */
   if (cs < 0)
   {
      cs = -cs;
      Q2.set(-right);      
   }
   else Q2 = right;

   // calculate coefficients
   if (1.0-cs > epsilon) 
   {
      theta = acos(cs);
      sx = sin(theta);
      scale0 = sin((1.0-u) * theta) / sx;
      scale1 = sin(u * theta) / sx;
   }
   else
   {
      scale0 = 1.0 - u;
      scale1 = u;
   }
   
   result.set( scale0*left + scale1*Q2);
}



}

---