DX8数学库（2013-08-03 11:48:34）


#include<cmath>

#include<iostream>

using namespace std;

/*

参考了程序员David Adam写的DX8数学库。

http://www.winehq.org/pipermail/wine-cvs/2007-October/037587.html

http://source.winehq.org/git/wine.git/blob/4cd47e8b6451e16985ebc4377f4ebba2b5511ae3:/dlls/d3dx8/math.c

http://source.winehq.org/git/wine.git/blob/a394fef4b75f6a1024644d1ac8d4302811e16722:/include/d3dx8math.inl

http://source.winehq.org/git/wine.git/blob/41029cc88c7592adc392f94b83120682cf3906d9:/dlls/d3dx8/math.c

实现DX矩阵API、向量API、四元数API，去掉Windows平台依赖，适应跨平台需要，正式使用时可以加上一个名字空间Han

目前实现的矩阵API有：

D3DXMatrixIdentity 单位矩阵

D3DXMatrixInverse 逆矩阵

D3DXMatrixTranslation 平移矩阵

D3DXMatrixRotationY 绕Y轴转

D3DXMatrixRotationX 绕X轴转

D3DXMatrixRotationZ 绕Z轴转

D3DXMatrixScaling 缩放变换

目前实现的向量API有：

D3DXVECTOR3 * D3DXVec3Cross向量叉乘

D3DXVECTOR3 * D3DXVec3Lerp数乘

D3DXVECTOR3 * D3DXVec3Maximize取最大值

D3DXVECTOR3 * D3DXVec3Minimize取最小值

D3DXVECTOR3 * D3DXVec3Scale比例

float D3DXVec3Dot(pv1,pv2)点乘

目前实现的四元数API有：

D3DXQuaternionBaryCentric

D3DXQuaternionConjugate

D3DXQuaternionDot

D3DXQuaternionExp

D3DXQuaternionIdentity

D3DXQuaternionInverse

D3DXQuaternionIsIdentity

D3DXQuaternionLength

D3DXQuaternionLengthSq

D3DXQuaternionLn

D3DXQuaternionMultiply合成四元数

D3DXQuaternionNormalize

D3DXQuaternionRotationAxis给一个定义旋转轴的向量增加一个旋转值，并在由D3DXQUTERNION结构定义的四元数中返回结果。

D3DXQuaternionRotationMatrix

D3DXQuaternionRotationYawPitchRoll

D3DXQuaternionSlerp在两个四元数间进行球面线性插值（spherical linear interpolation）

D3DXQuaternionSquad

D3DXQuaternionToAxisAngle

*/

/*

VB6中使用DX8VB

'Dim objDX As New DxVBLibA.DirectX8

'Private Sub Command1_Click()

'Dim a As D3DVECTOR, b As D3DVECTOR, c As D3DVECTOR, magnitude!, dot!

'a.x = 1#

'a.y = 2#

'a.z = 3#

'b.x = 0#

'b.y = -1#

'b.z = 5#

'magnitude = objDX.VectorModulus(a)

'MsgBox CStr(magnitude)

''MsgBox CStr(magnitude ^ 2) '14

''objDX.VectorNormalize a

''MsgBox CStr(a.x)

''MsgBox CStr(a.y)

''MsgBox CStr(a.z)

''MsgBox CStr(objDX.VectorModulus(a)) '1

''objDX.VectorAdd c, a, b

''MsgBox CStr(c.x)

''MsgBox CStr(c.y)

''MsgBox CStr(c.z)

''objDX.VectorSubtract c, a, b

''MsgBox CStr(c.x)

''MsgBox CStr(c.y)

''MsgBox CStr(c.z)

''dot = objDX.VectorDotProduct(a, b)

''MsgBox CStr(dot)

''objDX.VectorCrossProduct c, a, b

''MsgBox CStr(c.x)

''MsgBox CStr(c.y)

''MsgBox CStr(c.z)

'End Sub

Dim objDX As DxVBLibA.DirectX8

Private Sub Command1_Click()

Dim a As D3DVECTOR, b As D3DVECTOR, c As D3DVECTOR, magnitude!, dot!

a.x = 1#

a.y = 2#

a.z = 3#

b.x = 0#

b.y = -1#

b.z = 5#

Dim ret&

'ret = D3DXVec3Normalize(c, a)

'MsgBox CStr(c.x)

'MsgBox CStr(c.y)

'MsgBox CStr(c.z)

ret = D3DXVec3Add(c, a, b)

MsgBox CStr(c.x)

MsgBox CStr(c.y)

MsgBox CStr(c.z)

End Sub

*/

