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
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
|
#include "dolphin/mtx/vec.h"
#include "math.h"
#define R_RET fp1
#define FP2 fp2
#define FP3 fp3
#define FP4 fp4
#define FP5 fp5
#define FP6 fp6
#define FP7 fp7
#define FP8 fp8
#define FP9 fp9
#define FP10 fp10
#define FP11 fp11
#define FP12 fp12
#define FP13 fp13
/* 80347090-803470B4 3419D0 0024+00 1/1 103/103 679/679 .text PSVECAdd */
asm void PSVECAdd(const register Vec* vec1, const register Vec* vec2, register Vec* ret) {
#ifdef __MWERKS__ // clang-format off
nofralloc;
psq_l FP2, 0(vec1), 0, 0;
psq_l FP4, 0(vec2), 0, 0;
ps_add FP6, FP2, FP4;
psq_st FP6, 0(ret), 0, 0;
psq_l FP3, 8(vec1), 1, 0;
psq_l FP5, 8(vec2), 1, 0;
ps_add FP7, FP3, FP5;
psq_st FP7, 8(ret), 1, 0;
blr
#endif // clang-format on
}
/* 803470B4-803470D8 3419F4 0024+00 0/0 60/60 59/59 .text PSVECSubtract */
asm void PSVECSubtract(const register Vec* vec1, const register Vec* vec2, register Vec* ret) {
#ifdef __MWERKS__ // clang-format off
nofralloc;
psq_l FP2, 0(vec1), 0, 0;
psq_l FP4, 0(vec2), 0, 0;
ps_sub FP6, FP2, FP4;
psq_st FP6, 0(ret), 0, 0;
psq_l FP3, 8(vec1), 1, 0;
psq_l FP5, 8(vec2), 1, 0;
ps_sub FP7, FP3, FP5;
psq_st FP7, 8(ret), 1, 0;
blr
#endif // clang-format on
}
/* 803470D8-803470F4 341A18 001C+00 0/0 58/58 101/101 .text PSVECScale */
asm void PSVECScale(register const Vec* src, register Vec* dst, register f32 scale) {
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f0, 0(src), 0, 0
psq_l f2, 8(src), 1, 0
ps_muls0 f0, f0, f1
psq_st f0, 0(dst), 0, 0
ps_muls0 f0, f2, f1
psq_st f0, 8(dst), 1, 0
blr
#endif // clang-format on
}
void C_VECScale(const Vec* src, Vec* dst, f32 scale) {
f32 s;
s = 1.0f / sqrtf(src->z * src->z + src->x * src->x + src->y * src->y);
dst->x = src->x * s;
dst->y = src->y * s;
dst->z = src->z * s;
}
/* 803470F4-80347138 341A34 0044+00 2/2 16/16 0/0 .text PSVECNormalize */
void PSVECNormalize(const register Vec* vec1, register Vec* ret) {
#ifdef __MWERKS__ // clang-format off
register f32 half = 0.5f;
register f32 three = 3.0f;
register f32 xx_zz, xx_yy;
register f32 square_sum;
register f32 ret_sqrt;
register f32 n_0, n_1;
asm {
psq_l FP2, 0(vec1), 0, 0;
ps_mul xx_yy, FP2, FP2;
psq_l FP3, 8(vec1), 1, 0;
ps_madd xx_zz, FP3, FP3, xx_yy;
ps_sum0 square_sum, xx_zz, FP3, xx_yy;
frsqrte ret_sqrt, square_sum;
fmuls n_0, ret_sqrt, ret_sqrt;
fmuls n_1, ret_sqrt, half;
fnmsubs n_0, n_0, square_sum, three;
fmuls ret_sqrt, n_0, n_1;
ps_muls0 FP2, FP2, ret_sqrt;
psq_st FP2, 0(ret), 0, 0;
ps_muls0 FP3, FP3, ret_sqrt;
psq_st FP3, 8(ret), 1, 0;
}
#endif // clang-format on
}
/* 80347138-80347150 341A78 0018+00 0/0 140/140 727/727 .text PSVECSquareMag */
asm f32 PSVECSquareMag(register const Vec* v){
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f0, 0(v), 0, 0
ps_mul f0, f0, f0
lfs f1, 8(v)
ps_madd f1, f1, f1, f0
ps_sum0 f1, f1, f0, f0
blr
#endif // clang-format on
}
/* 80347150-80347194 341A90 0044+00 0/0 24/24 0/0 .text PSVECMag */
f32 PSVECMag(const register Vec* v) {
register f32 v_xy, v_zz, square_mag;
register f32 ret_mag, n_0, n_1;
register f32 three, half, zero;
half = 0.5f;
#ifdef __MWERKS__ // clang-format off
asm {
psq_l v_xy, 0(v), 0, 0
ps_mul v_xy, v_xy, v_xy
lfs v_zz, 8(v)
fsubs zero, half, half
ps_madd square_mag, v_zz, v_zz, v_xy
ps_sum0 square_mag, square_mag, v_xy, v_xy
fcmpu cr0, square_mag, zero
beq- __exit
frsqrte ret_mag, square_mag
}
#endif // clang-format on
three = 3.0f;
#ifdef __MWERKS__ // clang-format off
asm {
fmuls n_0, ret_mag, ret_mag
fmuls n_1, ret_mag, half
fnmsubs n_0, n_0, square_mag, three
fmuls ret_mag, n_0, n_1
fmuls square_mag, square_mag, ret_mag
__exit:
}
#endif // clang-format on
return square_mag;
}
/* 80347194-803471B4 341AD4 0020+00 2/2 39/39 15/15 .text PSVECDotProduct */
asm f32 PSVECDotProduct(const register Vec* vec1, const register Vec* vec2) {
#ifdef __MWERKS__ // clang-format off
nofralloc;
psq_l f2, 4(r3), 0, 0 /* qr0 */
psq_l f3, 4(r4), 0, 0 /* qr0 */
ps_mul f2, f2, f3
psq_l f5, 0(r3), 0, 0 /* qr0 */
psq_l f4, 0(r4), 0, 0 /* qr0 */
ps_madd f3, f5, f4, f2
ps_sum0 f1, f3, f2, f2
blr
#endif // clang-format on
}
/* 803471B4-803471F0 341AF4 003C+00 0/0 20/20 3/3 .text PSVECCrossProduct */
asm void PSVECCrossProduct(register const Vec* a, register const Vec* b, register Vec* axb) {
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f1, 0(b), 0, 0
lfs f2, 8(a)
psq_l f0, 0(a), 0, 0
ps_merge10 f6, f1, f1
lfs f3, 8(b)
ps_mul f4, f1, f2
ps_muls0 f7, f1, f0
ps_msub f5, f0, f3, f4
ps_msub f8, f0, f6, f7
ps_merge11 f9, f5, f5
ps_merge01 f10, f5, f8
psq_st f9, 0(axb), 1, 0
ps_neg f10, f10
psq_st f10, 4(axb), 0, 0
blr
#endif // clang-format on
}
/* 803471F0-803472C8 341B30 00D8+00 0/0 1/1 0/0 .text C_VECHalfAngle */
void C_VECHalfAngle(const Vec* a, const Vec* b, Vec* half) {
Vec a0;
Vec b0;
Vec ab;
a0.x = -a->x;
a0.y = -a->y;
a0.z = -a->z;
b0.x = -b->x;
b0.y = -b->y;
b0.z = -b->z;
VECNormalize(&a0, &a0);
VECNormalize(&b0, &b0);
VECAdd(&a0, &b0, &ab);
if (VECDotProduct(&ab, &ab) > 0.0f) {
VECNormalize(&ab, half);
} else {
*half = ab;
}
}
/* 803472C8-8034739C 341C08 00D4+00 0/0 1/1 11/11 .text C_VECReflect */
void C_VECReflect(const Vec* src, const Vec* normal, Vec* dst) {
Vec a0;
Vec b0;
f32 dot;
a0.x = -src->x;
a0.y = -src->y;
a0.z = -src->z;
VECNormalize(&a0, &a0);
VECNormalize(normal, &b0);
dot = VECDotProduct(&a0, &b0);
dst->x = b0.x * 2.0f * dot - a0.x;
dst->y = b0.y * 2.0f * dot - a0.y;
dst->z = b0.z * 2.0f * dot - a0.z;
VECNormalize(dst, dst);
}
/* 8034739C-803473C4 341CDC 0028+00 0/0 107/107 446/446 .text PSVECSquareDistance */
asm f32 PSVECSquareDistance(register const Vec* a, register const Vec* b){
#ifdef __MWERKS__ // clang-format off
nofralloc
psq_l f0, 4(a), 0, 0
psq_l f1, 4(b), 0, 0
ps_sub f2, f0, f1
psq_l f0, 0(a), 0, 0
psq_l f1, 0(b), 0, 0
ps_mul f2, f2, f2
ps_sub f0, f0, f1
ps_madd f1, f0, f0, f2
ps_sum0 f1, f1, f2, f2
blr
#endif // clang-format on
}
/* 803473C4-80347418 341D04 0054+00 0/0 4/4 0/0 .text PSVECDistance */
f32 PSVECDistance(register const Vec* a, register const Vec* b) {
register f32 half_c;
register f32 three_c;
register f32 dist;
#ifdef __MWERKS__ // clang-format off
asm {
psq_l f0, 4(a), 0, 0 /* qr0 */
psq_l f1, 4(b), 0, 0 /* qr0 */
ps_sub f2, f0, f1
psq_l f0, 0(a), 0, 0 /* qr0 */
psq_l f1, 0(b), 0, 0 /* qr0 */
ps_mul f2, f2, f2
ps_sub f0, f0, f1
}
half_c = 0.5f;
asm {
ps_madd dist, f0, f0, f2
fsubs f0, half_c, half_c
ps_sum0 dist, dist, f2, f2
fcmpu cr0, f0, dist
beq exit
}
three_c = 3.0f;
asm {
frsqrte f0, dist
fmuls f2, f0, f0
fmuls f0, f0, half_c
fnmsubs f2, f2, dist, three_c
fmuls f0, f2, f0
fmuls dist, dist, f0
exit:
}
return dist;
#endif // clang-format on
}
|
