1 /*---------------------------------------------------------------------------*
2 Project: TwlSDK - GX -
3 File: fx_mtx43.c
4
5 Copyright 2003-2008 Nintendo. All rights reserved.
6
7 These coded instructions, statements, and computer programs contain
8 proprietary information of Nintendo of America Inc. and/or Nintendo
9 Company Ltd., and are protected by Federal copyright law. They may
10 not be disclosed to third parties or copied or duplicated in any form,
11 in whole or in part, without the prior written consent of Nintendo.
12
13 $Date:: 2008-09-18#$
14 $Rev: 8573 $
15 $Author: okubata_ryoma $
16 *---------------------------------------------------------------------------*/
17
18 #include <nitro/code32.h> // Always generate ARM binary for efficiency
19
20 #include <nitro/fx/fx_mtx43.h>
21 #include <nitro/fx/fx_mtx33.h>
22 #include <nitro/fx/fx_vec.h>
23 #include <nitro/fx/fx_cp.h>
24 #include <nitro/mi/memory.h>
25
MTX_Identity43_(register MtxFx43 * pDst)26 asm void MTX_Identity43_(register MtxFx43* pDst)
27 {
28 mov r2, #4096 // FX32_ONE
29 mov r3, #0
30 stmia r0!, {r2, r3}
31 mov r1, #0
32 stmia r0!, {r1, r3}
33 stmia r0!, {r2, r3}
34 stmia r0!, {r1, r3}
35 stmia r0!, {r2, r3}
36 stmia r0!, {r1, r3}
37
38 bx lr
39 }
40
MTX_Copy43To44_(register const MtxFx43 * pSrc,register MtxFx44 * pDst)41 asm void MTX_Copy43To44_(register const MtxFx43* pSrc, register MtxFx44* pDst)
42 {
43 stmfd sp!, {r4}
44
45 mov r12, #0
46 ldmia r0!, {r2-r4}
47 stmia r1!, {r2-r4, r12}
48 ldmia r0!, {r2-r4}
49 stmia r1!, {r2-r4, r12}
50 ldmia r0!, {r2-r4}
51 stmia r1!, {r2-r4, r12}
52 mov r12, #4096 // FX32_ONE
53 ldmia r0!, {r2-r4}
54 stmia r1!, {r2-r4, r12}
55
56 ldmfd sp!, {r4}
57 bx lr
58 }
59
60
MTX_Transpose43_(register const MtxFx43 * pSrc,register MtxFx43 * pDst)61 asm void MTX_Transpose43_(register const MtxFx43* pSrc, register MtxFx43* pDst)
62 {
63 stmfd sp!, {r4-r9}
64
65 ldmia r0, {r2-r9, r12}
66 stmia r1!, {r2, r5, r8}
67 stmia r1!, {r3, r6, r9}
68 stmia r1!, {r4, r7, r12}
69
70 mov r0, #0
71 str r0, [r1, #0]
72 str r0, [r1, #4]
73 str r0, [r1, #8]
74
75 ldmfd sp!, {r4-r9}
76 bx lr
77 }
78
79 /*---------------------------------------------------------------------------*
80 Name: MTX_TransApply43
81
82 Description: Calc T(x, y, z) * pSrc = pDst
83
84 Arguments: pSrc a pointer to a 4x3 matrix
85 pDst a pointer to a 4x3 matrix
86 x x trans factor
87 y y trans factor
88 z z trans factor
89
90 Returns: none
91 *---------------------------------------------------------------------------*/
MTX_TransApply43(const MtxFx43 * pSrc,MtxFx43 * pDst,fx32 x,fx32 y,fx32 z)92 void MTX_TransApply43(const MtxFx43 *pSrc, MtxFx43 *pDst, fx32 x, fx32 y, fx32 z)
93 {
94 SDK_NULL_ASSERT(pSrc);
95 SDK_NULL_ASSERT(pDst);
96
97 if (pSrc != pDst)
98 {
99 MI_Copy36B(pSrc, pDst);
100 }
101 {
102 fx64 xx = x;
103 fx64 yy = y;
104 fx64 zz = z;
105
106 pDst->_30 =
107 pSrc->_30 + (fx32)((xx * pSrc->_00 + yy * pSrc->_10 + zz * pSrc->_20) >> FX32_SHIFT);
108 pDst->_31 =
109 pSrc->_31 + (fx32)((xx * pSrc->_01 + yy * pSrc->_11 + zz * pSrc->_21) >> FX32_SHIFT);
110 pDst->_32 =
111 pSrc->_32 + (fx32)((xx * pSrc->_02 + yy * pSrc->_12 + zz * pSrc->_22) >> FX32_SHIFT);
112 }
113 }
114
115 #include <nitro/code16.h>
MTX_Scale43_(register MtxFx43 * pDst,register fx32 x,register fx32 y,register fx32 z)116 asm void MTX_Scale43_(register MtxFx43* pDst, register fx32 x, register fx32 y, register fx32 z)
117 {
118 stmia r0!, {r1}
119 mov r1, #0
120 str r3, [r0, #28]
121 mov r3, #0
122 stmia r0!, {r1, r3}
123 stmia r0!, {r1, r2, r3}
124 mov r2, #0
125 stmia r0!, {r1, r3}
126 add r0, #4
127 stmia r0!, {r1, r2, r3}
128
129 bx lr
130 }
131 #include <nitro/code32.h>
132
133
134 /*---------------------------------------------------------------------------*
135 Name: MTX_ScaleApply43
136
137 Description: This function performs the operation equivalent to
138 MTX_Scale43 + MTX_Concat43.
139
140 Arguments: pSrc a pointer to a 4x3 matrix
141 pDst a pointer to a 4x3 matrix
142 x x scale factor
143 y y scale factor
144 z z scale factor
145
146 Returns: none
147 *---------------------------------------------------------------------------*/
MTX_ScaleApply43(const MtxFx43 * pSrc,MtxFx43 * pDst,fx32 x,fx32 y,fx32 z)148 void MTX_ScaleApply43(const MtxFx43 *pSrc, MtxFx43 *pDst, fx32 x, fx32 y, fx32 z)
149 {
150 MTX_ScaleApply33((const MtxFx33 *)pSrc, (MtxFx33 *)pDst, x, y, z);
151
152 pDst->_30 = pSrc->_30;
153 pDst->_31 = pSrc->_31;
154 pDst->_32 = pSrc->_32;
155 }
156
157
158 #include <nitro/code16.h>
MTX_RotX43_(register MtxFx43 * pDst,register fx32 sinVal,register fx32 cosVal)159 asm void MTX_RotX43_(register MtxFx43 * pDst, register fx32 sinVal, register fx32 cosVal)
160 {
161 str r1, [r0, #20]
162 neg r1, r1
163 str r1, [r0, #28]
164 mov r1, #1
165 lsl r1, r1, #12
166 stmia r0!, {r1}
167 mov r3, #0
168 mov r1, #0
169 stmia r0!, {r1,r3}
170 stmia r0!, {r1,r2}
171 str r1, [r0, #4]
172 add r0, #12
173 stmia r0!, {r2,r3}
174 stmia r0!, {r1,r3}
175
176 bx lr
177 }
178 #include <nitro/code32.h>
179
180 #include <nitro/code16.h>
MTX_RotY43_(register MtxFx43 * pDst,register fx32 sinVal,register fx32 cosVal)181 asm void MTX_RotY43_(register MtxFx43 * pDst, register fx32 sinVal, register fx32 cosVal)
182 {
183 str r1, [r0, #24]
184 mov r3, #0
185 stmia r0!, {r2, r3}
186 neg r1, r1
187 stmia r0!, {r1, r3}
188 mov r1, #1
189 lsl r1, r1, #12
190 stmia r0!, {r1, r3}
191 add r0, #4
192 mov r1, #0
193 stmia r0!, {r1, r2, r3}
194 stmia r0!, {r1, r3}
195
196 bx lr
197 }
198 #include <nitro/code32.h>
199
200
201 #include <nitro/code16.h>
MTX_RotZ43_(register MtxFx43 * pDst,register fx32 sinVal,register fx32 cosVal)202 asm void MTX_RotZ43_(register MtxFx43 * pDst, register fx32 sinVal, register fx32 cosVal)
203 {
204 stmia r0!, {r2}
205 mov r3, #0
206 stmia r0!, {r1, r3}
207 neg r1, r1
208 stmia r0!, {r1, r2, r3}
209 mov r1, #0
210 mov r2, #0
211 mov r3, #1
212 lsl r3, r3, #12
213 stmia r0!, {r1, r2, r3}
214 mov r3, #0
215 stmia r0!, {r1, r2, r3}
216
217 bx lr
218 }
219 #include <nitro/code32.h>
220
221
222 /*---------------------------------------------------------------------------*
223 Name: MTX_RotAxis43
224
225 Description: Sets a rotation matrix about an arbitrary axis.
226
227 Arguments: pDst a pointer to a 4x3 matrix
228 vec a pointer to a vector containing the x,y,z axis
229 components. *vec must be a unit vector.
230 sinVal sine of an angle of rotation
231 cosVal cosine of an angle of rotation
232
233 Returns: none
234 *---------------------------------------------------------------------------*/
MTX_RotAxis43(MtxFx43 * pDst,const VecFx32 * vec,fx32 sinVal,fx32 cosVal)235 void MTX_RotAxis43(MtxFx43 *pDst, const VecFx32 *vec, fx32 sinVal, fx32 cosVal)
236 {
237 MTX_RotAxis33((MtxFx33 *)pDst, vec, sinVal, cosVal);
238 pDst->_30 = 0;
239 pDst->_31 = 0;
240 pDst->_32 = 0;
241 }
242
243
244 /*---------------------------------------------------------------------------*
245 Name: MTX_Inverse43
246
247 Description: Computes an inverse of a 4x3 matrix.
248
249 Arguments: pSrc a pointer to a 4x3 matrix
250 pDst a pointer to a 4x3 matrix
251
252 Returns: -1 if *pSrc is not invertible.
253 0 if success.
254 *---------------------------------------------------------------------------*/
MTX_Inverse43(const MtxFx43 * pSrc,MtxFx43 * pDst)255 int MTX_Inverse43(const MtxFx43 *pSrc, MtxFx43 *pDst)
256 {
257 MtxFx43 tmp;
258 MtxFx43 *p;
259 fx32 det;
260 fx32 det00, det10, det20;
261 fx32 tmp01, tmp02, tmp11, tmp12;
262 fx32 tmp21, tmp22;
263
264 SDK_NULL_ASSERT(pSrc);
265 SDK_NULL_ASSERT(pDst);
266
267 if (pSrc == pDst)
268 {
269 p = &tmp;
270 }
271 else
272 {
273 p = pDst;
274 }
275
276 // Computes the sub-determinant
277 det00 = (fx32)(((fx64)pSrc->_11 * pSrc->_22 -
278 (fx64)pSrc->_12 * pSrc->_21 + (fx64)(FX32_ONE >> 1)) >> FX32_SHIFT);
279 det10 = (fx32)(((fx64)pSrc->_10 * pSrc->_22 -
280 (fx64)pSrc->_12 * pSrc->_20 + (fx64)(FX32_ONE >> 1)) >> FX32_SHIFT);
281 det20 = (fx32)(((fx64)pSrc->_10 * pSrc->_21 -
282 (fx64)pSrc->_11 * pSrc->_20 + (fx64)(FX32_ONE >> 1)) >> FX32_SHIFT);
283
284 // Comptutes the determinant
285 det = (fx32)(((fx64)pSrc->_00 * det00 -
286 (fx64)pSrc->_01 * det10 +
287 (fx64)pSrc->_02 * det20 + (fx64)(FX32_ONE >> 1)) >> FX32_SHIFT);
288 if (0 == det)
289 {
290 return -1;
291 }
292
293 FX_InvAsync(det);
294
295 tmp01 = (fx32)(((fx64)pSrc->_01 * pSrc->_22 - (fx64)pSrc->_21 * pSrc->_02) >> FX32_SHIFT);
296 tmp02 = (fx32)(((fx64)pSrc->_01 * pSrc->_12 - (fx64)pSrc->_11 * pSrc->_02) >> FX32_SHIFT);
297 tmp11 = (fx32)(((fx64)pSrc->_00 * pSrc->_22 - (fx64)pSrc->_20 * pSrc->_02) >> FX32_SHIFT);
298 tmp12 = (fx32)(((fx64)pSrc->_00 * pSrc->_12 - (fx64)pSrc->_10 * pSrc->_02) >> FX32_SHIFT);
299
300 det = FX_GetDivResult();
301 p->_00 = (fx32)(((fx64)det * det00) >> FX32_SHIFT);
302 p->_01 = -(fx32)(((fx64)det * tmp01) >> FX32_SHIFT);
303 p->_02 = (fx32)(((fx64)det * tmp02) >> FX32_SHIFT);
304
305 p->_10 = -(fx32)(((fx64)det * det10) >> FX32_SHIFT);
306 p->_11 = (fx32)(((fx64)det * tmp11) >> FX32_SHIFT);
307 p->_12 = -(fx32)(((fx64)det * tmp12) >> FX32_SHIFT);
308
309 p->_20 = (fx32)(((fx64)det * det20) >> FX32_SHIFT);
310
311 tmp21 = (fx32)(((fx64)pSrc->_00 * pSrc->_21 - (fx64)pSrc->_20 * pSrc->_01) >> FX32_SHIFT);
312 p->_21 = -(fx32)(((fx64)det * tmp21) >> FX32_SHIFT);
313
314 tmp22 = (fx32)(((fx64)pSrc->_00 * pSrc->_11 - (fx64)pSrc->_10 * pSrc->_01) >> FX32_SHIFT);
315 p->_22 = (fx32)(((fx64)det * tmp22) >> FX32_SHIFT);
316
317 // compute (invA)*(-C)
318 p->_30 = -(fx32)(((fx64)p->_00 * pSrc->_30 +
319 (fx64)p->_10 * pSrc->_31 + (fx64)p->_20 * pSrc->_32) >> FX32_SHIFT);
320
321 p->_31 = -(fx32)(((fx64)p->_01 * pSrc->_30 +
322 (fx64)p->_11 * pSrc->_31 + (fx64)p->_21 * pSrc->_32) >> FX32_SHIFT);
323
324 p->_32 = -(fx32)(((fx64)p->_02 * pSrc->_30 +
325 (fx64)p->_12 * pSrc->_31 + (fx64)p->_22 * pSrc->_32) >> FX32_SHIFT);
326
327 if (p == &tmp)
328 {
329 MI_Copy48B(&tmp, pDst);
330 }
331 return 0;
332 }
333
334
335 /*---------------------------------------------------------------------------*
336 Name: MTX_Concat43
337
338 Description: Concatenates two 4x3 matrices.
339 The order of operation is A x B = AB.
340 OK for any of ab == a == b.
341
342 Arguments: a a pointer to a 4x3 matrix
343 b a pointer to a 4x3 matrix
344 ab a pointer to the resultant matrix
345
346 Returns: none
347 *---------------------------------------------------------------------------*/
MTX_Concat43(const MtxFx43 * a,const MtxFx43 * b,MtxFx43 * ab)348 void MTX_Concat43(const MtxFx43 *a, const MtxFx43 *b, MtxFx43 *ab)
349 {
350 MtxFx43 tmp;
351 MtxFx43 *p;
352
353 register fx32 x, y, z;
354 register fx32 xx, yy, zz;
355
356 SDK_NULL_ASSERT(a);
357 SDK_NULL_ASSERT(b);
358 SDK_NULL_ASSERT(ab);
359
360 if (ab == b)
361 {
362 p = &tmp;
363 }
364 else
365 {
366 p = ab;
367 }
368
369 // compute (a x b) -> p
370
371 // row 0
372 x = a->_00;
373 y = a->_01;
374 z = a->_02;
375
376 p->_00 = (fx32)(((fx64)x * b->_00 + (fx64)y * b->_10 + (fx64)z * b->_20) >> FX32_SHIFT);
377 p->_01 = (fx32)(((fx64)x * b->_01 + (fx64)y * b->_11 + (fx64)z * b->_21) >> FX32_SHIFT);
378
379 xx = b->_02;
380 yy = b->_12;
381 zz = b->_22;
382
383 p->_02 = (fx32)(((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT);
384
385 // row 1
386 x = a->_10;
387 y = a->_11;
388 z = a->_12;
389
390 p->_12 = (fx32)(((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT);
391 p->_11 = (fx32)(((fx64)x * b->_01 + (fx64)y * b->_11 + (fx64)z * b->_21) >> FX32_SHIFT);
392
393 xx = b->_00;
394 yy = b->_10;
395 zz = b->_20;
396
397 p->_10 = (fx32)(((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT);
398
399 // row 2
400 x = a->_20;
401 y = a->_21;
402 z = a->_22;
403
404 p->_20 = (fx32)(((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT);
405 p->_21 = (fx32)(((fx64)x * b->_01 + (fx64)y * b->_11 + (fx64)z * b->_21) >> FX32_SHIFT);
406 xx = b->_02;
407 yy = b->_12;
408 zz = b->_22;
409
410 p->_22 = (fx32)(((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT);
411
412 // row 3
413 x = a->_30;
414 y = a->_31;
415 z = a->_32;
416
417 p->_32 = (fx32)((((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT) + b->_32);
418 p->_31 =
419 (fx32)((((fx64)x * b->_01 + (fx64)y * b->_11 + (fx64)z * b->_21) >> FX32_SHIFT) + b->_31);
420 p->_30 =
421 (fx32)((((fx64)x * b->_00 + (fx64)y * b->_10 + (fx64)z * b->_20) >> FX32_SHIFT) + b->_30);
422
423 if (p == &tmp)
424 {
425 *ab = tmp;
426 }
427 }
428
429
430 /*---------------------------------------------------------------------------*
431 Name: MTX_MultVec43
432
433 Description: Multiplies a vector by a 4x3 matrix.
434 vec x m = dst
435
436 Arguments: vec a pointer to the source vector to multiply
437 m a pointer to a 4x3 matrix
438 dst a pointer to the result vector
439
440 Returns: none
441 *---------------------------------------------------------------------------*/
MTX_MultVec43(const VecFx32 * vec,const MtxFx43 * m,VecFx32 * dst)442 void MTX_MultVec43(const VecFx32 *vec, const MtxFx43 *m, VecFx32 *dst)
443 {
444 register fx32 x, y, z;
445 SDK_NULL_ASSERT(vec);
446 SDK_NULL_ASSERT(m);
447 SDK_NULL_ASSERT(dst);
448
449 x = vec->x;
450 y = vec->y;
451 z = vec->z;
452
453 dst->x = (fx32)(((fx64)x * m->_00 + (fx64)y * m->_10 + (fx64)z * m->_20) >> FX32_SHIFT);
454 dst->x += m->_30;
455
456 dst->y = (fx32)(((fx64)x * m->_01 + (fx64)y * m->_11 + (fx64)z * m->_21) >> FX32_SHIFT);
457 dst->y += m->_31;
458
459 dst->z = (fx32)(((fx64)x * m->_02 + (fx64)y * m->_12 + (fx64)z * m->_22) >> FX32_SHIFT);
460 dst->z += m->_32;
461 }
462
463
464 /*---------------------------------------------------------------------------*
465 Name: MTX_LookAt
466
467 Description: Computes a matrix to transform points to camera coordinates.
468
469 Arguments: camPos camera position
470 camUp camera 'up' direction
471 target camera aim point
472 mtx a 4x3 matrix to be set if not NULL
473
474 Returns: none
475 *---------------------------------------------------------------------------*/
MTX_LookAt(const VecFx32 * camPos,const VecFx32 * camUp,const VecFx32 * target,MtxFx43 * mtx)476 void MTX_LookAt(const VecFx32 *camPos, const VecFx32 *camUp, const VecFx32 *target, MtxFx43 *mtx)
477 {
478 VecFx32 vLook, vRight, vUp;
479
480 SDK_NULL_ASSERT(camPos);
481 SDK_NULL_ASSERT(camUp);
482 SDK_NULL_ASSERT(target);
483 SDK_NULL_ASSERT(mtx);
484
485 vLook.x = camPos->x - target->x;
486 vLook.y = camPos->y - target->y;
487 vLook.z = camPos->z - target->z;
488 VEC_Normalize(&vLook, &vLook);
489 VEC_CrossProduct(camUp, &vLook, &vRight);
490 VEC_Normalize(&vRight, &vRight);
491 VEC_CrossProduct(&vLook, &vRight, &vUp);
492
493 mtx->_00 = vRight.x;
494 mtx->_01 = vUp.x;
495 mtx->_02 = vLook.x;
496 mtx->_10 = vRight.y;
497 mtx->_11 = vUp.y;
498 mtx->_12 = vLook.y;
499 mtx->_20 = vRight.z;
500 mtx->_21 = vUp.z;
501 mtx->_22 = vLook.z;
502 mtx->_30 = -VEC_DotProduct(camPos, &vRight);
503 mtx->_31 = -VEC_DotProduct(camPos, &vUp);
504 mtx->_32 = -VEC_DotProduct(camPos, &vLook);
505 }
506