typedef struct _D3DMATRIX

{

union {

struct

{

float _11, _12, _13, _14;

float _21, _22, _23, _24;

float _31, _32, _33, _34;

float _41, _42, _43, _44;

};

float m[4][4];

};

}D3DMATRIX;

typedef struct D3DXMATRIX : public D3DMATRIX

{

public:

D3DXMATRIX(){};

D3DXMATRIX(const float*);

D3DXMATRIX(const D3DMATRIX&);

D3DXMATRIX(float _11, float _12, float _13, float _14,

float _21, float _22, float _23, float _24,

float _31, float _32, float _33, float _34,

float _41, float _42, float _43, float _44);

// access grants

float& operator () (unsigned int Row, unsigned int Col);

float operator () (unsigned int Row, unsigned int Col) const;

// casting operators

operator float* ();

operator const float* () const;

// assignment operators

D3DXMATRIX& operator *= (const D3DXMATRIX&);

D3DXMATRIX& operator += (const D3DXMATRIX&);

D3DXMATRIX& operator -= (const D3DXMATRIX&);

D3DXMATRIX& operator *= (float);

D3DXMATRIX& operator /= (float);

// unary operators

D3DXMATRIX operator + () const;

D3DXMATRIX operator - () const;

// binary operators

D3DXMATRIX operator * (const D3DXMATRIX&) const;

D3DXMATRIX operator + (const D3DXMATRIX&) const;

D3DXMATRIX operator - (const D3DXMATRIX&) const;

D3DXMATRIX operator * (float) const;

D3DXMATRIX operator / (float) const;

friend D3DXMATRIX operator * (float, const D3DXMATRIX&);

bool operator == (const D3DXMATRIX&) const;

bool operator != (const D3DXMATRIX&) const;

}

D3DXMATRIX, *LPD3DXMATRIX;

//D3DXMatrixIdentity将一个矩阵单位化

D3DXMATRIX* D3DXMatrixIdentity(D3DXMATRIX *pOut)

{

//identity matrix

memset(pOut,0,sizeof(D3DXMATRIX));

pOut->m[0][0]=1;

pOut->m[1][1]=1;

pOut->m[2][2]=1;

pOut->m[3][3]=1;

return pOut;

}

//D3DXMATRIX* D3DXMatrixTranslation（输出矩阵，X，Y，Z） 平移变换

D3DXMATRIX* __stdcall D3DXMatrixTranslation(D3DXMATRIX *pout, float x, float y, float z)

{

D3DXMatrixIdentity(pout);

pout->m[3][0] = x;

pout->m[3][1] = y;

pout->m[3][2] = z;

return pout;

}

D3DXMATRIX* __stdcall D3DXMatrixMultiply(D3DXMATRIX *pout, const D3DXMATRIX *pm1, const D3DXMATRIX *pm2)

{

int i,j;

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

{

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

{

pout->m[i][j] = pm1->m[i][0] * pm2->m[0][j] + pm1->m[i][1] * pm2->m[1][j] + pm1->m[i][2] * pm2->m[2][j] + pm1->m[i][3] * pm2->m[3][j];

}

}

return pout;

}

//D3DXMATRIX * D3DXMatrixScaling缩放变换

D3DXMATRIX* __stdcall D3DXMatrixScaling(D3DXMATRIX *pout, float sx, float sy, float sz)

{

D3DXMatrixIdentity(pout);

pout->m[0][0] = sx;

pout->m[1][1] = sy;

pout->m[2][2] = sz;

return pout;

}

//D3DXMATRIX * D3DXMatrixTranspose矩阵转置

D3DXMATRIX* __stdcall D3DXMatrixTranspose(D3DXMATRIX *pout, const D3DXMATRIX *pm)

{

int i,j;

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

{

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

{

pout->m[i][j] = pm->m[j][i];

}

}

return pout;

}

/*

D3DXVECTOR3是从D3DVECTOR派生过来的，说明它和D3DVECTOR一样，有x、y、z这三个成员，

除此之外，D3DXVECTOR3还重载了小部分算术运算符，

这样我们就可以像对待整型那样对D3DXVECTOR3的对象进行加减乘除以及判断是否相等的运算了。

同时，由于D3DXVECTOR3是从D3DVECTOR派生过来的，所以两者的对象可以互相赋值，在这两种类型中随便转换。

*/

