arch/win32/d3dframe/d3dmath.cpp

   1 //-----------------------------------------------------------------------------
   2 // File: D3DMath.cpp
   3 //
   4 // Desc: Shortcut macros and functions for using DX objects
   5 //
   6 //
   7 // Copyright (c) 1997-1998 Microsoft Corporation. All rights reserved
   8 //-----------------------------------------------------------------------------
   9
  10 #define D3D_OVERLOADS
  11 #include <math.h>
  12 #include <stdio.h>
  13 #include "D3DMath.h"
  14
  15
  16
  17
  18 //-----------------------------------------------------------------------------
  19 // Name: D3DMath_MatrixMultiply()
  20 // Desc: Does the matrix operation: [Q] = [A] * [B].
  21 //-----------------------------------------------------------------------------
  22 VOID D3DMath_MatrixMultiply( D3DMATRIX& q, D3DMATRIX& a, D3DMATRIX& b )
  23 {
  24     FLOAT* pA = (FLOAT*)&a;
  25     FLOAT* pB = (FLOAT*)&b;
  26     FLOAT  pM[16];
  27
  28     ZeroMemory( pM, sizeof(D3DMATRIX) );
  29
  30     for( WORD i=0; i<4; i++ )
  31         for( WORD j=0; j<4; j++ )
  32             for( WORD k=0; k<4; k++ )
  33                 pM[4*i+j] += pA[4*k+j] * pB[4*i+k];
  34
  35     memcpy( &q, pM, sizeof(D3DMATRIX) );
  36 }
  37
  38
  39
  40
  41 //-----------------------------------------------------------------------------
  42 // Name: D3DMath_MatrixInvert()
  43 // Desc: Does the matrix operation: [Q] = inv[A]. Note: this function only
  44 //       works for matrices with [0 0 0 1] for the 4th column.
  45 //-----------------------------------------------------------------------------
  46 HRESULT D3DMath_MatrixInvert( D3DMATRIX& q, D3DMATRIX& a )
  47 {
  48     if( fabs(a._44 - 1.0f) > .001f)
  49         return E_INVALIDARG;
  50     if( fabs(a._14) > .001f || fabs(a._24) > .001f || fabs(a._34) > .001f )
  51         return E_INVALIDARG;
  52
  53     FLOAT fDetInv = 1.0f / ( a._11 * ( a._22 * a._33 - a._23 * a._32 ) -
  54                              a._12 * ( a._21 * a._33 - a._23 * a._31 ) +
  55                              a._13 * ( a._21 * a._32 - a._22 * a._31 ) );
  56
  57     q._11 =  fDetInv * ( a._22 * a._33 - a._23 * a._32 );
  58     q._12 = -fDetInv * ( a._12 * a._33 - a._13 * a._32 );
  59     q._13 =  fDetInv * ( a._12 * a._23 - a._13 * a._22 );
  60     q._14 = 0.0f;
  61
  62     q._21 = -fDetInv * ( a._21 * a._33 - a._23 * a._31 );
  63     q._22 =  fDetInv * ( a._11 * a._33 - a._13 * a._31 );
  64     q._23 = -fDetInv * ( a._11 * a._23 - a._13 * a._21 );
  65     q._24 = 0.0f;
  66
  67     q._31 =  fDetInv * ( a._21 * a._32 - a._22 * a._31 );
  68     q._32 = -fDetInv * ( a._11 * a._32 - a._12 * a._31 );
  69     q._33 =  fDetInv * ( a._11 * a._22 - a._12 * a._21 );
  70     q._34 = 0.0f;
  71
  72     q._41 = -( a._41 * q._11 + a._42 * q._21 + a._43 * q._31 );
  73     q._42 = -( a._41 * q._12 + a._42 * q._22 + a._43 * q._32 );
  74     q._43 = -( a._41 * q._13 + a._42 * q._23 + a._43 * q._33 );
  75     q._44 = 1.0f;
  76
  77     return S_OK;
  78 }
  79
  80
  81
  82
  83 //-----------------------------------------------------------------------------
  84 // Name: D3DMath_VectorMatrixMultiply()
  85 // Desc: Multiplies a vector by a matrix
  86 //-----------------------------------------------------------------------------
  87 HRESULT D3DMath_VectorMatrixMultiply( D3DVECTOR& vDest, D3DVECTOR& vSrc,
  88                                       D3DMATRIX& mat)
  89 {
  90     FLOAT x = vSrc.x*mat._11 + vSrc.y*mat._21 + vSrc.z* mat._31 + mat._41;
  91     FLOAT y = vSrc.x*mat._12 + vSrc.y*mat._22 + vSrc.z* mat._32 + mat._42;
  92     FLOAT z = vSrc.x*mat._13 + vSrc.y*mat._23 + vSrc.z* mat._33 + mat._43;
  93     FLOAT w = vSrc.x*mat._14 + vSrc.y*mat._24 + vSrc.z* mat._34 + mat._44;
  94
  95     if( fabs( w ) < g_EPSILON )
  96         return E_INVALIDARG;
  97
  98     vDest.x = x/w;
  99     vDest.y = y/w;
 100     vDest.z = z/w;
 101
 102     return S_OK;
 103 }
 104
 105
 106
 107
 108 //-----------------------------------------------------------------------------
 109 // Name: D3DMath_VertexMatrixMultiply()
 110 // Desc: Multiplies a vertex by a matrix
 111 //-----------------------------------------------------------------------------
 112 HRESULT D3DMath_VertexMatrixMultiply( D3DVERTEX& vDest, D3DVERTEX& vSrc,
 113                                       D3DMATRIX& mat )
 114 {
 115     HRESULT    hr;
 116     D3DVECTOR* pSrcVec  = (D3DVECTOR*)&vSrc.x;
 117     D3DVECTOR* pDestVec = (D3DVECTOR*)&vDest.x;
 118
 119     if( SUCCEEDED( hr = D3DMath_VectorMatrixMultiply( *pDestVec, *pSrcVec,
 120                                                       mat ) ) )
 121     {
 122         pSrcVec  = (D3DVECTOR*)&vSrc.nx;
 123         pDestVec = (D3DVECTOR*)&vDest.nx;
 124         hr = D3DMath_VectorMatrixMultiply( *pDestVec, *pSrcVec, mat );
 125     }
 126     return hr;
 127 }
 128
 129
 130
 131
 132 //-----------------------------------------------------------------------------
 133 // Name: D3DMath_QuaternionFromRotation()
 134 // Desc: Converts a normalized axis and angle to a unit quaternion.
 135 //-----------------------------------------------------------------------------
 136 VOID D3DMath_QuaternionFromRotation( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
 137                                      D3DVECTOR& v, FLOAT fTheta )
 138 {
 139     x = (FLOAT)sin(fTheta/2) * v.x;
 140     y = (FLOAT)sin(fTheta/2) * v.y;
 141     z = (FLOAT)sin(fTheta/2) * v.z;
 142     w = (FLOAT)cos(fTheta/2);
 143 }
 144
 145
 146
 147
 148 //-----------------------------------------------------------------------------
 149 // Name: D3DMath_RotationFromQuaternion()
 150 // Desc: Converts a normalized axis and angle to a unit quaternion.
 151 //-----------------------------------------------------------------------------
 152 VOID D3DMath_RotationFromQuaternion( D3DVECTOR& v, FLOAT& fTheta,
 153                                      FLOAT x, FLOAT y, FLOAT z, FLOAT w )
 154
 155 {
 156     fTheta = (FLOAT)( acos(w) * 2 );
 157     v.x    = (FLOAT)( x / sin(fTheta/2) );
 158     v.y    = (FLOAT)( y / sin(fTheta/2) );
 159     v.z    = (FLOAT)( z / sin(fTheta/2) );
 160 }
 161
 162
 163
 164
 165 //-----------------------------------------------------------------------------
 166 // Name: D3DMath_QuaternionFromAngles()
 167 // Desc: Converts euler angles to a unit quaternion.
 168 //-----------------------------------------------------------------------------
 169 VOID D3DMath_QuaternionFromAngles( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
 170                                    FLOAT fYaw, FLOAT fPitch, FLOAT fRoll )
 171
 172 {
 173     FLOAT fSinYaw   = (FLOAT)sin(fYaw/2);
 174     FLOAT fSinPitch = (FLOAT)sin(fPitch/2);
 175     FLOAT fSinRoll  = (FLOAT)sin(fRoll/2);
 176     FLOAT fCosYaw   = (FLOAT)cos(fYaw/2);
 177     FLOAT fCosPitch = (FLOAT)cos(fPitch/2);
 178     FLOAT fCosRoll  = (FLOAT)cos(fRoll/2);
 179
 180     x = fSinRoll * fCosPitch * fCosYaw - fCosRoll * fSinPitch * fSinYaw;
 181     y = fCosRoll * fSinPitch * fCosYaw + fSinRoll * fCosPitch * fSinYaw;
 182     z = fCosRoll * fCosPitch * fSinYaw - fSinRoll * fSinPitch * fCosYaw;
 183     w = fCosRoll * fCosPitch * fCosYaw + fSinRoll * fSinPitch * fSinYaw;
 184 }
 185
 186
 187
 188
 189 //-----------------------------------------------------------------------------
 190 // Name: D3DMath_MatrixFromQuaternion()
 191 // Desc: Converts a unit quaternion into a rotation matrix.
 192 //-----------------------------------------------------------------------------
 193 VOID D3DMath_MatrixFromQuaternion( D3DMATRIX& mat, FLOAT x, FLOAT y, FLOAT z,
 194                                    FLOAT w )
 195 {
 196     FLOAT xx = x*x; FLOAT yy = y*y; FLOAT zz = z*z;
 197     FLOAT xy = x*y; FLOAT xz = x*z; FLOAT yz = y*z;
 198     FLOAT wx = w*x; FLOAT wy = w*y; FLOAT wz = w*z;
 199
 200     mat._11 = 1 - 2 * ( yy + zz );
 201     mat._12 =     2 * ( xy - wz );
 202     mat._13 =     2 * ( xz + wy );
 203
 204     mat._21 =     2 * ( xy + wz );
 205     mat._22 = 1 - 2 * ( xx + zz );
 206     mat._23 =     2 * ( yz - wx );
 207
 208     mat._31 =     2 * ( xz - wy );
 209     mat._32 =     2 * ( yz + wx );
 210     mat._33 = 1 - 2 * ( xx + yy );
 211
 212     mat._14 = mat._24 = mat._34 = 0.0f;
 213     mat._41 = mat._42 = mat._43 = 0.0f;
 214     mat._44 = 1.0f;
 215 }
 216
 217
 218
 219
 220 //-----------------------------------------------------------------------------
 221 // Name: D3DMath_QuaternionFromMatrix()
 222 // Desc: Converts a rotation matrix into a unit quaternion.
 223 //-----------------------------------------------------------------------------
 224 VOID D3DMath_QuaternionFromMatrix( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
 225                                    D3DMATRIX& mat )
 226 {
 227     if( mat._11 + mat._22 + mat._33 > 0.0f )
 228     {
 229         FLOAT s = (FLOAT)sqrt( mat._11 + mat._22 + mat._33 + mat._44 );
 230
 231         x = (mat._23-mat._32) / (2*s);
 232         y = (mat._31-mat._13) / (2*s);
 233         z = (mat._12-mat._21) / (2*s);
 234         w = 0.5f * s;
 235     }
 236     else
 237     {
 238
 239
 240     }
 241     FLOAT xx = x*x; FLOAT yy = y*y; FLOAT zz = z*z;
 242     FLOAT xy = x*y; FLOAT xz = x*z; FLOAT yz = y*z;
 243     FLOAT wx = w*x; FLOAT wy = w*y; FLOAT wz = w*z;
 244
 245     mat._11 = 1 - 2 * ( yy + zz );
 246     mat._12 =     2 * ( xy - wz );
 247     mat._13 =     2 * ( xz + wy );
 248
 249     mat._21 =     2 * ( xy + wz );
 250     mat._22 = 1 - 2 * ( xx + zz );
 251     mat._23 =     2 * ( yz - wx );
 252
 253     mat._31 =     2 * ( xz - wy );
 254     mat._32 =     2 * ( yz + wx );
 255     mat._33 = 1 - 2 * ( xx + yy );
 256
 257     mat._14 = mat._24 = mat._34 = 0.0f;
 258     mat._41 = mat._42 = mat._43 = 0.0f;
 259     mat._44 = 1.0f;
 260 }
 261
 262
 263
 264
 265 //-----------------------------------------------------------------------------
 266 // Name: D3DMath_QuaternionMultiply()
 267 // Desc: Mulitples two quaternions together as in {Q} = {A} * {B}.
 268 //-----------------------------------------------------------------------------
 269 VOID D3DMath_QuaternionMultiply( FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw,
 270                                   FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw,
 271                                   FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw )
 272 {
 273     FLOAT Dx = Bw*Ax + Bx*Aw + By*Az + Bz*Ay;
 274     FLOAT Dy = Bw*Ay + By*Aw + Bz*Ax + Bx*Az;
 275     FLOAT Dz = Bw*Az + Bz*Aw + Bx*Ay + By*Ax;
 276     FLOAT Dw = Bw*Aw + Bx*Ax + By*Ay + Bz*Az;
 277
 278     Qx = Dx; Qy = Dy; Qz = Dz; Qw = Dw;
 279 }
 280
 281
 282
 283
 284 //-----------------------------------------------------------------------------
 285 // Name: D3DMath_SlerpQuaternions()
 286 // Desc: Compute a quaternion which is the spherical linear interpolation
 287 //       between two other quaternions by dvFraction.
 288 //-----------------------------------------------------------------------------
 289 VOID D3DMath_QuaternionSlerp( FLOAT& Qx, FLOAT& Qy, FLOAT& Qz, FLOAT& Qw,
 290                               FLOAT Ax, FLOAT Ay, FLOAT Az, FLOAT Aw,
 291                               FLOAT Bx, FLOAT By, FLOAT Bz, FLOAT Bw,
 292                               FLOAT fAlpha )
 293 {
 294     FLOAT fScale1;
 295     FLOAT fScale2;
 296
 297     // Compute dot product, aka cos(theta):
 298     FLOAT fCosTheta = Ax*Bx + Ay*By + Az*Bz + Aw*Bw;
 299
 300     if( fCosTheta < 0.0f )
 301     {
 302         // Flip start quaternion
 303         Ax = -Ax; Ay = -Ay; Ax = -Az; Aw = -Aw;
 304         fCosTheta = -fCosTheta;
 305     }
 306
 307     if( fCosTheta + 1.0f > 0.05f )
 308     {
 309         // If the quaternions are close, use linear interploation
 310         if( 1.0f - fCosTheta < 0.05f )
 311         {
 312             fScale1 = 1.0f - fAlpha;
 313             fScale2 = fAlpha;
 314         }
 315         else // Otherwise, do spherical interpolation
 316         {
 317             FLOAT fTheta    = (FLOAT)acos( fCosTheta );
 318             FLOAT fSinTheta = (FLOAT)sin( fTheta );
 319
 320             fScale1 = (FLOAT)sin( fTheta * (1.0f-fAlpha) ) / fSinTheta;
 321             fScale2 = (FLOAT)sin( fTheta * fAlpha ) / fSinTheta;
 322         }
 323     }
 324     else
 325     {
 326         Bx = -Ay;
 327         By =  Ax;
 328         Bz = -Aw;
 329         Bw =  Az;
 330         fScale1 = (FLOAT)sin( g_PI * (0.5f - fAlpha) );
 331         fScale2 = (FLOAT)sin( g_PI * fAlpha );
 332     }
 333
 334     Qx = fScale1 * Ax + fScale2 * Bx;
 335     Qy = fScale1 * Ay + fScale2 * By;
 336     Qz = fScale1 * Az + fScale2 * Bz;
 337     Qw = fScale1 * Aw + fScale2 * Bw;
 338 }
 339
 340
 341