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>::makeQT(Base& o, const Quat& q, const Vec3& t)
1220{
1221 // Assuming the quaternion "q" is normalized
1222
1223 const T yy = 2 * q.y * q.y;
1224 const T zz = 2 * q.z * q.z;
1225 const T xx = 2 * q.x * q.x;
1226 const T xy = 2 * q.x * q.y;
1227 const T xz = 2 * q.x * q.z;
1228 const T yz = 2 * q.y * q.z;
1229 const T wz = 2 * q.w * q.z;
1230 const T wx = 2 * q.w * q.x;
1231 const T wy = 2 * q.w * q.y;
1232
1233 o.m[0][0] = 1 - yy - zz;
1234 o.m[0][1] = xy - wz;
1235 o.m[0][2] = xz + wy;
1236
1237 o.m[1][0] = xy + wz;
1238 o.m[1][1] = 1 - xx - zz;
1239 o.m[1][2] = yz - wx;
1240
1241 o.m[2][0] = xz - wy;
1242 o.m[2][1] = yz + wx;
1243 o.m[2][2] = 1 - xx - yy;
1244
1245 o.m[0][3] = t.x;
1246 o.m[1][3] = t.y;
1247 o.m[2][3] = t.z;
1248}
1249
1250template <typename T>
1251void Matrix34CalcCommon<T>::makeR(Base& o, const Vec3& r)
1252{
1253 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
1254 MathCalcCommon<T>::sin(r.y),
1255 MathCalcCommon<T>::sin(r.z) };
1256
1257 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
1258 MathCalcCommon<T>::cos(r.y),
1259 MathCalcCommon<T>::cos(r.z) };
1260
1261 o.m[0][0] = (cosV[1] * cosV[2]);
1262 o.m[1][0] = (cosV[1] * sinV[2]);
1263 o.m[2][0] = -sinV[1];
1264
1265 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1266 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1267 o.m[2][1] = (sinV[0] * cosV[1]);
1268
1269 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1270 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1271 o.m[2][2] = (cosV[0] * cosV[1]);
1272
1273 o.m[0][3] = 0;
1274 o.m[1][3] = 0;
1275 o.m[2][3] = 0;
1276}
1277
1278template <typename T>
1279void Matrix34CalcCommon<T>::makeRIdx(Base& o, u32 xr, u32 yr, u32 zr)
1280{
1281 T sinV[3];
1282 T cosV[3];
1283
1284 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
1285 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
1286 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
1287
1288 o.m[0][0] = (cosV[1] * cosV[2]);
1289 o.m[1][0] = (cosV[1] * sinV[2]);
1290 o.m[2][0] = -sinV[1];
1291
1292 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1293 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1294 o.m[2][1] = (sinV[0] * cosV[1]);
1295
1296 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1297 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1298 o.m[2][2] = (cosV[0] * cosV[1]);
1299
1300 o.m[0][3] = 0;
1301 o.m[1][3] = 0;
1302 o.m[2][3] = 0;
1303}
1304
1305template <typename T>
1306void Matrix34CalcCommon<T>::makeRT(Base& o, const Vec3& r, const Vec3& t)
1307{
1308 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
1309 MathCalcCommon<T>::sin(r.y),
1310 MathCalcCommon<T>::sin(r.z) };
1311
1312 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
1313 MathCalcCommon<T>::cos(r.y),
1314 MathCalcCommon<T>::cos(r.z) };
1315
1316 o.m[0][0] = (cosV[1] * cosV[2]);
1317 o.m[1][0] = (cosV[1] * sinV[2]);
1318 o.m[2][0] = -sinV[1];
1319
1320 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1321 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1322 o.m[2][1] = (sinV[0] * cosV[1]);
1323
1324 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1325 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1326 o.m[2][2] = (cosV[0] * cosV[1]);
1327
1328 o.m[0][3] = t.x;
1329 o.m[1][3] = t.y;
1330 o.m[2][3] = t.z;
1331}
1332
1333template <typename T>
1334void Matrix34CalcCommon<T>::makeRTIdx(Base& o, const Vector3<u32>& r, const Vec3& t)
1335{
1336 T sinV[3];
1337 T cosV[3];
1338
1339 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1340 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1341 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1342
1343 o.m[0][0] = (cosV[1] * cosV[2]);
1344 o.m[1][0] = (cosV[1] * sinV[2]);
1345 o.m[2][0] = -sinV[1];
1346
1347 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1348 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1349 o.m[2][1] = (sinV[0] * cosV[1]);
1350
1351 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1352 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1353 o.m[2][2] = (cosV[0] * cosV[1]);
1354
1355 o.m[0][3] = t.x;
1356 o.m[1][3] = t.y;
1357 o.m[2][3] = t.z;
1358}
1359
1360template <typename T>
1361void Matrix34CalcCommon<T>::makeRzxyIdx(Base& o, u32 xr, u32 yr, u32 zr)
1362{
1363 T sinV[3];
1364 T cosV[3];
1365
1366 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
1367 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
1368 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
1369
1370 o.m[2][2] = (cosV[0] * cosV[1]);
1371 o.m[0][2] = (cosV[0] * sinV[1]);
1372 o.m[1][2] = -sinV[0];
1373
1374 o.m[2][0] = (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
1375 o.m[0][0] = (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
1376 o.m[1][0] = (cosV[0] * sinV[2]);
1377
1378 o.m[2][1] = (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
1379 o.m[0][1] = (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
1380 o.m[1][1] = (cosV[0] * cosV[2]);
1381
1382 o.m[0][3] = 0;
1383 o.m[1][3] = 0;
1384 o.m[2][3] = 0;
1385}
1386
1387template <typename T>
1388void Matrix34CalcCommon<T>::makeRzxyTIdx(Base& o, const Vector3<u32>& r, const Vec3& t)
1389{
1390 T sinV[3];
1391 T cosV[3];
1392
1393 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1394 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1395 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1396
1397 o.m[2][2] = (cosV[0] * cosV[1]);
1398 o.m[0][2] = (cosV[0] * sinV[1]);
1399 o.m[1][2] = -sinV[0];
1400
1401 o.m[2][0] = (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
1402 o.m[0][0] = (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
1403 o.m[1][0] = (cosV[0] * sinV[2]);
1404
1405 o.m[2][1] = (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
1406 o.m[0][1] = (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
1407 o.m[1][1] = (cosV[0] * cosV[2]);
1408
1409 o.m[0][3] = t.x;
1410 o.m[1][3] = t.y;
1411 o.m[2][3] = t.z;
1412}
1413
1414template <typename T>
1415void Matrix34CalcCommon<T>::makeS(Base& o, const Vec3& s)
1416{
1417 o.m[0][0] = s.x;
1418 o.m[1][0] = 0;
1419 o.m[2][0] = 0;
1420
1421 o.m[0][1] = 0;
1422 o.m[1][1] = s.y;
1423 o.m[2][1] = 0;
1424
1425 o.m[0][2] = 0;
1426 o.m[1][2] = 0;
1427 o.m[2][2] = s.z;
1428
1429 o.m[0][3] = 0;
1430 o.m[1][3] = 0;
1431 o.m[2][3] = 0;
1432}
1433
1434#ifdef cafe
1435
1436template <>
1437inline void
1438Matrix34CalcCommon<f32>::makeS(Base& o, const Vec3& s)
1439{
1440 ASM_MTXScale(o.m, s.x, s.y, s.z);
1441}
1442
1443#endif // cafe
1444
1445template <typename T>
1446void Matrix34CalcCommon<T>::makeSQ(Base& o, const Vec3& s, const Quat& q)
1447{
1448 // Assuming the quaternion "q" is normalized
1449
1450 const T yy = 2 * q.y * q.y;
1451 const T zz = 2 * q.z * q.z;
1452 const T xx = 2 * q.x * q.x;
1453 const T xy = 2 * q.x * q.y;
1454 const T xz = 2 * q.x * q.z;
1455 const T yz = 2 * q.y * q.z;
1456 const T wz = 2 * q.w * q.z;
1457 const T wx = 2 * q.w * q.x;
1458 const T wy = 2 * q.w * q.y;
1459
1460 o.m[0][0] = s.x * (1 - yy - zz);
1461 o.m[0][1] = s.x * ( xy - wz);
1462 o.m[0][2] = s.x * ( xz + wy);
1463
1464 o.m[1][0] = s.y * ( xy + wz);
1465 o.m[1][1] = s.y * (1 - xx - zz);
1466 o.m[1][2] = s.y * ( yz - wx);
1467
1468 o.m[2][0] = s.z * ( xz - wy);
1469 o.m[2][1] = s.z * ( yz + wx);
1470 o.m[2][2] = s.z * (1 - xx - yy);
1471
1472 o.m[0][3] = 0;
1473 o.m[1][3] = 0;
1474 o.m[2][3] = 0;
1475}
1476
1477template <typename T>
1478void Matrix34CalcCommon<T>::makeSQT(Base& o, const Vec3& s, const Quat& q, const Vec3& t)
1479{
1480 // Assuming the quaternion "q" is normalized
1481
1482 const T yy = 2 * q.y * q.y;
1483 const T zz = 2 * q.z * q.z;
1484 const T xx = 2 * q.x * q.x;
1485 const T xy = 2 * q.x * q.y;
1486 const T xz = 2 * q.x * q.z;
1487 const T yz = 2 * q.y * q.z;
1488 const T wz = 2 * q.w * q.z;
1489 const T wx = 2 * q.w * q.x;
1490 const T wy = 2 * q.w * q.y;
1491
1492 o.m[0][0] = s.x * (1 - yy - zz);
1493 o.m[0][1] = s.x * ( xy - wz);
1494 o.m[0][2] = s.x * ( xz + wy);
1495
1496 o.m[1][0] = s.y * ( xy + wz);
1497 o.m[1][1] = s.y * (1 - xx - zz);
1498 o.m[1][2] = s.y * ( yz - wx);
1499
1500 o.m[2][0] = s.z * ( xz - wy);
1501 o.m[2][1] = s.z * ( yz + wx);
1502 o.m[2][2] = s.z * (1 - xx - yy);
1503
1504 o.m[0][3] = t.x;
1505 o.m[1][3] = t.y;
1506 o.m[2][3] = t.z;
1507}
1508
1509template <typename T>
1510void Matrix34CalcCommon<T>::makeSR(Base& o, const Vec3& s, const Vec3& r)
1511{
1512 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
1513 MathCalcCommon<T>::sin(r.y),
1514 MathCalcCommon<T>::sin(r.z) };
1515
1516 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
1517 MathCalcCommon<T>::cos(r.y),
1518 MathCalcCommon<T>::cos(r.z) };
1519
1520 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
1521 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
1522 o.m[2][0] = s.x * -sinV[1];
1523
1524 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1525 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1526 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
1527
1528 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1529 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1530 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1531
1532 o.m[0][3] = 0;
1533 o.m[1][3] = 0;
1534 o.m[2][3] = 0;
1535}
1536
1537template <typename T>
1538void Matrix34CalcCommon<T>::makeSRIdx(Base& o, const Vec3& s, const Vector3<u32>& r)
1539{
1540 T sinV[3];
1541 T cosV[3];
1542
1543 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1544 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1545 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1546
1547 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
1548 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
1549 o.m[2][0] = s.x * -sinV[1];
1550
1551 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1552 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1553 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
1554
1555 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1556 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1557 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1558
1559 o.m[0][3] = 0;
1560 o.m[1][3] = 0;
1561 o.m[2][3] = 0;
1562}
1563
1564template <typename T>
1565void Matrix34CalcCommon<T>::makeSRT(Base& o, const Vec3& s, const Vec3& r, const Vec3& t)
1566{
1567 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
1568 MathCalcCommon<T>::sin(r.y),
1569 MathCalcCommon<T>::sin(r.z) };
1570
1571 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
1572 MathCalcCommon<T>::cos(r.y),
1573 MathCalcCommon<T>::cos(r.z) };
1574
1575 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
1576 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
1577 o.m[2][0] = s.x * -sinV[1];
1578
1579 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1580 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1581 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
1582
1583 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1584 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1585 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1586
1587 o.m[0][3] = t.x;
1588 o.m[1][3] = t.y;
1589 o.m[2][3] = t.z;
1590}
1591
1592template <typename T>
1593void Matrix34CalcCommon<T>::makeSRTIdx(Base& o, const Vec3& s, const Vector3<u32>& r, const Vec3& t)
1594{
1595 T sinV[3];
1596 T cosV[3];
1597
1598 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1599 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1600 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1601
1602 o.m[0][0] = s.x * (cosV[1] * cosV[2]);
1603 o.m[1][0] = s.x * (cosV[1] * sinV[2]);
1604 o.m[2][0] = s.x * -sinV[1];
1605
1606 o.m[0][1] = s.y * (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
1607 o.m[1][1] = s.y * (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
1608 o.m[2][1] = s.y * (sinV[0] * cosV[1]);
1609
1610 o.m[0][2] = s.z * (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
1611 o.m[1][2] = s.z * (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
1612 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1613
1614 o.m[0][3] = t.x;
1615 o.m[1][3] = t.y;
1616 o.m[2][3] = t.z;
1617}
1618
1619template <typename T>
1620void Matrix34CalcCommon<T>::makeSRzxyIdx(Base& o, const Vec3& s, const Vector3<u32>& r)
1621{
1622 T sinV[3];
1623 T cosV[3];
1624
1625 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1626 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1627 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1628
1629 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1630 o.m[0][2] = s.z * (cosV[0] * sinV[1]);
1631 o.m[1][2] = s.z * -sinV[0];
1632
1633 o.m[2][0] = s.x * (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
1634 o.m[0][0] = s.x * (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
1635 o.m[1][0] = s.x * (cosV[0] * sinV[2]);
1636
1637 o.m[2][1] = s.y * (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
1638 o.m[0][1] = s.y * (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
1639 o.m[1][1] = s.y * (cosV[0] * cosV[2]);
1640
1641 o.m[0][3] = 0;
1642 o.m[1][3] = 0;
1643 o.m[2][3] = 0;
1644}
1645
1646template <typename T>
1647void Matrix34CalcCommon<T>::makeSRzxyTIdx(Base& o, const Vec3& s, const Vector3<u32>& r, const Vec3& t)
1648{
1649 T sinV[3];
1650 T cosV[3];
1651
1652 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], r.x);
1653 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], r.y);
1654 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], r.z);
1655
1656 o.m[2][2] = s.z * (cosV[0] * cosV[1]);
1657 o.m[0][2] = s.z * (cosV[0] * sinV[1]);
1658 o.m[1][2] = s.z * -sinV[0];
1659
1660 o.m[2][0] = s.x * (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
1661 o.m[0][0] = s.x * (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
1662 o.m[1][0] = s.x * (cosV[0] * sinV[2]);
1663
1664 o.m[2][1] = s.y * (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
1665 o.m[0][1] = s.y * (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
1666 o.m[1][1] = s.y * (cosV[0] * cosV[2]);
1667
1668 o.m[0][3] = t.x;
1669 o.m[1][3] = t.y;
1670 o.m[2][3] = t.z;
1671}
1672
1673template <typename T>
1674void Matrix34CalcCommon<T>::makeST(Base& o, const Vec3& s, const Vec3& t)
1675{
1676 o.m[0][0] = s.x;
1677 o.m[1][0] = 0;
1678 o.m[2][0] = 0;
1679
1680 o.m[0][1] = 0;
1681 o.m[1][1] = s.y;
1682 o.m[2][1] = 0;
1683
1684 o.m[0][2] = 0;
1685 o.m[1][2] = 0;
1686 o.m[2][2] = s.z;
1687
1688 o.m[0][3] = t.x;
1689 o.m[1][3] = t.y;
1690 o.m[2][3] = t.z;
1691}
1692
1693template <typename T>
1694void Matrix34CalcCommon<T>::makeT(Base& o, const Vec3& t)
1695{
1696 o.m[0][0] = 1;
1697 o.m[1][0] = 0;
1698 o.m[2][0] = 0;
1699
1700 o.m[0][1] = 0;
1701 o.m[1][1] = 1;
1702 o.m[2][1] = 0;
1703
1704 o.m[0][2] = 0;
1705 o.m[1][2] = 0;
1706 o.m[2][2] = 1;
1707
1708 o.m[0][3] = t.x;
1709 o.m[1][3] = t.y;
1710 o.m[2][3] = t.z;
1711}
1712
1713#ifdef cafe
1714
1715template <>
1716inline void
1717Matrix34CalcCommon<f32>::makeT(Base& o, const Vec3& t)
1718{
1719 ASM_MTXTrans(o.m, t.x, t.y, t.z);
1720}
1721
1722#endif // cafe
1723
1724template <typename T>
1725void Matrix34CalcCommon<T>::toQuat(Quat& q, const Base& n)
1726{
1727 const T a11 = n.m[0][0];
1728 const T a12 = n.m[0][1];
1729 const T a13 = n.m[0][2];
1730
1731 const T a21 = n.m[1][0];
1732 const T a22 = n.m[1][1];
1733 const T a23 = n.m[1][2];
1734
1735 const T a31 = n.m[2][0];
1736 const T a32 = n.m[2][1];
1737 const T a33 = n.m[2][2];
1738
1739 const T t = a11 + a22 + a33;
1740 T w, x, y, z;
1741
1742 if (t > 0)
1743 {
1744 T s = MathCalcCommon<T>::sqrt(t + 1);
1745
1746 w = s * 0.5f;
1747
1748 //if (s != 0)
1749 s = 0.5f / s;
1750
1751 x = (a32 - a23) * s;
1752 y = (a13 - a31) * s;
1753 z = (a21 - a12) * s;
1754 }
1755 else if (a22 > a11 && a33 <= a22)
1756 {
1757 T s = MathCalcCommon<T>::sqrt(a22 - (a33 + a11) + 1);
1758
1759 y = s * 0.5f;
1760
1761 if (s != 0)
1762 s = 0.5f / s;
1763
1764 w = (a13 - a31) * s;
1765 x = (a21 + a12) * s;
1766 z = (a23 + a32) * s;
1767 }
1768 else if (a22 > a11 || a33 > a11)
1769 {
1770 T s = MathCalcCommon<T>::sqrt(a33 - (a11 + a22) + 1);
1771
1772 z = s * 0.5f;
1773
1774 if (s != 0)
1775 s = 0.5f / s;
1776
1777 w = (a21 - a12) * s;
1778 x = (a31 + a13) * s;
1779 y = (a32 + a23) * s;
1780 }
1781 else
1782 {
1783 T s = MathCalcCommon<T>::sqrt(a11 - (a22 + a33) + 1);
1784
1785 x = s * 0.5f;
1786
1787 if (s != 0)
1788 s = 0.5f / s;
1789
1790 w = (a32 - a23) * s;
1791 y = (a12 + a21) * s;
1792 z = (a13 + a31) * s;
1793 }
1794
1795 q.w = w;
1796 q.x = x;
1797 q.y = y;
1798 q.z = z;
1799}
1800
1801template <typename T>
1802void Matrix34CalcCommon<T>::multScaleLocal(Base& o, const Base& n, const Vec3& s)
1803{
1804 o.m[0][0] = n.m[0][0] * s.x;
1805 o.m[1][0] = n.m[1][0] * s.x;
1806 o.m[2][0] = n.m[2][0] * s.x;
1807
1808 o.m[0][1] = n.m[0][1] * s.y;
1809 o.m[1][1] = n.m[1][1] * s.y;
1810 o.m[2][1] = n.m[2][1] * s.y;
1811
1812 o.m[0][2] = n.m[0][2] * s.z;
1813 o.m[1][2] = n.m[1][2] * s.z;
1814 o.m[2][2] = n.m[2][2] * s.z;
1815
1816 o.m[0][3] = n.m[0][3];
1817 o.m[1][3] = n.m[1][3];
1818 o.m[2][3] = n.m[2][3];
1819}
1820
1821template <typename T>
1822void Matrix34CalcCommon<T>::multTranslationLocal(Base& o, const Base& n, const Vec3& t)
1823{
1824 o.m[0][0] = n.m[0][0];
1825 o.m[0][1] = n.m[0][1];
1826 o.m[0][2] = n.m[0][2];
1827 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];
1828
1829 o.m[1][0] = n.m[1][0];
1830 o.m[1][1] = n.m[1][1];
1831 o.m[1][2] = n.m[1][2];
1832 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];
1833
1834 o.m[2][0] = n.m[2][0];
1835 o.m[2][1] = n.m[2][1];
1836 o.m[2][2] = n.m[2][2];
1837 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];
1838}
1839
1840template <typename T>
1841void Matrix34CalcCommon<T>::multScaleWorld(Base& o, const Vec3& s, const Base& n)
1842{
1843 o.m[0][0] = n.m[0][0] * s.x;
1844 o.m[0][1] = n.m[0][1] * s.x;
1845 o.m[0][2] = n.m[0][2] * s.x;
1846 o.m[0][3] = n.m[0][3] * s.x;
1847
1848 o.m[1][0] = n.m[1][0] * s.y;
1849 o.m[1][1] = n.m[1][1] * s.y;
1850 o.m[1][2] = n.m[1][2] * s.y;
1851 o.m[1][3] = n.m[1][3] * s.y;
1852
1853 o.m[2][0] = n.m[2][0] * s.z;
1854 o.m[2][1] = n.m[2][1] * s.z;
1855 o.m[2][2] = n.m[2][2] * s.z;
1856 o.m[2][3] = n.m[2][3] * s.z;
1857}
1858
1859template <typename T>
1860void Matrix34CalcCommon<T>::multTranslationWorld(Base& o, const Vec3& t, const Base& n)
1861{
1862 o.m[0][0] = n.m[0][0];
1863 o.m[0][1] = n.m[0][1];
1864 o.m[0][2] = n.m[0][2];
1865 o.m[0][3] = n.m[0][3] + t.x;
1866
1867 o.m[1][0] = n.m[1][0];
1868 o.m[1][1] = n.m[1][1];
1869 o.m[1][2] = n.m[1][2];
1870 o.m[1][3] = n.m[1][3] + t.y;
1871
1872 o.m[2][0] = n.m[2][0];
1873 o.m[2][1] = n.m[2][1];
1874 o.m[2][2] = n.m[2][2];
1875 o.m[2][3] = n.m[2][3] + t.z;
1876}
1877
1878template <typename T>
1879void Matrix34CalcCommon<T>::getBase(Vec3& v, const Base& n, s32 axis)
1880{
1881 v.x = n.m[0][axis];
1882 v.y = n.m[1][axis];
1883 v.z = n.m[2][axis];
1884}
1885
1886template <typename T>
1887void Matrix34CalcCommon<T>::getRow(Vec4& v, const Base& n, s32 row)
1888{
1889 v.x = n.m[row][0];
1890 v.y = n.m[row][1];
1891 v.z = n.m[row][2];
1892 v.w = n.m[row][3];
1893}
1894
1895template <typename T>
1896void Matrix34CalcCommon<T>::getTranslation(Vec3& v, const Base& n)
1897{
1898 getBase(v, n, 3);
1899}
1900
1901template <typename T>
1902void Matrix34CalcCommon<T>::scaleAllElements(Base& n, T s)
1903{
1904 n.m[0][0] *= s;
1905 n.m[0][1] *= s;
1906 n.m[0][2] *= s;
1907 n.m[0][3] *= s;
1908
1909 n.m[1][0] *= s;
1910 n.m[1][1] *= s;
1911 n.m[1][2] *= s;
1912 n.m[1][3] *= s;
1913
1914 n.m[2][0] *= s;
1915 n.m[2][1] *= s;
1916 n.m[2][2] *= s;
1917 n.m[2][3] *= s;
1918}
1919
1920template <typename T>
1921void Matrix34CalcCommon<T>::scaleBases(Base& n, T sx, T sy, T sz)
1922{
1923 n.m[0][0] *= sx;
1924 n.m[1][0] *= sx;
1925 n.m[2][0] *= sx;
1926
1927 n.m[0][1] *= sy;
1928 n.m[1][1] *= sy;
1929 n.m[2][1] *= sy;
1930
1931 n.m[0][2] *= sz;
1932 n.m[1][2] *= sz;
1933 n.m[2][2] *= sz;
1934}
1935
1936template <typename T>
1937void Matrix34CalcCommon<T>::setBase(Base& n, s32 axis, const Vec3& v)
1938{
1939 n.m[0][axis] = v.x;
1940 n.m[1][axis] = v.y;
1941 n.m[2][axis] = v.z;
1942}
1943
1944template <typename T>
1945void Matrix34CalcCommon<T>::setRow(Base& n, const Vec4& v, s32 row)
1946{
1947 n.m[row][0] = v.x;
1948 n.m[row][1] = v.y;
1949 n.m[row][2] = v.z;
1950 n.m[row][3] = v.w;
1951}
1952
1953template <typename T>
1954void Matrix34CalcCommon<T>::setTranslation(Base& n, const Vec3& v)
1955{
1956 setBase(n, 3, v);
1957}
1958
1959template <typename T>
1960void Matrix44CalcCommon<T>::makeIdentity(Base& o)
1961{
1962 o.m[0][0] = 1;
1963 o.m[0][1] = 0;
1964 o.m[0][2] = 0;
1965 o.m[0][3] = 0;
1966
1967 o.m[1][0] = 0;
1968 o.m[1][1] = 1;
1969 o.m[1][2] = 0;
1970 o.m[1][3] = 0;
1971
1972 o.m[2][0] = 0;
1973 o.m[2][1] = 0;
1974 o.m[2][2] = 1;
1975 o.m[2][3] = 0;
1976
1977 o.m[3][0] = 0;
1978 o.m[3][1] = 0;
1979 o.m[3][2] = 0;
1980 o.m[3][3] = 1;
1981}
1982
1983#ifdef cafe
1984
1985template <>
1986inline void
1991
1992#endif // cafe
1993
1994template <typename T>
1995void Matrix44CalcCommon<T>::makeZero(Base& o)
1996{
1997 o.m[0][0] = 0;
1998 o.m[0][1] = 0;
1999 o.m[0][2] = 0;
2000 o.m[0][3] = 0;
2001
2002 o.m[1][0] = 0;
2003 o.m[1][1] = 0;
2004 o.m[1][2] = 0;
2005 o.m[1][3] = 0;
2006
2007 o.m[2][0] = 0;
2008 o.m[2][1] = 0;
2009 o.m[2][2] = 0;
2010 o.m[2][3] = 0;
2011
2012 o.m[3][0] = 0;
2013 o.m[3][1] = 0;
2014 o.m[3][2] = 0;
2015 o.m[3][3] = 0;
2016}
2017
2018template <typename T>
2019void Matrix44CalcCommon<T>::copy(Base& o, const Base& n)
2020{
2021 o.m[0][0] = n.m[0][0];
2022 o.m[0][1] = n.m[0][1];
2023 o.m[0][2] = n.m[0][2];
2024 o.m[0][3] = n.m[0][3];
2025
2026 o.m[1][0] = n.m[1][0];
2027 o.m[1][1] = n.m[1][1];
2028 o.m[1][2] = n.m[1][2];
2029 o.m[1][3] = n.m[1][3];
2030
2031 o.m[2][0] = n.m[2][0];
2032 o.m[2][1] = n.m[2][1];
2033 o.m[2][2] = n.m[2][2];
2034 o.m[2][3] = n.m[2][3];
2035
2036 o.m[3][0] = n.m[3][0];
2037 o.m[3][1] = n.m[3][1];
2038 o.m[3][2] = n.m[3][2];
2039 o.m[3][3] = n.m[3][3];
2040}
2041
2042#ifdef cafe
2043
2044template <>
2045inline void
2046Matrix44CalcCommon<f32>::copy(Base& o, const Base& n)
2047{
2048 ASM_MTX44Copy(const_cast<f32(*)[4]>(n.m), o.m);
2049}
2050
2051#endif // cafe
2052
2053template <typename T>
2054void Matrix44CalcCommon<T>::copy(Base& o, const Mtx33& n, const Vec3& t, const Vec4& v)
2055{
2056 o.m[0][0] = n.m[0][0];
2057 o.m[0][1] = n.m[0][1];
2058 o.m[0][2] = n.m[0][2];
2059 o.m[0][3] = t.x;
2060
2061 o.m[1][0] = n.m[1][0];
2062 o.m[1][1] = n.m[1][1];
2063 o.m[1][2] = n.m[1][2];
2064 o.m[1][3] = t.y;
2065
2066 o.m[2][0] = n.m[2][0];
2067 o.m[2][1] = n.m[2][1];
2068 o.m[2][2] = n.m[2][2];
2069 o.m[2][3] = t.z;
2070
2071 o.m[3][0] = v.x;
2072 o.m[3][1] = v.y;
2073 o.m[3][2] = v.z;
2074 o.m[3][3] = v.w;
2075}
2076
2077template <typename T>
2078void Matrix44CalcCommon<T>::copy(Base& o, const Mtx34& n, const Vec4& v)
2079{
2080 o.m[0][0] = n.m[0][0];
2081 o.m[0][1] = n.m[0][1];
2082 o.m[0][2] = n.m[0][2];
2083 o.m[0][3] = n.m[0][3];
2084
2085 o.m[1][0] = n.m[1][0];
2086 o.m[1][1] = n.m[1][1];
2087 o.m[1][2] = n.m[1][2];
2088 o.m[1][3] = n.m[1][3];
2089
2090 o.m[2][0] = n.m[2][0];
2091 o.m[2][1] = n.m[2][1];
2092 o.m[2][2] = n.m[2][2];
2093 o.m[2][3] = n.m[2][3];
2094
2095 o.m[3][0] = v.x;
2096 o.m[3][1] = v.y;
2097 o.m[3][2] = v.z;
2098 o.m[3][3] = v.w;
2099}
2100
2101#ifdef cafe
2102
2103// Nintendo did not actually use this for the cafe f32 specialization
2104//
2105//template <>
2106//inline void
2107//Matrix44CalcCommon<f32>::copy(Base& o, const Mtx34& n, const Vec4& v)
2108//{
2109// ASM_MTXCopy(const_cast<f32(*)[4]>(n.m), o.m);
2110//
2111// o.m[3][0] = v.x;
2112// o.m[3][1] = v.y;
2113// o.m[3][2] = v.z;
2114// o.m[3][3] = v.w;
2115//}
2116
2117#endif // cafe
2118
2119template <typename T>
2120void Matrix44CalcCommon<T>::inverse(Base& o, const Base& n)
2121{
2122 const T a11 = n.m[0][0];
2123 const T a12 = n.m[0][1];
2124 const T a13 = n.m[0][2];
2125 const T a14 = n.m[0][3];
2126
2127 const T a21 = n.m[1][0];
2128 const T a22 = n.m[1][1];
2129 const T a23 = n.m[1][2];
2130 const T a24 = n.m[1][3];
2131
2132 const T a31 = n.m[2][0];
2133 const T a32 = n.m[2][1];
2134 const T a33 = n.m[2][2];
2135 const T a34 = n.m[2][3];
2136
2137 const T a41 = n.m[3][0];
2138 const T a42 = n.m[3][1];
2139 const T a43 = n.m[3][2];
2140 const T a44 = n.m[3][3];
2141
2142 T det = a11 * (a22 * a33 * a44 + a23 * a34 * a42 + a24 * a32 * a43)
2143 + a12 * (a21 * a34 * a43 + a23 * a31 * a44 + a24 * a33 * a41)
2144 + a13 * (a21 * a32 * a44 + a22 * a34 * a41 + a24 * a31 * a42)
2145 + a14 * (a21 * a33 * a42 + a22 * a31 * a43 + a23 * a32 * a41)
2146 - a11 * (a22 * a34 * a43 + a23 * a32 * a44 + a24 * a33 * a42)
2147 - a12 * (a21 * a33 * a44 + a23 * a34 * a41 + a24 * a31 * a43)
2148 - a13 * (a21 * a34 * a42 + a22 * a31 * a44 + a24 * a32 * a41)
2149 - a14 * (a21 * a32 * a43 + a22 * a33 * a41 + a23 * a31 * a42);
2150
2151 if (det == 0)
2152 return makeIdentity(o);
2153
2154 det = 1 / det;
2155
2156 const T a33xa44_a34xa43 = a33 * a44 - a34 * a43;
2157 const T a32xa44_a34xa42 = a32 * a44 - a34 * a42;
2158 const T a33xa42_a32xa43 = a33 * a42 - a32 * a43;
2159 const T a33xa41_a31xa43 = a33 * a41 - a31 * a43;
2160 const T a31xa44_a34xa41 = a31 * a44 - a34 * a41;
2161 const T a32xa41_a31xa42 = a32 * a41 - a31 * a42;
2162 const T a23xa44_a24xa43 = a23 * a44 - a24 * a43;
2163 const T a24xa33_a23xa34 = a24 * a33 - a23 * a34;
2164 const T a24xa42_a22xa44 = a24 * a42 - a22 * a44;
2165 const T a22xa43_a23xa42 = a22 * a43 - a23 * a42;
2166 const T a22xa34_a24xa32 = a22 * a34 - a24 * a32;
2167 const T a23xa32_a22xa33 = a23 * a32 - a22 * a33;
2168 const T a21xa44_a24xa41 = a21 * a44 - a24 * a41;
2169 const T a23xa41_a21xa43 = a23 * a41 - a21 * a43;
2170 const T a24xa31_a21xa34 = a24 * a31 - a21 * a34;
2171 const T a21xa33_a23xa31 = a21 * a33 - a23 * a31;
2172 const T a21xa42_a22xa41 = a21 * a42 - a22 * a41;
2173 const T a22xa31_a21xa32 = a22 * a31 - a21 * a32;
2174
2175 o.m[0][0] = (a22 * a33xa44_a34xa43 - a23 * a32xa44_a34xa42 - a24 * a33xa42_a32xa43) * det;
2176 o.m[0][1] = (a13 * a32xa44_a34xa42 + a14 * a33xa42_a32xa43 - a12 * a33xa44_a34xa43) * det;
2177 o.m[0][2] = (a12 * a23xa44_a24xa43 + a13 * a24xa42_a22xa44 + a14 * a22xa43_a23xa42) * det;
2178 o.m[0][3] = (a12 * a24xa33_a23xa34 + a13 * a22xa34_a24xa32 + a14 * a23xa32_a22xa33) * det;
2179
2180 o.m[1][0] = (a23 * a31xa44_a34xa41 + a24 * a33xa41_a31xa43 - a21 * a33xa44_a34xa43) * det;
2181 o.m[1][1] = (a11 * a33xa44_a34xa43 - a13 * a31xa44_a34xa41 - a14 * a33xa41_a31xa43) * det;
2182 o.m[1][2] = (a13 * a21xa44_a24xa41 + a14 * a23xa41_a21xa43 - a11 * a23xa44_a24xa43) * det;
2183 o.m[1][3] = (a13 * a24xa31_a21xa34 + a14 * a21xa33_a23xa31 - a11 * a24xa33_a23xa34) * det;
2184
2185 o.m[2][0] = (a21 * a32xa44_a34xa42 - a22 * a31xa44_a34xa41 - a24 * a32xa41_a31xa42) * det;
2186 o.m[2][1] = (a12 * a31xa44_a34xa41 + a14 * a32xa41_a31xa42 - a11 * a32xa44_a34xa42) * det;
2187 o.m[2][2] = (a14 * a21xa42_a22xa41 - a11 * a24xa42_a22xa44 - a12 * a21xa44_a24xa41) * det;
2188 o.m[2][3] = (a14 * a22xa31_a21xa32 - a11 * a22xa34_a24xa32 - a12 * a24xa31_a21xa34) * det;
2189
2190 o.m[3][0] = (a21 * a33xa42_a32xa43 - a22 * a33xa41_a31xa43 + a23 * a32xa41_a31xa42) * det;
2191 o.m[3][1] = (a12 * a33xa41_a31xa43 - a13 * a32xa41_a31xa42 - a11 * a33xa42_a32xa43) * det;
2192 o.m[3][2] = (-a13 * a21xa42_a22xa41 - a11 * a22xa43_a23xa42 - a12 * a23xa41_a21xa43) * det;
2193 o.m[3][3] = (-a13 * a22xa31_a21xa32 - a11 * a23xa32_a22xa33 - a12 * a21xa33_a23xa31) * det;
2194}
2195
2196template <typename T>
2197void Matrix44CalcCommon<T>::inverseTranspose(Base& o, const Base& n)
2198{
2199 const T a11 = n.m[0][0];
2200 const T a12 = n.m[0][1];
2201 const T a13 = n.m[0][2];
2202 const T a14 = n.m[0][3];
2203
2204 const T a21 = n.m[1][0];
2205 const T a22 = n.m[1][1];
2206 const T a23 = n.m[1][2];
2207 const T a24 = n.m[1][3];
2208
2209 const T a31 = n.m[2][0];
2210 const T a32 = n.m[2][1];
2211 const T a33 = n.m[2][2];
2212 const T a34 = n.m[2][3];
2213
2214 const T a41 = n.m[3][0];
2215 const T a42 = n.m[3][1];
2216 const T a43 = n.m[3][2];
2217 const T a44 = n.m[3][3];
2218
2219 T det = a11 * (a22 * a33 * a44 + a23 * a34 * a42 + a24 * a32 * a43)
2220 + a12 * (a21 * a34 * a43 + a23 * a31 * a44 + a24 * a33 * a41)
2221 + a13 * (a21 * a32 * a44 + a22 * a34 * a41 + a24 * a31 * a42)
2222 + a14 * (a21 * a33 * a42 + a22 * a31 * a43 + a23 * a32 * a41)
2223 - a11 * (a22 * a34 * a43 + a23 * a32 * a44 + a24 * a33 * a42)
2224 - a12 * (a21 * a33 * a44 + a23 * a34 * a41 + a24 * a31 * a43)
2225 - a13 * (a21 * a34 * a42 + a22 * a31 * a44 + a24 * a32 * a41)
2226 - a14 * (a21 * a32 * a43 + a22 * a33 * a41 + a23 * a31 * a42);
2227
2228 if (det == 0)
2229 return makeIdentity(o);
2230
2231 det = 1 / det;
2232
2233 const T a33xa44_a34xa43 = a33 * a44 - a34 * a43;
2234 const T a32xa44_a34xa42 = a32 * a44 - a34 * a42;
2235 const T a33xa42_a32xa43 = a33 * a42 - a32 * a43;
2236 const T a33xa41_a31xa43 = a33 * a41 - a31 * a43;
2237 const T a31xa44_a34xa41 = a31 * a44 - a34 * a41;
2238 const T a32xa41_a31xa42 = a32 * a41 - a31 * a42;
2239 const T a23xa44_a24xa43 = a23 * a44 - a24 * a43;
2240 const T a24xa33_a23xa34 = a24 * a33 - a23 * a34;
2241 const T a24xa42_a22xa44 = a24 * a42 - a22 * a44;
2242 const T a22xa43_a23xa42 = a22 * a43 - a23 * a42;
2243 const T a22xa34_a24xa32 = a22 * a34 - a24 * a32;
2244 const T a23xa32_a22xa33 = a23 * a32 - a22 * a33;
2245 const T a21xa44_a24xa41 = a21 * a44 - a24 * a41;
2246 const T a23xa41_a21xa43 = a23 * a41 - a21 * a43;
2247 const T a24xa31_a21xa34 = a24 * a31 - a21 * a34;
2248 const T a21xa33_a23xa31 = a21 * a33 - a23 * a31;
2249 const T a21xa42_a22xa41 = a21 * a42 - a22 * a41;
2250 const T a22xa31_a21xa32 = a22 * a31 - a21 * a32;
2251
2252 o.m[0][0] = (a22 * a33xa44_a34xa43 - a23 * a32xa44_a34xa42 - a24 * a33xa42_a32xa43) * det;
2253 o.m[0][1] = (a23 * a31xa44_a34xa41 + a24 * a33xa41_a31xa43 - a21 * a33xa44_a34xa43) * det;
2254 o.m[0][2] = (a21 * a32xa44_a34xa42 - a22 * a31xa44_a34xa41 - a24 * a32xa41_a31xa42) * det;
2255 o.m[0][3] = (a21 * a33xa42_a32xa43 - a22 * a33xa41_a31xa43 + a23 * a32xa41_a31xa42) * det;
2256
2257 o.m[1][0] = (a13 * a32xa44_a34xa42 + a14 * a33xa42_a32xa43 - a12 * a33xa44_a34xa43) * det;
2258 o.m[1][1] = (a11 * a33xa44_a34xa43 - a13 * a31xa44_a34xa41 - a14 * a33xa41_a31xa43) * det;
2259 o.m[1][2] = (a12 * a31xa44_a34xa41 + a14 * a32xa41_a31xa42 - a11 * a32xa44_a34xa42) * det;
2260 o.m[1][3] = (a12 * a33xa41_a31xa43 - a13 * a32xa41_a31xa42 - a11 * a33xa42_a32xa43) * det;
2261
2262 o.m[2][0] = (a12 * a23xa44_a24xa43 + a13 * a24xa42_a22xa44 + a14 * a22xa43_a23xa42) * det;
2263 o.m[2][1] = (a13 * a21xa44_a24xa41 + a14 * a23xa41_a21xa43 - a11 * a23xa44_a24xa43) * det;
2264 o.m[2][2] = (a14 * a21xa42_a22xa41 - a11 * a24xa42_a22xa44 - a12 * a21xa44_a24xa41) * det;
2265 o.m[2][3] = (-a13 * a21xa42_a22xa41 - a11 * a22xa43_a23xa42 - a12 * a23xa41_a21xa43) * det;
2266
2267 o.m[3][0] = (a12 * a24xa33_a23xa34 + a13 * a22xa34_a24xa32 + a14 * a23xa32_a22xa33) * det;
2268 o.m[3][1] = (a13 * a24xa31_a21xa34 + a14 * a21xa33_a23xa31 - a11 * a24xa33_a23xa34) * det;
2269 o.m[3][2] = (a14 * a22xa31_a21xa32 - a11 * a22xa34_a24xa32 - a12 * a24xa31_a21xa34) * det;
2270 o.m[3][3] = (-a13 * a22xa31_a21xa32 - a11 * a23xa32_a22xa33 - a12 * a21xa33_a23xa31) * det;
2271}
2272
2273template <typename T>
2274void Matrix44CalcCommon<T>::multiply(Base& o, const Base& a, const Base& b)
2275{
2276 const T a11 = a.m[0][0];
2277 const T a12 = a.m[0][1];
2278 const T a13 = a.m[0][2];
2279 const T a14 = a.m[0][3];
2280
2281 const T a21 = a.m[1][0];
2282 const T a22 = a.m[1][1];
2283 const T a23 = a.m[1][2];
2284 const T a24 = a.m[1][3];
2285
2286 const T a31 = a.m[2][0];
2287 const T a32 = a.m[2][1];
2288 const T a33 = a.m[2][2];
2289 const T a34 = a.m[2][3];
2290
2291 const T a41 = a.m[3][0];
2292 const T a42 = a.m[3][1];
2293 const T a43 = a.m[3][2];
2294 const T a44 = a.m[3][3];
2295
2296 const T b11 = b.m[0][0];
2297 const T b12 = b.m[0][1];
2298 const T b13 = b.m[0][2];
2299 const T b14 = b.m[0][3];
2300
2301 const T b21 = b.m[1][0];
2302 const T b22 = b.m[1][1];
2303 const T b23 = b.m[1][2];
2304 const T b24 = b.m[1][3];
2305
2306 const T b31 = b.m[2][0];
2307 const T b32 = b.m[2][1];
2308 const T b33 = b.m[2][2];
2309 const T b34 = b.m[2][3];
2310
2311 const T b41 = b.m[3][0];
2312 const T b42 = b.m[3][1];
2313 const T b43 = b.m[3][2];
2314 const T b44 = b.m[3][3];
2315
2316 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41;
2317 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42;
2318 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43;
2319 o.m[0][3] = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44;
2320
2321 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41;
2322 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42;
2323 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43;
2324 o.m[1][3] = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44;
2325
2326 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41;
2327 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42;
2328 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43;
2329 o.m[2][3] = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44;
2330
2331 o.m[3][0] = a41 * b11 + a42 * b21 + a43 * b31 + a44 * b41;
2332 o.m[3][1] = a41 * b12 + a42 * b22 + a43 * b32 + a44 * b42;
2333 o.m[3][2] = a41 * b13 + a42 * b23 + a43 * b33 + a44 * b43;
2334 o.m[3][3] = a41 * b14 + a42 * b24 + a43 * b34 + a44 * b44;
2335}
2336
2337#ifdef cafe
2338
2339template <>
2340inline void
2341Matrix44CalcCommon<f32>::multiply(Base& o, const Base& a, const Base& b)
2342{
2343 ASM_MTX44Concat(const_cast<f32(*)[4]>(a.m), const_cast<f32(*)[4]>(b.m), o.m);
2344}
2345
2346#endif // cafe
2347
2348template <typename T>
2349void Matrix44CalcCommon<T>::multiply(Base& o, const Mtx34& a, const Base& b)
2350{
2351 const T a11 = a.m[0][0];
2352 const T a12 = a.m[0][1];
2353 const T a13 = a.m[0][2];
2354 const T a14 = a.m[0][3];
2355
2356 const T a21 = a.m[1][0];
2357 const T a22 = a.m[1][1];
2358 const T a23 = a.m[1][2];
2359 const T a24 = a.m[1][3];
2360
2361 const T a31 = a.m[2][0];
2362 const T a32 = a.m[2][1];
2363 const T a33 = a.m[2][2];
2364 const T a34 = a.m[2][3];
2365
2366 const T b11 = b.m[0][0];
2367 const T b12 = b.m[0][1];
2368 const T b13 = b.m[0][2];
2369 const T b14 = b.m[0][3];
2370
2371 const T b21 = b.m[1][0];
2372 const T b22 = b.m[1][1];
2373 const T b23 = b.m[1][2];
2374 const T b24 = b.m[1][3];
2375
2376 const T b31 = b.m[2][0];
2377 const T b32 = b.m[2][1];
2378 const T b33 = b.m[2][2];
2379 const T b34 = b.m[2][3];
2380
2381 const T b41 = b.m[3][0];
2382 const T b42 = b.m[3][1];
2383 const T b43 = b.m[3][2];
2384 const T b44 = b.m[3][3];
2385
2386 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31 + a14 * b41;
2387 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32 + a14 * b42;
2388 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33 + a14 * b43;
2389 o.m[0][3] = a11 * b14 + a12 * b24 + a13 * b34 + a14 * b44;
2390
2391 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31 + a24 * b41;
2392 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32 + a24 * b42;
2393 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33 + a24 * b43;
2394 o.m[1][3] = a21 * b14 + a22 * b24 + a23 * b34 + a24 * b44;
2395
2396 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31 + a34 * b41;
2397 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32 + a34 * b42;
2398 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33 + a34 * b43;
2399 o.m[2][3] = a31 * b14 + a32 * b24 + a33 * b34 + a34 * b44;
2400
2401 o.m[3][0] = b41;
2402 o.m[3][1] = b42;
2403 o.m[3][2] = b43;
2404 o.m[3][3] = b44;
2405}
2406
2407template <typename T>
2408void Matrix44CalcCommon<T>::multiply(Base& o, const Base& a, const Mtx34& b)
2409{
2410 const T a11 = a.m[0][0];
2411 const T a12 = a.m[0][1];
2412 const T a13 = a.m[0][2];
2413 const T a14 = a.m[0][3];
2414
2415 const T a21 = a.m[1][0];
2416 const T a22 = a.m[1][1];
2417 const T a23 = a.m[1][2];
2418 const T a24 = a.m[1][3];
2419
2420 const T a31 = a.m[2][0];
2421 const T a32 = a.m[2][1];
2422 const T a33 = a.m[2][2];
2423 const T a34 = a.m[2][3];
2424
2425 const T a41 = a.m[3][0];
2426 const T a42 = a.m[3][1];
2427 const T a43 = a.m[3][2];
2428 const T a44 = a.m[3][3];
2429
2430 const T b11 = b.m[0][0];
2431 const T b12 = b.m[0][1];
2432 const T b13 = b.m[0][2];
2433 const T b14 = b.m[0][3];
2434
2435 const T b21 = b.m[1][0];
2436 const T b22 = b.m[1][1];
2437 const T b23 = b.m[1][2];
2438 const T b24 = b.m[1][3];
2439
2440 const T b31 = b.m[2][0];
2441 const T b32 = b.m[2][1];
2442 const T b33 = b.m[2][2];
2443 const T b34 = b.m[2][3];
2444
2445 o.m[0][0] = a11 * b11 + a12 * b21 + a13 * b31;
2446 o.m[0][1] = a11 * b12 + a12 * b22 + a13 * b32;
2447 o.m[0][2] = a11 * b13 + a12 * b23 + a13 * b33;
2448 o.m[0][3] = a11 * b14 + a12 * b24 + a13 * b34 + a14;
2449
2450 o.m[1][0] = a21 * b11 + a22 * b21 + a23 * b31;
2451 o.m[1][1] = a21 * b12 + a22 * b22 + a23 * b32;
2452 o.m[1][2] = a21 * b13 + a22 * b23 + a23 * b33;
2453 o.m[1][3] = a21 * b14 + a22 * b24 + a23 * b34 + a24;
2454
2455 o.m[2][0] = a31 * b11 + a32 * b21 + a33 * b31;
2456 o.m[2][1] = a31 * b12 + a32 * b22 + a33 * b32;
2457 o.m[2][2] = a31 * b13 + a32 * b23 + a33 * b33;
2458 o.m[2][3] = a31 * b14 + a32 * b24 + a33 * b34 + a34;
2459
2460 o.m[3][0] = a41 * b11 + a42 * b21 + a43 * b31;
2461 o.m[3][1] = a41 * b12 + a42 * b22 + a43 * b32;
2462 o.m[3][2] = a41 * b13 + a42 * b23 + a43 * b33;
2463 o.m[3][3] = a41 * b14 + a42 * b24 + a43 * b34 + a44;
2464}
2465
2466template <typename T>
2467void Matrix44CalcCommon<T>::transpose(Base& o)
2468{
2469 const T a12 = o.m[0][1];
2470 const T a13 = o.m[0][2];
2471 const T a14 = o.m[0][3];
2472
2473 const T a21 = o.m[1][0];
2474 const T a23 = o.m[1][2];
2475 const T a24 = o.m[1][3];
2476
2477 const T a31 = o.m[2][0];
2478 const T a32 = o.m[2][1];
2479 const T a34 = o.m[2][3];
2480
2481 const T a41 = o.m[3][0];
2482 const T a42 = o.m[3][1];
2483 const T a43 = o.m[3][2];
2484
2485 o.m[0][1] = a21;
2486 o.m[0][2] = a31;
2487 o.m[0][3] = a41;
2488
2489 o.m[1][0] = a12;
2490 o.m[1][2] = a32;
2491 o.m[1][3] = a42;
2492
2493 o.m[2][0] = a13;
2494 o.m[2][1] = a23;
2495 o.m[2][3] = a43;
2496
2497 o.m[3][0] = a14;
2498 o.m[3][1] = a24;
2499 o.m[3][2] = a34;
2500}
2501
2502template <typename T>
2503void Matrix44CalcCommon<T>::transposeTo(Base& o, const Base& n)
2504{
2505 SEAD_ASSERT(&o != &n);
2506
2507 o.m[0][0] = n.m[0][0];
2508 o.m[0][1] = n.m[1][0];
2509 o.m[0][2] = n.m[2][0];
2510 o.m[0][3] = n.m[3][0];
2511
2512 o.m[1][0] = n.m[0][1];
2513 o.m[1][1] = n.m[1][1];
2514 o.m[1][2] = n.m[2][1];
2515 o.m[1][3] = n.m[3][1];
2516
2517 o.m[2][0] = n.m[0][2];
2518 o.m[2][1] = n.m[1][2];
2519 o.m[2][2] = n.m[2][2];
2520 o.m[2][3] = n.m[3][2];
2521
2522 o.m[3][0] = n.m[0][3];
2523 o.m[3][1] = n.m[1][3];
2524 o.m[3][2] = n.m[2][3];
2525 o.m[3][3] = n.m[3][3];
2526}
2527
2528template <typename T>
2529void Matrix44CalcCommon<T>::makeQ(Base& o, const Quat& q)
2530{
2531 // Assuming the quaternion "q" is normalized
2532
2533 const T yy = 2 * q.y * q.y;
2534 const T zz = 2 * q.z * q.z;
2535 const T xx = 2 * q.x * q.x;
2536 const T xy = 2 * q.x * q.y;
2537 const T xz = 2 * q.x * q.z;
2538 const T yz = 2 * q.y * q.z;
2539 const T wz = 2 * q.w * q.z;
2540 const T wx = 2 * q.w * q.x;
2541 const T wy = 2 * q.w * q.y;
2542
2543 o.m[0][0] = 1 - yy - zz;
2544 o.m[0][1] = xy - wz;
2545 o.m[0][2] = xz + wy;
2546
2547 o.m[1][0] = xy + wz;
2548 o.m[1][1] = 1 - xx - zz;
2549 o.m[1][2] = yz - wx;
2550
2551 o.m[2][0] = xz - wy;
2552 o.m[2][1] = yz + wx;
2553 o.m[2][2] = 1 - xx - yy;
2554
2555 o.m[0][3] = 0;
2556 o.m[1][3] = 0;
2557 o.m[2][3] = 0;
2558
2559 o.m[3][0] = 0;
2560 o.m[3][1] = 0;
2561 o.m[3][2] = 0;
2562 o.m[3][3] = 1;
2563}
2564
2565template <typename T>
2566void Matrix44CalcCommon<T>::makeR(Base& o, const Vec3& r)
2567{
2568 const T sinV[3] = { MathCalcCommon<T>::sin(r.x),
2569 MathCalcCommon<T>::sin(r.y),
2570 MathCalcCommon<T>::sin(r.z) };
2571
2572 const T cosV[3] = { MathCalcCommon<T>::cos(r.x),
2573 MathCalcCommon<T>::cos(r.y),
2574 MathCalcCommon<T>::cos(r.z) };
2575
2576 o.m[0][0] = (cosV[1] * cosV[2]);
2577 o.m[1][0] = (cosV[1] * sinV[2]);
2578 o.m[2][0] = -sinV[1];
2579
2580 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
2581 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
2582 o.m[2][1] = (sinV[0] * cosV[1]);
2583
2584 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
2585 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
2586 o.m[2][2] = (cosV[0] * cosV[1]);
2587
2588 o.m[0][3] = 0;
2589 o.m[1][3] = 0;
2590 o.m[2][3] = 0;
2591
2592 o.m[3][0] = 0;
2593 o.m[3][1] = 0;
2594 o.m[3][2] = 0;
2595 o.m[3][3] = 1;
2596}
2597
2598template <typename T>
2599void Matrix44CalcCommon<T>::makeRIdx(Base& o, u32 xr, u32 yr, u32 zr)
2600{
2601 T sinV[3];
2602 T cosV[3];
2603
2604 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
2605 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
2606 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
2607
2608 o.m[0][0] = (cosV[1] * cosV[2]);
2609 o.m[1][0] = (cosV[1] * sinV[2]);
2610 o.m[2][0] = -sinV[1];
2611
2612 o.m[0][1] = (sinV[0] * sinV[1] * cosV[2] - cosV[0] * sinV[2]);
2613 o.m[1][1] = (sinV[0] * sinV[1] * sinV[2] + cosV[0] * cosV[2]);
2614 o.m[2][1] = (sinV[0] * cosV[1]);
2615
2616 o.m[0][2] = (cosV[0] * cosV[2] * sinV[1] + sinV[0] * sinV[2]);
2617 o.m[1][2] = (cosV[0] * sinV[2] * sinV[1] - sinV[0] * cosV[2]);
2618 o.m[2][2] = (cosV[0] * cosV[1]);
2619
2620 o.m[0][3] = 0;
2621 o.m[1][3] = 0;
2622 o.m[2][3] = 0;
2623
2624 o.m[3][0] = 0;
2625 o.m[3][1] = 0;
2626 o.m[3][2] = 0;
2627 o.m[3][3] = 1;
2628}
2629
2630template <typename T>
2631void Matrix44CalcCommon<T>::makeRzxyIdx(Base& o, u32 xr, u32 yr, u32 zr)
2632{
2633 T sinV[3];
2634 T cosV[3];
2635
2636 MathCalcCommon<T>::sinCosIdx(&sinV[0], &cosV[0], xr);
2637 MathCalcCommon<T>::sinCosIdx(&sinV[1], &cosV[1], yr);
2638 MathCalcCommon<T>::sinCosIdx(&sinV[2], &cosV[2], zr);
2639
2640 o.m[2][2] = (cosV[0] * cosV[1]);
2641 o.m[0][2] = (cosV[0] * sinV[1]);
2642 o.m[1][2] = -sinV[0];
2643
2644 o.m[2][0] = (sinV[1] * cosV[2] - sinV[0] * cosV[1] * sinV[2]);
2645 o.m[0][0] = (cosV[1] * cosV[2] + sinV[0] * sinV[1] * sinV[2]);
2646 o.m[1][0] = (cosV[0] * sinV[2]);
2647
2648 o.m[2][1] = (sinV[1] * sinV[2] + sinV[0] * cosV[1] * cosV[2]);
2649 o.m[0][1] = (cosV[1] * sinV[2] - sinV[0] * sinV[1] * cosV[2]);
2650 o.m[1][1] = (cosV[0] * cosV[2]);
2651
2652 o.m[0][3] = 0;
2653 o.m[1][3] = 0;
2654 o.m[2][3] = 0;
2655
2656 o.m[3][0] = 0;
2657 o.m[3][1] = 0;
2658 o.m[3][2] = 0;
2659 o.m[3][3] = 1;
2660}
2661
2662template <typename T>
2663void Matrix44CalcCommon<T>::toQuat(Quat& q, const Base& n)
2664{
2665 const T a11 = n.m[0][0];
2666 const T a12 = n.m[0][1];
2667 const T a13 = n.m[0][2];
2668
2669 const T a21 = n.m[1][0];
2670 const T a22 = n.m[1][1];
2671 const T a23 = n.m[1][2];
2672
2673 const T a31 = n.m[2][0];
2674 const T a32 = n.m[2][1];
2675 const T a33 = n.m[2][2];
2676
2677 const T t = a11 + a22 + a33;
2678 T w, x, y, z;
2679
2680 if (t > 0)
2681 {
2682 T s = MathCalcCommon<T>::sqrt(t + 1);
2683
2684 w = s * 0.5f;
2685
2686 //if (s != 0)
2687 s = 0.5f / s;
2688
2689 x = (a32 - a23) * s;
2690 y = (a13 - a31) * s;
2691 z = (a21 - a12) * s;
2692 }
2693 else if (a22 > a11 && a33 <= a22)
2694 {
2695 T s = MathCalcCommon<T>::sqrt(a22 - (a33 + a11) + 1);
2696
2697 y = s * 0.5f;
2698
2699 if (s != 0)
2700 s = 0.5f / s;
2701
2702 w = (a13 - a31) * s;
2703 x = (a21 + a12) * s;
2704 z = (a23 + a32) * s;
2705 }
2706 else if (a22 > a11 || a33 > a11)
2707 {
2708 T s = MathCalcCommon<T>::sqrt(a33 - (a11 + a22) + 1);
2709
2710 z = s * 0.5f;
2711
2712 if (s != 0)
2713 s = 0.5f / s;
2714
2715 w = (a21 - a12) * s;
2716 x = (a31 + a13) * s;
2717 y = (a32 + a23) * s;
2718 }
2719 else
2720 {
2721 T s = MathCalcCommon<T>::sqrt(a11 - (a22 + a33) + 1);
2722
2723 x = s * 0.5f;
2724
2725 if (s != 0)
2726 s = 0.5f / s;
2727
2728 w = (a32 - a23) * s;
2729 y = (a12 + a21) * s;
2730 z = (a13 + a31) * s;
2731 }
2732
2733 q.w = w;
2734 q.x = x;
2735 q.y = y;
2736 q.z = z;
2737}
2738
2739template <typename T>
2740void Matrix44CalcCommon<T>::getCol(Vec4& v, const Base& n, s32 axis)
2741{
2742 v.x = n.m[0][axis];
2743 v.y = n.m[1][axis];
2744 v.z = n.m[2][axis];
2745 v.w = n.m[3][axis];
2746}
2747
2748template <typename T>
2749void Matrix44CalcCommon<T>::getRow(Vec4& v, const Base& n, s32 row)
2750{
2751 v.x = n.m[row][0];
2752 v.y = n.m[row][1];
2753 v.z = n.m[row][2];
2754 v.w = n.m[row][3];
2755}
2756
2757template <typename T>
2758void Matrix44CalcCommon<T>::scaleAllElements(Base& n, T s)
2759{
2760 n.m[0][0] *= s;
2761 n.m[0][1] *= s;
2762 n.m[0][2] *= s;
2763 n.m[0][3] *= s;
2764
2765 n.m[1][0] *= s;
2766 n.m[1][1] *= s;
2767 n.m[1][2] *= s;
2768 n.m[1][3] *= s;
2769
2770 n.m[2][0] *= s;
2771 n.m[2][1] *= s;
2772 n.m[2][2] *= s;
2773 n.m[2][3] *= s;
2774
2775 n.m[3][0] *= s;
2776 n.m[3][1] *= s;
2777 n.m[3][2] *= s;
2778 n.m[3][3] *= s;
2779}
2780
2781template <typename T>
2782void Matrix44CalcCommon<T>::scaleBases(Base& n, T sx, T sy, T sz, T sw)
2783{
2784 n.m[0][0] *= sx;
2785 n.m[1][0] *= sx;
2786 n.m[2][0] *= sx;
2787 n.m[3][0] *= sx;
2788
2789 n.m[0][1] *= sy;
2790 n.m[1][1] *= sy;
2791 n.m[2][1] *= sy;
2792 n.m[3][1] *= sy;
2793
2794 n.m[0][2] *= sz;
2795 n.m[1][2] *= sz;
2796 n.m[2][2] *= sz;
2797 n.m[3][2] *= sz;
2798
2799 n.m[0][3] *= sw;
2800 n.m[1][3] *= sw;
2801 n.m[2][3] *= sw;
2802 n.m[3][3] *= sw;
2803}
2804
2805template <typename T>
2806void Matrix44CalcCommon<T>::setCol(Base& n, s32 axis, const Vec4& v)
2807{
2808 n.m[0][axis] = v.x;
2809 n.m[1][axis] = v.y;
2810 n.m[2][axis] = v.z;
2811 n.m[3][axis] = v.w;
2812}
2813
2814template <typename T>
2815void Matrix44CalcCommon<T>::setRow(Base& n, const Vec4& v, s32 row)
2816{
2817 n.m[row][0] = v.x;
2818 n.m[row][1] = v.y;
2819 n.m[row][2] = v.z;
2820 n.m[row][3] = v.w;
2821}
2822
2823} // namespace sead
sead
Definition seadAssert.h:44
SEAD_ASSERT
#define SEAD_ASSERT(condition)
Definition seadAssert.h:24