//Private Type D3DVECTOR

// x As Single

// y As Single

// z As Single

//End Type

typedef float SCALAR;

//

// A 3D vector

//

class D3DVECTOR

{

public:

SCALAR x,y,z; //x,y,z coordinates

public:

D3DVECTOR() : x(0), y(0), z(0) {}

D3DVECTOR( const SCALAR& a, const SCALAR& b, const SCALAR& c ) : x(a), y(b), z(c) {}

//index a component

//NOTE: returning a reference allows

//you to assign the indexed element

SCALAR& operator [] ( const long i )

{

return *((&x) + i);

}

//compare

const bool operator == ( const D3DVECTOR& v ) const

{

return (v.x==x && v.y==y && v.z==z);

}

const bool operator != ( const D3DVECTOR& v ) const

{

return !(v == *this);

}

//negate

const D3DVECTOR operator - () const

{

return D3DVECTOR( -x, -y, -z );

}

//assign

const D3DVECTOR& operator = ( const D3DVECTOR& v )

{

x = v.x;

y = v.y;

z = v.z;

return *this;

}

//increment

const D3DVECTOR& operator += ( const D3DVECTOR& v )

{

x+=v.x;

y+=v.y;

z+=v.z;

return *this;

}

//decrement

const D3DVECTOR& operator -= ( const D3DVECTOR& v )

{

x-=v.x;

y-=v.y;

z-=v.z;

return *this;

}

//self-multiply

const D3DVECTOR& operator *= ( const SCALAR& s )

{

x*=s;

y*=s;

z*=s;

return *this;

}

//self-divide

const D3DVECTOR& operator /= ( const SCALAR& s )

{

const SCALAR r = 1 / s;

x *= r;

y *= r;

z *= r;

return *this;

}

//add

const D3DVECTOR operator + ( const D3DVECTOR& v ) const

{

return D3DVECTOR(x + v.x, y + v.y, z + v.z);

}

//subtract

const D3DVECTOR operator - ( const D3DVECTOR& v ) const

{

return D3DVECTOR(x - v.x, y - v.y, z - v.z);

}

//post-multiply by a scalar

const D3DVECTOR operator * ( const SCALAR& s ) const

{

return D3DVECTOR( x*s, y*s, z*s );

}

//pre-multiply by a scalar

friend inline const D3DVECTOR operator * ( const SCALAR& s, const D3DVECTOR& v )

{

return v * s;

}

//divide

const D3DVECTOR operator / (SCALAR s) const

{

s = 1/s;

return D3DVECTOR( s*x, s*y, s*z );

}

//cross product

const D3DVECTOR cross( const D3DVECTOR& v ) const

{

//Davis, Snider, "Introduction to Vector Analysis", p. 44

return D3DVECTOR( y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x );

}

//scalar dot product

const SCALAR dot( const D3DVECTOR& v ) const

{

return x*v.x + y*v.y + z*v.z;

}

//length

const SCALAR length() const

{

return (SCALAR)sqrt( (double)this->dot(*this) );

}

//unit vector

const D3DVECTOR unit() const

{

return (*this) / length();

}

//make this a unit vector

void normalize()

{

(*this) /= length();

}

//equal within an error 慹?

const bool nearlyEquals( const D3DVECTOR& v, const SCALAR e ) const

{

return fabs(x-v.x)<e && fabs(y-v.y)<e && fabs(z-v.z)<e;

}

};

typedef D3DVECTOR D3DXVECTOR3;

//

// A 4D vector

//

class D3DXVECTOR4

