Using Quaternions Efficiently in Real-time Applications

#ifndef _MATH_H
#define _MATH_H

#include <d3dx9math.h>


const D3DXQUATERNION operator *( const D3DXQUATERNION &q, const D3DXVECTOR3 &v )
{
    return D3DXQUATERNION(   q.w * v.x + q.y * v.z - q.z * v.y,
                             q.w * v.y + q.z * v.x - q.x * v.z,
                             q.w * v.z + q.x * v.y - q.y * v.x,
                          -( q.x * v.x + q.y * v.y + q.z * v.z )
                         );
}
void rotateVectorWithQuat( D3DXQUATERNION q, D3DXVECTOR3 &v )
{
    D3DXVECTOR3 uv, uuv;
    D3DXVECTOR3 qvec(q.x, q.y, q.z);
    D3DXVec3Cross( &uv, &qvec, &v );
    D3DXVec3Cross( &uuv, &qvec, &uv );

    float w2 = 2.0f * q.w;
    uv.x *= w2;
    uv.y *= w2;
    uv.z *= w2;

    uuv.x *= 2.0f;
    uuv.y *= 2.0f;
    uuv.z *= 2.0f;

    v.x = v.x + uv.x + uuv.x;
    v.y = v.y + uv.y + uuv.y;
    v.z = v.z + uv.z + uuv.z;
}

const D3DXVECTOR3 operator *( const D3DXVECTOR3 &u, const D3DXMATRIX &m )
{
    return D3DXVECTOR3( u.x*m(0,0)+ u.y*m(1,0) + u.z*m(2,0) + m(3,0),
                        u.x*m(0,1) + u.y*m(1,1) + u.z*m(2,1) + m(3,1),
                        u.x*m(0,2) + u.y*m(1,2) + u.z*m(2,2) + m(3,2)
                   );
}

const D3DXMATRIX quatToMatrix( const float *q )
{
    float x2 = q[0] + q[0]; 
    float y2 = q[1] + q[1]; 
    float z2 = q[2] + q[2]; 
    float w2 = q[3] + q[3]; 

    float yy2 = q[1] * y2; 
    float xy2 = q[0] * y2; 
    float xz2 = q[0] * z2; 
    float yz2 = q[1] * z2; 

    float zz2 = q[2] * z2; 
    float wz2 = q[3] * z2; 
    float wy2 = q[3] * y2; 
    float wx2 = q[3] * x2; 

    float xx2 = q[0] * x2; 

    D3DXMATRIX retM;
    float *m = retM;
    m[0*4+0] = - yy2 - zz2 + 1.0f; 
    m[0*4+1] = xy2 + wz2; 
    m[0*4+2] = xz2 - wy2; 

    m[1*4+0] = xy2 - wz2; 
    m[1*4+1] = - xx2 - zz2 + 1.0f; 
    m[1*4+2] = yz2 + wx2; 

    m[2*4+0] = xz2 + wy2; 
    m[2*4+1] = yz2 - wx2; 
    m[2*4+2] = - xx2 - yy2 + 1.0f; 

    return retM;
}

void quatFromEuler( D3DXQUATERNION &q, float yaw, float pitch, float roll)
{
    float cosY = cosf(yaw * 0.5f);    
    float sinY = sinf(yaw * 0.5f);    
    float cosP = cosf(pitch * 0.5f);    
    float sinP = sinf(pitch * 0.5f);    
    float cosR = cosf(roll * 0.5f);    
    float sinR = sinf(roll * 0.5f);
    
    q.x = (cosR * sinP * cosY + sinR * cosP * sinY);            
    q.y = (cosR * cosP * sinY - sinR * sinP * cosY);            
    q.z = (sinR * cosP * cosY - cosR * sinP * sinY);
    q.w = cosR * cosP * cosY + sinR * sinP * sinY;
}

#endif