windows-xp/Source/XPSP1/NT/shell/osshell/control/scrnsave/dolphin/d3dmath.cpp

//-----------------------------------------------------------------------------
// File: D3DMath.cpp
//
// Desc: Shortcut macros and functions for using DX objects
//
// Copyright (c) 1997-1999 Microsoft Corporation. All rights reserved
//-----------------------------------------------------------------------------
#define D3D_OVERLOADS
#define STRICT

#include "StdAfx.h"

#include <math.h>
#include "D3DMath.h"




//-----------------------------------------------------------------------------
// Name: D3DMath_MatrixMultiply()
// Desc: Does the matrix operation: [Q] = [A] * [B]. Note that the order of
//       this operation was changed from the previous version of the DXSDK.
//-----------------------------------------------------------------------------
VOID D3DMath_MatrixMultiply(D3DMATRIX& q, D3DMATRIX& a, D3DMATRIX& b)
{
    FLOAT* pA = (FLOAT*)&a;
    FLOAT* pB = (FLOAT*)&b;
    FLOAT  pM[16];

    ZeroMemory(pM, sizeof(D3DMATRIX));

    for(WORD i=0; i<4; i++) 
        for(WORD j=0; j<4; j++) 
            for(WORD k=0; k<4; k++) 
                pM[4*i+j] +=  pA[4*i+k] * pB[4*k+j];

    memcpy(&q, pM, sizeof(D3DMATRIX));
}




//-----------------------------------------------------------------------------
// Name: D3DMath_MatrixInvert()
// Desc: Does the matrix operation: [Q] = inv[A]. Note: this function only
//       works for matrices with [0 0 0 1] for the 4th column.
//-----------------------------------------------------------------------------
HRESULT D3DMath_MatrixInvert(D3DMATRIX& q, D3DMATRIX& a)
{
    if (fabs(a._44 - 1.0f) > .001f)
        return E_INVALIDARG;
    if (fabs(a._14) > .001f || fabs(a._24) > .001f || fabs(a._34) > .001f)
        return E_INVALIDARG;

    FLOAT fDetInv = 1.0f / (a._11 * (a._22 * a._33 - a._23 * a._32) -
                             a._12 * (a._21 * a._33 - a._23 * a._31) +
                             a._13 * (a._21 * a._32 - a._22 * a._31));

    q._11 =  fDetInv * (a._22 * a._33 - a._23 * a._32);
    q._12 = -fDetInv * (a._12 * a._33 - a._13 * a._32);
    q._13 =  fDetInv * (a._12 * a._23 - a._13 * a._22);
    q._14 = 0.0f;

    q._21 = -fDetInv * (a._21 * a._33 - a._23 * a._31);
    q._22 =  fDetInv * (a._11 * a._33 - a._13 * a._31);
    q._23 = -fDetInv * (a._11 * a._23 - a._13 * a._21);
    q._24 = 0.0f;

    q._31 =  fDetInv * (a._21 * a._32 - a._22 * a._31);
    q._32 = -fDetInv * (a._11 * a._32 - a._12 * a._31);
    q._33 =  fDetInv * (a._11 * a._22 - a._12 * a._21);
    q._34 = 0.0f;

    q._41 = -(a._41 * q._11 + a._42 * q._21 + a._43 * q._31);
    q._42 = -(a._41 * q._12 + a._42 * q._22 + a._43 * q._32);
    q._43 = -(a._41 * q._13 + a._42 * q._23 + a._43 * q._33);
    q._44 = 1.0f;

    return S_OK;
}




//-----------------------------------------------------------------------------
// Name: D3DMath_VectorMatrixMultiply()
// Desc: Multiplies a vector by a matrix
//-----------------------------------------------------------------------------
HRESULT D3DMath_VectorMatrixMultiply(D3DVECTOR& vDest, D3DVECTOR& vSrc,
                                      D3DMATRIX& mat)
{
    FLOAT x = vSrc.x*mat._11 + vSrc.y*mat._21 + vSrc.z* mat._31 + mat._41;
    FLOAT y = vSrc.x*mat._12 + vSrc.y*mat._22 + vSrc.z* mat._32 + mat._42;
    FLOAT z = vSrc.x*mat._13 + vSrc.y*mat._23 + vSrc.z* mat._33 + mat._43;
    FLOAT w = vSrc.x*mat._14 + vSrc.y*mat._24 + vSrc.z* mat._34 + mat._44;
    
    if (fabs(w) < g_EPSILON)
        return E_INVALIDARG;

    vDest.x = x/w;
    vDest.y = y/w;
    vDest.z = z/w;

    return S_OK;
}