{

public:

SCALAR x,y,z,w; //x,y,z,w coordinates

public:

D3DXVECTOR4() : x(0), y(0), z(0),w(0) {}

D3DXVECTOR4( const SCALAR& a, const SCALAR& b, const SCALAR& c , const SCALAR& d) : x(a), y(b), z(c), w(d) {}

//index a component

//NOTE: returning a reference allows

//you to assign the indexed element

SCALAR& operator [] ( const long i )

{

return *((&x) + i);

}

//compare

const bool operator == ( const D3DXVECTOR4& v ) const

{

return (v.x==x && v.y==y && v.z==z && v.w==w);

}

const bool operator != ( const D3DXVECTOR4& v ) const

{

return !(v == *this);

}

//negate

const D3DXVECTOR4 operator - () const

{

return D3DXVECTOR4( -x, -y, -z ,-w);

}

//assign

const D3DXVECTOR4& operator = ( const D3DXVECTOR4& v )

{

x = v.x;

y = v.y;

z = v.z;

w = v.w;

return *this;

}

//increment

const D3DXVECTOR4& operator += ( const D3DXVECTOR4& v )

{

x+=v.x;

y+=v.y;

z+=v.z;

w+=v.w;

return *this;

}

//decrement

const D3DXVECTOR4& operator -= ( const D3DXVECTOR4& v )

{

x-=v.x;

y-=v.y;

z-=v.z;

w-=v.w;

return *this;

}

//self-multiply

const D3DXVECTOR4& operator *= ( const SCALAR& s )

{

x*=s;

y*=s;

z*=s;

w*=s;

return *this;

}

//self-divide

const D3DXVECTOR4& operator /= ( const SCALAR& s )

{

const SCALAR r = 1 / s;

x *= r;

y *= r;

z *= r;

w *= r;

return *this;

}

//add

const D3DXVECTOR4 operator + ( const D3DXVECTOR4& v ) const

{

return D3DXVECTOR4(x + v.x, y + v.y, z + v.z,w+v.w);

}

//subtract

const D3DXVECTOR4 operator - ( const D3DXVECTOR4& v ) const

{

return D3DXVECTOR4(x - v.x, y - v.y, z - v.z,w-v.w);

}

//post-multiply by a scalar

const D3DXVECTOR4 operator * ( const SCALAR& s ) const

{

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

}

//pre-multiply by a scalar

friend inline const D3DXVECTOR4 operator * ( const SCALAR& s, const D3DXVECTOR4& v )

{

return v * s;

}

//divide

const D3DXVECTOR4 operator / (SCALAR s) const

{

s = 1/s;

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

}

////cross product，没有4维叉积

//

// const VECTOR cross( const VECTOR& v ) const

// {

// //Davis, Snider, "Introduction to Vector Analysis", p. 44

// return VECTOR( y*v.z - z*v.y, z*v.x - x*v.z, x*v.y - y*v.x );

// }

//scalar dot product

const SCALAR dot( const D3DXVECTOR4& v ) const

{

return x*v.x + y*v.y + z*v.z+w*v.w;

}

//length

const SCALAR length() const

{

return (SCALAR)sqrt( (double)this->dot(*this) );

}

//unit vector

const D3DXVECTOR4 unit() const

{

return (*this) / length();

}

//make this a unit vector

void normalize()

{

(*this) /= length();

}

//equal within an error 慹?

const bool nearlyEquals( const D3DXVECTOR4& v, const SCALAR e ) const

{

return fabs(x-v.x)<e && fabs(y-v.y)<e && fabs(z-v.z)<e && fabs(w-v.w)<e;

}

};

D3DXVECTOR4* __stdcall D3DXVec4Cross(D3DXVECTOR4 *pout, const D3DXVECTOR4 *pv1, const D3DXVECTOR4 *pv2, const D3DXVECTOR4 *pv3)

{

pout->x = pv1->y * (pv2->z * pv3->w - pv3->z * pv2->w) - pv1->z * (pv2->y * pv3->w - pv3->y * pv2->w) + pv1->w * (pv2->y * pv3->z - pv2->z *pv3->y);

pout->y = -(pv1->x * (pv2->z * pv3->w - pv3->z * pv2->w) - pv1->z * (pv2->x * pv3->w - pv3->x * pv2->w) + pv1->w * (pv2->x * pv3->z - pv3->x * pv2->z));

pout->z = pv1->x * (pv2->y * pv3->w - pv3->y * pv2->w) - pv1->y * (pv2->x *pv3->w - pv3->x * pv2->w) + pv1->w * (pv2->x * pv3->y - pv3->x * pv2->y);

pout->w = -(pv1->x * (pv2->y * pv3->z - pv3->y * pv2->z) - pv1->y * (pv2->x * pv3->z - pv3->x *pv2->z) + pv1->z * (pv2->x * pv3->y - pv3->x * pv2->y));

return pout;

}

float __stdcall D3DXMatrixfDeterminant(const D3DXMATRIX *pm)

