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