//-----------------------------------------------------------------------------
// Name: D3DMath_VertexMatrixMultiply()
// Desc: Multiplies a vertex by a matrix
//-----------------------------------------------------------------------------
HRESULT D3DMath_VertexMatrixMultiply(D3DVERTEX& vDest, D3DVERTEX& vSrc,
                                      D3DMATRIX& mat)
{
    HRESULT    hr;
    D3DVECTOR* pSrcVec  = (D3DVECTOR*)&vSrc.x;
    D3DVECTOR* pDestVec = (D3DVECTOR*)&vDest.x;

    if (SUCCEEDED(hr = D3DMath_VectorMatrixMultiply(*pDestVec, *pSrcVec,
                                                      mat)))
    {
        pSrcVec  = (D3DVECTOR*)&vSrc.nx;
        pDestVec = (D3DVECTOR*)&vDest.nx;
        hr = D3DMath_VectorMatrixMultiply(*pDestVec, *pSrcVec, mat);
    }
    return hr;
}




//-----------------------------------------------------------------------------
// Name: D3DMath_QuaternionFromRotation()
// Desc: Converts a normalized axis and angle to a unit quaternion.
//-----------------------------------------------------------------------------
VOID D3DMath_QuaternionFromRotation(FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
                                     D3DVECTOR& v, FLOAT fTheta)
{
    x = sinf(fTheta/2.0f) * v.x;
    y = sinf(fTheta/2.0f) * v.y;
    z = sinf(fTheta/2.0f) * v.z;
    w = cosf(fTheta/2.0f);
}




//-----------------------------------------------------------------------------
// Name: D3DMath_RotationFromQuaternion()
// Desc: Converts a normalized axis and angle to a unit quaternion.
//-----------------------------------------------------------------------------
VOID D3DMath_RotationFromQuaternion(D3DVECTOR& v, FLOAT& fTheta,
                                     FLOAT x, FLOAT y, FLOAT z, FLOAT w)
                                      
{
    fTheta = acosf(w) * 2.0f;
    v.x    = x / sinf(fTheta/2.0f);
    v.y    = y / sinf(fTheta/2.0f);
    v.z    = z / sinf(fTheta/2.0f);
}




//-----------------------------------------------------------------------------
// Name: D3DMath_QuaternionFromAngles()
// Desc: Converts euler angles to a unit quaternion.
//-----------------------------------------------------------------------------
VOID D3DMath_QuaternionFromAngles(FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
                                   FLOAT fYaw, FLOAT fPitch, FLOAT fRoll)
                                        
{
    FLOAT fSinYaw   = sinf(fYaw/2.0f);
    FLOAT fSinPitch = sinf(fPitch/2.0f);
    FLOAT fSinRoll  = sinf(fRoll/2.0f);
    FLOAT fCosYaw   = cosf(fYaw/2.0f);
    FLOAT fCosPitch = cosf(fPitch/2.0f);
    FLOAT fCosRoll  = cosf(fRoll/2.0f);

    x = fSinRoll * fCosPitch * fCosYaw - fCosRoll * fSinPitch * fSinYaw;
    y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
    z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
    w = fCosRoll * fCosPitch * fCosYaw + fSinRoll * fSinPitch * fSinYaw;
}




//-----------------------------------------------------------------------------
// Name: D3DMath_MatrixFromQuaternion()
// Desc: Converts a unit quaternion into a rotation matrix.
//-----------------------------------------------------------------------------
VOID D3DMath_MatrixFromQuaternion(D3DMATRIX& mat, FLOAT x, FLOAT y, FLOAT z,
                                   FLOAT w)
{
    FLOAT xx = x*x; FLOAT yy = y*y; FLOAT zz = z*z;
    FLOAT xy = x*y; FLOAT xz = x*z; FLOAT yz = y*z;
    FLOAT wx = w*x; FLOAT wy = w*y; FLOAT wz = w*z;
    
    mat._11 = 1 - 2 * (yy + zz); 
    mat._12 =     2 * (xy - wz);
    mat._13 =     2 * (xz + wy);

    mat._21 =     2 * (xy + wz);
    mat._22 = 1 - 2 * (xx + zz);
    mat._23 =     2 * (yz - wx);

    mat._31 =     2 * (xz - wy);
    mat._32 =     2 * (yz + wx);
    mat._33 = 1 - 2 * (xx + yy);

    mat._14 = mat._24 = mat._34 = 0.0f;
    mat._41 = mat._42 = mat._43 = 0.0f;
    mat._44 = 1.0f;
}