{

D3DXVECTOR4 minor, v1, v2, v3;

float det;

v1.x = pm->m[0][0]; v1.y = pm->m[1][0]; v1.z = pm->m[2][0]; v1.w = pm->m[3][0];

v2.x = pm->m[0][1]; v2.y = pm->m[1][1]; v2.z = pm->m[2][1]; v2.w = pm->m[3][1];

v3.x = pm->m[0][2]; v3.y = pm->m[1][2]; v3.z = pm->m[2][2]; v3.w = pm->m[3][2];

D3DXVec4Cross(&minor,&v1,&v2,&v3);

det = - (pm->m[0][3] * minor.x + pm->m[1][3] * minor.y + pm->m[2][3] * minor.z + pm->m[3][3] * minor.w);

return det;

}

//D3DXVECTOR3 * D3DXVec3Normalize（返回指针，V） 单位化

//返回3D向量的规格化向量

//Private Declare Function D3DXVec3Normalize Lib "DX8VB.DLL" Alias "VB_D3DXVec3Normalize" (VOut As D3DVECTOR, v As D3DVECTOR) As Long

D3DXVECTOR3* __stdcall D3DXVec3Normalize(D3DXVECTOR3 *pout, const D3DXVECTOR3 *pv)

{

float norm;

//norm = D3DXVec3Length(pv);

norm = pv->length();

if ( !norm )

{

pout->x = 0.0f;

pout->y = 0.0f;

pout->z = 0.0f;

}

else

{

pout->x = pv->x / norm;

pout->y = pv->y / norm;

pout->z = pv->z / norm;

}

return pout;

}

//D3DXVECTOR3 * D3DXVec3Add(返回的指针，u,v) 向量加法

//Private Declare Function D3DXVec3Add Lib "DX8VB.DLL" Alias "VB_D3DXVec3Add" (VOut As D3DVECTOR, v1 As D3DVECTOR, V2 As D3DVECTOR) As Long

static inline D3DXVECTOR3* D3DXVec3Add(D3DXVECTOR3 *pout, const D3DXVECTOR3 *pv1, const D3DXVECTOR3 *pv2)

{

if ( !pout || !pv1 || !pv2)

return NULL;

pout->x = pv1->x + pv2->x;

pout->y = pv1->y + pv2->y;

pout->z = pv1->z + pv2->z;

return pout;

}

//D3DXVECTOR3 * D3DXVec3Cross(同上) 向量X乘

static inline D3DXVECTOR3* D3DXVec3Cross(D3DXVECTOR3 *pout, const D3DXVECTOR3 *pv1, const D3DXVECTOR3 *pv2)

{

if ( !pout || !pv1 || !pv2)

return NULL;

pout->x = (pv1->y) * (pv2->z) - (pv1->z) * (pv2->y);

pout->y = (pv1->z) * (pv2->x) - (pv1->x) * (pv2->z);

pout->z = (pv1->x) * (pv2->y) - (pv1->y) * (pv2->x);

return pout;

}

//FLOAT D3DXVec3Dot(pv1,pv2) 点乘

static inline float D3DXVec3Dot(const D3DXVECTOR3 *pv1, const D3DXVECTOR3 *pv2)

{

if ( !pv1 || !pv2 )

return 0.0f;

return (pv1->x) * (pv2->x) + (pv1->y) * (pv2->y) + (pv1->z) * (pv2->z);

}

//D3DXVECTOR3 * D3DXVer3Length(V) 向量模的计算

//Private Declare Function D3DXVec3Length Lib "DX8VB.DLL" Alias "VB_D3DXVec3Length" (v As D3DVECTOR) As Single

static inline float D3DXVec3Length(const D3DXVECTOR3 *pv)

{

if (!pv)

return 0.0f;

return sqrt( (pv->x) * (pv->x) + (pv->y) * (pv->y) + (pv->z) * (pv->z) );

}

//D3DXVECTOR3 * D3DXVec3Lerp(同上) 数乘

static inline D3DXVECTOR3* D3DXVec3Lerp(D3DXVECTOR3 *pout, const D3DXVECTOR3 *pv1, const D3DXVECTOR3 *pv2, float s)

{

if ( !pout || !pv1 || !pv2)

return NULL;

pout->x = (1-s) * (pv1->x) + s * (pv2->x);

pout->y = (1-s) * (pv1->y) + s * (pv2->y);

pout->z = (1-s) * (pv1->z) + s * (pv2->z);

return pout;

}

//D3DXVECTOR3 * D3DXVec3Maximize（同上） 取最大值

static inline D3DXVECTOR3* D3DXVec3Maximize(D3DXVECTOR3 *pout, const D3DXVECTOR3 *pv1, const D3DXVECTOR3 *pv2)

