向量旋转与四元组 的运算和转换

// 版权所有(C) 梁意, 2004

// 最后修改: 2004.8.杭州

#ifndef ROTATION_HEADER

#define ROTATION_HEADER

 

#include"matrix.h"

 

const double DELTA_ROT=1.0E-10;

 

 

template<typename T>

class quaternion;

template<typename T>

class rotation;

 

template<typename T>

quaternion RotationToQuaternion( const rotation &r) //r 已归一化

{

     T cosValue, sinValue, theta;

     theta    = r.angle/180.0f*M_PI;

     cosValue = T(cos(theta/2.0f)); 

     sinValue = T(sin(theta/2.0f)); 

 

     return quaternion (sinValue*r.axis.x,

         sinValue*r.axis.y,

         sinValue*r.axis.z,

         cosValue);

}

 

template<typename T>

rotation QuaternionToRotation( const quaternion &q) //q 已归一化

{

     if( ((q.w>1-DELTA_ROT)&&(q.w<1+DELTA_ROT)) ||

         ((q.w>-1-DELTA_ROT)&&(q.w<-1+DELTA_ROT)) )

     {

         return rotation ();

     }

 

     T sinValue, halfTheta;

     halfTheta= T(acos(Q.w));

     sinValue = T(sin(halfTheta)); 

    

     if(sinValue < DELTA_ROT)

     {

         return rotation ();

     }

     return rotation ( q.x/sinValue,

         q.y/sinValue,q.z/sinValue,

         halfTheta * 2.0f * 180.0f /3.14159f);

}

template<typename T>

void QuaternionToMatrix(T M[16], const quaternion & q) //q 已归一化

{

     T m[4][4]; 

     T wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

 

     // calculate coefficients

     x2 = q.x * 2.0f;

     y2 = q.y * 2.0f;

     z2 = q.z * 2.0f;

     xx = q.x * x2;   xy = q.x * y2;   xz = q.x * z2;

     yy = q.y * y2;   yz = q.y * z2;   zz = q.z * z2;

     wx = q.w * x2;   wy = q.w * y2;   wz = q.w * z2;

 

     m[0][0] = 1.0f - (yy + zz);     

     m[0][1] = xy + wz;

     m[0][2] = xz - wy;    

     m[0][3] = 0.0f;

 

     m[1][0] = xy - wz;    

     m[1][1] = 1.0f - (xx + zz);

     m[1][2] = yz + wx;    

     m[1][3] = 0.0f;

 

     m[2][0] = xz + wy;    

     m[2][1] = yz - wx;

     m[2][2] = 1.0f - (xx + yy);     

     m[2][3] = 0.0f;

 

     m[3][0] = 0;          

     m[3][1] = 0;

     m[3][2] = 0;          

     m[3][3] = 1;

 

     for(size_t i=0; i<4; i++)

     for(size_t j=0; j<4; j++)

         M[i*4+j]=m[i][j]; 

}

 

template<typename T>

void  QuaternionToMatrix_OpenGL(T M[16], const quaternion & q)

{

  T m[4][4];  //column major format (OpenGL format)

 

  T wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

 

  // calculate coefficients

  x2 = q.x * 2.0f;

  y2 = q.y * 2.0f;

  z2 = q.z * 2.0f;

  xx = q.x * x2;   xy = q.x * y2;   xz = q.x * z2;

  yy = q.y * y2;   yz = q.y * z2;   zz = q.z * z2;

  wx = q.w * x2;   wy = q.w * y2;   wz = q.w * z2;

 

  m[0][0] = 1.0f - (yy + zz);   

  m[0][1] = xy - wz;

  m[0][2] = xz + wy;       

  m[0][3] = 0.0f;

 

  m[1][0] = xy + wz;       

  m[1][1] = 1.0f - (xx + zz);

  m[1][2] = yz - wx;       

  m[1][3] = 0.0f;

 

  m[2][0] = xz - wy;       

  m[2][1] = yz + wx;

  m[2][2] = 1.0f - (xx + yy);       

  m[2][3] = 0.0f;

 

  m[3][0] = 0;             

  m[3][1] = 0;

  m[3][2] = 0;             

  m[3][3] = 1;

 

  for(int i=0; i<4; i++)

  for(int j=0; j<4; j++)

       M[i*4+j]=m[i][j];    //column major format (OpenGL format)

}

template<typename T>

quaternion VectorToQuaternion( const vector3d &v)

{

     return quaternion ( V.x,V.y,V.z,0);

}

 

template<typename T>

