1 /*---------------------------------------------------------------------------*
2 Project: TwlSDK - GX -
3 File: fx_mtx22.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/fx/fx_mtx22.h>
19 #include <nitro/fx/fx_const.h>
20 #include <nitro/fx/fx_cp.h>
21
mul64(fx64 x,fx32 y)22 inline fx32 mul64(fx64 x, fx32 y)
23 {
24 return (fx32)((x * y) >> FX32_SHIFT);
25 }
26
27 #include <nitro/code32.h> // Always generate ARM binary for efficiency
28
MTX_Identity22_(register MtxFx22 * pDst)29 asm void MTX_Identity22_(register MtxFx22* pDst)
30 {
31 mov r1, #0
32 mov r2, #4096 // FX32_ONE
33 mov r3, #0
34 stmia r0!, {r2, r3}
35 stmia r0!, {r1, r2}
36 bx lr
37 }
38
MTX_Transpose22_(const register MtxFx22 * pSrc,register MtxFx22 * pDst)39 asm void MTX_Transpose22_(const register MtxFx22* pSrc, register MtxFx22* pDst)
40 {
41 stmfd sp!, {r4}
42
43 ldmia r0, {r2-r4, r12}
44 stmia r1!, {r2, r4}
45 stmia r1!, {r3, r12}
46
47 ldmfd sp!, {r4}
48 bx lr
49 }
50
51 #include <nitro/code16.h>
MTX_Scale22_(register MtxFx22 * pDst,register fx32 x,register fx32 y)52 asm void MTX_Scale22_(register MtxFx22* pDst, register fx32 x, register fx32 y)
53 {
54 stmia r0!, {r1}
55 mov r1, #0
56 str r2, [r0, #8]
57 mov r2, #0
58 stmia r0!, {r1, r2}
59
60 bx lr
61 }
62
MTX_Rot22_(register MtxFx22 * pDst,register fx32 sinVal,register fx32 cosVal)63 asm void MTX_Rot22_(register MtxFx22 * pDst, register fx32 sinVal, register fx32 cosVal)
64 {
65 str r2, [r0, #0]
66 str r1, [r0, #4]
67 neg r1, r1
68 str r1, [r0, #8]
69 str r2, [r0, #12]
70
71 bx lr
72 }
73
74 #include <nitro/codereset.h>
75
76 /*---------------------------------------------------------------------------*
77 Name: MTX_ScaleApply22
78
79 Description: This function performs the operation equivalent to
80 MTX_Scale22 + MTX_Concat22.
81
82 Arguments: pSrc a pointer to a 2x2 matrix
83 pDst a pointer to a 2x2 matrix
84 x x scale factor
85 y y scale factor
86
87 Returns: none
88 *---------------------------------------------------------------------------*/
MTX_ScaleApply22(const MtxFx22 * pSrc,MtxFx22 * pDst,fx32 x,fx32 y)89 void MTX_ScaleApply22(const MtxFx22 *pSrc, MtxFx22 *pDst, fx32 x, fx32 y)
90 {
91 fx64 v;
92
93 SDK_NULL_ASSERT(pSrc);
94 SDK_NULL_ASSERT(pDst);
95
96 v = (fx64)x;
97 pDst->_00 = mul64(v, pSrc->_00);
98 pDst->_01 = mul64(v, pSrc->_01);
99
100 v = (fx64)y;
101 pDst->_10 = mul64(v, pSrc->_10);
102 pDst->_11 = mul64(v, pSrc->_11);
103 }
104
105
106 /*---------------------------------------------------------------------------*
107 Name: MTX_Inverse22
108
109 Description: Computes an inverse of a 2x2 matrix.
110
111 Arguments: pSrc a pointer to a 2x2 matrix
112 pDst a pointer to a 2x2 matrix
113
114 Returns: -1 if *pSrc is not invertible.
115 0 if success.
116 *---------------------------------------------------------------------------*/
MTX_Inverse22(const MtxFx22 * pSrc,MtxFx22 * pDst)117 int MTX_Inverse22(const MtxFx22 *pSrc, MtxFx22 *pDst)
118 {
119 MtxFx22 tmp;
120 MtxFx22 *p;
121 fx32 det;
122
123 SDK_NULL_ASSERT(pSrc);
124 SDK_NULL_ASSERT(pDst);
125
126 if (pSrc == pDst)
127 {
128 p = &tmp;
129 }
130 else
131 {
132 p = pDst;
133 }
134
135 // Comptutes the determinant
136 det = (fx32)(((fx64)pSrc->_00 * pSrc->_11 -
137 (fx64)pSrc->_01 * pSrc->_10 + (fx64)(FX32_ONE >> 1)) >> FX32_SHIFT);
138
139 if (0 == det)
140 {
141 return -1;
142 }
143
144 FX_InvAsync(det);
145
146 det = FX_GetDivResult();
147
148 p->_00 = (fx32)(((fx64)det * pSrc->_11) >> FX32_SHIFT);
149 p->_01 = -(fx32)(((fx64)det * pSrc->_01) >> FX32_SHIFT);
150
151 p->_10 = -(fx32)(((fx64)det * pSrc->_10) >> FX32_SHIFT);
152 p->_11 = (fx32)(((fx64)det * pSrc->_00) >> FX32_SHIFT);
153
154 if (p == &tmp)
155 {
156 MI_Copy16B(&tmp, pDst);
157 }
158 return 0;
159 }
160
161
162 /*---------------------------------------------------------------------------*
163 Name: MTX_Concat22
164
165 Description: Concatenates two 2x2 matrices.
166 The order of operation is A x B = AB.
167 OK for any of ab == a == b.
168
169 Arguments: a a pointer to a 2x2 matrix
170 b a pointer to a 2x2 matrix
171 ab a pointer to the resultant matrix
172
173 Returns: none
174 *---------------------------------------------------------------------------*/
MTX_Concat22(const MtxFx22 * a,const MtxFx22 * b,MtxFx22 * ab)175 void MTX_Concat22(const MtxFx22 *a, const MtxFx22 *b, MtxFx22 *ab)
176 {
177 MtxFx22 tmp;
178 MtxFx22 *p;
179
180 register fx32 x, y;
181
182 SDK_NULL_ASSERT(a);
183 SDK_NULL_ASSERT(b);
184 SDK_NULL_ASSERT(ab);
185
186 if (ab == b)
187 {
188 p = &tmp;
189 }
190 else
191 {
192 p = ab;
193 }
194
195 // compute (a x b) -> p
196
197 // row 0
198 x = a->_00;
199 y = a->_01;
200
201 p->_00 = (fx32)(((fx64)x * b->_00 + (fx64)y * b->_10) >> FX32_SHIFT);
202 p->_01 = (fx32)(((fx64)x * b->_01 + (fx64)y * b->_11) >> FX32_SHIFT);
203
204 // row 1
205 x = a->_10;
206 y = a->_11;
207 p->_10 = (fx32)(((fx64)x * b->_00 + (fx64)y * b->_10) >> FX32_SHIFT);
208 p->_11 = (fx32)(((fx64)x * b->_01 + (fx64)y * b->_11) >> FX32_SHIFT);
209
210 if (p == &tmp)
211 {
212 *ab = tmp;
213 }
214 }
215