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