1// tLinearAlgebra.cpp 
2// 
3// POD types for Vectors, Matrices and a function library to manipulate them. Includes geometrical transformations, 
4// cross/dot products, inversion functions, projections, normalization etc. These POD types are used as superclasses 
5// for the more object-oriented and complete derived types. eg. tVector3 derives from the POD type tVec2 found here. 
6// 
7// Copyright (c) 2004-2006, 2015, 2017 Tristan Grimmer. 
8// Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby 
9// granted, provided that the above copyright notice and this permission notice appear in all copies. 
10// 
11// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL 
12// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, 
13// INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN 
14// AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR 
15// PERFORMANCE OF THIS SOFTWARE. 
16 
17#include "Math/tLinearAlgebra.h" 
18#include "Math/tVector2.h" 
19#include "Math/tVector3.h" 
20#include "Math/tVector4.h" 
21#include "Math/tQuaternion.h" 
22#include "Math/tMatrix2.h" 
23#include "Math/tMatrix4.h" 
24 
25 
26const tMath::tVector2 tMath::tVector2::zero = { 0.0f, 0.0f }; 
27const tMath::tVector2 tMath::tVector2::i = { 1.0f, 0.0f }; 
28const tMath::tVector2 tMath::tVector2::j = { 0.0f, 1.0f }; 
29const tMath::tVector3 tMath::tVector3::zero = { 0.0f, 0.0f, 0.0f }; 
30const tMath::tVector3 tMath::tVector3::i = { 1.0f, 0.0f, 0.0f }; 
31const tMath::tVector3 tMath::tVector3::j = { 0.0f, 1.0f, 0.0f }; 
32const tMath::tVector3 tMath::tVector3::k = { 0.0f, 0.0f, 1.0f }; 
33const tMath::tVector4 tMath::tVector4::zero = { 0.0f, 0.0f, 0.0f, 0.0f }; 
34const tMath::tVector4 tMath::tVector4::i = { 1.0f, 0.0f, 0.0f, 1.0f }; 
35const tMath::tVector4 tMath::tVector4::j = { 0.0f, 1.0f, 0.0f, 1.0f }; 
36const tMath::tVector4 tMath::tVector4::k = { 0.0f, 0.0f, 1.0f, 1.0f }; 
37const tMath::tVector4 tMath::tVector4::origin = { 0.0f, 0.0f, 0.0f, 1.0f }; 
38const tMath::tQuaternion tMath::tQuaternion::zero = { 0.0f, 0.0f, 0.0f, 0.0f }; 
39const tMath::tQuaternion tMath::tQuaternion::unit = { 0.0f, 0.0f, 0.0f, 1.0f }; 
40const tMath::tMatrix2 tMath::tMatrix2::zero = { 0.0f, 0.0f, 0.0f, 0.0f }; 
41const tMath::tMatrix2 tMath::tMatrix2::identity = { 1.0f, 0.0f, 0.0f, 1.0f }; 
42const tMath::tMatrix4 tMath::tMatrix4::zero = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; 
43const tMath::tMatrix4 tMath::tMatrix4::identity = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; 
44 
45 
46void tMath::tSet(tQuat& d, const tMat4& m) 
47{ 
48 float trace = m.E[0] + m.E[5] + m.E[10]; 
49 
50 // The 0.0 here implies a decision that when w < 0.5 we consider rounding errors to be a potential issue. 
51 if (trace > 0.0f) 
52 { 
53 // w >= 0.5. Negligible error. 
54 float s = tSqrt(trace + 1.0f); 
55 d.w = 0.5f * s; 
56 s = 0.5f / s; 
57 d.x = (m.E[6] - m.E[9]) * s; 
58 d.y = (m.E[8] - m.E[2]) * s; 
59 d.z = (m.E[1] - m.E[4]) * s; 
60 } 
61 else 
62 { 
63 // w < 0.5. Find the largest of the x, y, z components. The remaining components get computed from this 
64 // to avoid rounding errors. 
65 if ((m.E[0] > m.E[5]) && (m.E[0] > m.E[10])) 
66 { 
67 float s = tSqrt((m.E[0] - (m.E[5] + m.E[10])) + 1.0f); 
68 d.x = 0.5f * s; 
69 s = 0.5f / s; 
70 d.y = (m.E[4] + m.E[1]) * s; 
71 d.z = (m.E[2] + m.E[8]) * s; 
72 d.w = (m.E[6] - m.E[9]) * s; 
73 } 
74 else if (m.E[5] > m.E[10]) 
75 { 
76 float s = tSqrt((m.E[5] - (m.E[10] + m.E[0])) + 1.0f); 
77 d.y = 0.5f * s; 
78 s = 0.5f / s; 
79 d.z = (m.E[9] + m.E[6]) * s; 
80 d.x = (m.E[4] + m.E[1]) * s; 
81 d.w = (m.E[8] - m.E[2]) * s; 
82 } 
83 else 
84 { 
85 float s = tSqrt((m.E[10] - (m.E[0] + m.E[5])) + 1.0f); 
86 d.z = 0.5f * s; 
87 s = 0.5f / s; 
88 d.x = (m.E[2] + m.E[8]) * s; 
89 d.y = (m.E[9] + m.E[6]) * s; 
90 d.w = (m.E[1] - m.E[4]) * s; 
91 } 
92 } 
93} 
94 
95 
96void tMath::tSet(tQuat& d, const tVec3& axis, float angle) 
97{ 
98 d.r = tCos(angle / 2.0f); 
99 float s = tSin(angle / 2.0f); 
100 tSet(d.v, axis.x * s, axis.y * s, axis.z * s); 
101} 
102 
103 
104void tMath::tSet(tMat4& d, const tQuat& s) 
105{ 
106 float yy = s.y * s.y; 
107 float xx = s.x * s.x; 
108 float zz = s.z * s.z; 
109 float xy = s.x * s.y; 
110 float yz = s.y * s.z; 
111 float wz = s.w * s.z; 
112 float xz = s.x * s.z; 
113 float wx = s.w * s.x; 
114 float wy = s.w * s.y; 
115 
116 d.E[0] = 1.0f - 2.0f*(yy + zz); 
117 d.E[1] = 2.0f*(xy + wz); 
118 d.E[2] = 2.0f*(xz - wy); 
119 d.E[3] = 0.0f; 
120 
121 d.E[4] = 2.0f*(xy - wz); 
122 d.E[5] = 1.0f - 2.0f*(xx + zz); 
123 d.E[6] = 2.0f*(yz + wx); 
124 d.E[7] = 0.0f; 
125 
126 d.E[8] = 2.0f*(xz + wy); 
127 d.E[9] = 2.0f*(yz - wx); 
128 d.E[10] = 1.0f - 2.0f*(xx + yy); 
129 d.E[11] = 0.0f; 
130 
131 d.E[12] = 0.0f; 
132 d.E[13] = 0.0f; 
133 d.E[14] = 0.0f; 
134 d.E[15] = 1.0f; 
135} 
136 
137 
138void tMath::tGet(tVec3& axis, float& angle, const tQuat& q) 
139{ 
140 angle = 2.0f * tArcCos(q.w); // Angle E [0, 2Pi] 
141 float s = tSqrt(1.0f - q.w*q.w); // Assumes normalised so Abs(w) < 1, w*w E [0,1], and 1-w*w > 0. 
142 if (s > 0.00001f) // Avoid infinity.  
143 tSet(axis, q.x/s, q.y/s, q.z/s); 
144 else 
145 tSet(axis, 1.0f, 0.0f, 0.0f); // The axis direction doesn't matter so (1,0,0) is fine. 
146 
147 if (angle > Pi) 
148 { 
149 // To return an angle E [0,Pi] we may have to reverse the axis direction. 
150 tNeg(axis); 
151 angle = TwoPi - angle; 
152 } 
153} 
154 
155 
156void tMath::tMul(tVec3& d, const tMat4& a, const tVec3& b) 
157{ 
158 // It would be nice to have an intrinsics-based implementation instead of this inline assembly. 
159 // Unfortunately inline asm is not supported on architectures like x64. 
160 float x = b.x*a.a11 + b.y*a.a12 + b.z*a.a13 + a.a14; 
161 float y = b.x*a.a21 + b.y*a.a22 + b.z*a.a23 + a.a24; 
162 float z = b.x*a.a31 + b.y*a.a32 + b.z*a.a33 + a.a34; 
163 tSet(d, x, y, z); 
164} 
165 
166 
167void tMath::tMul(tVec4& d, const tMat4& a, const tVec4& b) 
168{ 
169 // @todo Optimize this using intrinsics or SSE. 
170 float x = b.x*a.a11 + b.y*a.a12 + b.z*a.a13 + b.w*a.a14; 
171 float y = b.x*a.a21 + b.y*a.a22 + b.z*a.a23 + b.w*a.a24; 
172 float z = b.x*a.a31 + b.y*a.a32 + b.z*a.a33 + b.w*a.a34; 
173 float w = b.x*a.a41 + b.y*a.a42 + b.z*a.a43 + b.w*a.a44; 
174 
175 tSet(d, x, y, z, w); 
176} 
177 
178 
179void tMath::tMul(tMat4& d, const tMat4& a, const tMat4& b) 
180{ 
181 for (int i = 0; i < 4; ++i) 
182 { 
183 float a0i = a.A[0][i]; 
184 float a1i = a.A[1][i]; 
185 float a2i = a.A[2][i]; 
186 float a3i = a.A[3][i]; 
187 
188 d.A[0][i] = (a0i * b.a11) + (a1i * b.a21) + (a2i * b.a31) + (a3i * b.a41); 
189 d.A[1][i] = (a0i * b.a12) + (a1i * b.a22) + (a2i * b.a32) + (a3i * b.a42); 
190 d.A[2][i] = (a0i * b.a13) + (a1i * b.a23) + (a2i * b.a33) + (a3i * b.a43); 
191 d.A[3][i] = (a0i * b.a14) + (a1i * b.a24) + (a2i * b.a34) + (a3i * b.a44); 
192 } 
193} 
194 
195 
196void tMath::tSlerp(tQuat& d, const tQuat& a, const tQuat& b, float t) 
197{ 
198 if (t >= 1.0f) 
199 { 
200 d = b; 
201 return; 
202 } 
203 
204 if (t <= 0.0f) 
205 { 
206 d = a; 
207 return; 
208 } 
209 
210 float cosTheta = tDot(a, b); 
211 if (cosTheta < 0.0f) 
212 { 
213 cosTheta = -cosTheta; 
214 tNeg(d, b); 
215 } 
216 else 
217 { 
218 d = b; 
219 } 
220 
221 const float delta = 0.98f; 
222 float facA = 1.0f - t; 
223 float facB = t; 
224 if (cosTheta < delta) 
225 { 
226 float theta = tArcCos(cosTheta); 
227 float sinTheta = tSin(theta); 
228 float den = 1.0f / sinTheta; 
229 
230 facA = den * tSin((1.0f - t) * theta); 
231 facB = den * tSin(t * theta); 
232 } 
233 
234 d.x = facA*a.x + facB*d.x; 
235 d.y = facA*a.y + facB*d.y; 
236 d.z = facA*a.z + facB*d.z; 
237 d.w = facA*a.w + facB*d.w; 
238} 
239 
240 
241void tMath::tSlerp(tQuat& d, const tQuat& a, const tQuat& bb, float t, tOrientationPath path) 
242{ 
243 if (t >= 1.0f) 
244 { 
245 d = bb; 
246 return; 
247 } 
248 
249 if (t <= 0.0f) 
250 { 
251 d = a; 
252 return; 
253 } 
254 
255 tQuat b; 
256 tSet(b, bb); 
257 float cosTheta = tDot(a, b); 
258 
259 if 
260 ( 
261 ((path == tOrientationPath::Shortest) && (cosTheta < 0.0f)) || // Shortest spherical path. 
262 ((path == tOrientationPath::Longest) && (cosTheta > 0.0f)) || // Longest spherical path. 
263 ((path == tOrientationPath::Clockwise) && (a.w > b.w)) || // Clockwise spherical path. 
264 ((path == tOrientationPath::CounterClockwise) && (a.w < b.w)) // Counterclockwise spherical path. 
265 ) 
266 { 
267 cosTheta = -cosTheta; 
268 tNeg(b); 
269 } 
270 
271 // We need to separate out the top of the cos curve (where it's flat and the sin goes to 0). 
272 // In the same way that sin(x) ~= x for small x, cos(x) ~= 1 for small x. 
273 float thresh = 0.98f; 
274 float facA = 1.0f - t; 
275 float facB = t; 
276 if (cosTheta < thresh) 
277 { 
278 float theta = tArcCos(cosTheta); 
279 float sinTheta = tSin(theta); 
280 float inv = 1.0f / sinTheta; 
281 facA = inv * tSin((1.0f - t) * theta); 
282 facB = inv * tSin(t * theta); 
283 } 
284 
285 d.x = facA*a.x + facB*b.x; 
286 d.y = facA*a.y + facB*b.y; 
287 d.z = facA*a.z + facB*b.z; 
288 d.w = facA*a.w + facB*b.w; 
289} 
290 
291 
292void tMath::tNlerp(tQuat& d, const tQuat& a, const tQuat& bb, float t, tOrientationPath path) 
293{ 
294 if (t >= 1.0f) 
295 { 
296 d = bb; 
297 return; 
298 } 
299 
300 if (t <= 0.0f) 
301 { 
302 d = a; 
303 return; 
304 } 
305 
306 tQuat b; 
307 tSet(b, bb); 
308 float cosTheta = tDot(a, b); 
309 
310 if 
311 ( 
312 ((path == tOrientationPath::Shortest) && (cosTheta < 0.0f)) || // Shortest linear path. 
313 ((path == tOrientationPath::Longest) && (cosTheta > 0.0f)) || // Longest linear path. 
314 ((path == tOrientationPath::Clockwise) && (a.w > b.w)) || // Clockwise path. 
315 ((path == tOrientationPath::CounterClockwise) && (a.w < b.w)) // Counterclockwise path. 
316 ) 
317 { 
318 cosTheta = -cosTheta; 
319 tNeg(b); 
320 } 
321 
322 float facA = 1.0f - t; 
323 float facB = t; 
324 d.x = facA*a.x + facB*b.x; 
325 d.y = facA*a.y + facB*b.y; 
326 d.z = facA*a.z + facB*b.z; 
327 d.w = facA*a.w + facB*b.w; 
328 tNormalize(d); 
329} 
330 
331 
332float tMath::tDeterminant(const tMat4& m) 
333{ 
334 return 
335 m.A[0][0] * 
336 ( 
337 m.A[1][1] * (m.A[2][2] * m.A[3][3] - m.A[2][3] * m.A[3][2]) + 
338 m.A[2][1] * (m.A[1][3] * m.A[3][2] - m.A[1][2] * m.A[3][3]) + 
339 m.A[3][1] * (m.A[1][2] * m.A[2][3] - m.A[1][3] * m.A[2][2]) 
340 ) - 
341 m.A[1][0] * 
342 ( 
343 m.A[0][1] * (m.A[2][2] * m.A[3][3] - m.A[2][3] * m.A[3][2]) + 
344 m.A[2][1] * (m.A[0][3] * m.A[3][2] - m.A[0][2] * m.A[3][3]) + 
345 m.A[3][1] * (m.A[0][2] * m.A[2][3] - m.A[0][3] * m.A[2][2]) 
346 ) + 
347 m.A[2][0] * 
348 ( 
349 m.A[0][1] * (m.A[1][2] * m.A[3][3] - m.A[1][3] * m.A[3][2]) + 
350 m.A[1][1] * (m.A[0][3] * m.A[3][2] - m.A[0][2] * m.A[3][3]) + 
351 m.A[3][1] * (m.A[0][2] * m.A[1][3] - m.A[0][3] * m.A[1][2]) 
352 ) - 
353 m.A[3][0] * 
354 ( 
355 m.A[0][1] * (m.A[1][2] * m.A[2][3] - m.A[1][3] * m.A[2][2]) + 
356 m.A[1][1] * (m.A[0][3] * m.A[2][2] - m.A[0][2] * m.A[2][3]) + 
357 m.A[2][1] * (m.A[0][2] * m.A[1][3] - m.A[0][3] * m.A[1][2]) 
358 ); 
359} 
360 
361 
362bool tMath::tInvert(tMat4& m) 
363{ 
364 #if defined(PLATFORM_WINDOWS) 
365 if (tDeterminant(m) == 0.0f) 
366 return false; 
367 
368 // Naming: m for minor. de for determinant. t0 for temporary. r for row. 
369 __m128 m1, m2, m3, m4, r1, r3, de; 
370 __m128 r2 = _mm_setzero_ps(); 
371 __m128 r4 = _mm_setzero_ps(); 
372 __m128 t0 = _mm_setzero_ps(); 
373 
374 // Cramer's rule used here (not Gaussian Elim). 
375 t0 = _mm_loadh_pi(_mm_loadl_pi(t0, (__m64*)(m.E+ 0)), (__m64*)(m.E+ 4)); 
376 r2 = _mm_loadh_pi(_mm_loadl_pi(r2, (__m64*)(m.E+ 8)), (__m64*)(m.E+12)); 
377 r1 = _mm_shuffle_ps(t0, r2, 0x88); 
378 r2 = _mm_shuffle_ps(r2, t0, 0xDD); 
379 t0 = _mm_loadh_pi(_mm_loadl_pi(t0, (__m64*)(m.E+ 2)), (__m64*)(m.E+ 6)); 
380 r4 = _mm_loadh_pi(_mm_loadl_pi(r4, (__m64*)(m.E+10)), (__m64*)(m.E+14)); 
381 r3 = _mm_shuffle_ps(t0, r4, 0x88); 
382 r4 = _mm_shuffle_ps(r4, t0, 0xDD); 
383 t0 = _mm_mul_ps(r3, r4); 
384 t0 = _mm_shuffle_ps(t0, t0, 0xB1); 
385 m1 = _mm_mul_ps(r2, t0); 
386 m2 = _mm_mul_ps(r1, t0); 
387 t0 = _mm_shuffle_ps(t0, t0, 0x4E); 
388 m1 = _mm_sub_ps(_mm_mul_ps(r2, t0), m1); 
389 m2 = _mm_sub_ps(_mm_mul_ps(r1, t0), m2); 
390 m2 = _mm_shuffle_ps(m2, m2, 0x4E); 
391 t0 = _mm_mul_ps(r2, r3); 
392 t0 = _mm_shuffle_ps(t0, t0, 0xB1); 
393 m1 = _mm_add_ps(_mm_mul_ps(r4, t0), m1); 
394 m4 = _mm_mul_ps(r1, t0); 
395 t0 = _mm_shuffle_ps(t0, t0, 0x4E); 
396 m1 = _mm_sub_ps(m1, _mm_mul_ps(r4, t0)); 
397 m4 = _mm_sub_ps(_mm_mul_ps(r1, t0), m4); 
398 m4 = _mm_shuffle_ps(m4, m4, 0x4E); 
399 t0 = _mm_mul_ps(_mm_shuffle_ps(r2, r2, 0x4E), r4); 
400 t0 = _mm_shuffle_ps(t0, t0, 0xB1); 
401 r3 = _mm_shuffle_ps(r3, r3, 0x4E); 
402 m1 = _mm_add_ps(_mm_mul_ps(r3, t0), m1); 
403 m3 = _mm_mul_ps(r1, t0); 
404 t0 = _mm_shuffle_ps(t0, t0, 0x4E); 
405 m1 = _mm_sub_ps(m1, _mm_mul_ps(r3, t0)); 
406 m3 = _mm_sub_ps(_mm_mul_ps(r1, t0), m3); 
407 m3 = _mm_shuffle_ps(m3, m3, 0x4E); 
408 t0 = _mm_mul_ps(r1, r2); 
409 t0 = _mm_shuffle_ps(t0, t0, 0xB1); 
410 m3 = _mm_add_ps(_mm_mul_ps(r4, t0), m3); 
411 m4 = _mm_sub_ps(_mm_mul_ps(r3, t0), m4); 
412 t0 = _mm_shuffle_ps(t0, t0, 0x4E); 
413 m3 = _mm_sub_ps(_mm_mul_ps(r4, t0), m3); 
414 m4 = _mm_sub_ps(m4, _mm_mul_ps(r3, t0)); 
415 t0 = _mm_mul_ps(r1, r4); 
416 t0 = _mm_shuffle_ps(t0, t0, 0xB1); 
417 m2 = _mm_sub_ps(m2, _mm_mul_ps(r3, t0)); 
418 m3 = _mm_add_ps(_mm_mul_ps(r2, t0), m3); 
419 t0 = _mm_shuffle_ps(t0, t0, 0x4E); 
420 m2 = _mm_add_ps(_mm_mul_ps(r3, t0), m2); 
421 m3 = _mm_sub_ps(m3, _mm_mul_ps(r2, t0)); 
422 t0 = _mm_mul_ps(r1, r3); 
423 t0 = _mm_shuffle_ps(t0, t0, 0xB1); 
424 m2 = _mm_add_ps(_mm_mul_ps(r4, t0), m2); 
425 m4 = _mm_sub_ps(m4, _mm_mul_ps(r2, t0)); 
426 t0 = _mm_shuffle_ps(t0, t0, 0x4E); 
427 m2 = _mm_sub_ps(m2, _mm_mul_ps(r4, t0)); 
428 m4 = _mm_add_ps(_mm_mul_ps(r2, t0), m4); 
429 de = _mm_mul_ps(r1, m1); 
430 de = _mm_add_ps(_mm_shuffle_ps(de, de, 0x4E), de); 
431 de = _mm_add_ss(_mm_shuffle_ps(de, de, 0xB1), de); 
432 t0 = _mm_rcp_ss(de); 
433 de = _mm_sub_ss(_mm_add_ss(t0, t0), _mm_mul_ss(de, _mm_mul_ss(t0, t0))); 
434 de = _mm_shuffle_ps(de, de, 0x00); 
435 m1 = _mm_mul_ps(de, m1); 
436 
437 // Assign the result. 
438 _mm_storel_pi((__m64*)(m.E+ 0), m1); 
439 _mm_storeh_pi((__m64*)(m.E+ 2), m1); 
440 m2 = _mm_mul_ps(de, m2); 
441 _mm_storel_pi((__m64*)(m.E+ 4), m2); 
442 _mm_storeh_pi((__m64*)(m.E+ 6), m2); 
443 m3 = _mm_mul_ps(de, m3); 
444 _mm_storel_pi((__m64*)(m.E+ 8), m3); 
445 _mm_storeh_pi((__m64*)(m.E+10), m3); 
446 m4 = _mm_mul_ps(de, m4); 
447 _mm_storel_pi((__m64*)(m.E+12), m4); 
448 _mm_storeh_pi((__m64*)(m.E+14), m4); 
449 return true; 
450 
451 #else 
452 tMat4 s; 
453 tSet(s, m); 
454 return tInvert(m, s); 
455 
456 #endif 
457} 
458 
459 
460bool tMath::tInvert(tMat4& d, const tMat4& s) 
461{ 
462 #if defined(PLATFORM_WINDOWS) 
463 tSet(d, s); 
464 return tInvert(d); 
465  
466 #else 
467 tMat4 t; 
468 tTranspose(t, s); 
469 float prd[12]; 
470 
471 // Calculate intermediate products for the first set of 8 cofactors. 
472 prd[ 0] = t.E[10]*t.E[15]; prd[ 1] = t.E[11]*t.E[14]; prd[ 2] = t.E[ 9]*t.E[15]; prd[ 3] = t.E[11]*t.E[13]; 
473 prd[ 4] = t.E[ 9]*t.E[14]; prd[ 5] = t.E[10]*t.E[13]; prd[ 6] = t.E[ 8]*t.E[15]; prd[ 7] = t.E[11]*t.E[12]; 
474 prd[ 8] = t.E[ 8]*t.E[14]; prd[ 9] = t.E[10]*t.E[12]; prd[10] = t.E[ 8]*t.E[13]; prd[11] = t.E[ 9]*t.E[12]; 
475 
476 // Calculate first set of 8 cofactors. 
477 d.E[ 0] = (prd[ 0]*t.E[ 5] + prd[ 3]*t.E[ 6] + prd[ 4]*t.E[ 7]) - (prd[ 1]*t.E[ 5] + prd[ 2]*t.E[ 6] + prd[ 5]*t.E[ 7]); 
478 d.E[ 1] = (prd[ 1]*t.E[ 4] + prd[ 6]*t.E[ 6] + prd[ 9]*t.E[ 7]) - (prd[ 0]*t.E[ 4] + prd[ 7]*t.E[ 6] + prd[ 8]*t.E[ 7]); 
479 d.E[ 2] = (prd[ 2]*t.E[ 4] + prd[ 7]*t.E[ 5] + prd[10]*t.E[ 7]) - (prd[ 3]*t.E[ 4] + prd[ 6]*t.E[ 5] + prd[11]*t.E[ 7]); 
480 d.E[ 3] = (prd[ 5]*t.E[ 4] + prd[ 8]*t.E[ 5] + prd[11]*t.E[ 6]) - (prd[ 4]*t.E[ 4] + prd[ 9]*t.E[ 5] + prd[10]*t.E[ 6]); 
481 d.E[ 4] = (prd[ 1]*t.E[ 1] + prd[ 2]*t.E[ 2] + prd[ 5]*t.E[ 3]) - (prd[ 0]*t.E[ 1] + prd[ 3]*t.E[ 2] + prd[ 4]*t.E[ 3]); 
482 d.E[ 5] = (prd[ 0]*t.E[ 0] + prd[ 7]*t.E[ 2] + prd[ 8]*t.E[ 3]) - (prd[ 1]*t.E[ 0] + prd[ 6]*t.E[ 2] + prd[ 9]*t.E[ 3]); 
483 d.E[ 6] = (prd[ 3]*t.E[ 0] + prd[ 6]*t.E[ 1] + prd[11]*t.E[ 3]) - (prd[ 2]*t.E[ 0] + prd[ 7]*t.E[ 1] + prd[10]*t.E[ 3]); 
484 d.E[ 7] = (prd[ 4]*t.E[ 0] + prd[ 9]*t.E[ 1] + prd[10]*t.E[ 2]) - (prd[ 5]*t.E[ 0] + prd[ 8]*t.E[ 1] + prd[11]*t.E[ 2]); 
485 
486 // Calculate intermediate products for the second set of 8 cofactors. 
487 prd[ 0] = t.E[ 2]*t.E[ 7]; prd[ 1] = t.E[ 3]*t.E[ 6]; prd[ 2] = t.E[ 1]*t.E[ 7]; prd[ 3] = t.E[ 3]*t.E[ 5]; 
488 prd[ 4] = t.E[ 1]*t.E[ 6]; prd[ 5] = t.E[ 2]*t.E[ 5]; prd[ 6] = t.E[ 0]*t.E[ 7]; prd[ 7] = t.E[ 3]*t.E[ 4]; 
489 prd[ 8] = t.E[ 0]*t.E[ 6]; prd[ 9] = t.E[ 2]*t.E[ 4]; prd[10] = t.E[ 0]*t.E[ 5]; prd[11] = t.E[ 1]*t.E[ 4]; 
490 
491 // Calculate second set of 8 cofactors. 
492 d.E[ 8] = (prd[ 0]*t.E[13] + prd[ 3]*t.E[14] + prd[ 4]*t.E[15]) - (prd[ 1]*t.E[13] + prd[ 2]*t.E[14] + prd[ 5]*t.E[15]); 
493 d.E[ 9] = (prd[ 1]*t.E[12] + prd[ 6]*t.E[14] + prd[ 9]*t.E[15]) - (prd[ 0]*t.E[12] + prd[ 7]*t.E[14] + prd[ 8]*t.E[15]); 
494 d.E[10] = (prd[ 2]*t.E[12] + prd[ 7]*t.E[13] + prd[10]*t.E[15]) - (prd[ 3]*t.E[12] + prd[ 6]*t.E[13] + prd[11]*t.E[15]); 
495 d.E[11] = (prd[ 5]*t.E[12] + prd[ 8]*t.E[13] + prd[11]*t.E[14]) - (prd[ 4]*t.E[12] + prd[ 9]*t.E[13] + prd[10]*t.E[14]); 
496 d.E[12] = (prd[ 2]*t.E[10] + prd[ 5]*t.E[11] + prd[ 1]*t.E[ 9]) - (prd[ 4]*t.E[11] + prd[ 0]*t.E[ 9] + prd[ 3]*t.E[10]); 
497 d.E[13] = (prd[ 8]*t.E[11] + prd[ 0]*t.E[ 8] + prd[ 7]*t.E[10]) - (prd[ 6]*t.E[10] + prd[ 9]*t.E[11] + prd[ 1]*t.E[ 8]); 
498 d.E[14] = (prd[ 6]*t.E[ 9] + prd[11]*t.E[11] + prd[ 3]*t.E[ 8]) - (prd[10]*t.E[11] + prd[ 2]*t.E[ 8] + prd[ 7]*t.E[ 9]); 
499 d.E[15] = (prd[10]*t.E[10] + prd[ 4]*t.E[ 8] + prd[ 9]*t.E[ 9]) - (prd[ 8]*t.E[ 9] + prd[11]*t.E[10] + prd[ 5]*t.E[ 8]); 
500 
501 // Calculate determinant. 
502 float det = t.E[0]*d.E[0] + t.E[1]*d.E[1] + t.E[2]*d.E[2] + t.E[3]*d.E[3]; 
503 if (det == 0.0f) 
504 return false; 
505 
506 // Calculate inverse by multiplying by reciprocal of determinant. 
507 tDiv(d, det); 
508 #endif 
509 
510 return true; 
511} 
512 
513 
514void tMath::tInvertAffine(tMat4& d, const tMat4& m) 
515{ 
516 // Let A be a 4x4 affine matrix where R is a 3x3 orthonormal matrix (the rotation part), T is the 3x1 translation, 
517 // and the row 4 of A is U, where U = [ 0 0 0 1 ]. 
518 // A = [ R T ] 
519 // [ U ] 
520 // 
521 // then the inverse is given by 
522 // Inv(A) = [ Inv(R) -Inv(R)*T ] 
523 // [ U ] 
524 d.a11 = m.a11; d.a21 = m.a12; d.a31 = m.a13; 
525 d.a12 = m.a21; d.a22 = m.a22; d.a32 = m.a23; 
526 d.a13 = m.a31; d.a23 = m.a32; d.a33 = m.a33; 
527 
528 d.C4.x = -m.C1.x*m.C4.x + -m.C1.y*m.C4.y + -m.C1.z*m.C4.z; 
529 d.C4.y = -m.C2.x*m.C4.x + -m.C2.y*m.C4.y + -m.C2.z*m.C4.z; 
530 d.C4.z = -m.C3.x*m.C4.x + -m.C3.y*m.C4.y + -m.C3.z*m.C4.z; 
531} 
532 
533 
534void tMath::tMakeLookAt(tMat4& d, const tVec3& eye, const tVec3& look, const tVec3& up) 
535{ 
536 tVec3 z; 
537 tSub(z, eye, look); 
538 tNormalize(z); 
539 
540 tVec3 y(up); 
541 tVec3 x; 
542 tCross(x, y, z); 
543 tCross(y, z, x); 
544 
545 tNormalize(x); 
546 tNormalize(y); 
547 
548 tSet(d.C1, x.x, y.x, z.x, 0.0f); 
549 tSet(d.C2, x.y, y.y, z.y, 0.0f); 
550 tSet(d.C3, x.z, y.z, z.z, 0.0f); 
551 tSet(d.C4, 0.0f, 0.0f, 0.0f, 1.0f); 
552 
553 tVec3 neg; 
554 tNeg(neg, eye); 
555 d.E[12] = d.E[0]*neg.x + d.E[4]*neg.y + d.E[ 8]*neg.z; 
556 d.E[13] = d.E[1]*neg.x + d.E[5]*neg.y + d.E[ 9]*neg.z; 
557 d.E[14] = d.E[2]*neg.x + d.E[6]*neg.y + d.E[10]*neg.z; 
558} 
559 
560 
561void tMath::tMakeRotate(tMat4& d, const tVec3& axis, float angle) 
562{ 
563 float t = tLengthSq(axis); 
564 if (t < 0.00001f) 
565 { 
566 tIdentity(d); 
567 return; 
568 } 
569 
570 float x = axis.x; 
571 float y = axis.y; 
572 float z = axis.z; 
573 float s = tSin(angle); 
574 float c = tCos(angle); 
575 
576 t = tRecipSqrtFast(t); 
577 x *= t; y *= t; z *= t; 
578 float xx = x*x; float yy = y*y; float zz = z*z; 
579 float xy = x*y; float yz = y*z; float zx = z*x; 
580 float xs = x*s; float ys = y*s; float zs = z*s; 
581 float rc = 1.0f - c; 
582 
583 // Again, this is correct, even though at first it looks like the transpose. 
584 // It is being filled out in column major order for efficiency. 
585 d.a11 = rc*xx + c; d.a21 = rc*xy + zs; d.a31 = rc*zx - ys; d.a41 = 0.0f; 
586 d.a12 = rc*xy - zs; d.a22 = rc*yy + c; d.a32 = rc*yz + xs; d.a42 = 0.0f; 
587 d.a13 = rc*zx + ys; d.a23 = rc*yz - xs; d.a33 = rc*zz + c; d.a43 = 0.0f; 
588 d.a14 = 0.0f; d.a24 = 0.0f; d.a34 = 0.0f; d.a44 = 1.0f; 
589} 
590 
591 
592void tMath::tMakeRotate(tMat4& d, const tVec3& a, const tVec3& b) 
593{ 
594 // Based on "Efficiently building a matrix to rotate one vector to another vector", by Thomas Möller and John F. 
595 // Hughes, Journal of Graphics Tools, Vol 4, No 4, 1999. The method is also described in "Real-Time Rendering", by 
596 // Akenine-Möller and Haines, 2nd Edition, Section 3.3.2. 
597 float e = tDot(a, b); 
598 tVec3 v; tCross(v, a, b); 
599 
600 if (tLength(v) < Epsilon) // Parallel? 
601 { 
602 if (e > 0.0f) // Aligned? 
603 { 
604 tIdentity(d); 
605 return; 
606 } 
607 else if (e < 0.0f) 
608 { 
609 // Rotate by Pi radians around an axis perpendicular to a. 
610 float smallest = tMin(a.x, a.y, a.z); 
611 
612 // Make vector u perpendicular to a. 
613 tVec3 u; 
614 if (a.x == smallest) 
615 tSet(u, 0.0f, -a.z, a.y); 
616 else if (a.y == smallest) 
617 tSet(u, -a.z, 0.0f, a.x); 
618 else 
619 tSet(u, -a.y, a.z, 0.0f); 
620 
621 tMakeRotate(d, u, Pi); 
622 } 
623 } 
624 
625 // Not parallel to each other. 
626 float h = (1.0f - e) / tDot(v, v); 
627 d.a11 = e + h*v.x*v.x; d.a21 = h * v.x*v.y + v.z; d.a31 = h*v.x*v.z - v.y; d.a41 = 0.0f; 
628 d.a12 = h*v.x*v.y - v.z; d.a22 = e + h*v.y*v.y; d.a32 = h*v.y*v.z + v.x; d.a42 = 0.0f; 
629 d.a13 = h*v.x*v.z + v.y; d.a23 = h*v.y*v.z - v.x; d.a33 = e + h*v.z*v.z; d.a43 = 0.0f; 
630 d.a14 = 0.0f; d.a24 = 0.0f; d.a34 = 0.0f; d.a44 = 1.0f; 
631} 
632 
633 
634void tMath::tMakeRotateXYZ(tMat4& d, float a, float b, float c) 
635{ 
636 // An excellent reference can be found here: http://www.songho.ca/opengl/gl_anglestoaxes.html 
637 float ca = tCos(a); float sa = tSin(a); 
638 float cb = tCos(b); float sb = tSin(b); 
639 float cc = tCos(c); float sc = tSin(c); 
640 
641 // For all these, cos is written before sin, and secondarily, a before b before c. 
642 d.a11 = cb*cc; d.a21 = cb*sc; d.a31 = -sb; d.a41 = 0.0f; 
643 d.a12 = cc*sa*sb - sc*ca; d.a22 = ca*cc + sa*sb*sc; d.a32 = cb*sa; d.a42 = 0.0f; 
644 d.a13 = sa*sc + ca*cc*sb; d.a23 = ca*sb*sc - cc*sa; d.a33 = ca*cb; d.a43 = 0.0f; 
645 d.a14 = 0.0f; d.a24 = 0.0f; d.a34 = 0.0f; d.a44 = 1.0f; 
646} 
647 
648 
649void tMath::tMakeRotateYZX(tMat4& d, float a, float b, float c) 
650{ 
651 float ca = tCos(a); float sa = tSin(a); 
652 float cb = tCos(b); float sb = tSin(b); 
653 float cc = tCos(c); float sc = tSin(c); 
654 
655 d.a11 = cb*cc; d.a21 = ca*cb*sc + sa*sb; d.a31 = cb*sa*sc - ca*sb; d.a41 = 0.0f; 
656 d.a12 = -sc; d.a22 = ca*cc; d.a32 = cc*sa; d.a42 = 0.0f; 
657 d.a13 = cc*sb; d.a23 = ca*sb*sc - cb*sa; d.a33 = sa*sb*sc + ca*cb; d.a43 = 0.0f; 
658 d.a14 = 0.0f; d.a24 = 0.0f; d.a34 = 0.0f; d.a44 = 1.0f; 
659} 
660 
661 
662void tMath::tMakeRotateZXY(tMat4& d, float a, float b, float c) 
663{ 
664 float ca = tCos(a); float sa = tSin(a); 
665 float cb = tCos(b); float sb = tSin(b); 
666 float cc = tCos(c); float sc = tSin(c); 
667 
668 d.a11 = cb*cc + sa*sb*sc; d.a21 = ca*sc; d.a31 = cb*sa*sc - cc*sb; d.a41 = 0.0f; 
669 d.a12 = cc*sa*sb - cb*sc; d.a22 = ca*cc; d.a32 = sb*sc + cb*cc*sa; d.a42 = 0.0f; 
670 d.a13 = ca*sb; d.a23 = -sa; d.a33 = ca*cb; d.a43 = 0.0f; 
671 d.a14 = 0.0f; d.a24 = 0.0f; d.a34 = 0.0f; d.a44 = 1.0f; 
672} 
673 
674 
675void tMath::tMakeRotateZYX(tMat4& d, float a, float b, float c) 
676{ 
677 float ca = tCos(a); float sa = tSin(a); 
678 float cb = tCos(b); float sb = tSin(b); 
679 float cc = tCos(c); float sc = tSin(c); 
680 
681 d.a11 = cb*cc; d.a21 = cc*sa*sb + ca*sc; d.a31 = sa*sc - ca*cc*sb; d.a41 = 0.0f; 
682 d.a12 = -cb*sc; d.a22 = ca*cc - sa*sb*sc; d.a32 = ca*sb*sc + cc*sa; d.a42 = 0.0f; 
683 d.a13 = sb; d.a23 = -cb*sa; d.a33 = ca*cb; d.a43 = 0.0f; 
684 d.a14 = 0.0f; d.a24 = 0.0f; d.a34 = 0.0f; d.a44 = 1.0f; 
685} 
686 
687 
688void tMath::tMakeRotateXZY(tMat4& d, float a, float b, float c) 
689{ 
690 float ca = tCos(a); float sa = tSin(a); 
691 float cb = tCos(b); float sb = tSin(b); 
692 float cc = tCos(c); float sc = tSin(c); 
693 
694 // Note there is (was?) a typo for a12 at the site: http://www.songho.ca/opengl/gl_anglestoaxes.html 
695 // I posted a correction. It may or may not have been updated. In any case, a12 is correct below. 
696 d.a11 = cb*cc; d.a21 = sc; d.a31 = -cc*sb; d.a41 = 0.0f; 
697 d.a12 = sa*sb - ca*cb*sc; d.a22 = ca*cc; d.a32 = ca*sb*sc + cb*sa; d.a42 = 0.0f; 
698 d.a13 = cb*sa*sc + ca*sb; d.a23 = -cc*sa; d.a33 = ca*cb - sa*sb*sc; d.a43 = 0.0f; 
699 d.a14 = 0.0f; d.a24 = 0.0f; d.a34 = 0.0f; d.a44 = 1.0f; 
700} 
701 
702 
703void tMath::tMakeRotateYXZ(tMat4& d, float a, float b, float c) 
704{ 
705 float ca = tCos(a); float sa = tSin(a); 
706 float cb = tCos(b); float sb = tSin(b); 
707 float cc = tCos(c); float sc = tSin(c); 
708 
709 d.a11 = cb*cc - sa*sb*sc; d.a21 = cb*sc + cc*sa*sb; d.a31 = -ca*sb; d.a41 = 0.0f; 
710 d.a12 = -ca*sc; d.a22 = ca*cc; d.a32 = sa; d.a42 = 0.0f; 
711 d.a13 = cc*sb + cb*sa*sc; d.a23 = sb*sc - cb*cc*sa; d.a33 = ca*cb; d.a43 = 0.0f; 
712 d.a14 = 0.0f; d.a24 = 0.0f; d.a34 = 0.0f; d.a44 = 1.0f; 
713} 
714 
715 
716void tMath::tMakeProjPersp(tMat4& d, float left, float right, float bottom, float top, float near, float far) 
717{ 
718 // Generates: 2n/(r-l) 0 (r+l)/(r-l) 0  
719 // 0 2n/(t-b) (t+b)/(t-b) 0 
720 // 0 0 -(f+n)/(f-n) -2fn/(f-n) 
721 // 0 0 -1 0 
722 tZero(d); 
723 float lr = right-left; float bt = top-bottom; 
724 float nf = far-near; float n2 = 2.0f*near; 
725 
726 d.a11 = n2 / lr; 
727 d.a22 = n2 / bt; 
728 d.a13 = (right+left) / lr; 
729 d.a23 = (top+bottom) / bt; 
730 d.a33 = -(far+near) / nf; 
731 d.a43 = -1.0f; 
732 d.a34 = -n2*far / nf; 
733} 
734 
735 
736void tMath::tMakeProjPersp(tMat4& d, const tVec3& boxMin, const tVec3& boxMax) 
737{ 
738 float left = boxMin.x; 
739 float right = boxMax.x; 
740 float bottom = boxMin.y; 
741 float top = boxMax.y; 
742 float nearPlane = -boxMax.z; 
743 float farPlane = -boxMin.z; 
744 tMakeProjPersp(d, left, right, bottom, top, nearPlane, farPlane); 
745} 
746 
747 
748void tMath::tMakeProjPerspSym(tMat4& d, float right, float top, float nearPlane, float farPlane) 
749{ 
750 // Symmetry allows a faster implementation since left = -right and bottom = -top. 
751 // Generates: n/r 0 0 0  
752 // 0 n/t 0 0 
753 // 0 0 -(f+n)/(f-n) -2fn/(f-n) 
754 // 0 0 -1 0 
755 tZero(d); 
756 d.a11 = nearPlane/right; 
757 d.a22 = nearPlane/top; 
758 d.a33 = -(farPlane+nearPlane)/(farPlane-nearPlane); 
759 d.a34 = -2.0f*farPlane*nearPlane/(farPlane-nearPlane); 
760 d.a43 = -1.0f; 
761} 
762 
763 
764void tMath::tMakeProjPerspSymFovV(tMat4& d, float fovX, float aspect, float nearPlane, float farPlane) 
765{ 
766 tAssert(aspect > 0.0f); 
767 float top = nearPlane*tTan(fovX/2.0f); 
768 float right = top*aspect; 
769 tMakeProjPerspSym(d, right, top, nearPlane, farPlane); 
770} 
771 
772 
773void tMath::tMakeProjPerspSymFovH(tMat4& d, float fovY, float aspect, float nearPlane, float farPlane) 
774{ 
775 tAssert(aspect > 0.0f); 
776 float right = nearPlane*tTan(fovY/2.0f); 
777 float top = right/aspect; 
778 tMakeProjPerspSym(d, right, top, nearPlane, farPlane); 
779} 
780 
781 
782void tMath::tMakeProjPerspOffset(tMat4& d, float right, float top, float nearPlane, float farPlane, float offsetX, float offsetY) 
783{ 
784 float offsetH = -offsetX*right; 
785 float offsetV = -offsetY*top; 
786 tMakeProjPersp(d, offsetH-right, offsetH+right, offsetV-top, offsetV+top, nearPlane, farPlane); 
787} 
788 
789 
790void tMath::tMakeProjPerspFovVOffset(tMat4& d, float fovX, float aspect, float nearPlane, float farPlane, float offsetX, float offsetY) 
791{ 
792 tAssert(aspect > 0.0f); 
793 float top = nearPlane*tTan(fovX/2.0f); 
794 float right = top*aspect; 
795 
796 float offsetH = -offsetX*right; 
797 float offsetV = -offsetY*top; 
798 tMakeProjPersp(d, offsetH-right, offsetH+right, offsetV-top, offsetV+top, nearPlane, farPlane); 
799} 
800 
801 
802void tMath::tMakeProjPerspFovHOffset(tMat4& d, float fovY, float aspect, float nearPlane, float farPlane, float offsetX, float offsetY) 
803{ 
804 tAssert(aspect > 0.0f); 
805 float right = nearPlane*tTan(fovY/2.0f); 
806 float top = right/aspect; 
807 
808 float offsetH = -offsetX*right; 
809 float offsetV = -offsetY*top; 
810 tMakeProjPersp(d, offsetH-right, offsetH+right, offsetV-top, offsetV+top, nearPlane, farPlane); 
811} 
812 
813 
814void tMath::tMakeProjParallel(tMat4& d, const tVec3& boxMin, const tVec3& boxMax) 
815{ 
816 tZero(d); 
817 d.a11 = 2.0f / (boxMax.x-boxMin.x); 
818 d.a14 = -(boxMax.x+boxMin.x) / (boxMax.x-boxMin.x); 
819 d.a22 = 2.0f / (boxMax.y-boxMin.y); 
820 d.a24 = -(boxMax.y+boxMin.y) / (boxMax.y-boxMin.y); 
821 d.a33 = -2.0f / (boxMax.z-boxMin.z); 
822 d.a34 = -(boxMax.z+boxMin.z) / (boxMax.z-boxMin.z); 
823 d.a44 = 1.0f; 
824} 
825 
826 
827void tMath::tMakeOrthoNormal(tMat4& m) 
828{ 
829 tVec3 c1; 
830 tSet(c1, m.C1); 
831 tNormalize(c1); 
832 
833 tVec3 t2; 
834 tSet(t2, m.C2); 
835 
836 tVec3 c3; 
837 tCross(c3, c1, t2); 
838 tNormalize(c3); 
839 
840 tVec3 c2; 
841 tCross(c2, c3, c1); 
842 
843 tSet(m.C1, c1); 
844 tSet(m.C2, c2); 
845 tSet(m.C3, c3); 
846 
847 m.C1.w = m.C2.w = m.C3.w = 0.0f; m.C4.w = 1.0f; 
848} 
849 
850 
851void tMath::tMakeOrthoUniform(tMat4& m) 
852{ 
853 tVec3 c1; 
854 tSet(c1, m.C1); 
855 float scale = tLength(c1); // We use the length of the first column for all columns. 
856 
857 tVec3 c2; 
858 tSet(c2, m.C2); 
859 
860 tVec3 c3; 
861 tCross(c3, c1, c2); 
862 tNormalizeScale(c3, scale); 
863 tCross(c2, c3, c1); 
864 tNormalizeScale(c2, scale); 
865 
866 tSet(m.C2, c2); 
867 tSet(m.C3, c3); 
868 m.C1.w = m.C2.w = m.C3.w = 0.0f; m.C4.w = 1.0f; 
869} 
870 
871 
872void tMath::tMakeOrthoNonUniform(tMat4& m) 
873{ 
874 tVec3 c1; 
875 tSet(c1, m.C1); 
876 
877 tVec3 c2; 
878 tSet(c2, m.C2); 
879 float scale2 = tLength(c2); 
880 
881 m.C3.w = 0.0f; 
882 float scale3 = tLength(m.C3); 
883 
884 tVec3 c3; 
885 tCross(c3, c1, c2); 
886 tNormalizeScale(c3, scale3); 
887 
888 tCross(c2, c3, c1); 
889 tNormalizeScale(c2, scale2); 
890 
891 tSet(m.C2, c2); 
892 tSet(m.C3, c3); 
893 m.C1.w = m.C2.w = 0.0f; m.C4.w = 1.0f; 
894} 
895 
896 
897void tMath::tExtractProjectionPlanes(tVec4 planes[6], const tMat4& m, bool outwardNormals, bool normalizePlanes) 
898{ 
899 // Order is: Right, Left, Top, Bottom, Near, Far. 
900 tSet(planes[0], m.C1.d - m.C1.a, m.C2.d - m.C2.a, m.C3.d - m.C3.a, m.C4.d - m.C4.a ); 
901 tSet(planes[1], m.C1.d + m.C1.a, m.C2.d + m.C2.a, m.C3.d + m.C3.a, m.C4.d + m.C4.a ); 
902 tSet(planes[2], m.C1.d - m.C1.b, m.C2.d - m.C2.b, m.C3.d - m.C3.b, m.C4.d - m.C4.b ); 
903 tSet(planes[3], m.C1.d + m.C1.b, m.C2.d + m.C2.b, m.C3.d + m.C3.b, m.C4.d + m.C4.b ); 
904 tSet(planes[4], m.C1.d + m.C1.c, m.C2.d + m.C2.c, m.C3.d + m.C3.c, m.C4.d + m.C4.c ); 
905 tSet(planes[5], m.C1.d - m.C1.c, m.C2.d - m.C2.c, m.C3.d - m.C3.c, m.C4.d - m.C4.c ); 
906 
907 if (outwardNormals) 
908 { 
909 for (int p = 0; p < 6; p++) 
910 tNeg(planes[p]); 
911 } 
912 
913 if (normalizePlanes) 
914 { 
915 for (int p = 0; p < 6; p++) 
916 { 
917 tVec3 point; 
918 tSet(point, planes[p]); 
919 float length = tNormalizeGetLength(point); 
920 tDiv(planes[p], length); 
921 } 
922 } 
923} 
924 
925 
926bool tMath::tExtractRotationEulerXYZ 
927( 
928 tVec3& sol1, tVec3& sol2, const tMat4& rot, 
929 float gimbalZ, tIntervalBias bias 
930) 
931{ 
932 // Extraction based on http://staff.city.ac.uk/~sbbh653/publications/euler.pdf 
933 float gimbalEpsilon = 0.000001f; 
934 bool gimbalNeg = tApproxEqual(rot.a31, -1.0f, gimbalEpsilon); 
935 bool gimbalPos = tApproxEqual(rot.a31, 1.0f, gimbalEpsilon); 
936 if (!gimbalNeg && !gimbalPos) 
937 { 
938 sol1.y = -tArcSin(rot.a31); // [-Pi/2, Pi/2] 
939 sol2.y = tNormalizedAngle( Pi - sol1.y, bias ); 
940 
941 float ct1 = tCos(sol1.y); // Cos(theta1) 
942 float ct2 = tCos(sol2.y); // Cos(theta2) 
943 sol1.x = tNormalizedAngle( tArcTan(rot.a32/ct1, rot.a33/ct1), bias ); 
944 sol2.x = tNormalizedAngle( tArcTan(rot.a32/ct2, rot.a33/ct2), bias ); 
945 sol1.z = tNormalizedAngle( tArcTan(rot.a21/ct1, rot.a11/ct1), bias ); 
946 sol2.z = tNormalizedAngle( tArcTan(rot.a21/ct2, rot.a11/ct2), bias ); 
947 } 
948 else 
949 { 
950 // User choice of particular solution returned. 
951 sol1.z = gimbalZ; 
952 if (gimbalNeg) 
953 { 
954 sol1.y = PiOver2; 
955 sol1.x = tNormalizedAngle(sol1.z + tArcTan(rot.a12, rot.a13), bias); 
956 } 
957 else 
958 { 
959 sol1.y = -PiOver2; 
960 sol1.x = tNormalizedAngle(-sol1.z + tArcTan(-rot.a12, -rot.a13), bias); 
961 } 
962 sol2 = sol1; 
963 } 
964 
965 return gimbalNeg || gimbalPos; 
966} 
967 
968 
969bool tMath::tExtractAffineEulerXYZ(tVec3& sol1, tVec3& sol2, const tMat4& affine, float gimbalZ, tIntervalBias bias) 
970{ 
971 tMat4 r; tSet(r, affine); 
972 tNormalize(r.C1); 
973 tNormalize(r.C2); 
974 tNormalize(r.C3); 
975 return tExtractRotationEulerXYZ(sol1, sol2, r, gimbalZ, bias); 
976} 
977 
978 
979bool tMath::tApproachAngle(float& angle, float dest, float rate, float dt) 
980{ 
981 float diff = dest - angle; 
982 while (diff > Pi) 
983 diff -= TwoPi; 
984 
985 while (diff < -Pi) 
986 diff += TwoPi; 
987 
988 float delta = rate*dt; 
989 if (tAbs(diff) <= delta) 
990 { 
991 angle = dest; 
992 return true; 
993 } 
994 
995 angle += tSign(diff)*delta; 
996 return false; 
997} 
998 
999 
1000bool tMath::tApproachOrientation(tQuat& curr, const tQuat& dest, float rate, float dt) 
1001{ 
1002 // Here is the quaternion that will rotate from curr to dest. q' = q1^-1 * q2. 
1003 tQuat rot; 
1004 tConjugate(rot, curr); 
1005 tMul(rot, dest); 
1006 
1007 // Angle is in [0,Pi]. It will be unit length. 
1008 float angle; 
1009 tVec3 axis; 
1010 tGet(axis, angle, rot); 
1011 if (angle <= (rate*dt)) 
1012 { 
1013 curr = dest; 
1014 return true; 
1015 } 
1016 
1017 tQuat diff; 
1018 tSet(diff, axis, rate*dt); 
1019 tMul(curr, diff); 
1020 tNormalize(curr); 
1021 return false; 
1022} 
1023