1 /*---------------------------------------------------------------------------*
2 Project: TwlSDK - GX -
3 File: fx_mtx33.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 #include <nitro/code32.h> // Always generate ARM binary for efficiency
18
19 #include <nitro/fx/fx_mtx33.h>
20 #include <nitro/fx/fx_const.h>
21 #include <nitro/fx/fx_cp.h>
22
mul64(fx64 x,fx32 y)23 inline fx32 mul64(fx64 x, fx32 y)
24 {
25 return (fx32)((x * y) >> FX32_SHIFT);
26 }
27
MTX_Identity33_(register MtxFx33 * pDst)28 asm void MTX_Identity33_(register MtxFx33* pDst)
29 {
30 mov r2, #4096 // FX32_ONE
31 str r2, [r0, #32]
32 mov r3, #0
33 stmia r0!, {r2, r3}
34 mov r1, #0
35 stmia r0!, {r1, r3}
36 stmia r0!, {r2, r3}
37 stmia r0!, {r1, r3}
38 bx lr
39 }
40
MTX_Copy33To43_(const register MtxFx33 * pSrc,register MtxFx43 * pDst)41 asm void MTX_Copy33To43_(const register MtxFx33* pSrc, register MtxFx43* pDst)
42 {
43 ldmia r0!, {r2-r3, r12}
44 stmia r1!, {r2-r3, r12}
45 ldmia r0!, {r2-r3, r12}
46 stmia r1!, {r2-r3, r12}
47 ldmia r0!, {r2-r3, r12}
48 stmia r1!, {r2-r3, r12}
49
50 mov r2, #0
51 str r2, [r1, #0]
52 str r2, [r1, #4]
53 str r2, [r1, #8]
54
55 bx lr
56 }
57
58
MTX_Copy33To44_(const register MtxFx33 * pSrc,register MtxFx44 * pDst)59 asm void MTX_Copy33To44_(const register MtxFx33* pSrc, register MtxFx44* pDst)
60 {
61 stmfd sp!, {r4}
62
63 mov r12, #0
64 ldmia r0!, {r2-r4}
65 stmia r1!, {r2-r4, r12}
66 ldmia r0!, {r2-r4}
67 stmia r1!, {r2-r4, r12}
68 ldmia r0!, {r2-r4}
69 stmia r1!, {r2-r4, r12}
70
71 mov r2, #4096 // FX32_ONE
72 str r12, [r1, #0]
73 str r12, [r1, #4]
74 str r12, [r1, #8]
75 str r2, [r1, #12]
76
77 ldmfd sp!, {r4}
78 bx lr
79 }
80
81
MTX_Transpose33_(const register MtxFx33 * pSrc,register MtxFx33 * pDst)82 asm void MTX_Transpose33_(const register MtxFx33* pSrc, register MtxFx33* pDst)
83 {
84 stmfd sp!, {r4-r9}
85
86 ldmia r0, {r2-r9, r12}
87 stmia r1!, {r2, r5, r8}
88 stmia r1!, {r3, r6, r9}
89 stmia r1!, {r4, r7, r12}
90
91 ldmfd sp!, {r4-r9}
92 bx lr
93 }
94
95 #include <nitro/code16.h>
MTX_Scale33_(register MtxFx33 * pDst,register fx32 x,register fx32 y,register fx32 z)96 asm void MTX_Scale33_(register MtxFx33* pDst, register fx32 x, register fx32 y, register fx32 z)
97 {
98 stmia r0!, {r1}
99 mov r1, #0
100 str r3, [r0, #28]
101 mov r3, #0
102 stmia r0!, {r1, r3}
103 stmia r0!, {r1, r2, r3}
104 mov r2, #0
105 stmia r0!, {r1, r3}
106
107 bx lr
108 }
109 #include <nitro/code32.h>
110
111 /*---------------------------------------------------------------------------*
112 Name: MTX_ScaleApply33
113
114 Description: This function performs the operation equivalent to
115 MTX_Scale33 + MTX_Concat33.
116
117 Arguments: pSrc a pointer to a 3x3 matrix
118 pDst a pointer to a 3x3 matrix
119 x x scale factor
120 y y scale factor
121 z z scale factor
122
123 Returns: none
124 *---------------------------------------------------------------------------*/
MTX_ScaleApply33(const MtxFx33 * pSrc,MtxFx33 * pDst,fx32 x,fx32 y,fx32 z)125 void MTX_ScaleApply33(const MtxFx33 *pSrc, MtxFx33 *pDst, fx32 x, fx32 y, fx32 z)
126 {
127 fx64 v;
128
129 SDK_NULL_ASSERT(pSrc);
130 SDK_NULL_ASSERT(pDst);
131
132 v = (fx64)x;
133 pDst->_00 = mul64(v, pSrc->_00);
134 pDst->_01 = mul64(v, pSrc->_01);
135 pDst->_02 = mul64(v, pSrc->_02);
136
137 v = (fx64)y;
138 pDst->_10 = mul64(v, pSrc->_10);
139 pDst->_11 = mul64(v, pSrc->_11);
140 pDst->_12 = mul64(v, pSrc->_12);
141
142 v = (fx64)z;
143 pDst->_20 = mul64(v, pSrc->_20);
144 pDst->_21 = mul64(v, pSrc->_21);
145 pDst->_22 = mul64(v, pSrc->_22);
146 }
147
148 #include <nitro/code16.h>
MTX_RotX33_(register MtxFx33 * pDst,register fx32 sinVal,register fx32 cosVal)149 asm void MTX_RotX33_(register MtxFx33 * pDst, register fx32 sinVal, register fx32 cosVal)
150 {
151 mov r3, #1
152 lsl r3, r3, #12
153 str r3, [r0, #0]
154 mov r3, #0
155 str r3, [r0, #4]
156 str r3, [r0, #8]
157 str r3, [r0, #12]
158 str r2, [r0, #16]
159 str r1, [r0, #20]
160 str r3, [r0, #24]
161 neg r1, r1
162 str r1, [r0, #28]
163 str r2, [r0, #32]
164
165 bx lr
166 }
167 #include <nitro/code32.h>
168
169 #include <nitro/code16.h>
MTX_RotY33_(register MtxFx33 * pDst,register fx32 sinVal,register fx32 cosVal)170 asm void MTX_RotY33_(register MtxFx33 * pDst, register fx32 sinVal, register fx32 cosVal)
171 {
172 str r2, [r0, #0]
173 str r2, [r0, #32]
174 mov r3, #0
175 str r3, [r0, #4]
176 str r3, [r0, #12]
177 str r3, [r0, #20]
178 str r3, [r0, #28]
179 neg r2, r1
180 mov r3, #1
181 lsl r3, r3, #12
182 str r1, [r0, #24]
183 str r2, [r0, #8]
184 str r3, [r0, #16]
185
186 bx lr
187 }
188 #include <nitro/code32.h>
189
190 #include <nitro/code16.h>
MTX_RotZ33_(register MtxFx33 * pDst,register fx32 sinVal,register fx32 cosVal)191 asm void MTX_RotZ33_(register MtxFx33 * pDst, register fx32 sinVal, register fx32 cosVal)
192 {
193 stmia r0!, {r2}
194 mov r3, #0
195 stmia r0!, {r1, r3}
196 neg r1, r1
197 stmia r0!, {r1, r2}
198 mov r1, #1
199 lsl r1, r1, #12
200 str r3, [r0, #0]
201 str r3, [r0, #4]
202 str r3, [r0, #8]
203 str r1, [r0, #12]
204
205 bx lr
206 }
207 #include <nitro/code32.h>
208
209 /*---------------------------------------------------------------------------*
210 Name: MTX_RotAxis33
211
212 Description: Sets a rotation matrix about an arbitrary axis
213
214 Arguments: pDst a pointer to a 3x3 matrix
215 vec a pointer to a vector containing the x,y,z axis
216 components. *vec must be a unit vector.
217 sinVal sine of an angle of rotation
218 cosVal cosine of an angle of rotation
219
220 Returns: none
221 *---------------------------------------------------------------------------*/
MTX_RotAxis33(MtxFx33 * pDst,const VecFx32 * vec,fx32 sinVal,fx32 cosVal)222 void MTX_RotAxis33(MtxFx33 *pDst, const VecFx32 *vec, fx32 sinVal, fx32 cosVal)
223 {
224 fx64 t, ss, cc, xx, yy, zz;
225 fx32 t01;
226 fx32 s2;
227 SDK_NULL_ASSERT(pDst);
228 SDK_NULL_ASSERT(vec);
229
230 t = (fx64)(FX32_ONE - (fx32)cosVal);
231 ss = (fx64)sinVal;
232 cc = (fx64)cosVal;
233 xx = (fx64)vec->x;
234 yy = (fx64)vec->y;
235 zz = (fx64)vec->z;
236
237 t01 = (fx32)((xx * xx * t) >> (FX64_SHIFT + FX64_SHIFT));
238 pDst->_00 = t01 + cosVal;
239
240 t01 = (fx32)((yy * yy * t) >> (FX64_SHIFT + FX64_SHIFT));
241 pDst->_11 = t01 + cosVal;
242
243 t01 = (fx32)((zz * zz * t) >> (FX64_SHIFT + FX64_SHIFT));
244 pDst->_22 = t01 + cosVal;
245
246 t01 = (fx32)((t * xx * yy) >> (FX64_SHIFT + FX64_SHIFT));
247 s2 = (fx32)((ss * zz) >> FX64_SHIFT);
248 pDst->_01 = t01 + s2;
249 pDst->_10 = t01 - s2;
250
251 t01 = (fx32)((t * xx * zz) >> (FX64_SHIFT + FX64_SHIFT));
252 s2 = (fx32)((ss * yy) >> FX64_SHIFT);
253 pDst->_02 = t01 - s2;
254 pDst->_20 = t01 + s2;
255
256 t01 = (fx32)((t * yy * zz) >> (FX64_SHIFT + FX64_SHIFT));
257 s2 = (fx32)((ss * xx) >> FX64_SHIFT);
258 pDst->_12 = t01 + s2;
259 pDst->_21 = t01 - s2;
260 }
261
262
263 /*---------------------------------------------------------------------------*
264 Name: MTX_Inverse33
265
266 Description: Computes an inverse of a 3x3 matrix.
267
268 Arguments: pSrc a pointer to a 3x3 matrix
269 pDst a pointer to a 3x3 matrix
270
271 Returns: -1 if *pSrc is not invertible.
272 0 if success.
273 *---------------------------------------------------------------------------*/
MTX_Inverse33(const MtxFx33 * pSrc,MtxFx33 * pDst)274 int MTX_Inverse33(const MtxFx33 *pSrc, MtxFx33 *pDst)
275 {
276 MtxFx33 tmp;
277 MtxFx33 *p;
278 fx32 det;
279 fx32 det00, det10, det20;
280 fx32 tmp01, tmp02, tmp11, tmp12;
281 fx32 tmp21, tmp22;
282
283 SDK_NULL_ASSERT(pSrc);
284 SDK_NULL_ASSERT(pDst);
285
286 if (pSrc == pDst)
287 {
288 p = &tmp;
289 }
290 else
291 {
292 p = pDst;
293 }
294
295 // Computes the sub-determinant
296 det00 = (fx32)(((fx64)pSrc->_11 * pSrc->_22 -
297 (fx64)pSrc->_12 * pSrc->_21 + (fx64)(FX32_ONE >> 1)) >> FX32_SHIFT);
298 det10 = (fx32)(((fx64)pSrc->_10 * pSrc->_22 -
299 (fx64)pSrc->_12 * pSrc->_20 + (fx64)(FX32_ONE >> 1)) >> FX32_SHIFT);
300 det20 = (fx32)(((fx64)pSrc->_10 * pSrc->_21 -
301 (fx64)pSrc->_11 * pSrc->_20 + (fx64)(FX32_ONE >> 1)) >> FX32_SHIFT);
302
303 // Comptutes the determinant
304 det = (fx32)(((fx64)pSrc->_00 * det00 -
305 (fx64)pSrc->_01 * det10 +
306 (fx64)pSrc->_02 * det20 + (fx64)(FX32_ONE >> 1)) >> FX32_SHIFT);
307 if (0 == det)
308 {
309 return -1;
310 }
311
312 FX_InvAsync(det);
313
314 tmp01 = (fx32)(((fx64)pSrc->_01 * pSrc->_22 - (fx64)pSrc->_21 * pSrc->_02) >> FX32_SHIFT);
315 tmp02 = (fx32)(((fx64)pSrc->_01 * pSrc->_12 - (fx64)pSrc->_11 * pSrc->_02) >> FX32_SHIFT);
316 tmp11 = (fx32)(((fx64)pSrc->_00 * pSrc->_22 - (fx64)pSrc->_20 * pSrc->_02) >> FX32_SHIFT);
317 tmp12 = (fx32)(((fx64)pSrc->_00 * pSrc->_12 - (fx64)pSrc->_10 * pSrc->_02) >> FX32_SHIFT);
318
319 det = FX_GetDivResult();
320 p->_00 = (fx32)(((fx64)det * det00) >> FX32_SHIFT);
321 p->_01 = -(fx32)(((fx64)det * tmp01) >> FX32_SHIFT);
322 p->_02 = (fx32)(((fx64)det * tmp02) >> FX32_SHIFT);
323
324 p->_10 = -(fx32)(((fx64)det * det10) >> FX32_SHIFT);
325 p->_11 = (fx32)(((fx64)det * tmp11) >> FX32_SHIFT);
326 p->_12 = -(fx32)(((fx64)det * tmp12) >> FX32_SHIFT);
327
328 p->_20 = (fx32)(((fx64)det * det20) >> FX32_SHIFT);
329
330 tmp21 = (fx32)(((fx64)pSrc->_00 * pSrc->_21 - (fx64)pSrc->_20 * pSrc->_01) >> FX32_SHIFT);
331 p->_21 = -(fx32)(((fx64)det * tmp21) >> FX32_SHIFT);
332
333 tmp22 = (fx32)(((fx64)pSrc->_00 * pSrc->_11 - (fx64)pSrc->_10 * pSrc->_01) >> FX32_SHIFT);
334 p->_22 = (fx32)(((fx64)det * tmp22) >> FX32_SHIFT);
335
336 if (p == &tmp)
337 {
338 MI_Copy36B(&tmp, pDst);
339 }
340 return 0;
341 }
342
343
344 /*---------------------------------------------------------------------------*
345 Name: MTX_Concat33
346
347 Description: Concatenates two 3x3 matrices.
348 The order of operation is A x B = AB.
349 OK for any of ab == a == b.
350
351 Arguments: a a pointer to a 3x3 matrix
352 b a pointer to a 3x3 matrix
353 ab a pointer to the resultant matrix
354
355 Returns: none
356 *---------------------------------------------------------------------------*/
MTX_Concat33(const MtxFx33 * a,const MtxFx33 * b,MtxFx33 * ab)357 void MTX_Concat33(const MtxFx33 *a, const MtxFx33 *b, MtxFx33 *ab)
358 {
359 MtxFx33 tmp;
360 MtxFx33 *p;
361
362 register fx32 x, y, z;
363 register fx32 xx, yy, zz;
364
365 SDK_NULL_ASSERT(a);
366 SDK_NULL_ASSERT(b);
367 SDK_NULL_ASSERT(ab);
368
369 if (ab == b)
370 {
371 p = &tmp;
372 }
373 else
374 {
375 p = ab;
376 }
377
378 // compute (a x b) -> p
379
380 // row 0
381 x = a->_00;
382 y = a->_01;
383 z = a->_02;
384
385 p->_00 = (fx32)(((fx64)x * b->_00 + (fx64)y * b->_10 + (fx64)z * b->_20) >> FX32_SHIFT);
386 p->_01 = (fx32)(((fx64)x * b->_01 + (fx64)y * b->_11 + (fx64)z * b->_21) >> FX32_SHIFT);
387
388 xx = b->_02;
389 yy = b->_12;
390 zz = b->_22;
391
392 p->_02 = (fx32)(((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT);
393
394 // row 1
395 x = a->_10;
396 y = a->_11;
397 z = a->_12;
398
399 p->_12 = (fx32)(((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT);
400 p->_11 = (fx32)(((fx64)x * b->_01 + (fx64)y * b->_11 + (fx64)z * b->_21) >> FX32_SHIFT);
401
402 xx = b->_00;
403 yy = b->_10;
404 zz = b->_20;
405
406 p->_10 = (fx32)(((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT);
407
408 // row 2
409 x = a->_20;
410 y = a->_21;
411 z = a->_22;
412
413 p->_20 = (fx32)(((fx64)x * xx + (fx64)y * yy + (fx64)z * zz) >> FX32_SHIFT);
414 p->_21 = (fx32)(((fx64)x * b->_01 + (fx64)y * b->_11 + (fx64)z * b->_21) >> FX32_SHIFT);
415 p->_22 = (fx32)(((fx64)x * b->_02 + (fx64)y * b->_12 + (fx64)z * b->_22) >> FX32_SHIFT);
416
417 if (p == &tmp)
418 {
419 *ab = tmp;
420 }
421 }
422
423
424 /*---------------------------------------------------------------------------*
425 Name: MTX_MultVec33
426
427 Description: Multiplies a vector by a 3x3 matrix.
428 vec x m = dst
429
430 Arguments: vec a pointer to the source vector to multiply
431 m a pointer to a 3x3 matrix
432 dst a pointer to the result vector
433
434 Returns: none
435 *---------------------------------------------------------------------------*/
MTX_MultVec33(const VecFx32 * vec,const MtxFx33 * m,VecFx32 * dst)436 void MTX_MultVec33(const VecFx32 *vec, const MtxFx33 *m, VecFx32 *dst)
437 {
438 register fx32 x, y, z;
439 SDK_NULL_ASSERT(vec);
440 SDK_NULL_ASSERT(m);
441 SDK_NULL_ASSERT(dst);
442
443 x = vec->x;
444 y = vec->y;
445 z = vec->z;
446
447 dst->x = (fx32)(((fx64)x * m->_00 + (fx64)y * m->_10 + (fx64)z * m->_20) >> FX32_SHIFT);
448 dst->y = (fx32)(((fx64)x * m->_01 + (fx64)y * m->_11 + (fx64)z * m->_21) >> FX32_SHIFT);
449 dst->z = (fx32)(((fx64)x * m->_02 + (fx64)y * m->_12 + (fx64)z * m->_22) >> FX32_SHIFT);
450 }
451
452 #include <nitro/codereset.h>
453