xquaternion.cpp

XMathLib是一个通用的3D图形数学库。 其中包含两个部分： XMathLib和XGeomLib。分别处理数学和几何运算。 数学部分包含向量、矩阵、四元数的运算。以及其它的运算。 几何部分

/********************************************************************************
声明：
　　这里的代码版权归潘李亮所有．你可以自由使用分发这些代码．但是不得用于商业用途．
  如有需要请于作者联系．
  　在你使用本代码时候，请务必保留本声明
   　潘李亮　2003-10
	Stanly Lee. 2003-10

	Email : [e-mail]


	数学库的代码：
	　　　本数学库的目的是方便的实现一个图形几何管道．

	   对矩阵的操作函数．

	   四元数操作的实现。




	   *******************************************************************************/

#include "XMathLib.h"

namespace XMathLib
{

	XQuaternion XQuaternion::operator * (XQuaternion& q2)
	{
		XQuaternion ret ;
		ret.x = w*q2.x + x*q2.w + y*q2.z - z*q2.y;
		ret.y = w*q2.y + y*q2.w + z*q2.x - x*q2.z;
		ret.z = w*q2.z + z*q2.w + x*q2.y - y*q2.x;
		ret.w = w*q2.w - x*q2.x - y*q2.y - z*q2.z;
		return ret;
	}


	XVector    XQuaternion::Rotate(XVector& vIn)
	{
		XVector vTemp;
		XM_Rotate(*this,vIn,vTemp);
		return vTemp;
	}





    XMatrix XQuaternion::toMatrix()
	{
          XMatrix mOut;
		  toMatrix(mOut);
		  return mOut;
	}



	void     XQuaternion::toMatrix(XMatrix& mOut)
	{
#define M(row,col)  mOut.m [row ] [ col ]
         
		float x2 = x*x;
		float y2 = y*y;
		float z2 = z*z;
		float w2 = w*w;
		float xy = 2*x*y;
		float wz = 2*w*z;
		float wy = 2*w*y;
		float xz = 2*x*z;
		float yz = 2*y*z;
		float wx = 2*x*w;

		M(0,0) = w2 + x2 - y2 - z2;
		M(1,0) = xy - wz;
		M(2,0) = xz + wy;
		M(3,0) = 0;

        M(0,1) = xy + wz;
		M(1,1) = w2 - x2 + y2 - z2;
		M(2,1) = yz - wx;
		M(3,1) = 0;

		M(0,2) = xz - wy;
		M(1,2) = yz + wx;
		M(2,2) = w2 - x2 - y2 + z2;
		M(3,2) = 0;

		M(0,3) = 0;
		M(1,3) = 0;
		M(2,3) = 0;
		M(3,3) = 1;
#undef M
	}

	XQuaternion XQuaternion::Slerp(float t,XQuaternion& q2)
	{
		XQuaternion qOut;
		XM_Slerp(t,*this,q2,qOut);
		return qOut;
	}


    void    _MATH_LIB_EXPORT_ XM_Mul(XQuaternion& qA,XQuaternion& qB,XQuaternion& qOut)
    {
        qOut.x = qA.w*qB.x + qA.x*qB.w + qA.y*qB.z - qA.z*qB.y;
        qOut.y = qA.w*qB.y + qA.y*qB.w + qA.z*qB.x - qA.x*qB.z;
        qOut.z = qA.w*qB.z + qA.z*qB.w + qA.x*qB.y - qA.y*qB.x;
        qOut.w = qA.w*qB.w - qA.x*qB.x - qA.y*qB.y - qA.z*qB.z;
    }

    void    _MATH_LIB_EXPORT_ XM_Add(XQuaternion& qA,XQuaternion& qB,XQuaternion& qOut)
    {
        qOut.x = qA.x + qB.x  ;
        qOut.y = qA.y + qB.y  ;
        qOut.z = qA.z + qB.z  ;
        qOut.w = qA.w + qB.w  ;
    }

    void    _MATH_LIB_EXPORT_ XM_Inverse(XQuaternion& qA ,XQuaternion& qOut)
    {
        float n = qA.x * qA.x + qA.y * qA.y + qA.z * qA.z + qA.w * qA.w;
        n = -1.0f / n ;
        qOut = qA;
        qOut.x *= n;
        qOut.y *= n;
        qOut.z *= n;
        qOut.w *= n;
    }

    /*********************************************************
    将一个顶点绕一个轴旋转。
    这个旋转用四元数来表示。
    参数: q

    **********************************************************/
    void    _MATH_LIB_EXPORT_ XM_Rotate(XQuaternion& q,XVector& v,XVector& vOut)
    {
        XQuaternion qn = q;

        //单位化四元数。
        qn.normalize();
        //求逆
        XQuaternion qc(-qn.x,-qn.y,-qn.z,qn.w);
        //把顶点表示成四元数的形式。
        XQuaternion qv(v.x,v.y,v.z,1);

        //保存中间数据
        XQuaternion qtemp;
        //结果。
        XQuaternion qresult;
        //  [qn * V ] * qn(-1)
        XM_Mul(qn,qv,qtemp);
        XM_Mul(qtemp,qc,qresult);

        vOut.x = qresult.x;
        vOut.y = qresult.y;
        vOut.z = qresult.z;
        vOut.w = 1;
    }

    void    _MATH_LIB_EXPORT_ XM_Slerp(float t,XQuaternion& q1,XQuaternion& q2,XQuaternion& qOut)
    {
        /*
        XQuaternion q1 = qA;
        XQuaternion q2 = qB;

        q1.Normalize();
        q2.Normalize();
        */

        float c = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;

        float s = sqrt(1 - c*c);
        float a = acos(c);
        float sa1_t = sin( a - a*t);
        float sat = sin(a*t);

        float k1 = sa1_t / s;
        float k2 = sat / s;

        qOut.x = k1 * q1.x + k2 * q2.x ;
        qOut.y = k1 * q1.y + k2 * q2.y ;
        qOut.z = k1 * q1.z + k2 * q2.z ;
        qOut.w = k1 * q1.w + k2 * q2.w ;

    }


};