{

if ( !pout || !pv1 || !pv2)

return NULL;

pout->x = max(pv1->x , pv2->x);

pout->y = max(pv1->y , pv2->y);

pout->z = max(pv1->z , pv2->z);

return pout;

}

//D3DXVECTOR3 * D3DXVec3Minimize（同上） 取最小值

static inline D3DXVECTOR3* D3DXVec3Minimize(D3DXVECTOR3 *pout, const D3DXVECTOR3 *pv1, const D3DXVECTOR3 *pv2)

{

if ( !pout || !pv1 || !pv2)

return NULL;

pout->x = min(pv1->x , pv2->x);

pout->y = min(pv1->y , pv2->y);

pout->z = min(pv1->z , pv2->z);

return pout;

}

//D3DXVECTOR3 * D3DXVec3Scale（返回指针，PV，FLOAT） 比例

static inline D3DXVECTOR3* D3DXVec3Scale(D3DXVECTOR3 *pout, const D3DXVECTOR3 *pv, float s)

{

if ( !pout || !pv)

return NULL;

pout->x = s * (pv->x);

pout->y = s * (pv->y);

pout->z = s * (pv->z);

return pout;

}

//D3DXVECTOR3 * D3DXVec3Subtract（同上） 减法

//Private Declare Function D3DXVec3Subtract Lib "DX8VB.DLL" Alias "VB_D3DXVec3Subtract" (VOut As D3DVECTOR, v1 As D3DVECTOR, V2 As D3DVECTOR) As Long

static inline D3DXVECTOR3* D3DXVec3Subtract(D3DXVECTOR3 *pout, const D3DXVECTOR3 *pv1, const D3DXVECTOR3 *pv2)

{

if ( !pout || !pv1 || !pv2)

return NULL;

pout->x = pv1->x - pv2->x;

pout->y = pv1->y - pv2->y;

pout->z = pv1->z - pv2->z;

return pout;

}

typedef D3DXVECTOR4 D3DXQUATERNION;

static inline D3DXQUATERNION* D3DXQuaternionConjugate(D3DXQUATERNION *pout, const D3DXQUATERNION *pq)

{

if ( !pout || !pq)

return NULL;

pout->x = -pq->x;

pout->y = -pq->y;

pout->z = -pq->z;

pout->w = pq->w;

return pout;

}

static inline float D3DXQuaternionDot(const D3DXQUATERNION *pq1, const D3DXQUATERNION *pq2)

{

if ( !pq1 || !pq2 )

return 0.0f;

return (pq1->x) * (pq2->x) + (pq1->y) * (pq2->y) + (pq1->z) * (pq2->z) + (pq1->w) * (pq2->w);

}

static inline float D3DXQuaternionLength(const D3DXQUATERNION *pq)

{

if (!pq)

return 0.0f;

return sqrt( (pq->x) * (pq->x) + (pq->y) * (pq->y) + (pq->z) * (pq->z) + (pq->w) * (pq->w) );

}

static inline float D3DXQuaternionLengthSq(const D3DXQUATERNION *pq)

{

if (!pq)

return 0.0f;

return (pq->x) * (pq->x) + (pq->y) * (pq->y) + (pq->z) * (pq->z) + (pq->w) * (pq->w);

}

D3DXQUATERNION* __stdcall D3DXQuaternionInverse(D3DXQUATERNION *pout, const D3DXQUATERNION *pq)

{

D3DXQUATERNION temp;

float norm;

norm = D3DXQuaternionLengthSq(pq);

if ( !norm )

{

pout->x = 0.0f;

pout->y = 0.0f;

pout->z = 0.0f;

pout->w = 0.0f;

}

else

{

D3DXQuaternionConjugate(&temp, pq);

pout->x = temp.x / norm;

pout->y = temp.y / norm;

pout->z = temp.z / norm;

pout->w = temp.w / norm;

}

return pout;

}

D3DXQUATERNION* __stdcall D3DXQuaternionMultiply(D3DXQUATERNION *pout, const D3DXQUATERNION *pq1, const D3DXQUATERNION *pq2)

{

pout->x = pq2->w * pq1->x + pq2->x * pq1->w + pq2->y * pq1->z - pq2->z * pq1->y;

pout->y = pq2->w * pq1->y - pq2->x * pq1->z + pq2->y * pq1->w + pq2->z * pq1->x;

pout->z = pq2->w * pq1->z + pq2->x * pq1->y - pq2->y * pq1->x + pq2->z * pq1->w;

pout->w = pq2->w * pq1->w - pq2->x * pq1->x - pq2->y * pq1->y - pq2->z * pq1->z;

return pout;

}

