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;
}