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 //-----------------------------------------------------------------------------
D3DMath_MatrixMultiply(D3DMATRIX & q,D3DMATRIX & a,D3DMATRIX & b)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 //-----------------------------------------------------------------------------
D3DMath_MatrixInvert(D3DMATRIX & q,D3DMATRIX & a)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 //-----------------------------------------------------------------------------
D3DMath_VectorMatrixMultiply(D3DVECTOR & vDest,D3DVECTOR & vSrc,D3DMATRIX & mat)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 //-----------------------------------------------------------------------------
D3DMath_VertexMatrixMultiply(D3DVERTEX & vDest,D3DVERTEX & vSrc,D3DMATRIX & mat)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 //-----------------------------------------------------------------------------
D3DMath_QuaternionFromRotation(FLOAT & x,FLOAT & y,FLOAT & z,FLOAT & w,D3DVECTOR & v,FLOAT fTheta)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 //-----------------------------------------------------------------------------
D3DMath_RotationFromQuaternion(D3DVECTOR & v,FLOAT & fTheta,FLOAT x,FLOAT y,FLOAT z,FLOAT w)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 //-----------------------------------------------------------------------------
D3DMath_QuaternionFromAngles(FLOAT & x,FLOAT & y,FLOAT & z,FLOAT & w,FLOAT fYaw,FLOAT fPitch,FLOAT fRoll)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 //-----------------------------------------------------------------------------
D3DMath_MatrixFromQuaternion(D3DMATRIX & mat,FLOAT x,FLOAT y,FLOAT z,FLOAT w)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 //-----------------------------------------------------------------------------
D3DMath_QuaternionFromMatrix(FLOAT & x,FLOAT & y,FLOAT & z,FLOAT & w,D3DMATRIX & mat)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 //-----------------------------------------------------------------------------
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)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 //-----------------------------------------------------------------------------
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)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
342