D3DXQUATERNION* __stdcall D3DXQuaternionNormalize(D3DXQUATERNION *pout, const D3DXQUATERNION *pq)

{

float norm;

norm = D3DXQuaternionLength(pq);

if ( !norm )

{

pout->x = 0.0f;

pout->y = 0.0f;

pout->z = 0.0f;

pout->w = 0.0f;

}

else

{

pout->x = pq->x / norm;

pout->y = pq->y / norm;

pout->z = pq->z / norm;

pout->w = pq->w / norm;

}

return pout;

}

D3DXQUATERNION* __stdcall D3DXQuaternionLn(D3DXQUATERNION *pout, const D3DXQUATERNION *pq)

{

float norm, normvec, theta;

norm = D3DXQuaternionLengthSq(pq);

if ( norm > 1.0001f )

{

pout->x = pq->x;

pout->y = pq->y;

pout->z = pq->z;

pout->w = 0.0f;

}

else if( norm > 0.99999f)

{

normvec = sqrt( pq->x * pq->x + pq->y * pq->y + pq->z * pq->z );

theta = atan2(normvec, pq->w) / normvec;

pout->x = theta * pq->x;

pout->y = theta * pq->y;

pout->z = theta * pq->z;

pout->w = 0.0f;

}

else

{

//FIXME("The quaternion (%f, %f, %f, %f) has a norm <1. This should not happen. Windows returns a result anyway. This case is not implemented yet.\n", pq->x, pq->y, pq->z, pq->w);

}

return pout;

}

D3DXQUATERNION* __stdcall D3DXQuaternionRotationAxis(D3DXQUATERNION *pout, const D3DXVECTOR3 *pv, float angle)

{

D3DXVECTOR3 temp;

D3DXVec3Normalize(&temp, pv);

pout->x = sin( angle / 2.0f ) * temp.x;

pout->y = sin( angle / 2.0f ) * temp.y;

pout->z = sin( angle / 2.0f ) * temp.z;

pout->w = cos( angle / 2.0f );

return pout;

}

D3DXQUATERNION* __stdcall D3DXQuaternionRotationMatrix(D3DXQUATERNION *pout, const D3DXMATRIX *pm)

{

int i, maxi;

float maxdiag, S, trace;

trace = pm->m[0][0] + pm->m[1][1] + pm->m[2][2] + 1.0f;

if ( trace > 0.0f)

{

pout->x = ( pm->m[1][2] - pm->m[2][1] ) / ( 2.0f * sqrt(trace) );

pout->y = ( pm->m[2][0] - pm->m[0][2] ) / ( 2.0f * sqrt(trace) );

pout->z = ( pm->m[0][1] - pm->m[1][0] ) / ( 2.0f * sqrt(trace) );

pout->w = sqrt(trace) / 2.0f;

return pout;

}

maxi = 0;

maxdiag = pm->m[0][0];

for (i=1; i<3; i++)

{

if ( pm->m[i][i] > maxdiag )

{

maxi = i;

maxdiag = pm->m[i][i];

}

}

switch( maxi )

{

case 0:

S = 2.0f * sqrt(1.0f + pm->m[0][0] - pm->m[1][1] - pm->m[2][2]);

pout->x = 0.25f * S;

pout->y = ( pm->m[0][1] + pm->m[1][0] ) / S;

pout->z = ( pm->m[0][2] + pm->m[2][0] ) / S;

pout->w = ( pm->m[1][2] - pm->m[2][1] ) / S;

break;

case 1:

S = 2.0f * sqrt(1.0f + pm->m[1][1] - pm->m[0][0] - pm->m[2][2]);

pout->x = ( pm->m[0][1] + pm->m[1][0] ) / S;

pout->y = 0.25f * S;

pout->z = ( pm->m[1][2] + pm->m[2][1] ) / S;

pout->w = ( pm->m[2][0] - pm->m[0][2] ) / S;

break;

case 2:

S = 2.0f * sqrt(1.0f + pm->m[2][2] - pm->m[0][0] - pm->m[1][1]);

pout->x = ( pm->m[0][2] + pm->m[2][0] ) / S;

pout->y = ( pm->m[1][2] + pm->m[2][1] ) / S;

pout->z = 0.25f * S;

pout->w = ( pm->m[0][1] - pm->m[1][0] ) / S;

break;

}

return pout;

}

