// 版權所有(C) 樑意, 2004

// 最後修改: 2004.8.杭州

#ifndef ROTATION_HEADER<?xml:namespace prefix = o ns = "urn:schemas-microsoft-com:office:office" />

#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>

voidQuaternionToMatrix_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);

}

quaternionoperator +(quaternion &q)

{

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

}

quaternionoperator -(quaternion &q)

{

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

}

quaternionoperator *(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);

}

quaternionoperator *(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)

{

Tto1[4];

Tomega, 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