diff --git a/configure.py b/configure.py index 545739bb..4fe28a70 100644 --- a/configure.py +++ b/configure.py @@ -344,6 +344,16 @@ def DolphinLib(lib_name, objects): } +def DolphinLibUnpatched(lib_name, objects): + return { + "lib": lib_name, + "mw_version": "GC/1.2.5", + "cflags": cflags_dolphin, + "host": False, + "objects": objects, + } + + def MusyX(objects, mw_version="GC/1.3.2", debug=False, major=1, minor=5, patch=4): cflags = cflags_musyx if not debug else cflags_musyx_debug return { @@ -514,14 +524,14 @@ config.libs = [ Object(MatchingFor("GMPE01_00", "GMPE01_01"), "dolphin/db.c"), ], ), - DolphinLib( + DolphinLibUnpatched( "mtx", [ - Object(NonMatching, "dolphin/mtx/mtx.c"), + Object(MatchingFor("GMPE01_00", "GMPE01_01"), "dolphin/mtx/mtx.c"), Object(MatchingFor("GMPE01_00", "GMPE01_01"), "dolphin/mtx/mtxvec.c"), Object(MatchingFor("GMPE01_00", "GMPE01_01"), "dolphin/mtx/mtx44.c"), - Object(NonMatching, "dolphin/mtx/vec.c"), - Object(NonMatching, "dolphin/mtx/quat.c"), + Object(MatchingFor("GMPE01_00", "GMPE01_01"), "dolphin/mtx/vec.c"), + Object(MatchingFor("GMPE01_00", "GMPE01_01"), "dolphin/mtx/quat.c"), Object(MatchingFor("GMPE01_00", "GMPE01_01"), "dolphin/mtx/psmtx.c"), ], ), @@ -554,7 +564,7 @@ config.libs = [ DolphinLib( "pad", [ - Object(NonMatching, "dolphin/pad/Padclamp.c"), + Object(MatchingFor("GMPE01_00", "GMPE01_01"), "dolphin/pad/Padclamp.c"), Object(MatchingFor("GMPE01_00", "GMPE01_01"), "dolphin/pad/Pad.c"), ], ), diff --git a/include/dolphin/math.h b/include/dolphin/math.h new file mode 100644 index 00000000..483d8081 --- /dev/null +++ b/include/dolphin/math.h @@ -0,0 +1,85 @@ +#ifndef _DOLPHIN_MATH +#define _DOLPHIN_MATH + +// this file is necessary to match mtx/quat.c + +#define M_PI 3.141592653589793 + +#ifndef _MATH_INLINE +#define _MATH_INLINE static inline +#endif + +extern int __float_nan[]; +extern int __float_huge[]; +extern int __double_huge[]; + +#define INFINITY (*(float *)__float_huge) +#define NAN (*(float *)__float_nan) +#define HUGE_VAL (*(double *)__double_huge) + +#ifdef __MWERKS__ +extern inline double sqrt(double x) +{ + if (x > 0.0) { + double guess = __frsqrte(x); /* returns an approximation to */ + guess = .5 * guess * (3.0 - guess * guess * x); /* now have 8 sig bits */ + guess = .5 * guess * (3.0 - guess * guess * x); /* now have 16 sig bits */ + guess = .5 * guess * (3.0 - guess * guess * x); /* now have 32 sig bits */ + guess = .5 * guess * (3.0 - guess * guess * x); /* now have > 53 sig bits */ + return x * guess; + } + else if (x == 0) + return 0; + else if (x) + return NAN; + + return INFINITY; +} + +extern inline float sqrtf(float x) +{ + const double _half = .5; + const double _three = 3.0; + volatile float y; + if (x > 0.0f) { + double guess = __frsqrte((double)x); // returns an approximation to + guess = _half * guess * (_three - guess * guess * x); // now have 12 sig bits + guess = _half * guess * (_three - guess * guess * x); // now have 24 sig bits + guess = _half * guess * (_three - guess * guess * x); // now have 32 sig bits + y = (float)(x * guess); + return y; + } + return x; +} +#else +double sqrt(double x); +float sqrtf(float x); +#endif + +double atan(double x); +double copysign(double x, double y); +double cos(double x); +double floor(double x); +double frexp(double x, int *exp); +double ldexp(double x, int exp); +double modf(double x, double *intpart); +double sin(double x); +double tan(double x); +double acos(double x); +double asin(double x); +double atan2(double y, double x); +double fmod(double x, double y); +double log(double x); +double pow(double x, double y); +float tanf(float x); + +#ifdef __MWERKS__ +extern inline double fabs(double x) +{ + return __fabs(x); +} +#else +double fabs(double x); +#endif + +#endif diff --git a/include/dolphin/mtx.h b/include/dolphin/mtx.h index 9aba8f15..49dc1b7e 100644 --- a/include/dolphin/mtx.h +++ b/include/dolphin/mtx.h @@ -118,7 +118,7 @@ void PSMTXScaleApply(const Mtx src, Mtx dst, f32 xS, f32 yS, f32 zS); void PSMTXRotRad(Mtx m, char axis, f32 rad); void PSMTXRotTrig(Mtx m, char axis, f32 sinA, f32 cosA); -void PSMTXRotAxisRad(Mtx m, const Vec* axis, f32 rad); +void PSMTXRotAxisRad(register Mtx m, const Vec* axis, register f32 rad); #endif #ifdef MTX_USE_PS diff --git a/src/dolphin/mtx/mtx.c b/src/dolphin/mtx/mtx.c index a8f2bb06..eb16a64f 100644 --- a/src/dolphin/mtx/mtx.c +++ b/src/dolphin/mtx/mtx.c @@ -3,6 +3,8 @@ static f32 Unit01[] = { 0.0f, 1.0f }; extern f32 sinf(f32); +extern f32 cosf(f32); +extern f32 tanf(f32); void C_MTXIdentity(Mtx mtx) { @@ -648,12 +650,12 @@ void C_MTXRotRad(Mtx m, char axis, f32 rad) #ifdef GEKKO void PSMTXRotRad(Mtx m, char axis, f32 rad) { - // f32 sinA, cosA; + f32 sinA, cosA; - // sinA = sinf(rad); - // cosA = cosf(rad); + sinA = sinf(rad); + cosA = cosf(rad); - // PSMTXRotTrig(m, axis, sinA, cosA); + PSMTXRotTrig(m, axis, sinA, cosA); } #endif @@ -717,68 +719,63 @@ void C_MTXRotTrig(Mtx m, char axis, f32 sinA, f32 cosA) #ifdef GEKKO void PSMTXRotTrig(register Mtx m, register char axis, register f32 sinA, register f32 cosA) { -// register f32 fc0, fc1, nsinA; -// register f32 fw0, fw1, fw2, fw3; -// // clang-format off -// asm -// { -// frsp sinA, sinA -// frsp cosA, cosA -// } + register f32 fc0, fc1, nsinA; + register f32 fw0, fw1, fw2, fw3; + // clang-format off -// fc0 = 0.0F; -// fc1 = 1.0F; -// asm -// { -// ori axis, axis, 0x20 -// ps_neg nsinA, sinA -// cmplwi axis, 'x' -// beq _case_x -// cmplwi axis, 'y' -// beq _case_y -// cmplwi axis, 'z' -// beq _case_z -// b _end + fc0 = 0.0F; + fc1 = 1.0F; + asm + { + ori axis, axis, 0x20 + ps_neg nsinA, sinA + cmplwi axis, 'x' + beq _case_x + cmplwi axis, 'y' + beq _case_y + cmplwi axis, 'z' + beq _case_z + b _end -// _case_x: -// psq_st fc1, 0(m), 1, 0 -// psq_st fc0, 4(m), 0, 0 -// ps_merge00 fw0, sinA, cosA -// psq_st fc0, 12(m), 0, 0 -// ps_merge00 fw1, cosA, nsinA -// psq_st fc0, 28(m), 0, 0 -// psq_st fc0, 44(m), 1, 0 -// psq_st fw0, 36(m), 0, 0 -// psq_st fw1, 20(m), 0, 0 -// b _end; +_case_x: + psq_st fc1, 0(m), 1, 0 + psq_st fc0, 4(m), 0, 0 + ps_merge00 fw0, sinA, cosA + psq_st fc0, 12(m), 0, 0 + ps_merge00 fw1, cosA, nsinA + psq_st fc0, 28(m), 0, 0 + psq_st fc0, 44(m), 1, 0 + psq_st fw0, 36(m), 0, 0 + psq_st fw1, 20(m), 0, 0 + b _end; -// _case_y: -// ps_merge00 fw0, cosA, fc0 -// ps_merge00 fw1, fc0, fc1 -// psq_st fc0, 24(m), 0, 0 -// psq_st fw0, 0(m), 0, 0 -// ps_merge00 fw2, nsinA, fc0 -// ps_merge00 fw3, sinA, fc0 -// psq_st fw0, 40(m), 0, 0; -// psq_st fw1, 16(m), 0, 0; -// psq_st fw3, 8(m), 0, 0; -// psq_st fw2, 32(m), 0, 0; -// b _end; +_case_y: + ps_merge00 fw0, cosA, fc0 + ps_merge00 fw1, fc0, fc1 + psq_st fc0, 24(m), 0, 0 + psq_st fw0, 0(m), 0, 0 + ps_merge00 fw2, nsinA, fc0 + ps_merge00 fw3, sinA, fc0 + psq_st fw0, 40(m), 0, 0; + psq_st fw1, 16(m), 0, 0; + psq_st fw3, 8(m), 0, 0; + psq_st fw2, 32(m), 0, 0; + b _end; -// _case_z: -// psq_st fc0, 8(m), 0, 0 -// ps_merge00 fw0, sinA, cosA -// ps_merge00 fw2, cosA, nsinA -// psq_st fc0, 24(m), 0, 0 -// psq_st fc0, 32(m), 0, 0 -// ps_merge00 fw1, fc1, fc0 -// psq_st fw0, 16(m), 0, 0 -// psq_st fw2, 0(m), 0, 0 -// psq_st fw1, 40(m), 0, 0 +_case_z: + psq_st fc0, 8(m), 0, 0 + ps_merge00 fw0, sinA, cosA + ps_merge00 fw2, cosA, nsinA + psq_st fc0, 24(m), 0, 0 + psq_st fc0, 32(m), 0, 0 + ps_merge00 fw1, fc1, fc0 + psq_st fw0, 16(m), 0, 0 + psq_st fw2, 0(m), 0, 0 + psq_st fw1, 40(m), 0, 0 -// _end: -// } -// // clang-format on +_end: + } + // clang-format on } #endif @@ -822,70 +819,58 @@ void C_MTXRotAxisRad(Mtx m, const Vec *axis, f32 rad) } #ifdef GEKKO -static void __PSMTXRotAxisRadInternal(register Mtx m, const register Vec *axis, register f32 sT, register f32 cT) +#define qr0 0 + +void PSMTXRotAxisRad(register Mtx m, const Vec *axis, register f32 rad) { - register f32 tT, fc0; register f32 tmp0, tmp1, tmp2, tmp3, tmp4; register f32 tmp5, tmp6, tmp7, tmp8, tmp9; - tmp9 = 0.5F; - tmp8 = 3.0F; - // clang-format off - asm - { - frsp cT, cT - psq_l tmp0, 0(axis), 0, 0 - frsp sT, sT - lfs tmp1, 8(axis) - ps_mul tmp2, tmp0, tmp0 - fadds tmp7, tmp9, tmp9 - ps_madd tmp3, tmp1, tmp1, tmp2 - fsubs fc0, tmp9, tmp9 - ps_sum0 tmp4, tmp3, tmp1, tmp2 - fsubs tT, tmp7, cT - frsqrte tmp5, tmp4 - fmuls tmp2, tmp5, tmp5 - fmuls tmp3, tmp5, tmp9 - fnmsubs tmp2, tmp2, tmp4, tmp8 - fmuls tmp5, tmp2, tmp3 - ps_merge00 cT, cT, cT - ps_muls0 tmp0, tmp0, tmp5 - ps_muls0 tmp1, tmp1, tmp5 - ps_muls0 tmp4, tmp0, tT - ps_muls0 tmp9, tmp0, sT - ps_muls0 tmp5, tmp1, tT - ps_muls1 tmp3, tmp4, tmp0 - ps_muls0 tmp2, tmp4, tmp0 - ps_muls0 tmp4, tmp4, tmp1 - fnmsubs tmp6, tmp1, sT, tmp3 - fmadds tmp7, tmp1, sT, tmp3 - ps_neg tmp0, tmp9 - ps_sum0 tmp8, tmp4, fc0, tmp9 - ps_sum0 tmp2, tmp2, tmp6, cT - ps_sum1 tmp3, cT, tmp7, tmp3 - ps_sum0 tmp6, tmp0, fc0 ,tmp4 - psq_st tmp8, 8(m), 0, 0 - ps_sum0 tmp0, tmp4, tmp4, tmp0 - psq_st tmp2, 0(m), 0, 0 - ps_muls0 tmp5, tmp5, tmp1 - psq_st tmp3, 16(m), 0, 0 - ps_sum1 tmp4, tmp9, tmp0, tmp4 - psq_st tmp6, 24(m), 0, 0 - ps_sum0 tmp5, tmp5, fc0, cT - psq_st tmp4, 32(m), 0, 0 - psq_st tmp5, 40(m), 0, 0 + register f32 sT; + register f32 cT; + register f32 oneMinusCosT; + register f32 zero; + Vec axisNormalized; + register Vec *axisNormalizedPtr; + + zero = 0.0f; + axisNormalizedPtr = &axisNormalized; + sT = sinf(rad); + cT = cosf(rad); + oneMinusCosT = 1.0f - cT; + + PSVECNormalize(axis, axisNormalizedPtr); + +#ifdef __MWERKS__ // clang-format off + asm { + psq_l rad, 0x0(axisNormalizedPtr), 0, qr0 + lfs tmp1, 0x8(axisNormalizedPtr) + ps_merge00 tmp0, cT, cT + ps_muls0 tmp4, rad, oneMinusCosT + ps_muls0 tmp5, tmp1, oneMinusCosT + ps_muls1 tmp3, tmp4, rad + ps_muls0 tmp2, tmp4, rad + ps_muls0 rad, rad, sT + ps_muls0 tmp4, tmp4, tmp1 + fnmsubs tmp6, tmp1, sT, tmp3 + fmadds tmp7, tmp1, sT, tmp3 + ps_neg tmp9, rad + ps_sum0 tmp8, tmp4, zero, rad + ps_sum0 tmp2, tmp2, tmp6, tmp0 + ps_sum1 tmp3, tmp0, tmp7, tmp3 + ps_sum0 tmp6, tmp9, zero, tmp4 + ps_sum0 tmp9, tmp4, tmp4, tmp9 + psq_st tmp8, 0x8(m), 0, qr0 + ps_muls0 tmp5, tmp5, tmp1 + psq_st tmp2, 0x0(m), 0, qr0 + ps_sum1 tmp4, rad, tmp9, tmp4 + psq_st tmp3, 0x10(m), 0, qr0 + ps_sum0 tmp5, tmp5, zero, tmp0 + psq_st tmp6, 0x18(m), 0, qr0 + psq_st tmp4, 0x20(m), 0, qr0 + psq_st tmp5, 0x28(m), 0, qr0 } - // clang-format on -} - -void PSMTXRotAxisRad(Mtx m, const Vec *axis, f32 rad) -{ - // f32 sinT, cosT; - - // sinT = sinf(rad); - // cosT = cosf(rad); - - // __PSMTXRotAxisRadInternal(m, axis, sinT, cosT); +#endif // clang-format on } #endif @@ -1219,30 +1204,30 @@ void PSMTXReflect(register Mtx m, const register Vec *p, const register Vec *n) void C_MTXLookAt(Mtx m, const Point3d *camPos, const Vec *camUp, const Point3d *target) { - // Vec vLook, vRight, vUp; + Vec vLook, vRight, vUp; - // vLook.x = camPos->x - target->x; - // vLook.y = camPos->y - target->y; - // vLook.z = camPos->z - target->z; - // VECNormalize(&vLook, &vLook); - // VECCrossProduct(camUp, &vLook, &vRight); - // VECNormalize(&vRight, &vRight); - // VECCrossProduct(&vLook, &vRight, &vUp); + vLook.x = camPos->x - target->x; + vLook.y = camPos->y - target->y; + vLook.z = camPos->z - target->z; + VECNormalize(&vLook, &vLook); + VECCrossProduct(camUp, &vLook, &vRight); + VECNormalize(&vRight, &vRight); + VECCrossProduct(&vLook, &vRight, &vUp); - // m[0][0] = vRight.x; - // m[0][1] = vRight.y; - // m[0][2] = vRight.z; - // m[0][3] = -(camPos->x * vRight.x + camPos->y * vRight.y + camPos->z * vRight.z); + m[0][0] = vRight.x; + m[0][1] = vRight.y; + m[0][2] = vRight.z; + m[0][3] = -(camPos->x * vRight.x + camPos->y * vRight.y + camPos->z * vRight.z); - // m[1][0] = vUp.x; - // m[1][1] = vUp.y; - // m[1][2] = vUp.z; - // m[1][3] = -(camPos->x * vUp.x + camPos->y * vUp.y + camPos->z * vUp.z); + m[1][0] = vUp.x; + m[1][1] = vUp.y; + m[1][2] = vUp.z; + m[1][3] = -(camPos->x * vUp.x + camPos->y * vUp.y + camPos->z * vUp.z); - // m[2][0] = vLook.x; - // m[2][1] = vLook.y; - // m[2][2] = vLook.z; - // m[2][3] = -(camPos->x * vLook.x + camPos->y * vLook.y + camPos->z * vLook.z); + m[2][0] = vLook.x; + m[2][1] = vLook.y; + m[2][2] = vLook.z; + m[2][3] = -(camPos->x * vLook.x + camPos->y * vLook.y + camPos->z * vLook.z); } void C_MTXLightFrustum(Mtx m, float t, float b, float l, float r, float n, float scaleS, float scaleT, float transS, float transT) @@ -1269,28 +1254,28 @@ void C_MTXLightFrustum(Mtx m, float t, float b, float l, float r, float n, float void C_MTXLightPerspective(Mtx m, f32 fovY, f32 aspect, float scaleS, float scaleT, float transS, float transT) { - // f32 angle; - // f32 cot; + f32 angle; + f32 cot; - // angle = fovY * 0.5f; - // angle = MTXDegToRad(angle); + angle = fovY * 0.5f; + angle = MTXDegToRad(angle); - // cot = 1.0f / tanf(angle); + cot = 1.0f / tanf(angle); - // m[0][0] = (cot / aspect) * scaleS; - // m[0][1] = 0.0f; - // m[0][2] = -transS; - // m[0][3] = 0.0f; + m[0][0] = (cot / aspect) * scaleS; + m[0][1] = 0.0f; + m[0][2] = -transS; + m[0][3] = 0.0f; - // m[1][0] = 0.0f; - // m[1][1] = cot * scaleT; - // m[1][2] = -transT; - // m[1][3] = 0.0f; + m[1][0] = 0.0f; + m[1][1] = cot * scaleT; + m[1][2] = -transT; + m[1][3] = 0.0f; - // m[2][0] = 0.0f; - // m[2][1] = 0.0f; - // m[2][2] = -1.0f; - // m[2][3] = 0.0f; + m[2][0] = 0.0f; + m[2][1] = 0.0f; + m[2][2] = -1.0f; + m[2][3] = 0.0f; } void C_MTXLightOrtho(Mtx m, f32 t, f32 b, f32 l, f32 r, float scaleS, float scaleT, float transS, float transT) diff --git a/src/dolphin/mtx/quat.c b/src/dolphin/mtx/quat.c index 66964bf0..f3e15a87 100644 --- a/src/dolphin/mtx/quat.c +++ b/src/dolphin/mtx/quat.c @@ -1,3 +1,4 @@ +#include "dolphin/math.h" #include "dolphin/mtx.h" float acosf(float x); @@ -5,6 +6,28 @@ float acosf(float x); float sinf(float x); float cosf(float x); +void C_QUATAdd(const Quaternion *p, const Quaternion *q, Qtrn *r) +{ + r->x = p->x + q->x; + r->y = p->y + q->y; + r->z = p->z + q->z; + r->w = p->w + q->w; +} + +void PSQUATAdd(register const Quaternion *p, register const Quaternion *q, register Quaternion *r) +{ + asm { + psq_l f0, 0x0(r3), 0, 0 + psq_l f1, 0x0(r4), 0, 0 + ps_add f0, f0, f1 + psq_st f0, 0x0(r5), 0, 0 + psq_l f0, 0x8(r3), 0, 0 + psq_l f1, 0x8(r4), 0, 0 + ps_add f0, f0, f1 + psq_st f0, 0x8(r5), 0, 0 + } +} + void PSQUATMultiply(register const Quaternion *a, register const Quaternion *b, register Quaternion *ab) { asm { @@ -33,6 +56,77 @@ void PSQUATMultiply(register const Quaternion *a, register const Quaternion *b, } } +void PSQUATNormalize(const register Quaternion *src, register Quaternion *unit) +{ + // sdata2 + (void)0.00001f; + (void)0.0f; + (void)0.5; + (void)3.0; + (void)1.0f; + (void)0.5f; + (void)3.0f; + { + register f32 vv1, vv2, vv3; + register f32 vv4, vv5, vv6; + register f32 vv7, vv8; + register f32 vv9 = 0.00001f; + register f32 vvA = 0.5F; + register f32 vvB = 3.0F; + asm + { + psq_l vv1, 0(src), 0, 0; + ps_mul vv3, vv1, vv1; + psq_l vv2, 8(src), 0, 0; + ps_sub vv6, vv9, vv9; + ps_madd vv3, vv2, vv2, vv3; + ps_sum0 vv3, vv3, vv3, vv3; + frsqrte vv4, vv3; + ps_sub vv5, vv3, vv9; + fmul vv7, vv4, vv4; + fmul vv8, vv4, vvA; + fnmsub vv7, vv7, vv3, vvB; + fmul vv4, vv7, vv8; + ps_sel vv4, vv5, vv4, vv6; + ps_muls0 vv1, vv1, vv4; + ps_muls0 vv2, vv2, vv4; + psq_st vv1, 0(unit), 0, 0; + psq_st vv2, 8(unit), 0, 0; + } + } +} + +void PSQUATInverse(const register Quaternion *src, register Quaternion *inv) +{ + register f32 vv1, vv2, vv3, vv4; + register f32 vv5, vv6, vv7, vv8, vv9, vvA, vvB; + register f32 vvC = 1.0F; + asm { + psq_l vv1, 0(src), 0, 0; + ps_mul vv5, vv1, vv1; + psq_l vv2, 8(src), 0, 0; + ps_madd vv5, vv2, vv2, vv5; + ps_add vvA, vvC, vvC; + ps_sum0 vv5, vv5, vv5, vv5; + fres vv7, vv5; + ps_neg vv6, vv5; + ps_nmsub vv9, vv5, vv7, vvA; + ps_mul vv7, vv7, vv9; + ps_sel vv7, vv6, vvC, vv7 + b loc1; + loc0: + fmr vv7, vvC; + loc1: + ps_neg vv8, vv7; + ps_muls1 vv4, vv7, vv2; + ps_muls0 vv1, vv1, vv8; + psq_st vv4, 12(inv), 1, 0; + ps_muls0 vv3, vv2, vv8; + psq_st vv1, 0(inv), 0, 0; + psq_st vv3, 8(inv), 1, 0; + } +} + void C_QUATRotAxisRad(Quaternion *q, const Vec *axis, f32 rad) { f32 tmp, tmp2, tmp3; @@ -52,6 +146,42 @@ void C_QUATRotAxisRad(Quaternion *q, const Vec *axis, f32 rad) q->w = tmp3; } +void C_QUATMtx(Quaternion *r, const Mtx m) +{ + f32 vv0, vv1; + s32 i, j, k; + s32 idx[3] = { 1, 2, 0 }; + f32 vec[3]; + vv0 = m[0][0] + m[1][1] + m[2][2]; + if (vv0 > 0.0f) { + vv1 = (f32)sqrtf(vv0 + 1.0f); + r->w = vv1 * 0.5f; + vv1 = 0.5f / vv1; + r->x = (m[2][1] - m[1][2]) * vv1; + r->y = (m[0][2] - m[2][0]) * vv1; + r->z = (m[1][0] - m[0][1]) * vv1; + } + else { + i = 0; + if (m[1][1] > m[0][0]) + i = 1; + if (m[2][2] > m[i][i]) + i = 2; + j = idx[i]; + k = idx[j]; + vv1 = (f32)sqrtf((m[i][i] - (m[j][j] + m[k][k])) + 1.0f); + vec[i] = vv1 * 0.5f; + if (vv1 != 0.0f) + vv1 = 0.5f / vv1; + r->w = (m[k][j] - m[j][k]) * vv1; + vec[j] = (m[i][j] + m[j][i]) * vv1; + vec[k] = (m[i][k] + m[k][i]) * vv1; + r->x = vec[0]; + r->y = vec[1]; + r->z = vec[2]; + } +} + void C_QUATSlerp(const Quaternion *p, const Quaternion *q, Quaternion *r, f32 t) { f32 ratioA, ratioB; diff --git a/src/dolphin/mtx/vec.c b/src/dolphin/mtx/vec.c index 985a6196..de5ad450 100644 --- a/src/dolphin/mtx/vec.c +++ b/src/dolphin/mtx/vec.c @@ -210,23 +210,23 @@ void C_VECHalfAngle(const Vec *a, const Vec *b, Vec *half) void C_VECReflect(const Vec *src, const Vec *normal, Vec *dst) { - // Vec a0; - // Vec b0; - // f32 dot; + Vec a0; + Vec b0; + f32 dot; - // a0.x = -src->x; - // a0.y = -src->y; - // a0.z = -src->z; + a0.x = -src->x; + a0.y = -src->y; + a0.z = -src->z; - // VECNormalize(&a0, &a0); - // VECNormalize(normal, &b0); + 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; + 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); + VECNormalize(dst, dst); } asm f32 PSVECSquareDistance(register const Vec *a, register const Vec *b) { diff --git a/src/dolphin/pad/Padclamp.c b/src/dolphin/pad/Padclamp.c index a96d75c4..bdc9af39 100644 --- a/src/dolphin/pad/Padclamp.c +++ b/src/dolphin/pad/Padclamp.c @@ -103,17 +103,34 @@ static void ClampTrigger(u8 *trigger, u8 min, u8 max) } } -void PADClamp(PADStatus *status) +void PADClamp(PADStatus* status) { - // int i; - // for (i = 0; i < PAD_CHANMAX; i++, status++) { - // if (status->err != PAD_ERR_NONE) { - // continue; - // } + int i; + for (i = 0; i < PAD_CHANMAX; i++, status++) { + if (status->err != PAD_ERR_NONE) { + continue; + } - // ClampStick(&status->stickX, &status->stickY, ClampRegion.maxStick, ClampRegion.xyStick, ClampRegion.minStick); - // ClampStick(&status->substickX, &status->substickY, ClampRegion.maxSubstick, ClampRegion.xySubstick, ClampRegion.minSubstick); - // ClampTrigger(&status->triggerL, ClampRegion.minTrigger, ClampRegion.maxTrigger); - // ClampTrigger(&status->triggerR, ClampRegion.minTrigger, ClampRegion.maxTrigger); - // } + ClampStick(&status->stickX, &status->stickY, ClampRegion.maxStick, + ClampRegion.xyStick, ClampRegion.minStick); + ClampStick(&status->substickX, &status->substickY, + ClampRegion.maxSubstick, ClampRegion.xySubstick, + ClampRegion.minSubstick); + if (status->triggerL <= ClampRegion.minTrigger) { + status->triggerL = 0; + } else { + if (ClampRegion.maxTrigger < status->triggerL) { + status->triggerL = ClampRegion.maxTrigger; + } + status->triggerL -= ClampRegion.minTrigger; + } + if (status->triggerR <= ClampRegion.minTrigger) { + status->triggerR = 0; + } else { + if (ClampRegion.maxTrigger < status->triggerR) { + status->triggerR = ClampRegion.maxTrigger; + } + status->triggerR -= ClampRegion.minTrigger; + } + } }