diff options
Diffstat (limited to 'include/lib/math.h')
| -rw-r--r-- | include/lib/math.h | 29 |
1 files changed, 21 insertions, 8 deletions
diff --git a/include/lib/math.h b/include/lib/math.h index a4f4086e..16b5cdd1 100644 --- a/include/lib/math.h +++ b/include/lib/math.h @@ -10,8 +10,15 @@ typedef s16 q4; #define INT_TO_Q20(n) ((s32)((n) << 12)) #define FLOAT_TO_Q20(n) ((s32)(((n) * 8192 + 1) / 2)) - #define ROUND_Q20(n) (((s32)(n) + 0x800) >> 12) +#define MUL_Q20(a,b) ROUND_Q20((a) * (b)) + +#define DEG_TO_ANG(n) ((n) * 0x10000 / 360) +#define SIN(n) (gSinCosTable[2 * ((n) >> 4)]) +#define COS(n) (gSinCosTable[2 * ((n) >> 4) + 1]) + +extern "C" s16 Atan2(s32 x, s32 y); +extern q4 gSinCosTable[]; typedef struct { /* 0 */ s8 x; @@ -39,6 +46,19 @@ extern "C" void Vec3p_Cross(Vec3p *a, Vec3p *b, Vec3p *out); extern "C" q20 Vec3p_Length(Vec3p *a); extern "C" void Vec3p_Normalize(Vec3p *vec, Vec3p *out); extern "C" void Vec3p_Axpy(q20 a, Vec3p *x, Vec3p *y, Vec3p *out); +extern "C" q20 Vec3p_Distance(Vec3p *a, Vec3p *b); +inline void Vec3p_Rotate(Vec3p *vec, s16 angle, Vec3p *out) { + q20 sin = SIN(angle); + q20 zSin = MUL_Q20(vec->z, sin); + out->x += zSin; + q20 cos = COS(angle); + q20 zCos = MUL_Q20(vec->z, cos); + out->z += zCos; + q20 xCos = MUL_Q20(vec->x, cos); + out->x += xCos; + q20 xSin = MUL_Q20(vec->x, -sin); + out->z += xSin; +} typedef struct { /* 00 */ q20 x; @@ -55,13 +75,6 @@ typedef struct { /* 24 */ } Mat3p; -#define DEG_TO_ANG(n) ((n) * 0x10000 / 360) -#define SIN(n) (gSinCosTable[2 * ((n) >> 4)]) -#define COS(n) (gSinCosTable[2 * ((n) >> 4) + 1]) - -extern "C" s16 Atan2(s32 x, s32 y); -extern s16 gSinCosTable[]; - extern "C" u32 FastDivide(u32 a, u32 b); extern "C" u32 Divide(u32 a, u32 b); extern "C" bool Approach(unk32 *src, unk32 dest, unk32 step); |