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
|
#ifndef VEC_H
#define VEC_H
#include "dolphin/types.h"
#ifdef __cplusplus
extern "C" {
#endif
typedef struct Vec {
f32 x, y, z;
} Vec;
typedef Vec* VecPtr;
typedef Vec Point3d;
typedef Vec* Point3dPtr;
typedef struct SVec {
s16 x, y, z;
} SVec;
void PSVECAdd(const Vec* a, const Vec* b, Vec* ab);
void PSVECSubtract(const Vec* a, const Vec* b, Vec* a_b);
void PSVECScale(const Vec* src, Vec* dst, f32 scale);
void PSVECNormalize(const Vec* src, Vec* unit);
f32 PSVECSquareMag(const Vec* v);
f32 PSVECMag(const Vec* v);
f32 PSVECDotProduct(const Vec* a, const Vec* b);
void PSVECCrossProduct(const Vec* a, const Vec* b, Vec* axb);
f32 PSVECSquareDistance(const Vec* a, const Vec* b);
f32 PSVECDistance(const Vec* a, const Vec* b);
void C_VECHalfAngle(const Vec* a, const Vec* b, Vec* half);
void C_VECReflect(const Vec* src, const Vec* normal, Vec* dst);
inline void C_VECAdd(register const Vec* a, register const Vec* b, register Vec* ab) {
register f32 axy;
register f32 bxy;
register f32 az;
register f32 sumz;
register f32 bz;
asm {
psq_l axy, 0(a), 0, 0
psq_l bxy, 0(b), 0, 0
ps_add axy, axy, bxy
psq_st axy, 0(ab), 0, 0
psq_l az, 8(a), 1, 0
psq_l bz, 8(b), 1, 0
ps_add sumz, az, bz
psq_st sumz, 8(ab), 1, 0
}
}
inline void C_VECSubtract(register const Vec* a, register const Vec* b, register Vec* ab) {
register f32 axy;
register f32 bxy;
register f32 az;
register f32 subz;
register f32 bz;
asm {
psq_l axy, 0(a), 0, 0
psq_l bxy, 0(b), 0, 0
ps_sub bxy, axy, bxy
psq_st bxy, 0(ab), 0, 0
psq_l az, 8(a), 1, 0
psq_l bz, 8(b), 1, 0
ps_sub subz, az, bz
psq_st subz, 8(ab), 1, 0
}
}
inline f32 C_VECSquareMag(const Vec* v) {
register f32 x_y;
register f32 z;
register f32 res;
register const f32* src = &v->x;
asm {
psq_l x_y, 0(src), 0, 0
ps_mul x_y, x_y, x_y
lfs z, 8(src)
ps_madd res, z, z, x_y
ps_sum0 res, res, x_y, x_y
}
;
return res;
}
/* When compiling in debug mode, use C implementations */
#ifdef DEBUG
// TODO: Add debug rom C implementations
/* #define VECAdd C_VECAdd
#define VECSubtract C_VECSubtract
#define VECScale C_VECScale
#define VECNormalize C_VECNormalize
#define VECSquareMag C_VECSquareMag
#define VECMag C_VECMag
#define VECDotProduct C_VECDotProduct
#define VECCrossProduct C_VECCrossProduct
#define VECSquareDistance C_VECSquareDistance
#define VECDistance C_VECDistance */
#define VECAdd PSVECAdd
#define VECSubtract PSVECSubtract
#define VECScale PSVECScale
#define VECNormalize PSVECNormalize
#define VECSquareMag PSVECSquareMag
#define VECMag PSVECMag
#define VECDotProduct PSVECDotProduct
#define VECCrossProduct PSVECCrossProduct
#define VECSquareDistance PSVECSquareDistance
#define VECDistance PSVECDistance
#else
#define VECAdd PSVECAdd
#define VECSubtract PSVECSubtract
#define VECScale PSVECScale
#define VECNormalize PSVECNormalize
#define VECSquareMag PSVECSquareMag
#define VECMag PSVECMag
#define VECDotProduct PSVECDotProduct
#define VECCrossProduct PSVECCrossProduct
#define VECSquareDistance PSVECSquareDistance
#define VECDistance PSVECDistance
#endif
#ifdef __cplusplus
};
#endif
#endif /* VEC_H */
|