vector3d QuaternionToVector( const quaternion Q)

{

     return vector3d (Q.x,Q.y,Q.z);

}

 

// V 经过 Q 变换后为 V', 函数返回 V'

template<typename T>

vector3d VectorTransform( const vector3d & V, const quaternion &Q)

{

     if( ((Q.w>1-DELTA_ROT)&&(Q.w<1+DELTA_ROT)) ||

         ((Q.w>-1-DELTA_ROT)&&(Q.w<-1+DELTA_ROT)) ) return V;

 

     quaternion A, B, C;

     A=VectorToQuaternion(V);

     B=Q;

     B.Inverse()

     C=Q*A*B;

     return QuaternionToVector(C);

}

 

 

template<typename T>

class rotation

{

public:

     vector3d axis;

     T angle;

public:

     rotation(T x=0,T y=0,T z=1.0,T _angle=0)

     {

         axis.x=x;

         axis.y=y;

         axis.z=z;

         angle=_angle;

     }

     rotation(vector3d &v,T _angle=0)

     {

         axis=v;

         angle=_angle;

     }

};

 

 

template<typename T>

class quaternion

{

public:

     T x,y,z,w;

 

public:

     quaternion(T _x=0,T _y=0,T _z=1,T _w=0)

     {

         x=_x;

         y=_y;

         z=_z;

         w=_w;

     }

     T Magnitude()

     {

         return sqrt(x*x+y*y+z*z+w*w);

     }

     void Normalize(quaternion &out)

     {

         T m=Magnitude();

         if(m>0)

         {

         out.x=x/m;

         out.y=y/m;

         out.z=z/m;

         out.w=w/m;

         }

     }

     void Normalize()

     {

         Normalize(*this);

     }

     void Inverse(quaternion &out)

     {

         T m=Magnitude();

         if(m>0)

         {

         out.x=-x/m;

         out.y=-y/m;

         out.z=-z/m;

         out.w=-w/m;

         }

        

     }

     void Inverse()

     {

         Inverse(*this);

     }

     void Conjugate(quaternion &out)

     {

         out.x=-x;

         out.y=-y;

         out.z=-z;

     }

     void Conjugate()

     {

         Conjugate(*this);

     }

 

     quaternion operator +(quaternion &q)

     {

         return quaternion (x+q.x,y+q.y,z+q.z,w+q.w);

     }

     quaternion operator -(quaternion &q)

     {

         return quaternion (x-q.x,y-q.y,z-q.z,w-q.w);

     }

     quaternion operator *( const quaternion &q)

     {

         return quaternion (w *q.x +x *q.w + y * q.z - z * q.y,

              w *q.y + y * q.w + z * q.x - x * q.z,

              w *q.z + z * q.w + x * q.y - y * q.x,

              w *q.w - x * q.x - y * q.y - z * q.z);

     }

     quaternion operator *(T s)

     {

         return quaternion (x*s,y*s,z*s,w*s);

     }

     // Nick Bobick 编写

     // Purpose:        Spherical Linear Interpolation Between two Quaternions

     // Arguments: Two Quaternions, blend factor

     quaternion slerp( const quaternion &from, const quaternion &to,T t)

     {

         T  to1[4];

         T  omega, cosom, sinom, scale0, scale1;

 

         // calc cosine

         cosom = from.x * to.x + from.y * to.y + from.z * to.z

                   + from.w * to.w;

 

         // adjust signs (if necessary)

         if ( cosom <0.0 ){ cosom = -cosom; to1[0] = - to.x;

         to1[1] = - to.y;

         to1[2] = - to.z;

         to1[3] = - to.w;

         } else  {

         to1[0] = to.x;

         to1[1] = to.y;

         to1[2] = to.z;

         to1[3] = to.w;

         }

 

         // calculate coefficients

 

         if ( (1.0 - cosom) > DELTA_ROT ) {

                       // standard case (slerp)

                       omega = T(acos(cosom));

                       sinom = T(sin(omega));

                       scale0 = T(sin((1.0 - t) * omega) / sinom);

                       scale1 = T(sin(t * omega) / sinom);

 

              } else {       

         // "from" and "to" quaternions are very close

              //  ... so we can do a linear interpolation

                       scale0 = 1.0f - t;

                       scale1 = t;

              }

         // calculate final values

         return(scale0 * from.x + scale1 * to1[0],

              scale0 * from.y + scale1 * to1[1],

              scale0 * from.z + scale1 * to1[2],

              scale0 * from.w + scale1 * to1[3]);

     }

};

 

#endif