diff --git a/include/ctr/coll.h b/include/ctr/coll.h new file mode 100644 index 000000000..eb2af865e --- /dev/null +++ b/include/ctr/coll.h @@ -0,0 +1,5 @@ +#pragma once + +#include + +void COLL_ProjectPointToEdge(SVec3* out, const SVec3* v1, const SVec3* v2, const SVec3* point); \ No newline at end of file diff --git a/include/ctr/gte.h b/include/ctr/gte.h index 7ca41078e..0797d71e8 100644 --- a/include/ctr/gte.h +++ b/include/ctr/gte.h @@ -23,18 +23,32 @@ typedef enum GTE_VECTOR GTE_VECTOR_0, /* SVec */ GTE_VECTOR_1, /* SVec */ GTE_VECTOR_2, /* SVec */ - GTE_VECTOR_IR, /* Vec */ + GTE_VECTOR_IR, /* SVec */ + GTE_VECTOR_MAC, /* Vec */ } GTE_VECTOR; +typedef enum GTE_IR +{ + GTE_IR_0, /* s16 */ + GTE_IR_1, /* s16 */ + GTE_IR_2, /* s16 */ + GTE_IR_3, /* s16 */ +} GTE_IR; + typedef enum GTE_MAC { - GTE_MAC_0, - GTE_MAC_1, - GTE_MAC_2, - GTE_MAC_3, - GTE_MAC_VEC, /* 1-3, Vec */ + GTE_MAC_0, /* s32 */ + GTE_MAC_1, /* s32 */ + GTE_MAC_2, /* s32 */ + GTE_MAC_3, /* s32 */ } GTE_MAC; +typedef enum GTE_INTERPOLATE +{ + GTE_INTERPOLATE_INT, + GTE_INTERPOLATE_FLOATING_POINT, +} GTE_INTERPOLATE; + /* HELPERS */ #define _CAT(a, b) a##b #define CAT(a, b) _CAT(a, b) @@ -44,23 +58,46 @@ typedef enum GTE_MAC #define _gte_loadSVecMatrix_GTE_MATRIX_ROT_GTE_ROW_INDEX_0(v) gte_ldsvrtrow0(v) #define _gte_loadSVecMatrix_GTE_MATRIX_LIGHT_GTE_ROW_INDEX_0(v) gte_ldsvllrow0(v) #define _gte_loadSVecMatrix_GTE_MATRIX_COLOR_GTE_ROW_INDEX_0(v) gte_ldsvlcrow0(v) -#define _gte_loadVec_GTE_VECTOR_0(v) gte_ldv0(v) -#define _gte_loadVec_GTE_VECTOR_1(v) gte_ldv1(v) -#define _gte_loadVec_GTE_VECTOR_2(v) gte_ldv2(v) +#define _gte_loadSVec_GTE_VECTOR_0(v) gte_ldv0(v) +#define _gte_loadSVec_GTE_VECTOR_1(v) gte_ldv1(v) +#define _gte_loadSVec_GTE_VECTOR_2(v) gte_ldv2(v) +#define _gte_loadSVec_GTE_VECTOR_IR(v) gte_ldsv(v) #define _gte_loadVec_GTE_VECTOR_IR(v) gte_ldlvl(v) +#define _gte_loadVec_GTE_VECTOR_MAC(v) __asm__ volatile ( \ + "lwc2 $25, 0( %0 );" \ + "lwc2 $26, 4( %0 );" \ + "lwc2 $27, 8( %0 );" \ + : \ + : "r"( v ) ) #define _gte_readMac_GTE_MAC_0(out) gte_stopz(out) #define _gte_readMac_GTE_MAC_1(out) gte_stlvnl0(out) #define _gte_readMac_GTE_MAC_2(out) gte_stlvnl1(out) #define _gte_readMac_GTE_MAC_3(out) gte_stlvnl2(out) -#define _gte_readMac_GTE_MAC_VEC(out) gte_stlvnl(out) +#define _gte_readMac_GTE_VECTOR_MAC(out) gte_stlvnl(out) +#define _gte_loadIR_GTE_IR_0(in) gte_lddp(in) #define _gte_dotProduct_GTE_ROW_INDEX_0(out, matrixType, vecType) gte_mvmva(0, matrixType, vecType, 3, 0); _gte_readMac_GTE_MAC_1(out) #define _gte_dotProduct_GTE_ROW_INDEX_1(out, matrixType, vecType) gte_mvmva(0, matrixType, vecType, 3, 0); _gte_readMac_GTE_MAC_2(out) #define _gte_dotProduct_GTE_ROW_INDEX_2(out, matrixType, vecType) gte_mvmva(0, matrixType, vecType, 3, 0); _gte_readMac_GTE_MAC_3(out) -#define _gte_mulMatrixVec(out, matrixType, vecType, shift) gte_mvmva(shift, matrixType, vecType, 3, 0); _gte_readMac_GTE_MAC_VEC(out) +#define _gte_mulMatrixVec(out, matrixType, vecType, shift) gte_mvmva(shift, matrixType, vecType, 3, 0); _gte_readMac_GTE_VECTOR_MAC(out) +#define _gte_interpolate_GTE_INTERPOLATE_INT() gte_gpl0() +#define _gte_interpolate_GTE_INTERPOLATE_FLOATING_POINT() gte_gpl12() +#define _gte_leadingZeroes(out, in) __asm__ volatile ( \ + "mtc2 %1, $30;" \ + "nop;" \ + "nop;" \ + "swc2 $31, 0( %0 );" \ + : \ + : "r"( out ), "r"( in ) \ + : "memory" \ + ) /* API */ #define gte_readMac(out, macType) CAT(_gte_readMac_, macType)(out) +#define gte_loadIR(in, irType) CAT(_gte_loadIR_, irType)(in) +#define gte_loadSVec(v, vecType) CAT(_gte_loadSVec_, vecType)(v) #define gte_loadVec(v, vecType) CAT(_gte_loadVec_, vecType)(v) #define gte_loadRowMatrix(v, rowIndex, matrixType) CAT3(_gte_loadSVecMatrix_, matrixType, _##rowIndex)(v) #define gte_mulMatrixVec(out, matrixType, vecType) _gte_mulMatrixVec(out, matrixType, vecType, 1) -#define gte_dotProduct(out, rowIndex, matrixType, vecType) CAT(_gte_dotProduct_, rowIndex)(out, matrixType, vecType) \ No newline at end of file +#define gte_dotProduct(out, rowIndex, matrixType, vecType) CAT(_gte_dotProduct_, rowIndex)(out, matrixType, vecType) +#define gte_leadingZeroes(out, in) _gte_leadingZeroes(out, in) +#define gte_interpolate(out, interpolationType) CAT(_gte_interpolate_, interpolationType)(); _gte_readMac_GTE_VECTOR_MAC(out) \ No newline at end of file diff --git a/include/ctr/nd.h b/include/ctr/nd.h index 37bafed0d..8fa28ac9b 100644 --- a/include/ctr/nd.h +++ b/include/ctr/nd.h @@ -23,4 +23,7 @@ void ND_MATH_MatrixMultiplication(Matrix* out, const Matrix* m, const Matrix* n) u32 ND_RNG_Rand(); s32 ND_RNG_RandInt(u32 n); u16 ND_RNG_PseudoRand(u16 n); -u32 ND_RNG_Random(RNGSeed* seed); \ No newline at end of file +u32 ND_RNG_Random(RNGSeed* seed); + +/* COLL */ +void ND_COLL_ProjectPointToEdge(SVec3* out, const SVec3* v1, const SVec3* v2, const SVec3* point); \ No newline at end of file diff --git a/include/ctr/test.h b/include/ctr/test.h index fea8b193b..23e5702c1 100644 --- a/include/ctr/test.h +++ b/include/ctr/test.h @@ -6,6 +6,7 @@ #include #include #include +#include void LoadTestPatches(); u32 PatchFunction_Beg(u32* index); @@ -17,6 +18,7 @@ u32 PrintMatrixDiff(const char* name, const Matrix* expected, const Matrix* ret, #define TEST_MATH_IMPL #define TEST_RNG_IMPL +#define TEST_COLL_IMPL #ifdef TEST_MATH_IMPL void TEST_MATH_Sin(u32 angle, s32 ret); @@ -52,4 +54,10 @@ u32 PrintMatrixDiff(const char* name, const Matrix* expected, const Matrix* ret, #define TEST_RNG_RandInt(n, ret) #define TEST_RNG_PseudoRand(n, ret) #define TEST_RNG_Random(seed, ret) +#endif + +#ifdef TEST_COLL_IMPL + void TEST_COLL_ProjectPointToEdge(const SVec3* v1, const SVec3* v2, const SVec3* point, const SVec3* ret); +#else + #define TEST_COLL_ProjectPointToEdge(out, v1, v2, point) #endif \ No newline at end of file diff --git a/rewrite/src/exe/coll.c b/rewrite/src/exe/coll.c new file mode 100644 index 000000000..b46ff8702 --- /dev/null +++ b/rewrite/src/exe/coll.c @@ -0,0 +1,39 @@ +#include +#include +#include + +/* Address: 0x8001ede4 */ +void COLL_ProjectPointToEdge(SVec3* out, const SVec3* v1, const SVec3* v2, const SVec3* point) +{ + const SVec3 edge = { v2->x - v1->x, v2->y - v1->y, v2->z - v1->z }; + const Matrix m = + { + .m[0][0] = edge.x, .m[0][1] = edge.y, .m[0][2] = edge.z, + .m[1][0] = point->x - v1->x, .m[1][1] = point->y - v1->y, .m[1][2] = point->z - v1->z, + }; + gte_SetRotMatrix(m.m); + gte_loadSVec(edge.v, GTE_VECTOR_0); + s32 edgeDot, pointDot; + gte_dotProduct(&edgeDot, GTE_ROW_INDEX_0, GTE_MATRIX_ROT, GTE_VECTOR_0); + gte_readMac(&pointDot, GTE_MAC_2); + s32 leadingZeroes; + gte_leadingZeroes(&leadingZeroes, pointDot); + + const s32 shift = clamp(leadingZeroes - 2, 0, FRACTIONAL_BITS); + if (shift < FRACTIONAL_BITS) { edgeDot >>= (FRACTIONAL_BITS - shift); } + + s32 barycentricFactor = 0; + if (edgeDot != 0) { barycentricFactor = clamp((pointDot << shift) / edgeDot, 0, FP_ONE); } + + const Vec3 V1 = { .x = v1->x, .y = v1->y, .z = v1->z }; + gte_loadIR(barycentricFactor, GTE_IR_0); + gte_loadSVec(edge.v, GTE_VECTOR_IR); + gte_loadVec(V1.v, GTE_VECTOR_MAC); + + Vec3 coords; + gte_interpolate(coords.v, GTE_INTERPOLATE_FLOATING_POINT); + out->x = coords.x; + out->y = coords.y; + out->z = coords.z; + TEST_COLL_ProjectPointToEdge(v1, v2, point, out); +} \ No newline at end of file diff --git a/rewrite/src/exe/math.c b/rewrite/src/exe/math.c index 7af0d019c..ea4387bc6 100644 --- a/rewrite/src/exe/math.c +++ b/rewrite/src/exe/math.c @@ -71,7 +71,7 @@ void MATH_GetInverseMatrixTransformation(Matrix* out, const Matrix* matrix) } const SVec3 t = { .x = (-matrix->t.x) & 0xFFFF, .y = (-matrix->t.y) & 0xFFFF, .z = (-matrix->t.z) & 0xFFFF }; gte_SetRotMatrix(out->m); - gte_loadVec(&t, GTE_VECTOR_0); + gte_loadSVec(&t, GTE_VECTOR_0); gte_mulMatrixVec(out->t.v, GTE_MATRIX_ROT, GTE_VECTOR_0); TEST_MATH_GetInverseMatrixTransformation(matrix, out); } @@ -80,7 +80,7 @@ void MATH_GetInverseMatrixTransformation(Matrix* out, const Matrix* matrix) s32 MATH_VectorLength(const SVec3* vector) { gte_loadRowMatrix(vector, GTE_ROW_INDEX_0, GTE_MATRIX_ROT); - gte_loadVec(vector, GTE_VECTOR_0); + gte_loadSVec(vector, GTE_VECTOR_0); s32 lengthSquared; gte_dotProduct(&lengthSquared, GTE_ROW_INDEX_0, GTE_MATRIX_ROT, GTE_VECTOR_0); const s32 len = ND_SquareRoot0_stub(lengthSquared); @@ -142,11 +142,11 @@ void MATH_MatrixMultiplication(Matrix* out, const Matrix* m, const Matrix* n) const SVec3 v1 = { .x = n->m[0][1], .y = n->m[1][1], .z = n->m[2][1] }; const SVec3 v2 = { .x = n->m[0][2], .y = n->m[1][2], .z = n->m[2][2] }; gte_SetRotMatrix(m->m); - gte_loadVec(&v0, GTE_VECTOR_0); + gte_loadSVec(&v0, GTE_VECTOR_0); gte_mulMatrixVec(res[0].v, GTE_MATRIX_ROT, GTE_VECTOR_0); - gte_loadVec(&v1, GTE_VECTOR_1); + gte_loadSVec(&v1, GTE_VECTOR_1); gte_mulMatrixVec(res[1].v, GTE_MATRIX_ROT, GTE_VECTOR_1); - gte_loadVec(&v2, GTE_VECTOR_2); + gte_loadSVec(&v2, GTE_VECTOR_2); gte_mulMatrixVec(res[2].v, GTE_MATRIX_ROT, GTE_VECTOR_2); out->m[0][0] = res[0].x; out->m[0][1] = res[1].x; out->m[0][2] = res[2].x; out->m[1][0] = res[0].y; out->m[1][1] = res[1].y; out->m[1][2] = res[2].y; diff --git a/rewrite/src/tests/test.c b/rewrite/src/tests/test.c index f4604cd66..90e0485fc 100644 --- a/rewrite/src/tests/test.c +++ b/rewrite/src/tests/test.c @@ -29,6 +29,7 @@ FunctionPatch s_functions[] = TEST_FUNC(RNG_RandInt), TEST_FUNC(RNG_PseudoRand), TEST_FUNC(RNG_Random), + TEST_FUNC(COLL_ProjectPointToEdge), }; void LoadTestPatches() diff --git a/rewrite/src/tests/test_coll.c b/rewrite/src/tests/test_coll.c new file mode 100644 index 000000000..761632a4a --- /dev/null +++ b/rewrite/src/tests/test_coll.c @@ -0,0 +1,14 @@ +#include + +#ifdef TEST_COLL_IMPL + +void TEST_COLL_ProjectPointToEdge(const SVec3* v1, const SVec3* v2, const SVec3* point, const SVec3* ret) +{ + const u32 index = PatchFunction_Beg((u32*)(&ND_COLL_ProjectPointToEdge)); + SVec3 expected; + ND_COLL_ProjectPointToEdge(&expected, v1, v2, point); + PrintSVectorDiff("COLL_ProjectPointToEdge", &expected, ret); + PatchFunction_End(index); +} + +#endif // TEST_COLL_IMPL \ No newline at end of file diff --git a/symbols/gcc-syms-rewrite.txt b/symbols/gcc-syms-rewrite.txt index 057b69b50..e3448bfad 100644 --- a/symbols/gcc-syms-rewrite.txt +++ b/symbols/gcc-syms-rewrite.txt @@ -61,7 +61,7 @@ ND_COLL_FIXED_BotsSearch = 0x8001d77c; ND_COLL_FIXED_PlayerSearch = 0x8001d944; ND_COLL_SearchBSP_CallbackQUADBLK = 0x8001eb0c; ND_COLL_SearchBSP_CallbackPARAM = 0x8001ebec; -ND_COLL_FIXED_TRIANGL_Barycentrics = 0x8001ede4; +ND_COLL_ProjectPointToEdge = 0x8001ede4; ND_COLL_FIXED_TRIANGL_TestPoint = 0x8001ef50; ND_COLL_FIXED_TRIANGL_GetNormVec = 0x8001f2dc; ND_COLL_FIXED_QUADBLK_TestTriangles = 0x8001f41c;