//-----------------------------------------------------------------------------
// Name: D3DMath_QuaternionFromMatrix()
// Desc: Converts a rotation matrix into a unit quaternion.
//-----------------------------------------------------------------------------
VOID D3DMath_QuaternionFromMatrix(FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
                                   D3DMATRIX& mat)
{
    if (mat._11 + mat._22 + mat._33 > 0.0f)
    {
        FLOAT s = sqrtf(mat._11 + mat._22 + mat._33 + mat._44);

        x = (mat._23-mat._32) / (2*s);
        y = (mat._31-mat._13) / (2*s);
        z = (mat._12-mat._21) / (2*s);
        w = 0.5f * s;
    }
    else
    {


    }
    FLOAT xx = x*x; FLOAT yy = y*y; FLOAT zz = z*z;
    FLOAT xy = x*y; FLOAT xz = x*z; FLOAT yz = y*z;
    FLOAT wx = w*x; FLOAT wy = w*y; FLOAT wz = w*z;
    
    mat._11 = 1 - 2 * (yy + zz); 
    mat._12 =     2 * (xy - wz);
    mat._13 =     2 * (xz + wy);

    mat._21 =     2 * (xy + wz);
    mat._22 = 1 - 2 * (xx + zz);
    mat._23 =     2 * (yz - wx);

    mat._31 =     2 * (xz - wy);
    mat._32 =     2 * (yz + wx);
    mat._33 = 1 - 2 * (xx + yy);

    mat._14 = mat._24 = mat._34 = 0.0f;
    mat._41 = mat._42 = mat._43 = 0.0f;
    mat._44 = 1.0f;
}




//-----------------------------------------------------------------------------
// Name: D3DMath_QuaternionMultiply()
// Desc: Mulitples two quaternions together as in {Q} = {A} * {B}.
//-----------------------------------------------------------------------------
VOID D3DMath_QuaternionMultiply(FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw,
                                  FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw,
                                  FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw)
{
    FLOAT Dx =  Ax*Bw + Ay*Bz - Az*By + Aw*Bx;
    FLOAT Dy = -Ax*Bz + Ay*Bw + Az*Bx + Aw*By;
    FLOAT Dz =  Ax*By - Ay*Bx + Az*Bw + Aw*Bz;
    FLOAT Dw = -Ax*Bx - Ay*By - Az*Bz + Aw*Bw;

    Qx = Dx; Qy = Dy; Qz = Dz; Qw = Dw;
}




//-----------------------------------------------------------------------------
// Name: D3DMath_SlerpQuaternions()
// Desc: Compute a quaternion which is the spherical linear interpolation
//       between two other quaternions by dvFraction.
//-----------------------------------------------------------------------------
VOID D3DMath_QuaternionSlerp(FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw,
                              FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw,
                              FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw,
                              FLOAT fAlpha)
{
    // Compute dot product (equal to cosine of the angle between quaternions)
    FLOAT fCosTheta = Ax*Bx + Ay*By + Az*Bz + Aw*Bw;

    // Check angle to see if quaternions are in opposite hemispheres
    if (fCosTheta < 0.0f) 
    {
        // If so, flip one of the quaterions
        fCosTheta = -fCosTheta;
        Bx = -Bx; By = -By; Bz = -Bz; Bw = -Bw;
    }

    // Set factors to do linear interpolation, as a special case where the
    // quaternions are close together.
    FLOAT fBeta = 1.0f - fAlpha;
    
    // If the quaternions aren't close, proceed with spherical interpolation
    if (1.0f - fCosTheta > 0.001f) 
    {   
        FLOAT fTheta = acosf(fCosTheta);
        
        fBeta  = sinf(fTheta*fBeta) / sinf(fTheta);
        fAlpha = sinf(fTheta*fAlpha) / sinf(fTheta);
    }

    // Do the interpolation
    Qx = fBeta*Ax + fAlpha*Bx;
    Qy = fBeta*Ay + fAlpha*By;
    Qz = fBeta*Az + fAlpha*Bz;
    Qw = fBeta*Aw + fAlpha*Bw;
}