sead
Loading...
Searching...
No Matches
seadMatrixCalcCommon.hpp
Go to the documentation of this file.
1#pragma once
2
3#ifdef cafe
4#include <cafe.h>
5#endif // cafe
6
7#include <basis/seadAssert.h>
8#include <math/seadMathCalcCommon.h>
9
10namespace sead {
11
12template <typename T>
13void Matrix22CalcCommon<T>::makeIdentity(Base& o)
14{
15 o.m[0][0] = 1;
16 o.m[0][1] = 0;
17
18 o.m[1][0] = 0;
19 o.m[1][1] = 1;
20}
21
22template <typename T>
23void Matrix22CalcCommon<T>::makeZero(Base& o)
24{
25 o.m[0][0] = 0;
26 o.m[0][1] = 0;
27
28 o.m[1][0] = 0;
29 o.m[1][1] = 0;
30}
31
32template <typename T>
33void Matrix22CalcCommon<T>::copy(Base& o, const Base& n)
34{
35 o.m[0][0] = n.m[0][0];
36 o.m[0][1] = n.m[0][1];
37
38 o.m[1][0] = n.m[1][0];
39 o.m[1][1] = n.m[1][1];
40}
41
42template <typename T>
43void Matrix22CalcCommon<T>::inverse(Base& o, const Base& n)
44{
45 const T a11 = n.m[0][0];
46 const T a12 = n.m[0][1];
47
48 const T a21 = n.m[1][0];
49 const T a22 = n.m[1][1];
50
51 T det = a11 * a22 - a12 * a21;
52 if (det == 0)
53 return makeIdentity(o);
54
55 det = 1 / det;
56
57 o.m[0][0] = a22 * det;
58 o.m[0][1] = -a12 * det;
59
60 o.m[1][0] = -a21 * det;
61 o.m[1][1] = a11 * det;
62}
63
64template <typename T>
65void Matrix22CalcCommon<T>::inverseTranspose(Base& o, const Base& n)
66{
67 const T a11 = n.m[0][0];
68 const T a12 = n.m[0][1];
69
70 const T a21 = n.m[1][0];
71 const T a22 = n.m[1][1];
72
73 T det = a11 * a22 - a12 * a21;
74 if (det == 0)
75 return makeIdentity(o);
76
77 det = 1 / det;
78
79 o.m[0][0] = a22 * det;
80 o.m[0][1] = -a21 * det;
81
82 o.m[1][0] = -a12 * det;
83 o.m[1][1] = a11 * det;
84}
85
86template <typename T>
87void Matrix22CalcCommon<T>::multiply(Base& o, const Base& a, const Base& b)
88{
89 const T a11 = a.m[0][0];
90 const T a12 = a.m[0][1];
91
92 const T a21 = a.m[1][0];
93 const T a22 = a.m[1][1];
94
95 const T b11 = b.m[0][0];
96 const T b12 = b.m[0][1];
97
98 const T b21 = b.m[1][0];
99 const T b22 = b.m[1][1];
100
101 o.m[0][0] = a11 * b11 + a12 * b21;
102 o.m[0][1] = a11 * b12 + a12 * b22;
103
104 o.m[1][0] = a21 * b11 + a22 * b21;
105 o.m[1][1] = a21 * b12 + a22 * b22;
106}
107
108template <typename T>
109void Matrix22CalcCommon<T>::transpose(Base& o)
110{
111 const T a12 = o.m[0][1];
112 const T a21 = o.m[1][0];
113
114 o.m[0][1] = a21;
115 o.m[1][0] = a12;
116}
117
118template <typename T>
119void Matrix22CalcCommon<T>::transposeTo(Base& o, const Base& n)
120{
121 SEAD_ASSERT(&o != &n);
122
123 o.m[0][0] = n.m[0][0];
124 o.m[0][1] = n.m[1][0];
125
126 o.m[1][0] = n.m[0][1];
127 o.m[1][1] = n.m[1][1];
128}
129
130template <typename T>
131void Matrix22CalcCommon<T>::makeRIdx(Base& o, u32 r)
132{
133 T sinV;
134 T cosV;
135
136 MathCalcCommon<T>::sinCosIdx(&sinV, &cosV, r);
137
138 o.m[0][0] = cosV;
139 o.m[0][1] = -sinV;
140
141 o.m[1][0] = sinV;
142 o.m[1][1] = cosV;
143}
144
145template <typename T>
146void Matrix33CalcCommon<T>::makeIdentity(Base& o)
147{
148 o.m[0][0] = 1;
149 o.m[0][1] = 0;
150 o.m[0][2] = 0;
151
152 o.m[1][0] = 0;
153 o.m[1][1] = 1;
154 o.m[1][2] = 0;
155
156 o.m[2][0] = 0;
157 o.m[2][1] = 0;
158 o.m[2][2] = 1;
159}
160
161template <typename T>
162void Matrix33CalcCommon<T>::makeZero(Base& o)
163{
164 o.m[0][0] = 0;
165 o.m[0][1] = 0;
166 o.m[0][2] = 0;
167
168 o.m[1][0] = 0;
169 o.m[1][1] = 0;
170 o.m[1][2] = 0;
171
172 o.m[2][0] = 0;
173 o.m[2][1] = 0;
174 o.m[2][2] = 0;
175}
176
177template <typename T>
178void Matrix33CalcCommon<T>::copy(Base& o, const Base& n)
179{
180 o.m[0][0] = n.m[0][0];
181 o.m[0][1] = n.m[0][1];
182 o.m[0][2] = n.m[0][2];
183
184 o.m[1][0] = n.m[1][0];
185 o.m[1][1] = n.m[1][1];
186 o.m[1][2] = n.m[1][2];
187
188 o.m[2][0] = n.m[2][0];
189 o.m[2][1] = n.m[2][1];
190 o.m[2][2] = n.m[2][2];
191}
192
193template <typename T>
194void Matrix33CalcCommon<T>::copy(Base& o, const Mtx34& n)
195{
196 o.m[0][0] = n.m[0][0];
197 o.m[0][1] = n.m[0][1];
198 o.m[0][2] = n.m[0][2];
199
200 o.m[1][0] = n.m[1][0];
201 o.m[1][1] = n.m[1][1];
202 o.m[1][2] = n.m[1][2];
203
204 o.m[2][0] = n.m[2][0];
205 o.m[2][1] = n.m[2][1];
206 o.m[2][2] = n.m[2][2];
207}
208
209template <typename T>
210void Matrix33CalcCommon<T>::inverse(Base& o, const Base& n)
211{
212 const T a11 = n.m[0][0];
213 const T a12 = n.m[0][1];
214 const T a13 = n.m[0][2];
215
216 const T a21 = n.m[1][0];
217 const T a22 = n.m[1][1];
218 const T a23 = n.m[1][2];
219
220 const T a31 = n.m[2][0];
221 const T a32 = n.m[2][1];
222 const T a33 = n.m[2][2];
223
224 T det = (a11 * a22 * a33 - a31 * a22 * a13)
225 + (a12 * a23 * a31 - a21 * a12 * a33)
226 + (a13 * a21 * a32 - a11 * a32 * a23);
227
228 if (det == 0)
229 return makeIdentity(o);
230
231 det = 1 / det;
232
233 o.m[0][0] = (a22 * a33 - a32 * a23) * det;
234 o.m[0][1] = (a32 * a13 - a12 * a33) * det;
235 o.m[0][2] = (a12 * a23 - a22 * a13) * det;
236
237 o.m[1][0] = (a31 * a23 - a21 * a33) * det;
238 o.m[1][1] = (a11 * a33 - a31 * a13) * det;
239 o.m[1][2] = (a21 * a13 - a11 * a23) * det;
240
241 o.m[2][0] = (a21 * a32 - a31 * a22) * det;
242 o.m[2][1] = (a31 * a12 - a11 * a32) * det;
243 o.m[2][2] = (a11 * a22 - a21 * a12) * det;
244}
245
246template <typename T>
247void Matrix33CalcCommon<T>::inverseTranspose(Base& o, const Base& n)
248{
249 const T a11 = n.m[0][0];
250 const T a12 = n.m[0][1];
251 const T a13 = n.m[0][2];
252
253 const T a21 = n.m[1][0];
254 const T a22 = n.m[1][1];
255 const T a23 = n.m[1][2];
256
257 const T a31 = n.m[2][0];
258 const T a32 = n.m[2][1];
259 const T a33 = n.m[2][2];
260
261 T det = (a11 * a22 * a33 - a31 * a22 * a13)
262 + (a12 * a23 * a31 - a21 * a12 * a33)
263 + (a13 * a21 * a32 - a11 * a32 * a23);
264
265 if (det == 0)
266 return makeIdentity(o);
267
268 det = 1 / det;
269
270 o.m[0][0] = (a22 * a33 - a32 * a23) * det;
271 o.m[0][1] = (a31 * a23 - a21 * a33) * det;
272 o.m[0][2] = (a21 * a32 - a31 * a22) * det;
273
274 o.m[1][0] = (a32 * a13 - a12 * a33) * det;
275 o.m[1][1] = (a11 * a33 - a31 * a13) * det;
276 o.m[1][2] = (a31 * a12 - a11 * a32) * det;
277
278 o.m[2][0] = (a12 * a23 - a22 * a13) * det;
279 o.m[2][1] = (a21 * a13 - a11 * a23) * det;
280 o.m[2][2] = (a11 * a22 - a21 * a12) * det;
281}
282
283template <typename T>
284void Matrix33CalcCommon<T>::multiply(Base& o, const Base& a, const Base& b)
285{
286 const T a11 = a.m[0][0];
287 const T a12 = a.m[0][1];
288 const T a13 = a.m[0][2];
289
290 const T a21 = a.m[1][0];
291 const T a22 = a.m[1][1];
292 const T a23 = a.m[1][2];
293
294 const T a31 = a.m[2][0];
295 const T a32 = a.m[2][1];
296 const T a33 = a.m[2][2];
297
298 const T b11 = b.m[0][0];
299 const T b12 = b.m[0][1];
300 const T b13 = b.m[0][2];
301
302 const T b21 = b.m[1][0];
303 const T b22 = b.m[1][1];
304 const T b23 = b.m[1][2];
305
306 const T b31 = b.m[2][0];
307 const T b32 = b.m[2][1];
308 const T b33 = b.m[2][2];
309
310 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31;
311 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32;
312 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33;
313
314 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31;
315 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32;
316 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33;
317
318 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31;
319 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32;
320 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33;
321}
322
323template <typename T>
324void Matrix33CalcCommon<T>::multiply(Base& o, const Mtx34& a, const Base& b)
325{
326 const T a11 = a.m[0][0];
327 const T a12 = a.m[0][1];
328 const T a13 = a.m[0][2];
329
330 const T a21 = a.m[1][0];
331 const T a22 = a.m[1][1];
332 const T a23 = a.m[1][2];
333
334 const T a31 = a.m[2][0];
335 const T a32 = a.m[2][1];
336 const T a33 = a.m[2][2];
337
338 const T b11 = b.m[0][0];
339 const T b12 = b.m[0][1];
340 const T b13 = b.m[0][2];
341
342 const T b21 = b.m[1][0];
343 const T b22 = b.m[1][1];
344 const T b23 = b.m[1][2];
345
346 const T b31 = b.m[2][0];
347 const T b32 = b.m[2][1];
348 const T b33 = b.m[2][2];
349
350 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31;
351 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32;
352 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33;
353
354 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31;
355 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32;
356 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33;
357
358 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31;
359 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32;
360 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33;
361}
362
363template <typename T>
364void Matrix33CalcCommon<T>::multiply(Base& o, const Base& a, const Mtx34& b)
365{
366 const T a11 = a.m[0][0];
367 const T a12 = a.m[0][1];
368 const T a13 = a.m[0][2];
369
370 const T a21 = a.m[1][0];
371 const T a22 = a.m[1][1];
372 const T a23 = a.m[1][2];
373
374 const T a31 = a.m[2][0];
375 const T a32 = a.m[2][1];
376 const T a33 = a.m[2][2];
377
378 const T b11 = b.m[0][0];
379 const T b12 = b.m[0][1];
380 const T b13 = b.m[0][2];
381
382 const T b21 = b.m[1][0];
383 const T b22 = b.m[1][1];
384 const T b23 = b.m[1][2];
385
386 const T b31 = b.m[2][0];
387 const T b32 = b.m[2][1];
388 const T b33 = b.m[2][2];
389
390 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31;
391 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32;
392 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33;
393
394 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31;
395 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32;
396 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33;
397
398 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31;
399 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32;
400 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33;
401}
402
403template <typename T>
404void Matrix33CalcCommon<T>::transpose(Base& o)
405{
406 const T a12 = o.m[0][1];
407 const T a13 = o.m[0][2];
408
409 const T a21 = o.m[1][0];
410 const T a23 = o.m[1][2];
411
412 const T a31 = o.m[2][0];
413 const T a32 = o.m[2][1];
414
415 o.m[0][1] = a21;
416 o.m[0][2] = a31;
417
418 o.m[1][0] = a12;
419 o.m[1][2] = a32;
420
421 o.m[2][0] = a13;
422 o.m[2][1] = a23;
423}
424
425template <typename T>
426void Matrix33CalcCommon<T>::transposeTo(Base& o, const Base& n)
427{
428 SEAD_ASSERT(&o != &n);
429
430 o.m[0][0] = n.m[0][0];
431 o.m[0][1] = n.m[1][0];
432 o.m[0][2] = n.m[2][0];
433
434 o.m[1][0] = n.m[0][1];
435 o.m[1][1] = n.m[1][1];
436 o.m[1][2] = n.m[2][1];
437
438 o.m[2][0] = n.m[0][2];
439 o.m[2][1] = n.m[1][2];
440 o.m[2][2] = n.m[2][2];
441}
442
443template <typename T>
444void Matrix33CalcCommon<T>::makeQ(Base& o, const Quat& q)
445{
446 // Assuming the quaternion "q" is normalized
447
448 const T yy = 2 * q.y * q.y;
449 const T zz = 2 * q.z * q.z;
450 const T xx = 2 * q.x * q.x;
451 const T xy = 2 * q.x * q.y;
452 const T xz = 2 * q.x * q.z;
453 const T yz = 2 * q.y * q.z;
454 const T wz = 2 * q.w * q.z;
455 const T wx = 2 * q.w * q.x;
456 const T wy = 2 * q.w * q.y;
457
458 o.m[0][0] = 1 - yy - zz;
459 o.m[0][1] = xy - wz;
460 o.m[0][2] = xz + wy;
461
462 o.m[1][0] = xy + wz;
463 o.m[1][1] = 1 - xx - zz;
464 o.m[1][2] = yz - wx;
465
466 o.m[2][0] = xz - wy;
467 o.m[2][1] = yz + wx;
468 o.m[2][2] = 1 - xx - yy;
469}
470
471template <typename T>
472void Matrix33CalcCommon<T>::makeR(Base& o, const Vec3& r)
473{
474 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
475 MathCalcCommon<T>::sin(r.y),
476 MathCalcCommon<T>::sin(r.z) };
477
478 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
479 MathCalcCommon<T>::cos(r.y),
480 MathCalcCommon<T>::cos(r.z) };
481
482 o.m[0][0] = (cosV[1] * cosV[2]);
483 o.m[1][0] = (cosV[1] * sinV[2]);
484 o.m[2][0] = -sinV[1];
485
486 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
487 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
488 o.m[2][1] = (sinV[0] * cosV[1]);
489
490 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
491 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
492 o.m[2][2] = (cosV[0] * cosV[1]);
493}
494
495template <typename T>
496void Matrix33CalcCommon<T>::makeRIdx(Base& o, u32 xr, u32 yr, u32 zr)
497{
498 T sinV[3];
499 T cosV[3];
500
501 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
502 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
503 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
504
505 o.m[0][0] = (cosV[1] * cosV[2]);
506 o.m[1][0] = (cosV[1] * sinV[2]);
507 o.m[2][0] = -sinV[1];
508
509 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
510 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
511 o.m[2][1] = (sinV[0] * cosV[1]);
512
513 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
514 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
515 o.m[2][2] = (cosV[0] * cosV[1]);
516}
517
518template <typename T>
519void Matrix33CalcCommon<T>::makeRzxyIdx(Base& o, u32 xr, u32 yr, u32 zr)
520{
521 T sinV[3];
522 T cosV[3];
523
524 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
525 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
526 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
527
528 o.m[2][2] = (cosV[0] * cosV[1]);
529 o.m[0][2] = (cosV[0] * sinV[1]);
530 o.m[1][2] = -sinV[0];
531
532 o.m[2][0] = (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
533 o.m[0][0] = (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
534 o.m[1][0] = (cosV[0] * sinV[2]);
535
536 o.m[2][1] = (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
537 o.m[0][1] = (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
538 o.m[1][1] = (cosV[0] * cosV[2]);
539}
540
541template <typename T>
542void Matrix33CalcCommon<T>::makeS(Base& o, const Vec3& s)
543{
544 o.m[0][0] = s.x;
545 o.m[1][0] = 0;
546 o.m[2][0] = 0;
547
548 o.m[0][1] = 0;
549 o.m[1][1] = s.y;
550 o.m[2][1] = 0;
551
552 o.m[0][2] = 0;
553 o.m[1][2] = 0;
554 o.m[2][2] = s.z;
555}
556
557template <typename T>
558void Matrix33CalcCommon<T>::makeSR(Base& o, const Vec3& s, const Vec3& r)
559{
560 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
561 MathCalcCommon<T>::sin(r.y),
562 MathCalcCommon<T>::sin(r.z) };
563
564 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
565 MathCalcCommon<T>::cos(r.y),
566 MathCalcCommon<T>::cos(r.z) };
567
568 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
569 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
570 o.m[2][0] = s.x * -sinV[1];
571
572 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
573 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
574 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
575
576 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
577 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
578 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
579}
580
581template <typename T>
582void Matrix33CalcCommon<T>::makeSRIdx(Base& o, const Vec3& s, const Vector3<u32>& r)
583{
584 T sinV[3];
585 T cosV[3];
586
587 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
588 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
589 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
590
591 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
592 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
593 o.m[2][0] = s.x * -sinV[1];
594
595 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
596 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
597 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
598
599 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
600 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
601 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
602}
603
604template <typename T>
605void Matrix33CalcCommon<T>::makeSRzxyIdx(Base& o, const Vec3& s, const Vector3<u32>& r)
606{
607 T sinV[3];
608 T cosV[3];
609
610 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
611 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
612 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
613
614 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
615 o.m[0][2] = s.z * (cosV[0] * sinV[1]);
616 o.m[1][2] = s.z * -sinV[0];
617
618 o.m[2][0] = s.x * (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
619 o.m[0][0] = s.x * (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
620 o.m[1][0] = s.x * (cosV[0] * sinV[2]);
621
622 o.m[2][1] = s.y * (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
623 o.m[0][1] = s.y * (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
624 o.m[1][1] = s.y * (cosV[0] * cosV[2]);
625}
626
627template <typename T>
628void Matrix33CalcCommon<T>::toQuat(Quat& q, const Base& n)
629{
630 const T a11 = n.m[0][0];
631 const T a12 = n.m[0][1];
632 const T a13 = n.m[0][2];
633
634 const T a21 = n.m[1][0];
635 const T a22 = n.m[1][1];
636 const T a23 = n.m[1][2];
637
638 const T a31 = n.m[2][0];
639 const T a32 = n.m[2][1];
640 const T a33 = n.m[2][2];
641
642 const T t = a11 + a22 + a33;
643 T w, x, y, z;
644
645 if (t > 0)
646 {
647 T s = MathCalcCommon<T>::sqrt(t + 1);
648
649 w = s * 0.5f;
650
651 //if (s != 0)
652 s = 0.5f / s;
653
654 x = (a32 - a23) * s;
655 y = (a13 - a31) * s;
656 z = (a21 - a12) * s;
657 }
658 else if (a22 > a11 && a33 <= a22)
659 {
660 T s = MathCalcCommon<T>::sqrt(a22 - (a33 + a11) + 1);
661
662 y = s * 0.5f;
663
664 if (s != 0)
665 s = 0.5f / s;
666
667 w = (a13 - a31) * s;
668 x = (a21 + a12) * s;
669 z = (a23 + a32) * s;
670 }
671 else if (a22 > a11 || a33 > a11)
672 {
673 T s = MathCalcCommon<T>::sqrt(a33 - (a11 + a22) + 1);
674
675 z = s * 0.5f;
676
677 if (s != 0)
678 s = 0.5f / s;
679
680 w = (a21 - a12) * s;
681 x = (a31 + a13) * s;
682 y = (a32 + a23) * s;
683 }
684 else
685 {
686 T s = MathCalcCommon<T>::sqrt(a11 - (a22 + a33) + 1);
687
688 x = s * 0.5f;
689
690 if (s != 0)
691 s = 0.5f / s;
692
693 w = (a32 - a23) * s;
694 y = (a12 + a21) * s;
695 z = (a13 + a31) * s;
696 }
697
698 q.w = w;
699 q.x = x;
700 q.y = y;
701 q.z = z;
702}
703
704template <typename T>
705void Matrix34CalcCommon<T>::makeIdentity(Base& o)
706{
707 o.m[0][0] = 1;
708 o.m[0][1] = 0;
709 o.m[0][2] = 0;
710 o.m[0][3] = 0;
711
712 o.m[1][0] = 0;
713 o.m[1][1] = 1;
714 o.m[1][2] = 0;
715 o.m[1][3] = 0;
716
717 o.m[2][0] = 0;
718 o.m[2][1] = 0;
719 o.m[2][2] = 1;
720 o.m[2][3] = 0;
721}
722
723#ifdef cafe
724
725template <>
726inline void
731
732#endif // cafe
733
734template <typename T>
735void Matrix34CalcCommon<T>::makeZero(Base& o)
736{
737 o.m[0][0] = 0;
738 o.m[0][1] = 0;
739 o.m[0][2] = 0;
740 o.m[0][3] = 0;
741
742 o.m[1][0] = 0;
743 o.m[1][1] = 0;
744 o.m[1][2] = 0;
745 o.m[1][3] = 0;
746
747 o.m[2][0] = 0;
748 o.m[2][1] = 0;
749 o.m[2][2] = 0;
750 o.m[2][3] = 0;
751}
752
753template <typename T>
754void Matrix34CalcCommon<T>::copy(Base& o, const Base& n)
755{
756 o.m[0][0] = n.m[0][0];
757 o.m[0][1] = n.m[0][1];
758 o.m[0][2] = n.m[0][2];
759 o.m[0][3] = n.m[0][3];
760
761 o.m[1][0] = n.m[1][0];
762 o.m[1][1] = n.m[1][1];
763 o.m[1][2] = n.m[1][2];
764 o.m[1][3] = n.m[1][3];
765
766 o.m[2][0] = n.m[2][0];
767 o.m[2][1] = n.m[2][1];
768 o.m[2][2] = n.m[2][2];
769 o.m[2][3] = n.m[2][3];
770}
771
772#ifdef cafe
773
774template <>
775inline void
776Matrix34CalcCommon<f32>::copy(Base& o, const Base& n)
777{
778 ASM_MTXCopy(const_cast<f32(*)[4]>(n.m), o.m);
779}
780
781#endif // cafe
782
783template <typename T>
784void Matrix34CalcCommon<T>::copy(Base& o, const Mtx33& n, const Vec3& t)
785{
786 o.m[0][0] = n.m[0][0];
787 o.m[0][1] = n.m[0][1];
788 o.m[0][2] = n.m[0][2];
789 o.m[0][3] = t.x;
790
791 o.m[1][0] = n.m[1][0];
792 o.m[1][1] = n.m[1][1];
793 o.m[1][2] = n.m[1][2];
794 o.m[1][3] = t.y;
795
796 o.m[2][0] = n.m[2][0];
797 o.m[2][1] = n.m[2][1];
798 o.m[2][2] = n.m[2][2];
799 o.m[2][3] = t.z;
800}
801
802template <typename T>
803void Matrix34CalcCommon<T>::copy(Base& o, const Mtx44& n)
804{
805 o.m[0][0] = n.m[0][0];
806 o.m[0][1] = n.m[0][1];
807 o.m[0][2] = n.m[0][2];
808 o.m[0][3] = n.m[0][3];
809
810 o.m[1][0] = n.m[1][0];
811 o.m[1][1] = n.m[1][1];
812 o.m[1][2] = n.m[1][2];
813 o.m[1][3] = n.m[1][3];
814
815 o.m[2][0] = n.m[2][0];
816 o.m[2][1] = n.m[2][1];
817 o.m[2][2] = n.m[2][2];
818 o.m[2][3] = n.m[2][3];
819}
820
821#ifdef cafe
822
823// Nintendo did not actually use this for the cafe f32 specialization
824//
825//template <>
826//inline void
827//Matrix34CalcCommon<f32>::copy(Base& o, const Mtx44& n)
828//{
829// ASM_MTXCopy(const_cast<f32(*)[4]>(n.m), o.m);
830//}
831
832#endif // cafe
833
834template <typename T>
835void Matrix34CalcCommon<T>::inverse(Base& o, const Base& n)
836{
837 const T a11 = n.m[0][0];
838 const T a12 = n.m[0][1];
839 const T a13 = n.m[0][2];
840 const T a14 = n.m[0][3];
841
842 const T a21 = n.m[1][0];
843 const T a22 = n.m[1][1];
844 const T a23 = n.m[1][2];
845 const T a24 = n.m[1][3];
846
847 const T a31 = n.m[2][0];
848 const T a32 = n.m[2][1];
849 const T a33 = n.m[2][2];
850 const T a34 = n.m[2][3];
851
852 T det = (a11 * a22 * a33 - a31 * a22 * a13)
853 + (a12 * a23 * a31 - a21 * a12 * a33)
854 + (a13 * a21 * a32 - a11 * a32 * a23);
855
856 if (det == 0)
857 return makeIdentity(o);
858
859 det = 1 / det;
860
861 o.m[0][0] = (a22 * a33 - a32 * a23) * det;
862 o.m[0][1] = (a32 * a13 - a12 * a33) * det;
863 o.m[0][2] = (a12 * a23 - a22 * a13) * det;
864
865 o.m[1][0] = (a31 * a23 - a21 * a33) * det;
866 o.m[1][1] = (a11 * a33 - a31 * a13) * det;
867 o.m[1][2] = (a21 * a13 - a11 * a23) * det;
868
869 o.m[2][0] = (a21 * a32 - a31 * a22) * det;
870 o.m[2][1] = (a31 * a12 - a11 * a32) * det;
871 o.m[2][2] = (a11 * a22 - a21 * a12) * det;
872
873 o.m[0][3] = o.m[0][0] * -a14 + o.m[0][1] * -a24 + o.m[0][2] * -a34;
874 o.m[1][3] = o.m[1][0] * -a14 + o.m[1][1] * -a24 + o.m[1][2] * -a34;
875 o.m[2][3] = o.m[2][0] * -a14 + o.m[2][1] * -a24 + o.m[2][2] * -a34;
876}
877
878#ifdef cafe
879
880template <>
881inline void
882Matrix34CalcCommon<f32>::inverse(Base& o, const Base& n)
883{
884 u32 ret = ASM_MTXInverse(const_cast<f32(*)[4]>(n.m), o.m);
885
886 // Nintendo did not actually call makeIdentity() for the cafe f32 specialization
887 //if (!ret)
888 // return makeIdentity(o);
889}
890
891#endif // cafe
892
893template <typename T>
894void Matrix34CalcCommon<T>::inverse33(Base& o, const Base& n)
895{
896 const T a11 = n.m[0][0];
897 const T a12 = n.m[0][1];
898 const T a13 = n.m[0][2];
899
900 const T a21 = n.m[1][0];
901 const T a22 = n.m[1][1];
902 const T a23 = n.m[1][2];
903
904 const T a31 = n.m[2][0];
905 const T a32 = n.m[2][1];
906 const T a33 = n.m[2][2];
907
908 T det = (a11 * a22 * a33 - a31 * a22 * a13)
909 + (a12 * a23 * a31 - a21 * a12 * a33)
910 + (a13 * a21 * a32 - a11 * a32 * a23);
911
912 if (det == 0)
913 return makeIdentity(o);
914
915 det = 1 / det;
916
917 o.m[0][0] = (a22 * a33 - a32 * a23) * det;
918 o.m[0][1] = (a32 * a13 - a12 * a33) * det;
919 o.m[0][2] = (a12 * a23 - a22 * a13) * det;
920
921 o.m[1][0] = (a31 * a23 - a21 * a33) * det;
922 o.m[1][1] = (a11 * a33 - a31 * a13) * det;
923 o.m[1][2] = (a21 * a13 - a11 * a23) * det;
924
925 o.m[2][0] = (a21 * a32 - a31 * a22) * det;
926 o.m[2][1] = (a31 * a12 - a11 * a32) * det;
927 o.m[2][2] = (a11 * a22 - a21 * a12) * det;
928
929 o.m[0][3] = 0;
930 o.m[1][3] = 0;
931 o.m[2][3] = 0;
932}
933
934template <typename T>
935void Matrix34CalcCommon<T>::inverseTranspose(Base& o, const Base& n)
936{
937 const T a11 = n.m[0][0];
938 const T a12 = n.m[0][1];
939 const T a13 = n.m[0][2];
940
941 const T a21 = n.m[1][0];
942 const T a22 = n.m[1][1];
943 const T a23 = n.m[1][2];
944
945 const T a31 = n.m[2][0];
946 const T a32 = n.m[2][1];
947 const T a33 = n.m[2][2];
948
949 T det = (a11 * a22 * a33 - a31 * a22 * a13)
950 + (a12 * a23 * a31 - a21 * a12 * a33)
951 + (a13 * a21 * a32 - a11 * a32 * a23);
952
953 if (det == 0)
954 return makeIdentity(o);
955
956 det = 1 / det;
957
958 o.m[0][0] = (a22 * a33 - a23 * a32) * det;
959 o.m[0][1] = (a23 * a31 - a21 * a33) * det;
960 o.m[0][2] = (a21 * a32 - a22 * a31) * det;
961
962 o.m[1][0] = (a13 * a32 - a12 * a33) * det;
963 o.m[1][1] = (a11 * a33 - a13 * a31) * det;
964 o.m[1][2] = (a12 * a31 - a11 * a32) * det;
965
966 o.m[2][0] = (a12 * a23 - a13 * a22) * det;
967 o.m[2][1] = (a13 * a21 - a11 * a23) * det;
968 o.m[2][2] = (a11 * a22 - a12 * a21) * det;
969
970 o.m[0][3] = 0;
971 o.m[1][3] = 0;
972 o.m[2][3] = 0;
973}
974
975template <typename T>
976void Matrix34CalcCommon<T>::multiply(Base& o, const Base& a, const Base& b)
977{
978 const T a11 = a.m[0][0];
979 const T a12 = a.m[0][1];
980 const T a13 = a.m[0][2];
981 const T a14 = a.m[0][3];
982
983 const T a21 = a.m[1][0];
984 const T a22 = a.m[1][1];
985 const T a23 = a.m[1][2];
986 const T a24 = a.m[1][3];
987
988 const T a31 = a.m[2][0];
989 const T a32 = a.m[2][1];
990 const T a33 = a.m[2][2];
991 const T a34 = a.m[2][3];
992
993 const T b11 = b.m[0][0];
994 const T b12 = b.m[0][1];
995 const T b13 = b.m[0][2];
996 const T b14 = b.m[0][3];
997
998 const T b21 = b.m[1][0];
999 const T b22 = b.m[1][1];
1000 const T b23 = b.m[1][2];
1001 const T b24 = b.m[1][3];
1002
1003 const T b31 = b.m[2][0];
1004 const T b32 = b.m[2][1];
1005 const T b33 = b.m[2][2];
1006 const T b34 = b.m[2][3];
1007
1008 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31;
1009 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32;
1010 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33;
1011 o.m[0][3] = a11 * b14 + a12 * b24 + a13 * b34 + a14;
1012
1013 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31;
1014 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32;
1015 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33;
1016 o.m[1][3] = a21 * b14 + a22 * b24 + a23 * b34 + a24;
1017
1018 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31;
1019 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32;
1020 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33;
1021 o.m[2][3] = a31 * b14 + a32 * b24 + a33 * b34 + a34;
1022}
1023
1024#ifdef cafe
1025
1026template <>
1027inline void
1028Matrix34CalcCommon<f32>::multiply(Base& o, const Base& a, const Base& b)
1029{
1030 ASM_MTXConcat(const_cast<f32(*)[4]>(a.m), const_cast<f32(*)[4]>(b.m), o.m);
1031}
1032
1033#endif // cafe
1034
1035template <typename T>
1036void Matrix34CalcCommon<T>::multiply(Base& o, const Mtx33& a, const Base& b)
1037{
1038 const T a11 = a.m[0][0];
1039 const T a12 = a.m[0][1];
1040 const T a13 = a.m[0][2];
1041
1042 const T a21 = a.m[1][0];
1043 const T a22 = a.m[1][1];
1044 const T a23 = a.m[1][2];
1045
1046 const T a31 = a.m[2][0];
1047 const T a32 = a.m[2][1];
1048 const T a33 = a.m[2][2];
1049
1050 const T b11 = b.m[0][0];
1051 const T b12 = b.m[0][1];
1052 const T b13 = b.m[0][2];
1053 const T b14 = b.m[0][3];
1054
1055 const T b21 = b.m[1][0];
1056 const T b22 = b.m[1][1];
1057 const T b23 = b.m[1][2];
1058 const T b24 = b.m[1][3];
1059
1060 const T b31 = b.m[2][0];
1061 const T b32 = b.m[2][1];
1062 const T b33 = b.m[2][2];
1063 const T b34 = b.m[2][3];
1064
1065 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31;
1066 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32;
1067 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33;
1068 o.m[0][3] = a11 * b14 + a12 * b24 + a13 * b34;
1069
1070 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31;
1071 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32;
1072 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33;
1073 o.m[1][3] = a21 * b14 + a22 * b24 + a23 * b34;
1074
1075 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31;
1076 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32;
1077 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33;
1078 o.m[2][3] = a31 * b14 + a32 * b24 + a33 * b34;
1079}
1080
1081template <typename T>
1082void Matrix34CalcCommon<T>::multiply(Base& o, const Base& a, const Mtx33& b)
1083{
1084 const T a11 = a.m[0][0];
1085 const T a12 = a.m[0][1];
1086 const T a13 = a.m[0][2];
1087 const T a14 = a.m[0][3];
1088
1089 const T a21 = a.m[1][0];
1090 const T a22 = a.m[1][1];
1091 const T a23 = a.m[1][2];
1092 const T a24 = a.m[1][3];
1093
1094 const T a31 = a.m[2][0];
1095 const T a32 = a.m[2][1];
1096 const T a33 = a.m[2][2];
1097 const T a34 = a.m[2][3];
1098
1099 const T b11 = b.m[0][0];
1100 const T b12 = b.m[0][1];
1101 const T b13 = b.m[0][2];
1102
1103 const T b21 = b.m[1][0];
1104 const T b22 = b.m[1][1];
1105 const T b23 = b.m[1][2];
1106
1107 const T b31 = b.m[2][0];
1108 const T b32 = b.m[2][1];
1109 const T b33 = b.m[2][2];
1110
1111 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31;
1112 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32;
1113 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33;
1114 o.m[0][3] = a14;
1115
1116 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31;
1117 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32;
1118 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33;
1119 o.m[1][3] = a24;
1120
1121 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31;
1122 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32;
1123 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33;
1124 o.m[2][3] = a34;
1125}
1126
1127template <typename T>
1128void Matrix34CalcCommon<T>::transpose(Base& o)
1129{
1130 const T a12 = o.m[0][1];
1131 const T a13 = o.m[0][2];
1132
1133 const T a21 = o.m[1][0];
1134 const T a23 = o.m[1][2];
1135
1136 const T a31 = o.m[2][0];
1137 const T a32 = o.m[2][1];
1138
1139 o.m[0][1] = a21;
1140 o.m[0][2] = a31;
1141 o.m[0][3] = 0;
1142
1143 o.m[1][0] = a12;
1144 o.m[1][2] = a32;
1145 o.m[1][3] = 0;
1146
1147 o.m[2][0] = a13;
1148 o.m[2][1] = a23;
1149 o.m[2][3] = 0;
1150}
1151
1152template <typename T>
1153void Matrix34CalcCommon<T>::transposeTo(Base& o, const Base& n)
1154{
1155 SEAD_ASSERT(&o != &n);
1156
1157 o.m[0][0] = n.m[0][0];
1158 o.m[0][1] = n.m[1][0];
1159 o.m[0][2] = n.m[2][0];
1160 o.m[0][3] = 0;
1161
1162 o.m[1][0] = n.m[0][1];
1163 o.m[1][1] = n.m[1][1];
1164 o.m[1][2] = n.m[2][1];
1165 o.m[1][3] = 0;
1166
1167 o.m[2][0] = n.m[0][2];
1168 o.m[2][1] = n.m[1][2];
1169 o.m[2][2] = n.m[2][2];
1170 o.m[2][3] = 0;
1171}
1172
1173#ifdef cafe
1174
1175// Nintendo did not actually use this for the cafe f32 specialization
1176//
1177//template <>
1178//inline void
1179//Matrix34CalcCommon<f32>::transposeTo(Base& o, const Base& n)
1180//{
1181// ASM_MTXTranspose(const_cast<f32(*)[4]>(n.m), o.m);
1182//}
1183
1184#endif // cafe
1185
1186template <typename T>
1187void Matrix34CalcCommon<T>::makeQ(Base& o, const Quat& q)
1188{
1189 // Assuming the quaternion "q" is normalized
1190
1191 const T yy = 2 * q.y * q.y;
1192 const T zz = 2 * q.z * q.z;
1193 const T xx = 2 * q.x * q.x;
1194 const T xy = 2 * q.x * q.y;
1195 const T xz = 2 * q.x * q.z;
1196 const T yz = 2 * q.y * q.z;
1197 const T wz = 2 * q.w * q.z;
1198 const T wx = 2 * q.w * q.x;
1199 const T wy = 2 * q.w * q.y;
1200
1201 o.m[0][0] = 1 - yy - zz;
1202 o.m[0][1] = xy - wz;
1203 o.m[0][2] = xz + wy;
1204
1205 o.m[1][0] = xy + wz;
1206 o.m[1][1] = 1 - xx - zz;
1207 o.m[1][2] = yz - wx;
1208
1209 o.m[2][0] = xz - wy;
1210 o.m[2][1] = yz + wx;
1211 o.m[2][2] = 1 - xx - yy;
1212
1213 o.m[0][3] = 0;
1214 o.m[1][3] = 0;
1215 o.m[2][3] = 0;
1216}
1217
1218template <typename T>
1219void Matrix34CalcCommon<T>::makeR(Base& o, const Vec3& r)
1220{
1221 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
1222 MathCalcCommon<T>::sin(r.y),
1223 MathCalcCommon<T>::sin(r.z) };
1224
1225 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
1226 MathCalcCommon<T>::cos(r.y),
1227 MathCalcCommon<T>::cos(r.z) };
1228
1229 o.m[0][0] = (cosV[1] * cosV[2]);
1230 o.m[1][0] = (cosV[1] * sinV[2]);
1231 o.m[2][0] = -sinV[1];
1232
1233 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1234 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1235 o.m[2][1] = (sinV[0] * cosV[1]);
1236
1237 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1238 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1239 o.m[2][2] = (cosV[0] * cosV[1]);
1240
1241 o.m[0][3] = 0;
1242 o.m[1][3] = 0;
1243 o.m[2][3] = 0;
1244}
1245
1246template <typename T>
1247void Matrix34CalcCommon<T>::makeRIdx(Base& o, u32 xr, u32 yr, u32 zr)
1248{
1249 T sinV[3];
1250 T cosV[3];
1251
1252 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
1253 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
1254 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
1255
1256 o.m[0][0] = (cosV[1] * cosV[2]);
1257 o.m[1][0] = (cosV[1] * sinV[2]);
1258 o.m[2][0] = -sinV[1];
1259
1260 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1261 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1262 o.m[2][1] = (sinV[0] * cosV[1]);
1263
1264 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1265 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1266 o.m[2][2] = (cosV[0] * cosV[1]);
1267
1268 o.m[0][3] = 0;
1269 o.m[1][3] = 0;
1270 o.m[2][3] = 0;
1271}
1272
1273template <typename T>
1274void Matrix34CalcCommon<T>::makeRT(Base& o, const Vec3& r, const Vec3& t)
1275{
1276 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
1277 MathCalcCommon<T>::sin(r.y),
1278 MathCalcCommon<T>::sin(r.z) };
1279
1280 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
1281 MathCalcCommon<T>::cos(r.y),
1282 MathCalcCommon<T>::cos(r.z) };
1283
1284 o.m[0][0] = (cosV[1] * cosV[2]);
1285 o.m[1][0] = (cosV[1] * sinV[2]);
1286 o.m[2][0] = -sinV[1];
1287
1288 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1289 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1290 o.m[2][1] = (sinV[0] * cosV[1]);
1291
1292 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1293 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1294 o.m[2][2] = (cosV[0] * cosV[1]);
1295
1296 o.m[0][3] = t.x;
1297 o.m[1][3] = t.y;
1298 o.m[2][3] = t.z;
1299}
1300
1301template <typename T>
1302void Matrix34CalcCommon<T>::makeRTIdx(Base& o, const Vector3<u32>& r, const Vec3& t)
1303{
1304 T sinV[3];
1305 T cosV[3];
1306
1307 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1308 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1309 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1310
1311 o.m[0][0] = (cosV[1] * cosV[2]);
1312 o.m[1][0] = (cosV[1] * sinV[2]);
1313 o.m[2][0] = -sinV[1];
1314
1315 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1316 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1317 o.m[2][1] = (sinV[0] * cosV[1]);
1318
1319 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1320 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1321 o.m[2][2] = (cosV[0] * cosV[1]);
1322
1323 o.m[0][3] = t.x;
1324 o.m[1][3] = t.y;
1325 o.m[2][3] = t.z;
1326}
1327
1328template <typename T>
1329void Matrix34CalcCommon<T>::makeRzxyIdx(Base& o, u32 xr, u32 yr, u32 zr)
1330{
1331 T sinV[3];
1332 T cosV[3];
1333
1334 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
1335 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
1336 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
1337
1338 o.m[2][2] = (cosV[0] * cosV[1]);
1339 o.m[0][2] = (cosV[0] * sinV[1]);
1340 o.m[1][2] = -sinV[0];
1341
1342 o.m[2][0] = (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
1343 o.m[0][0] = (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
1344 o.m[1][0] = (cosV[0] * sinV[2]);
1345
1346 o.m[2][1] = (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
1347 o.m[0][1] = (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
1348 o.m[1][1] = (cosV[0] * cosV[2]);
1349
1350 o.m[0][3] = 0;
1351 o.m[1][3] = 0;
1352 o.m[2][3] = 0;
1353}
1354
1355template <typename T>
1356void Matrix34CalcCommon<T>::makeRzxyTIdx(Base& o, const Vector3<u32>& r, const Vec3& t)
1357{
1358 T sinV[3];
1359 T cosV[3];
1360
1361 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1362 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1363 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1364
1365 o.m[2][2] = (cosV[0] * cosV[1]);
1366 o.m[0][2] = (cosV[0] * sinV[1]);
1367 o.m[1][2] = -sinV[0];
1368
1369 o.m[2][0] = (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
1370 o.m[0][0] = (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
1371 o.m[1][0] = (cosV[0] * sinV[2]);
1372
1373 o.m[2][1] = (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
1374 o.m[0][1] = (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
1375 o.m[1][1] = (cosV[0] * cosV[2]);
1376
1377 o.m[0][3] = t.x;
1378 o.m[1][3] = t.y;
1379 o.m[2][3] = t.z;
1380}
1381
1382template <typename T>
1383void Matrix34CalcCommon<T>::makeS(Base& o, const Vec3& s)
1384{
1385 o.m[0][0] = s.x;
1386 o.m[1][0] = 0;
1387 o.m[2][0] = 0;
1388
1389 o.m[0][1] = 0;
1390 o.m[1][1] = s.y;
1391 o.m[2][1] = 0;
1392
1393 o.m[0][2] = 0;
1394 o.m[1][2] = 0;
1395 o.m[2][2] = s.z;
1396
1397 o.m[0][3] = 0;
1398 o.m[1][3] = 0;
1399 o.m[2][3] = 0;
1400}
1401
1402#ifdef cafe
1403
1404template <>
1405inline void
1406Matrix34CalcCommon<f32>::makeS(Base& o, const Vec3& s)
1407{
1408 ASM_MTXScale(o.m, s.x, s.y, s.z);
1409}
1410
1411#endif // cafe
1412
1413template <typename T>
1414void Matrix34CalcCommon<T>::makeSR(Base& o, const Vec3& s, const Vec3& r)
1415{
1416 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
1417 MathCalcCommon<T>::sin(r.y),
1418 MathCalcCommon<T>::sin(r.z) };
1419
1420 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
1421 MathCalcCommon<T>::cos(r.y),
1422 MathCalcCommon<T>::cos(r.z) };
1423
1424 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
1425 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
1426 o.m[2][0] = s.x * -sinV[1];
1427
1428 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1429 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1430 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
1431
1432 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1433 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1434 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1435
1436 o.m[0][3] = 0;
1437 o.m[1][3] = 0;
1438 o.m[2][3] = 0;
1439}
1440
1441template <typename T>
1442void Matrix34CalcCommon<T>::makeSRIdx(Base& o, const Vec3& s, const Vector3<u32>& r)
1443{
1444 T sinV[3];
1445 T cosV[3];
1446
1447 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1448 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1449 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1450
1451 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
1452 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
1453 o.m[2][0] = s.x * -sinV[1];
1454
1455 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1456 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1457 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
1458
1459 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1460 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1461 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1462
1463 o.m[0][3] = 0;
1464 o.m[1][3] = 0;
1465 o.m[2][3] = 0;
1466}
1467
1468template <typename T>
1469void Matrix34CalcCommon<T>::makeSRT(Base& o, const Vec3& s, const Vec3& r, const Vec3& t)
1470{
1471 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
1472 MathCalcCommon<T>::sin(r.y),
1473 MathCalcCommon<T>::sin(r.z) };
1474
1475 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
1476 MathCalcCommon<T>::cos(r.y),
1477 MathCalcCommon<T>::cos(r.z) };
1478
1479 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
1480 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
1481 o.m[2][0] = s.x * -sinV[1];
1482
1483 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1484 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1485 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
1486
1487 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1488 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1489 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1490
1491 o.m[0][3] = t.x;
1492 o.m[1][3] = t.y;
1493 o.m[2][3] = t.z;
1494}
1495
1496template <typename T>
1497void Matrix34CalcCommon<T>::makeSRTIdx(Base& o, const Vec3& s, const Vector3<u32>& r, const Vec3& t)
1498{
1499 T sinV[3];
1500 T cosV[3];
1501
1502 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1503 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1504 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1505
1506 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
1507 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
1508 o.m[2][0] = s.x * -sinV[1];
1509
1510 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1511 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1512 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
1513
1514 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1515 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1516 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1517
1518 o.m[0][3] = t.x;
1519 o.m[1][3] = t.y;
1520 o.m[2][3] = t.z;
1521}
1522
1523template <typename T>
1524void Matrix34CalcCommon<T>::makeSRzxyIdx(Base& o, const Vec3& s, const Vector3<u32>& r)
1525{
1526 T sinV[3];
1527 T cosV[3];
1528
1529 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1530 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1531 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1532
1533 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1534 o.m[0][2] = s.z * (cosV[0] * sinV[1]);
1535 o.m[1][2] = s.z * -sinV[0];
1536
1537 o.m[2][0] = s.x * (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
1538 o.m[0][0] = s.x * (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
1539 o.m[1][0] = s.x * (cosV[0] * sinV[2]);
1540
1541 o.m[2][1] = s.y * (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
1542 o.m[0][1] = s.y * (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
1543 o.m[1][1] = s.y * (cosV[0] * cosV[2]);
1544
1545 o.m[0][3] = 0;
1546 o.m[1][3] = 0;
1547 o.m[2][3] = 0;
1548}
1549
1550template <typename T>
1551void Matrix34CalcCommon<T>::makeSRzxyTIdx(Base& o, const Vec3& s, const Vector3<u32>& r, const Vec3& t)
1552{
1553 T sinV[3];
1554 T cosV[3];
1555
1556 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1557 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1558 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1559
1560 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1561 o.m[0][2] = s.z * (cosV[0] * sinV[1]);
1562 o.m[1][2] = s.z * -sinV[0];
1563
1564 o.m[2][0] = s.x * (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
1565 o.m[0][0] = s.x * (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
1566 o.m[1][0] = s.x * (cosV[0] * sinV[2]);
1567
1568 o.m[2][1] = s.y * (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
1569 o.m[0][1] = s.y * (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
1570 o.m[1][1] = s.y * (cosV[0] * cosV[2]);
1571
1572 o.m[0][3] = t.x;
1573 o.m[1][3] = t.y;
1574 o.m[2][3] = t.z;
1575}
1576
1577template <typename T>
1578void Matrix34CalcCommon<T>::makeST(Base& o, const Vec3& s, const Vec3& t)
1579{
1580 o.m[0][0] = s.x;
1581 o.m[1][0] = 0;
1582 o.m[2][0] = 0;
1583
1584 o.m[0][1] = 0;
1585 o.m[1][1] = s.y;
1586 o.m[2][1] = 0;
1587
1588 o.m[0][2] = 0;
1589 o.m[1][2] = 0;
1590 o.m[2][2] = s.z;
1591
1592 o.m[0][3] = t.x;
1593 o.m[1][3] = t.y;
1594 o.m[2][3] = t.z;
1595}
1596
1597template <typename T>
1598void Matrix34CalcCommon<T>::makeT(Base& o, const Vec3& t)
1599{
1600 o.m[0][0] = 1;
1601 o.m[1][0] = 0;
1602 o.m[2][0] = 0;
1603
1604 o.m[0][1] = 0;
1605 o.m[1][1] = 1;
1606 o.m[2][1] = 0;
1607
1608 o.m[0][2] = 0;
1609 o.m[1][2] = 0;
1610 o.m[2][2] = 1;
1611
1612 o.m[0][3] = t.x;
1613 o.m[1][3] = t.y;
1614 o.m[2][3] = t.z;
1615}
1616
1617#ifdef cafe
1618
1619template <>
1620inline void
1621Matrix34CalcCommon<f32>::makeT(Base& o, const Vec3& t)
1622{
1623 ASM_MTXTrans(o.m, t.x, t.y, t.z);
1624}
1625
1626#endif // cafe
1627
1628template <typename T>
1629void Matrix34CalcCommon<T>::toQuat(Quat& q, const Base& n)
1630{
1631 const T a11 = n.m[0][0];
1632 const T a12 = n.m[0][1];
1633 const T a13 = n.m[0][2];
1634
1635 const T a21 = n.m[1][0];
1636 const T a22 = n.m[1][1];
1637 const T a23 = n.m[1][2];
1638
1639 const T a31 = n.m[2][0];
1640 const T a32 = n.m[2][1];
1641 const T a33 = n.m[2][2];
1642
1643 const T t = a11 + a22 + a33;
1644 T w, x, y, z;
1645
1646 if (t > 0)
1647 {
1648 T s = MathCalcCommon<T>::sqrt(t + 1);
1649
1650 w = s * 0.5f;
1651
1652 //if (s != 0)
1653 s = 0.5f / s;
1654
1655 x = (a32 - a23) * s;
1656 y = (a13 - a31) * s;
1657 z = (a21 - a12) * s;
1658 }
1659 else if (a22 > a11 && a33 <= a22)
1660 {
1661 T s = MathCalcCommon<T>::sqrt(a22 - (a33 + a11) + 1);
1662
1663 y = s * 0.5f;
1664
1665 if (s != 0)
1666 s = 0.5f / s;
1667
1668 w = (a13 - a31) * s;
1669 x = (a21 + a12) * s;
1670 z = (a23 + a32) * s;
1671 }
1672 else if (a22 > a11 || a33 > a11)
1673 {
1674 T s = MathCalcCommon<T>::sqrt(a33 - (a11 + a22) + 1);
1675
1676 z = s * 0.5f;
1677
1678 if (s != 0)
1679 s = 0.5f / s;
1680
1681 w = (a21 - a12) * s;
1682 x = (a31 + a13) * s;
1683 y = (a32 + a23) * s;
1684 }
1685 else
1686 {
1687 T s = MathCalcCommon<T>::sqrt(a11 - (a22 + a33) + 1);
1688
1689 x = s * 0.5f;
1690
1691 if (s != 0)
1692 s = 0.5f / s;
1693
1694 w = (a32 - a23) * s;
1695 y = (a12 + a21) * s;
1696 z = (a13 + a31) * s;
1697 }
1698
1699 q.w = w;
1700 q.x = x;
1701 q.y = y;
1702 q.z = z;
1703}
1704
1705template <typename T>
1706void Matrix34CalcCommon<T>::multScaleLocal(Base& o, const Base& n, const Vec3& s)
1707{
1708 o.m[0][0] = n.m[0][0] * s.x;
1709 o.m[1][0] = n.m[1][0] * s.x;
1710 o.m[2][0] = n.m[2][0] * s.x;
1711
1712 o.m[0][1] = n.m[0][1] * s.y;
1713 o.m[1][1] = n.m[1][1] * s.y;
1714 o.m[2][1] = n.m[2][1] * s.y;
1715
1716 o.m[0][2] = n.m[0][2] * s.z;
1717 o.m[1][2] = n.m[1][2] * s.z;
1718 o.m[2][2] = n.m[2][2] * s.z;
1719
1720 o.m[0][3] = n.m[0][3];
1721 o.m[1][3] = n.m[1][3];
1722 o.m[2][3] = n.m[2][3];
1723}
1724
1725template <typename T>
1726void Matrix34CalcCommon<T>::multTranslationLocal(Base& o, const Base& n, const Vec3& t)
1727{
1728 o.m[0][0] = n.m[0][0];
1729 o.m[0][1] = n.m[0][1];
1730 o.m[0][2] = n.m[0][2];
1731 o.m[0][3] = n.m[0][0] * t.x + n.m[0][1] * t.y + n.m[0][2] * t.z + n.m[0][3];
1732
1733 o.m[1][0] = n.m[1][0];
1734 o.m[1][1] = n.m[1][1];
1735 o.m[1][2] = n.m[1][2];
1736 o.m[1][3] = n.m[1][0] * t.x + n.m[1][1] * t.y + n.m[1][2] * t.z + n.m[1][3];
1737
1738 o.m[2][0] = n.m[2][0];
1739 o.m[2][1] = n.m[2][1];
1740 o.m[2][2] = n.m[2][2];
1741 o.m[2][3] = n.m[2][0] * t.x + n.m[2][1] * t.y + n.m[2][2] * t.z + n.m[2][3];
1742}
1743
1744template <typename T>
1745void Matrix34CalcCommon<T>::multScaleWorld(Base& o, const Vec3& s, const Base& n)
1746{
1747 o.m[0][0] = n.m[0][0] * s.x;
1748 o.m[0][1] = n.m[0][1] * s.x;
1749 o.m[0][2] = n.m[0][2] * s.x;
1750 o.m[0][3] = n.m[0][3] * s.x;
1751
1752 o.m[1][0] = n.m[1][0] * s.y;
1753 o.m[1][1] = n.m[1][1] * s.y;
1754 o.m[1][2] = n.m[1][2] * s.y;
1755 o.m[1][3] = n.m[1][3] * s.y;
1756
1757 o.m[2][0] = n.m[2][0] * s.z;
1758 o.m[2][1] = n.m[2][1] * s.z;
1759 o.m[2][2] = n.m[2][2] * s.z;
1760 o.m[2][3] = n.m[2][3] * s.z;
1761}
1762
1763template <typename T>
1764void Matrix34CalcCommon<T>::multTranslationWorld(Base& o, const Vec3& t, const Base& n)
1765{
1766 o.m[0][0] = n.m[0][0];
1767 o.m[0][1] = n.m[0][1];
1768 o.m[0][2] = n.m[0][2];
1769 o.m[0][3] = n.m[0][3] + t.x;
1770
1771 o.m[1][0] = n.m[1][0];
1772 o.m[1][1] = n.m[1][1];
1773 o.m[1][2] = n.m[1][2];
1774 o.m[1][3] = n.m[1][3] + t.y;
1775
1776 o.m[2][0] = n.m[2][0];
1777 o.m[2][1] = n.m[2][1];
1778 o.m[2][2] = n.m[2][2];
1779 o.m[2][3] = n.m[2][3] + t.z;
1780}
1781
1782template <typename T>
1783void Matrix34CalcCommon<T>::getBase(Vec3& v, const Base& n, s32 axis)
1784{
1785 v.x = n.m[0][axis];
1786 v.y = n.m[1][axis];
1787 v.z = n.m[2][axis];
1788}
1789
1790template <typename T>
1791void Matrix34CalcCommon<T>::getRow(Vec4& v, const Base& n, s32 row)
1792{
1793 v.x = n.m[row][0];
1794 v.y = n.m[row][1];
1795 v.z = n.m[row][2];
1796 v.w = n.m[row][3];
1797}
1798
1799template <typename T>
1800void Matrix34CalcCommon<T>::getTranslation(Vec3& v, const Base& n)
1801{
1802 getBase(v, n, 3);
1803}
1804
1805template <typename T>
1806void Matrix34CalcCommon<T>::scaleAllElements(Base& n, T s)
1807{
1808 n.m[0][0] *= s;
1809 n.m[0][1] *= s;
1810 n.m[0][2] *= s;
1811 n.m[0][3] *= s;
1812
1813 n.m[1][0] *= s;
1814 n.m[1][1] *= s;
1815 n.m[1][2] *= s;
1816 n.m[1][3] *= s;
1817
1818 n.m[2][0] *= s;
1819 n.m[2][1] *= s;
1820 n.m[2][2] *= s;
1821 n.m[2][3] *= s;
1822}
1823
1824template <typename T>
1825void Matrix34CalcCommon<T>::scaleBases(Base& n, T sx, T sy, T sz)
1826{
1827 n.m[0][0] *= sx;
1828 n.m[1][0] *= sx;
1829 n.m[2][0] *= sx;
1830
1831 n.m[0][1] *= sy;
1832 n.m[1][1] *= sy;
1833 n.m[2][1] *= sy;
1834
1835 n.m[0][2] *= sz;
1836 n.m[1][2] *= sz;
1837 n.m[2][2] *= sz;
1838}
1839
1840template <typename T>
1841void Matrix34CalcCommon<T>::setBase(Base& n, s32 axis, const Vec3& v)
1842{
1843 n.m[0][axis] = v.x;
1844 n.m[1][axis] = v.y;
1845 n.m[2][axis] = v.z;
1846}
1847
1848template <typename T>
1849void Matrix34CalcCommon<T>::setRow(Base& n, const Vec4& v, s32 row)
1850{
1851 n.m[row][0] = v.x;
1852 n.m[row][1] = v.y;
1853 n.m[row][2] = v.z;
1854 n.m[row][3] = v.w;
1855}
1856
1857template <typename T>
1858void Matrix34CalcCommon<T>::setTranslation(Base& n, const Vec3& v)
1859{
1860 setBase(n, 3, v);
1861}
1862
1863template <typename T>
1864void Matrix44CalcCommon<T>::makeIdentity(Base& o)
1865{
1866 o.m[0][0] = 1;
1867 o.m[0][1] = 0;
1868 o.m[0][2] = 0;
1869 o.m[0][3] = 0;
1870
1871 o.m[1][0] = 0;
1872 o.m[1][1] = 1;
1873 o.m[1][2] = 0;
1874 o.m[1][3] = 0;
1875
1876 o.m[2][0] = 0;
1877 o.m[2][1] = 0;
1878 o.m[2][2] = 1;
1879 o.m[2][3] = 0;
1880
1881 o.m[3][0] = 0;
1882 o.m[3][1] = 0;
1883 o.m[3][2] = 0;
1884 o.m[3][3] = 1;
1885}
1886
1887#ifdef cafe
1888
1889template <>
1890inline void
1895
1896#endif // cafe
1897
1898template <typename T>
1899void Matrix44CalcCommon<T>::makeZero(Base& o)
1900{
1901 o.m[0][0] = 0;
1902 o.m[0][1] = 0;
1903 o.m[0][2] = 0;
1904 o.m[0][3] = 0;
1905
1906 o.m[1][0] = 0;
1907 o.m[1][1] = 0;
1908 o.m[1][2] = 0;
1909 o.m[1][3] = 0;
1910
1911 o.m[2][0] = 0;
1912 o.m[2][1] = 0;
1913 o.m[2][2] = 0;
1914 o.m[2][3] = 0;
1915
1916 o.m[3][0] = 0;
1917 o.m[3][1] = 0;
1918 o.m[3][2] = 0;
1919 o.m[3][3] = 0;
1920}
1921
1922template <typename T>
1923void Matrix44CalcCommon<T>::copy(Base& o, const Base& n)
1924{
1925 o.m[0][0] = n.m[0][0];
1926 o.m[0][1] = n.m[0][1];
1927 o.m[0][2] = n.m[0][2];
1928 o.m[0][3] = n.m[0][3];
1929
1930 o.m[1][0] = n.m[1][0];
1931 o.m[1][1] = n.m[1][1];
1932 o.m[1][2] = n.m[1][2];
1933 o.m[1][3] = n.m[1][3];
1934
1935 o.m[2][0] = n.m[2][0];
1936 o.m[2][1] = n.m[2][1];
1937 o.m[2][2] = n.m[2][2];
1938 o.m[2][3] = n.m[2][3];
1939
1940 o.m[3][0] = n.m[3][0];
1941 o.m[3][1] = n.m[3][1];
1942 o.m[3][2] = n.m[3][2];
1943 o.m[3][3] = n.m[3][3];
1944}
1945
1946#ifdef cafe
1947
1948template <>
1949inline void
1950Matrix44CalcCommon<f32>::copy(Base& o, const Base& n)
1951{
1952 ASM_MTX44Copy(const_cast<f32(*)[4]>(n.m), o.m);
1953}
1954
1955#endif // cafe
1956
1957template <typename T>
1958void Matrix44CalcCommon<T>::copy(Base& o, const Mtx33& n, const Vec3& t, const Vec4& v)
1959{
1960 o.m[0][0] = n.m[0][0];
1961 o.m[0][1] = n.m[0][1];
1962 o.m[0][2] = n.m[0][2];
1963 o.m[0][3] = t.x;
1964
1965 o.m[1][0] = n.m[1][0];
1966 o.m[1][1] = n.m[1][1];
1967 o.m[1][2] = n.m[1][2];
1968 o.m[1][3] = t.y;
1969
1970 o.m[2][0] = n.m[2][0];
1971 o.m[2][1] = n.m[2][1];
1972 o.m[2][2] = n.m[2][2];
1973 o.m[2][3] = t.z;
1974
1975 o.m[3][0] = v.x;
1976 o.m[3][1] = v.y;
1977 o.m[3][2] = v.z;
1978 o.m[3][3] = v.w;
1979}
1980
1981template <typename T>
1982void Matrix44CalcCommon<T>::copy(Base& o, const Mtx34& n, const Vec4& v)
1983{
1984 o.m[0][0] = n.m[0][0];
1985 o.m[0][1] = n.m[0][1];
1986 o.m[0][2] = n.m[0][2];
1987 o.m[0][3] = n.m[0][3];
1988
1989 o.m[1][0] = n.m[1][0];
1990 o.m[1][1] = n.m[1][1];
1991 o.m[1][2] = n.m[1][2];
1992 o.m[1][3] = n.m[1][3];
1993
1994 o.m[2][0] = n.m[2][0];
1995 o.m[2][1] = n.m[2][1];
1996 o.m[2][2] = n.m[2][2];
1997 o.m[2][3] = n.m[2][3];
1998
1999 o.m[3][0] = v.x;
2000 o.m[3][1] = v.y;
2001 o.m[3][2] = v.z;
2002 o.m[3][3] = v.w;
2003}
2004
2005#ifdef cafe
2006
2007// Nintendo did not actually use this for the cafe f32 specialization
2008//
2009//template <>
2010//inline void
2011//Matrix44CalcCommon<f32>::copy(Base& o, const Mtx34& n, const Vec4& v)
2012//{
2013// ASM_MTXCopy(const_cast<f32(*)[4]>(n.m), o.m);
2014//
2015// o.m[3][0] = v.x;
2016// o.m[3][1] = v.y;
2017// o.m[3][2] = v.z;
2018// o.m[3][3] = v.w;
2019//}
2020
2021#endif // cafe
2022
2023template <typename T>
2024void Matrix44CalcCommon<T>::inverse(Base& o, const Base& n)
2025{
2026 const T a11 = n.m[0][0];
2027 const T a12 = n.m[0][1];
2028 const T a13 = n.m[0][2];
2029 const T a14 = n.m[0][3];
2030
2031 const T a21 = n.m[1][0];
2032 const T a22 = n.m[1][1];
2033 const T a23 = n.m[1][2];
2034 const T a24 = n.m[1][3];
2035
2036 const T a31 = n.m[2][0];
2037 const T a32 = n.m[2][1];
2038 const T a33 = n.m[2][2];
2039 const T a34 = n.m[2][3];
2040
2041 const T a41 = n.m[3][0];
2042 const T a42 = n.m[3][1];
2043 const T a43 = n.m[3][2];
2044 const T a44 = n.m[3][3];
2045
2046 T det = a11 * (a22 * a33 * a44 + a23 * a34 * a42 + a24 * a32 * a43)
2047 + a12 * (a21 * a34 * a43 + a23 * a31 * a44 + a24 * a33 * a41)
2048 + a13 * (a21 * a32 * a44 + a22 * a34 * a41 + a24 * a31 * a42)
2049 + a14 * (a21 * a33 * a42 + a22 * a31 * a43 + a23 * a32 * a41)
2050 - a11 * (a22 * a34 * a43 + a23 * a32 * a44 + a24 * a33 * a42)
2051 - a12 * (a21 * a33 * a44 + a23 * a34 * a41 + a24 * a31 * a43)
2052 - a13 * (a21 * a34 * a42 + a22 * a31 * a44 + a24 * a32 * a41)
2053 - a14 * (a21 * a32 * a43 + a22 * a33 * a41 + a23 * a31 * a42);
2054
2055 if (det == 0)
2056 return makeIdentity(o);
2057
2058 det = 1 / det;
2059
2060 const T a33xa44_a34xa43 = a33 * a44 - a34 * a43;
2061 const T a32xa44_a34xa42 = a32 * a44 - a34 * a42;
2062 const T a33xa42_a32xa43 = a33 * a42 - a32 * a43;
2063 const T a33xa41_a31xa43 = a33 * a41 - a31 * a43;
2064 const T a31xa44_a34xa41 = a31 * a44 - a34 * a41;
2065 const T a32xa41_a31xa42 = a32 * a41 - a31 * a42;
2066 const T a23xa44_a24xa43 = a23 * a44 - a24 * a43;
2067 const T a24xa33_a23xa34 = a24 * a33 - a23 * a34;
2068 const T a24xa42_a22xa44 = a24 * a42 - a22 * a44;
2069 const T a22xa43_a23xa42 = a22 * a43 - a23 * a42;
2070 const T a22xa34_a24xa32 = a22 * a34 - a24 * a32;
2071 const T a23xa32_a22xa33 = a23 * a32 - a22 * a33;
2072 const T a21xa44_a24xa41 = a21 * a44 - a24 * a41;
2073 const T a23xa41_a21xa43 = a23 * a41 - a21 * a43;
2074 const T a24xa31_a21xa34 = a24 * a31 - a21 * a34;
2075 const T a21xa33_a23xa31 = a21 * a33 - a23 * a31;
2076 const T a21xa42_a22xa41 = a21 * a42 - a22 * a41;
2077 const T a22xa31_a21xa32 = a22 * a31 - a21 * a32;
2078
2079 o.m[0][0] = (a22 * a33xa44_a34xa43 - a23 * a32xa44_a34xa42 - a24 * a33xa42_a32xa43) * det;
2080 o.m[0][1] = (a13 * a32xa44_a34xa42 + a14 * a33xa42_a32xa43 - a12 * a33xa44_a34xa43) * det;
2081 o.m[0][2] = (a12 * a23xa44_a24xa43 + a13 * a24xa42_a22xa44 + a14 * a22xa43_a23xa42) * det;
2082 o.m[0][3] = (a12 * a24xa33_a23xa34 + a13 * a22xa34_a24xa32 + a14 * a23xa32_a22xa33) * det;
2083
2084 o.m[1][0] = (a23 * a31xa44_a34xa41 + a24 * a33xa41_a31xa43 - a21 * a33xa44_a34xa43) * det;
2085 o.m[1][1] = (a11 * a33xa44_a34xa43 - a13 * a31xa44_a34xa41 - a14 * a33xa41_a31xa43) * det;
2086 o.m[1][2] = (a13 * a21xa44_a24xa41 + a14 * a23xa41_a21xa43 - a11 * a23xa44_a24xa43) * det;
2087 o.m[1][3] = (a13 * a24xa31_a21xa34 + a14 * a21xa33_a23xa31 - a11 * a24xa33_a23xa34) * det;
2088
2089 o.m[2][0] = (a21 * a32xa44_a34xa42 - a22 * a31xa44_a34xa41 - a24 * a32xa41_a31xa42) * det;
2090 o.m[2][1] = (a12 * a31xa44_a34xa41 + a14 * a32xa41_a31xa42 - a11 * a32xa44_a34xa42) * det;
2091 o.m[2][2] = (a14 * a21xa42_a22xa41 - a11 * a24xa42_a22xa44 - a12 * a21xa44_a24xa41) * det;
2092 o.m[2][3] = (a14 * a22xa31_a21xa32 - a11 * a22xa34_a24xa32 - a12 * a24xa31_a21xa34) * det;
2093
2094 o.m[3][0] = (a21 * a33xa42_a32xa43 - a22 * a33xa41_a31xa43 + a23 * a32xa41_a31xa42) * det;
2095 o.m[3][1] = (a12 * a33xa41_a31xa43 - a13 * a32xa41_a31xa42 - a11 * a33xa42_a32xa43) * det;
2096 o.m[3][2] = (-a13 * a21xa42_a22xa41 - a11 * a22xa43_a23xa42 - a12 * a23xa41_a21xa43) * det;
2097 o.m[3][3] = (-a13 * a22xa31_a21xa32 - a11 * a23xa32_a22xa33 - a12 * a21xa33_a23xa31) * det;
2098}
2099
2100template <typename T>
2101void Matrix44CalcCommon<T>::inverseTranspose(Base& o, const Base& n)
2102{
2103 const T a11 = n.m[0][0];
2104 const T a12 = n.m[0][1];
2105 const T a13 = n.m[0][2];
2106 const T a14 = n.m[0][3];
2107
2108 const T a21 = n.m[1][0];
2109 const T a22 = n.m[1][1];
2110 const T a23 = n.m[1][2];
2111 const T a24 = n.m[1][3];
2112
2113 const T a31 = n.m[2][0];
2114 const T a32 = n.m[2][1];
2115 const T a33 = n.m[2][2];
2116 const T a34 = n.m[2][3];
2117
2118 const T a41 = n.m[3][0];
2119 const T a42 = n.m[3][1];
2120 const T a43 = n.m[3][2];
2121 const T a44 = n.m[3][3];
2122
2123 T det = a11 * (a22 * a33 * a44 + a23 * a34 * a42 + a24 * a32 * a43)
2124 + a12 * (a21 * a34 * a43 + a23 * a31 * a44 + a24 * a33 * a41)
2125 + a13 * (a21 * a32 * a44 + a22 * a34 * a41 + a24 * a31 * a42)
2126 + a14 * (a21 * a33 * a42 + a22 * a31 * a43 + a23 * a32 * a41)
2127 - a11 * (a22 * a34 * a43 + a23 * a32 * a44 + a24 * a33 * a42)
2128 - a12 * (a21 * a33 * a44 + a23 * a34 * a41 + a24 * a31 * a43)
2129 - a13 * (a21 * a34 * a42 + a22 * a31 * a44 + a24 * a32 * a41)
2130 - a14 * (a21 * a32 * a43 + a22 * a33 * a41 + a23 * a31 * a42);
2131
2132 if (det == 0)
2133 return makeIdentity(o);
2134
2135 det = 1 / det;
2136
2137 const T a33xa44_a34xa43 = a33 * a44 - a34 * a43;
2138 const T a32xa44_a34xa42 = a32 * a44 - a34 * a42;
2139 const T a33xa42_a32xa43 = a33 * a42 - a32 * a43;
2140 const T a33xa41_a31xa43 = a33 * a41 - a31 * a43;
2141 const T a31xa44_a34xa41 = a31 * a44 - a34 * a41;
2142 const T a32xa41_a31xa42 = a32 * a41 - a31 * a42;
2143 const T a23xa44_a24xa43 = a23 * a44 - a24 * a43;
2144 const T a24xa33_a23xa34 = a24 * a33 - a23 * a34;
2145 const T a24xa42_a22xa44 = a24 * a42 - a22 * a44;
2146 const T a22xa43_a23xa42 = a22 * a43 - a23 * a42;
2147 const T a22xa34_a24xa32 = a22 * a34 - a24 * a32;
2148 const T a23xa32_a22xa33 = a23 * a32 - a22 * a33;
2149 const T a21xa44_a24xa41 = a21 * a44 - a24 * a41;
2150 const T a23xa41_a21xa43 = a23 * a41 - a21 * a43;
2151 const T a24xa31_a21xa34 = a24 * a31 - a21 * a34;
2152 const T a21xa33_a23xa31 = a21 * a33 - a23 * a31;
2153 const T a21xa42_a22xa41 = a21 * a42 - a22 * a41;
2154 const T a22xa31_a21xa32 = a22 * a31 - a21 * a32;
2155
2156 o.m[0][0] = (a22 * a33xa44_a34xa43 - a23 * a32xa44_a34xa42 - a24 * a33xa42_a32xa43) * det;
2157 o.m[0][1] = (a23 * a31xa44_a34xa41 + a24 * a33xa41_a31xa43 - a21 * a33xa44_a34xa43) * det;
2158 o.m[0][2] = (a21 * a32xa44_a34xa42 - a22 * a31xa44_a34xa41 - a24 * a32xa41_a31xa42) * det;
2159 o.m[0][3] = (a21 * a33xa42_a32xa43 - a22 * a33xa41_a31xa43 + a23 * a32xa41_a31xa42) * det;
2160
2161 o.m[1][0] = (a13 * a32xa44_a34xa42 + a14 * a33xa42_a32xa43 - a12 * a33xa44_a34xa43) * det;
2162 o.m[1][1] = (a11 * a33xa44_a34xa43 - a13 * a31xa44_a34xa41 - a14 * a33xa41_a31xa43) * det;
2163 o.m[1][2] = (a12 * a31xa44_a34xa41 + a14 * a32xa41_a31xa42 - a11 * a32xa44_a34xa42) * det;
2164 o.m[1][3] = (a12 * a33xa41_a31xa43 - a13 * a32xa41_a31xa42 - a11 * a33xa42_a32xa43) * det;
2165
2166 o.m[2][0] = (a12 * a23xa44_a24xa43 + a13 * a24xa42_a22xa44 + a14 * a22xa43_a23xa42) * det;
2167 o.m[2][1] = (a13 * a21xa44_a24xa41 + a14 * a23xa41_a21xa43 - a11 * a23xa44_a24xa43) * det;
2168 o.m[2][2] = (a14 * a21xa42_a22xa41 - a11 * a24xa42_a22xa44 - a12 * a21xa44_a24xa41) * det;
2169 o.m[2][3] = (-a13 * a21xa42_a22xa41 - a11 * a22xa43_a23xa42 - a12 * a23xa41_a21xa43) * det;
2170
2171 o.m[3][0] = (a12 * a24xa33_a23xa34 + a13 * a22xa34_a24xa32 + a14 * a23xa32_a22xa33) * det;
2172 o.m[3][1] = (a13 * a24xa31_a21xa34 + a14 * a21xa33_a23xa31 - a11 * a24xa33_a23xa34) * det;
2173 o.m[3][2] = (a14 * a22xa31_a21xa32 - a11 * a22xa34_a24xa32 - a12 * a24xa31_a21xa34) * det;
2174 o.m[3][3] = (-a13 * a22xa31_a21xa32 - a11 * a23xa32_a22xa33 - a12 * a21xa33_a23xa31) * det;
2175}
2176
2177template <typename T>
2178void Matrix44CalcCommon<T>::multiply(Base& o, const Base& a, const Base& b)
2179{
2180 const T a11 = a.m[0][0];
2181 const T a12 = a.m[0][1];
2182 const T a13 = a.m[0][2];
2183 const T a14 = a.m[0][3];
2184
2185 const T a21 = a.m[1][0];
2186 const T a22 = a.m[1][1];
2187 const T a23 = a.m[1][2];
2188 const T a24 = a.m[1][3];
2189
2190 const T a31 = a.m[2][0];
2191 const T a32 = a.m[2][1];
2192 const T a33 = a.m[2][2];
2193 const T a34 = a.m[2][3];
2194
2195 const T a41 = a.m[3][0];
2196 const T a42 = a.m[3][1];
2197 const T a43 = a.m[3][2];
2198 const T a44 = a.m[3][3];
2199
2200 const T b11 = b.m[0][0];
2201 const T b12 = b.m[0][1];
2202 const T b13 = b.m[0][2];
2203 const T b14 = b.m[0][3];
2204
2205 const T b21 = b.m[1][0];
2206 const T b22 = b.m[1][1];
2207 const T b23 = b.m[1][2];
2208 const T b24 = b.m[1][3];
2209
2210 const T b31 = b.m[2][0];
2211 const T b32 = b.m[2][1];
2212 const T b33 = b.m[2][2];
2213 const T b34 = b.m[2][3];
2214
2215 const T b41 = b.m[3][0];
2216 const T b42 = b.m[3][1];
2217 const T b43 = b.m[3][2];
2218 const T b44 = b.m[3][3];
2219
2220 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41;
2221 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42;
2222 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43;
2223 o.m[0][3] = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44;
2224
2225 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41;
2226 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42;
2227 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43;
2228 o.m[1][3] = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44;
2229
2230 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41;
2231 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42;
2232 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43;
2233 o.m[2][3] = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44;
2234
2235 o.m[3][0] = a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41;
2236 o.m[3][1] = a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42;
2237 o.m[3][2] = a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43;
2238 o.m[3][3] = a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44;
2239}
2240
2241#ifdef cafe
2242
2243template <>
2244inline void
2245Matrix44CalcCommon<f32>::multiply(Base& o, const Base& a, const Base& b)
2246{
2247 ASM_MTX44Concat(const_cast<f32(*)[4]>(a.m), const_cast<f32(*)[4]>(b.m), o.m);
2248}
2249
2250#endif // cafe
2251
2252template <typename T>
2253void Matrix44CalcCommon<T>::multiply(Base& o, const Mtx34& a, const Base& b)
2254{
2255 const T a11 = a.m[0][0];
2256 const T a12 = a.m[0][1];
2257 const T a13 = a.m[0][2];
2258 const T a14 = a.m[0][3];
2259
2260 const T a21 = a.m[1][0];
2261 const T a22 = a.m[1][1];
2262 const T a23 = a.m[1][2];
2263 const T a24 = a.m[1][3];
2264
2265 const T a31 = a.m[2][0];
2266 const T a32 = a.m[2][1];
2267 const T a33 = a.m[2][2];
2268 const T a34 = a.m[2][3];
2269
2270 const T b11 = b.m[0][0];
2271 const T b12 = b.m[0][1];
2272 const T b13 = b.m[0][2];
2273 const T b14 = b.m[0][3];
2274
2275 const T b21 = b.m[1][0];
2276 const T b22 = b.m[1][1];
2277 const T b23 = b.m[1][2];
2278 const T b24 = b.m[1][3];
2279
2280 const T b31 = b.m[2][0];
2281 const T b32 = b.m[2][1];
2282 const T b33 = b.m[2][2];
2283 const T b34 = b.m[2][3];
2284
2285 const T b41 = b.m[3][0];
2286 const T b42 = b.m[3][1];
2287 const T b43 = b.m[3][2];
2288 const T b44 = b.m[3][3];
2289
2290 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41;
2291 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42;
2292 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43;
2293 o.m[0][3] = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44;
2294
2295 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41;
2296 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42;
2297 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43;
2298 o.m[1][3] = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44;
2299
2300 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41;
2301 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42;
2302 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43;
2303 o.m[2][3] = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44;
2304
2305 o.m[3][0] = b41;
2306 o.m[3][1] = b42;
2307 o.m[3][2] = b43;
2308 o.m[3][3] = b44;
2309}
2310
2311template <typename T>
2312void Matrix44CalcCommon<T>::multiply(Base& o, const Base& a, const Mtx34& b)
2313{
2314 const T a11 = a.m[0][0];
2315 const T a12 = a.m[0][1];
2316 const T a13 = a.m[0][2];
2317 const T a14 = a.m[0][3];
2318
2319 const T a21 = a.m[1][0];
2320 const T a22 = a.m[1][1];
2321 const T a23 = a.m[1][2];
2322 const T a24 = a.m[1][3];
2323
2324 const T a31 = a.m[2][0];
2325 const T a32 = a.m[2][1];
2326 const T a33 = a.m[2][2];
2327 const T a34 = a.m[2][3];
2328
2329 const T a41 = a.m[3][0];
2330 const T a42 = a.m[3][1];
2331 const T a43 = a.m[3][2];
2332 const T a44 = a.m[3][3];
2333
2334 const T b11 = b.m[0][0];
2335 const T b12 = b.m[0][1];
2336 const T b13 = b.m[0][2];
2337 const T b14 = b.m[0][3];
2338
2339 const T b21 = b.m[1][0];
2340 const T b22 = b.m[1][1];
2341 const T b23 = b.m[1][2];
2342 const T b24 = b.m[1][3];
2343
2344 const T b31 = b.m[2][0];
2345 const T b32 = b.m[2][1];
2346 const T b33 = b.m[2][2];
2347 const T b34 = b.m[2][3];
2348
2349 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31;
2350 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32;
2351 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33;
2352 o.m[0][3] = a11 * b14 + a12 * b24 + a13 * b34 + a14;
2353
2354 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31;
2355 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32;
2356 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33;
2357 o.m[1][3] = a21 * b14 + a22 * b24 + a23 * b34 + a24;
2358
2359 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31;
2360 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32;
2361 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33;
2362 o.m[2][3] = a31 * b14 + a32 * b24 + a33 * b34 + a34;
2363
2364 o.m[3][0] = a41 * b11 + a42 * b21 + a43 * b31;
2365 o.m[3][1] = a41 * b12 + a42 * b22 + a43 * b32;
2366 o.m[3][2] = a41 * b13 + a42 * b23 + a43 * b33;
2367 o.m[3][3] = a41 * b14 + a42 * b24 + a43 * b34 + a44;
2368}
2369
2370template <typename T>
2371void Matrix44CalcCommon<T>::transpose(Base& o)
2372{
2373 const T a12 = o.m[0][1];
2374 const T a13 = o.m[0][2];
2375 const T a14 = o.m[0][3];
2376
2377 const T a21 = o.m[1][0];
2378 const T a23 = o.m[1][2];
2379 const T a24 = o.m[1][3];
2380
2381 const T a31 = o.m[2][0];
2382 const T a32 = o.m[2][1];
2383 const T a34 = o.m[2][3];
2384
2385 const T a41 = o.m[3][0];
2386 const T a42 = o.m[3][1];
2387 const T a43 = o.m[3][2];
2388
2389 o.m[0][1] = a21;
2390 o.m[0][2] = a31;
2391 o.m[0][3] = a41;
2392
2393 o.m[1][0] = a12;
2394 o.m[1][2] = a32;
2395 o.m[1][3] = a42;
2396
2397 o.m[2][0] = a13;
2398 o.m[2][1] = a23;
2399 o.m[2][3] = a43;
2400
2401 o.m[3][0] = a14;
2402 o.m[3][1] = a24;
2403 o.m[3][2] = a34;
2404}
2405
2406template <typename T>
2407void Matrix44CalcCommon<T>::transposeTo(Base& o, const Base& n)
2408{
2409 SEAD_ASSERT(&o != &n);
2410
2411 o.m[0][0] = n.m[0][0];
2412 o.m[0][1] = n.m[1][0];
2413 o.m[0][2] = n.m[2][0];
2414 o.m[0][3] = n.m[3][0];
2415
2416 o.m[1][0] = n.m[0][1];
2417 o.m[1][1] = n.m[1][1];
2418 o.m[1][2] = n.m[2][1];
2419 o.m[1][3] = n.m[3][1];
2420
2421 o.m[2][0] = n.m[0][2];
2422 o.m[2][1] = n.m[1][2];
2423 o.m[2][2] = n.m[2][2];
2424 o.m[2][3] = n.m[3][2];
2425
2426 o.m[3][0] = n.m[0][3];
2427 o.m[3][1] = n.m[1][3];
2428 o.m[3][2] = n.m[2][3];
2429 o.m[3][3] = n.m[3][3];
2430}
2431
2432template <typename T>
2433void Matrix44CalcCommon<T>::makeQ(Base& o, const Quat& q)
2434{
2435 // Assuming the quaternion "q" is normalized
2436
2437 const T yy = 2 * q.y * q.y;
2438 const T zz = 2 * q.z * q.z;
2439 const T xx = 2 * q.x * q.x;
2440 const T xy = 2 * q.x * q.y;
2441 const T xz = 2 * q.x * q.z;
2442 const T yz = 2 * q.y * q.z;
2443 const T wz = 2 * q.w * q.z;
2444 const T wx = 2 * q.w * q.x;
2445 const T wy = 2 * q.w * q.y;
2446
2447 o.m[0][0] = 1 - yy - zz;
2448 o.m[0][1] = xy - wz;
2449 o.m[0][2] = xz + wy;
2450
2451 o.m[1][0] = xy + wz;
2452 o.m[1][1] = 1 - xx - zz;
2453 o.m[1][2] = yz - wx;
2454
2455 o.m[2][0] = xz - wy;
2456 o.m[2][1] = yz + wx;
2457 o.m[2][2] = 1 - xx - yy;
2458
2459 o.m[0][3] = 0;
2460 o.m[1][3] = 0;
2461 o.m[2][3] = 0;
2462
2463 o.m[3][0] = 0;
2464 o.m[3][1] = 0;
2465 o.m[3][2] = 0;
2466 o.m[3][3] = 1;
2467}
2468
2469template <typename T>
2470void Matrix44CalcCommon<T>::makeR(Base& o, const Vec3& r)
2471{
2472 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
2473 MathCalcCommon<T>::sin(r.y),
2474 MathCalcCommon<T>::sin(r.z) };
2475
2476 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
2477 MathCalcCommon<T>::cos(r.y),
2478 MathCalcCommon<T>::cos(r.z) };
2479
2480 o.m[0][0] = (cosV[1] * cosV[2]);
2481 o.m[1][0] = (cosV[1] * sinV[2]);
2482 o.m[2][0] = -sinV[1];
2483
2484 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
2485 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
2486 o.m[2][1] = (sinV[0] * cosV[1]);
2487
2488 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
2489 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
2490 o.m[2][2] = (cosV[0] * cosV[1]);
2491
2492 o.m[0][3] = 0;
2493 o.m[1][3] = 0;
2494 o.m[2][3] = 0;
2495
2496 o.m[3][0] = 0;
2497 o.m[3][1] = 0;
2498 o.m[3][2] = 0;
2499 o.m[3][3] = 1;
2500}
2501
2502template <typename T>
2503void Matrix44CalcCommon<T>::makeRIdx(Base& o, u32 xr, u32 yr, u32 zr)
2504{
2505 T sinV[3];
2506 T cosV[3];
2507
2508 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
2509 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
2510 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
2511
2512 o.m[0][0] = (cosV[1] * cosV[2]);
2513 o.m[1][0] = (cosV[1] * sinV[2]);
2514 o.m[2][0] = -sinV[1];
2515
2516 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
2517 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
2518 o.m[2][1] = (sinV[0] * cosV[1]);
2519
2520 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
2521 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
2522 o.m[2][2] = (cosV[0] * cosV[1]);
2523
2524 o.m[0][3] = 0;
2525 o.m[1][3] = 0;
2526 o.m[2][3] = 0;
2527
2528 o.m[3][0] = 0;
2529 o.m[3][1] = 0;
2530 o.m[3][2] = 0;
2531 o.m[3][3] = 1;
2532}
2533
2534template <typename T>
2535void Matrix44CalcCommon<T>::makeRzxyIdx(Base& o, u32 xr, u32 yr, u32 zr)
2536{
2537 T sinV[3];
2538 T cosV[3];
2539
2540 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
2541 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
2542 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
2543
2544 o.m[2][2] = (cosV[0] * cosV[1]);
2545 o.m[0][2] = (cosV[0] * sinV[1]);
2546 o.m[1][2] = -sinV[0];
2547
2548 o.m[2][0] = (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
2549 o.m[0][0] = (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
2550 o.m[1][0] = (cosV[0] * sinV[2]);
2551
2552 o.m[2][1] = (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
2553 o.m[0][1] = (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
2554 o.m[1][1] = (cosV[0] * cosV[2]);
2555
2556 o.m[0][3] = 0;
2557 o.m[1][3] = 0;
2558 o.m[2][3] = 0;
2559
2560 o.m[3][0] = 0;
2561 o.m[3][1] = 0;
2562 o.m[3][2] = 0;
2563 o.m[3][3] = 1;
2564}
2565
2566template <typename T>
2567void Matrix44CalcCommon<T>::toQuat(Quat& q, const Base& n)
2568{
2569 const T a11 = n.m[0][0];
2570 const T a12 = n.m[0][1];
2571 const T a13 = n.m[0][2];
2572
2573 const T a21 = n.m[1][0];
2574 const T a22 = n.m[1][1];
2575 const T a23 = n.m[1][2];
2576
2577 const T a31 = n.m[2][0];
2578 const T a32 = n.m[2][1];
2579 const T a33 = n.m[2][2];
2580
2581 const T t = a11 + a22 + a33;
2582 T w, x, y, z;
2583
2584 if (t > 0)
2585 {
2586 T s = MathCalcCommon<T>::sqrt(t + 1);
2587
2588 w = s * 0.5f;
2589
2590 //if (s != 0)
2591 s = 0.5f / s;
2592
2593 x = (a32 - a23) * s;
2594 y = (a13 - a31) * s;
2595 z = (a21 - a12) * s;
2596 }
2597 else if (a22 > a11 && a33 <= a22)
2598 {
2599 T s = MathCalcCommon<T>::sqrt(a22 - (a33 + a11) + 1);
2600
2601 y = s * 0.5f;
2602
2603 if (s != 0)
2604 s = 0.5f / s;
2605
2606 w = (a13 - a31) * s;
2607 x = (a21 + a12) * s;
2608 z = (a23 + a32) * s;
2609 }
2610 else if (a22 > a11 || a33 > a11)
2611 {
2612 T s = MathCalcCommon<T>::sqrt(a33 - (a11 + a22) + 1);
2613
2614 z = s * 0.5f;
2615
2616 if (s != 0)
2617 s = 0.5f / s;
2618
2619 w = (a21 - a12) * s;
2620 x = (a31 + a13) * s;
2621 y = (a32 + a23) * s;
2622 }
2623 else
2624 {
2625 T s = MathCalcCommon<T>::sqrt(a11 - (a22 + a33) + 1);
2626
2627 x = s * 0.5f;
2628
2629 if (s != 0)
2630 s = 0.5f / s;
2631
2632 w = (a32 - a23) * s;
2633 y = (a12 + a21) * s;
2634 z = (a13 + a31) * s;
2635 }
2636
2637 q.w = w;
2638 q.x = x;
2639 q.y = y;
2640 q.z = z;
2641}
2642
2643template <typename T>
2644void Matrix44CalcCommon<T>::getCol(Vec4& v, const Base& n, s32 axis)
2645{
2646 v.x = n.m[0][axis];
2647 v.y = n.m[1][axis];
2648 v.z = n.m[2][axis];
2649 v.w = n.m[3][axis];
2650}
2651
2652template <typename T>
2653void Matrix44CalcCommon<T>::getRow(Vec4& v, const Base& n, s32 row)
2654{
2655 v.x = n.m[row][0];
2656 v.y = n.m[row][1];
2657 v.z = n.m[row][2];
2658 v.w = n.m[row][3];
2659}
2660
2661template <typename T>
2662void Matrix44CalcCommon<T>::scaleAllElements(Base& n, T s)
2663{
2664 n.m[0][0] *= s;
2665 n.m[0][1] *= s;
2666 n.m[0][2] *= s;
2667 n.m[0][3] *= s;
2668
2669 n.m[1][0] *= s;
2670 n.m[1][1] *= s;
2671 n.m[1][2] *= s;
2672 n.m[1][3] *= s;
2673
2674 n.m[2][0] *= s;
2675 n.m[2][1] *= s;
2676 n.m[2][2] *= s;
2677 n.m[2][3] *= s;
2678
2679 n.m[3][0] *= s;
2680 n.m[3][1] *= s;
2681 n.m[3][2] *= s;
2682 n.m[3][3] *= s;
2683}
2684
2685template <typename T>
2686void Matrix44CalcCommon<T>::scaleBases(Base& n, T sx, T sy, T sz, T sw)
2687{
2688 n.m[0][0] *= sx;
2689 n.m[1][0] *= sx;
2690 n.m[2][0] *= sx;
2691 n.m[3][0] *= sx;
2692
2693 n.m[0][1] *= sy;
2694 n.m[1][1] *= sy;
2695 n.m[2][1] *= sy;
2696 n.m[3][1] *= sy;
2697
2698 n.m[0][2] *= sz;
2699 n.m[1][2] *= sz;
2700 n.m[2][2] *= sz;
2701 n.m[3][2] *= sz;
2702
2703 n.m[0][3] *= sw;
2704 n.m[1][3] *= sw;
2705 n.m[2][3] *= sw;
2706 n.m[3][3] *= sw;
2707}
2708
2709template <typename T>
2710void Matrix44CalcCommon<T>::setCol(Base& n, s32 axis, const Vec4& v)
2711{
2712 n.m[0][axis] = v.x;
2713 n.m[1][axis] = v.y;
2714 n.m[2][axis] = v.z;
2715 n.m[3][axis] = v.w;
2716}
2717
2718template <typename T>
2719void Matrix44CalcCommon<T>::setRow(Base& n, const Vec4& v, s32 row)
2720{
2721 n.m[row][0] = v.x;
2722 n.m[row][1] = v.y;
2723 n.m[row][2] = v.z;
2724 n.m[row][3] = v.w;
2725}
2726
2727} // namespace sead
sead
Definition seadAssert.h:44
SEAD_ASSERT
#define SEAD_ASSERT(condition)
Definition seadAssert.h:24