D3DXQUATERNION* __stdcall D3DXQuaternionExp(D3DXQUATERNION *pout, const D3DXQUATERNION *pq)

{

float norm;

norm = sqrt(pq->x * pq->x + pq->y * pq->y + pq->z * pq->z);

if (norm )

{

pout->x = sin(norm) * pq->x / norm;

pout->y = sin(norm) * pq->y / norm;

pout->z = sin(norm) * pq->z / norm;

pout->w = cos(norm);

}

else

{

pout->x = 0.0f;

pout->y = 0.0f;

pout->z = 0.0f;

pout->w = 1.0f;

}

return pout;

}

D3DXQUATERNION* __stdcall D3DXQuaternionRotationYawPitchRoll(D3DXQUATERNION *pout, float yaw, float pitch, float roll)

{

pout->x = sin( yaw / 2.0f) * cos(pitch / 2.0f) * sin(roll / 2.0f) + cos(yaw / 2.0f) * sin(pitch / 2.0f) * cos(roll / 2.0f);

pout->y = sin( yaw / 2.0f) * cos(pitch / 2.0f) * cos(roll / 2.0f) - cos(yaw / 2.0f) * sin(pitch / 2.0f) * sin(roll / 2.0f);

pout->z = cos(yaw / 2.0f) * cos(pitch / 2.0f) * sin(roll / 2.0f) - sin( yaw / 2.0f) * sin(pitch / 2.0f) * cos(roll / 2.0f);

pout->w = cos( yaw / 2.0f) * cos(pitch / 2.0f) * cos(roll / 2.0f) + sin(yaw / 2.0f) * sin(pitch / 2.0f) * sin(roll / 2.0f);

return pout;

}

D3DXQUATERNION* __stdcall D3DXQuaternionSlerp(D3DXQUATERNION *pout, const D3DXQUATERNION *pq1, const D3DXQUATERNION *pq2, float t)

{

float dot, epsilon;

epsilon = 1.0f;

dot = D3DXQuaternionDot(pq1, pq2);

if ( dot < 0.0f)

epsilon = -1.0f;

pout->x = (1.0f - t) * pq1->x + epsilon * t * pq2->x;

pout->y = (1.0f - t) * pq1->y + epsilon * t * pq2->y;

pout->z = (1.0f - t) * pq1->z + epsilon * t * pq2->z;

pout->w = (1.0f - t) * pq1->w + epsilon * t * pq2->w;

return pout;

}

D3DXQUATERNION* __stdcall D3DXQuaternionSquad(D3DXQUATERNION *pout, const D3DXQUATERNION *pq1, const D3DXQUATERNION *pq2, const D3DXQUATERNION *pq3, const D3DXQUATERNION *pq4, float t)

{

D3DXQUATERNION temp1, temp2;

D3DXQuaternionSlerp(pout, D3DXQuaternionSlerp(&temp1, pq1, pq4, t), D3DXQuaternionSlerp(&temp2, pq2, pq3, t), 2.0f * t * (1.0f - t));

return pout;

}

void __stdcall D3DXQuaternionToAxisAngle(const D3DXQUATERNION *pq, D3DXVECTOR3 *paxis, float *pangle)

{

float norm;

*pangle = 0.0f;

norm = D3DXQuaternionLength(pq);

if ( norm )

{

paxis->x = pq->x / norm;

paxis->y = pq->y / norm;

paxis->z = pq->z / norm;

if ( fabs( pq->w ) <= 1.0f )

*pangle = 2.0f * acos(pq->w);

}

else

{

paxis->x = 1.0f;

paxis->y = 0.0f;

paxis->z = 0.0f;

}

}

D3DXQUATERNION* __stdcall D3DXQuaternionBaryCentric(D3DXQUATERNION *pout, const D3DXQUATERNION *pq1, const D3DXQUATERNION *pq2, const D3DXQUATERNION *pq3, float f, float g)

{

D3DXQUATERNION temp1, temp2;

D3DXQuaternionSlerp(pout, D3DXQuaternionSlerp(&temp1, pq1, pq2, f + g), D3DXQuaternionSlerp(&temp2, pq1, pq3, f+g), g / (f + g));

return pout;

}

int main()

{

system("pause");

return 0;

}