1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
|
#include "global.h"
#include "main.h"
#include "fx.h"
void MI_Copy48B(void *src, void *dst);
ARM_FUNC void MTX_TransApply44(struct Mtx44 *mtx, struct Mtx44 *dst, fx32 x, fx32 y, fx32 z){
if(mtx != dst)
MI_Copy48B(mtx, dst);
dst->_[12] = mtx->_[12] + (fx32)(((fx64)x * mtx->_[0] + (fx64)y * mtx->_[4] + (fx64)z * mtx->_[8] ) >> FX32_INT_SHIFT);
dst->_[13] = mtx->_[13] + (fx32)(((fx64)x * mtx->_[1] + (fx64)y * mtx->_[5] + (fx64)z * mtx->_[9] ) >> FX32_INT_SHIFT);
dst->_[14] = mtx->_[14] + (fx32)(((fx64)x * mtx->_[2] + (fx64)y * mtx->_[6] + (fx64)z * mtx->_[10]) >> FX32_INT_SHIFT);
dst->_[15] = mtx->_[15] + (fx32)(((fx64)x * mtx->_[3] + (fx64)y * mtx->_[7] + (fx64)z * mtx->_[11]) >> FX32_INT_SHIFT);
}
ARM_FUNC void MTX_Concat44(struct Mtx44 *a, struct Mtx44 *b, struct Mtx44 *c){
struct Mtx44 temp;
struct Mtx44 *dst;
fx32 a0, a1, a2, a3;
fx32 b0, b1, b2, b3;
if (c == b)
dst = &temp;
else
dst = c;
a0 = a->_[0];
a1 = a->_[1];
a2 = a->_[2];
a3 = a->_[3];
dst->_[0] = (fx32)(((fx64)a0 * b->_[0] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[8] + (fx64)a3 * b->_[12]) >> FX32_INT_SHIFT);
dst->_[1] = (fx32)(((fx64)a0 * b->_[1] + (fx64)a1 * b->_[5] + (fx64)a2 * b->_[9] + (fx64)a3 * b->_[13]) >> FX32_INT_SHIFT);
dst->_[3] = (fx32)(((fx64)a0 * b->_[3] + (fx64)a1 * b->_[7] + (fx64)a2 * b->_[11] + (fx64)a3 * b->_[15]) >> FX32_INT_SHIFT);
b0 = b->_[2];
b1 = b->_[6];
b2 = b->_[10];
b3 = b->_[14];
dst->_[2] = (fx32)(((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT);
a0 = a->_[4];
a1 = a->_[5];
a2 = a->_[6];
a3 = a->_[7];
dst->_[6] = (fx32)(((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT);
dst->_[5] = (fx32)(((fx64)a0 * b->_[1] + (fx64)a1 * b->_[5] + (fx64)a2 * b->_[9] + (fx64)a3 * b->_[13]) >> FX32_INT_SHIFT);
dst->_[7] = (fx32)(((fx64)a0 * b->_[3] + (fx64)a1 * b->_[7] + (fx64)a2 * b->_[11] + (fx64)a3 * b->_[15]) >> FX32_INT_SHIFT);
b0 = b->_[0];
b1 = b->_[4];
b2 = b->_[8];
b3 = b->_[12];
dst->_[4] = (fx32)(((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT);
a0 = a->_[8];
a1 = a->_[9];
a2 = a->_[10];
a3 = a->_[11];
dst->_[8] = (fx32)(((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT);
dst->_[9] = (fx32)(((fx64)a0 * b->_[1] + (fx64)a1 * b->_[5] + (fx64)a2 * b->_[9] + (fx64)a3 * b->_[13]) >> FX32_INT_SHIFT);
dst->_[11] = (fx32)(((fx64)a0 * b->_[3] + (fx64)a1 * b->_[7] + (fx64)a2 * b->_[11] + (fx64)a3 * b->_[15]) >> FX32_INT_SHIFT);
b0 = b->_[2];
b1 = b->_[6];
b2 = b->_[10];
b3 = b->_[14];
dst->_[10] = (fx32)(((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT);
a0 = a->_[12];
a1 = a->_[13];
a2 = a->_[14];
a3 = a->_[15];
dst->_[14] = (fx32)(((fx64)a0 * b0 + (fx64)a1 * b1 + (fx64)a2 * b2 + (fx64)a3 * b3) >> FX32_INT_SHIFT);
dst->_[13] = (fx32)(((fx64)a0 * b->_[1] + (fx64)a1 * b->_[5] + (fx64)a2 * b->_[9] + (fx64)a3 * b->_[13]) >> FX32_INT_SHIFT);
dst->_[12] = (fx32)(((fx64)a0 * b->_[0] + (fx64)a1 * b->_[4] + (fx64)a2 * b->_[8] + (fx64)a3 * b->_[12]) >> FX32_INT_SHIFT);
dst->_[15] = (fx32)(((fx64)a0 * b->_[3] + (fx64)a1 * b->_[7] + (fx64)a2 * b->_[11] + (fx64)a3 * b->_[15]) >> FX32_INT_SHIFT);
if (dst == &temp)
*c = temp;
}
ARM_FUNC asm void MTX_Identity44_(struct Mtx44 *dst){
mov r2, #0x1000
mov r3, #0x0
stmia r0!, {r2-r3}
mov r1, #0x0
stmia r0!, {r1,r3}
stmia r0!, {r1-r3}
stmia r0!, {r1,r3}
stmia r0!, {r1-r3}
stmia r0!, {r1,r3}
stmia r0!, {r1-r2}
bx lr
}
ARM_FUNC asm void MTX_Copy44To43_(struct Mtx44 *src, struct Mtx43 *dst){
ldmia r0!, {r2-r3,r12}
add r0, r0, #0x4
stmia r1!, {r2-r3,r12}
ldmia r0!, {r2-r3,r12}
add r0, r0, #0x4
stmia r1!, {r2-r3,r12}
ldmia r0!, {r2-r3,r12}
add r0, r0, #0x4
stmia r1!, {r2-r3,r12}
ldmia r0!, {r2-r3,r12}
add r0, r0, #0x4
stmia r1!, {r2-r3,r12}
bx lr
}
THUMB_FUNC asm void MTX_RotX44_(struct Mtx44 *mtx, fx32 sinphi, fx32 cosphi){
str r2, [r0, #0x14]
str r2, [r0, #0x28]
str r1, [r0, #0x18]
neg r1, r1
str r1, [r0, #0x24]
mov r1, #0x1
mov r2, #0x0
lsl r1, r1, #0xc
mov r3, #0x0
stmia r0!, {r1-r3}
stmia r0!, {r2-r3}
add r0, #0x8
stmia r0!, {r2-r3}
add r0, #0x8
stmia r0!, {r2-r3}
stmia r0!, {r2-r3}
str r1, [r0, #0x0]
bx lr
}
THUMB_FUNC asm void MTX_RotY44_(struct Mtx44 *mtx, fx32 sinphi, fx32 cosphi){
str r2, [r0, #0x0]
str r2, [r0, #0x28]
str r1, [r0, #0x20]
neg r1, r1
str r1, [r0, #0x8]
mov r3, #0x1
mov r1, #0x0
lsl r3, r3, #0xc
mov r2, #0x0
str r2, [r0, #0x4]
add r0, #0xc
stmia r0!, {r1-r3}
stmia r0!, {r1-r2}
str r2, [r0, #0x4]
add r0, #0xc
stmia r0!, {r1-r2}
stmia r0!, {r1-r3}
bx lr
}
THUMB_FUNC asm void MTX_RotZ44_(struct Mtx44 *mtx, fx32 sinphi, fx32 cosphi){
str r2, [r0, #0x0]
str r2, [r0, #0x14]
str r1, [r0, #0x4]
neg r1, r1
str r1, [r0, #0x10]
mov r3, #0x1
mov r1, #0x0
lsl r3, r3, #0xc
mov r2, #0x0
add r0, #0x8
stmia r0!, {r1-r2}
add r0, #0x8
stmia r0!, {r1-r2}
stmia r0!, {r1-r3}
stmia r0!, {r1-r2}
stmia r0!, {r1-r3}
bx lr
}
|
