diff --git a/src/BRSRC13/CORE/MATH/matrix23.c b/src/BRSRC13/CORE/MATH/matrix23.c index 52e52d51..904583e9 100644 --- a/src/BRSRC13/CORE/MATH/matrix23.c +++ b/src/BRSRC13/CORE/MATH/matrix23.c @@ -1,29 +1,52 @@ #include "matrix23.h" #include "harness/trace.h" +#include + +#define A(x, y) A->m[x][y] +#define B(x, y) B->m[x][y] +#define C(x, y) C->m[x][y] +#define M(x, y) mat->m[x][y] + +#define BR_MAC2(a, b, c, d) ((a) * (b) + (c) * (d)) + br_matrix23 mattmp1_23; br_matrix23 mattmp2_23; // IDA: void __cdecl BrMatrix23Copy(br_matrix23 *A, br_matrix23 *B) void BrMatrix23Copy(br_matrix23* A, br_matrix23* B) { LOG_TRACE("(%p, %p)", A, B); - NOT_IMPLEMENTED(); + + A(0, 0) = B(0, 0); + A(0, 1) = B(0, 1); + A(1, 0) = B(1, 0); + A(1, 1) = B(1, 1); + A(2, 0) = B(2, 0); + A(2, 1) = B(2, 1); } // IDA: void __cdecl BrMatrix23Mul(br_matrix23 *A, br_matrix23 *B, br_matrix23 *C) void BrMatrix23Mul(br_matrix23* A, br_matrix23* B, br_matrix23* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A(0, 0) = BR_MAC2(B(0, 0), C(0, 0), B(0, 1), C(1, 0)); + A(0, 1) = BR_MAC2(B(0, 0), C(0, 1), B(0, 1), C(1, 1)); + A(1, 0) = BR_MAC2(B(1, 0), C(0, 0), B(1, 1), C(1, 0)); + A(1, 1) = BR_MAC2(B(1, 0), C(0, 1), B(1, 1), C(1, 1)); + A(2, 0) = BR_MAC2(B(2, 0), C(0, 0), B(2, 1), C(1, 0)) + C(2, 0); + A(2, 1) = BR_MAC2(B(2, 0), C(0, 1), B(2, 1), C(1, 1)) + C(2, 1); } // IDA: void __cdecl BrMatrix23Identity(br_matrix23 *mat) void BrMatrix23Identity(br_matrix23* mat) { - mat->m[0][0] = 1.0; - mat->m[0][1] = 0.0; - mat->m[1][0] = 0.0; - mat->m[1][1] = 1.0; - mat->m[2][0] = 0.0; - mat->m[2][1] = 0.0; + LOG_TRACE("(%p)", mat); + + M(0, 0) = 1.f; + M(0, 1) = 0.f; + M(1, 0) = 0.f; + M(1, 1) = 1.f; + M(2, 0) = 0.f; + M(2, 1) = 0.f; } // IDA: void __cdecl BrMatrix23Rotate(br_matrix23 *mat, br_angle rz) @@ -31,31 +54,64 @@ void BrMatrix23Rotate(br_matrix23* mat, br_angle rz) { br_scalar s; br_scalar c; LOG_TRACE("(%p, %d)", mat, rz); - NOT_IMPLEMENTED(); + + c = BR_COS(rz); + s = BR_SIN(rz); + + M(0, 0) = c; + M(0, 1) = s; + M(1, 0) = -s; + M(1, 1) = c; + M(2, 0) = BR_SCALAR(0.f); + M(2, 1) = BR_SCALAR(0.f); } // IDA: void __cdecl BrMatrix23Translate(br_matrix23 *mat, br_scalar dx, br_scalar dy) void BrMatrix23Translate(br_matrix23* mat, br_scalar dx, br_scalar dy) { LOG_TRACE("(%p, %f, %f)", mat, dx, dy); - NOT_IMPLEMENTED(); + + M(0, 0) = BR_SCALAR(1.f); + M(0, 1) = BR_SCALAR(0.f); + M(1, 0) = BR_SCALAR(0.f); + M(1, 1) = BR_SCALAR(1.f); + M(2, 0) = dx; + M(2, 1) = dy; } // IDA: void __cdecl BrMatrix23Scale(br_matrix23 *mat, br_scalar sx, br_scalar sy) void BrMatrix23Scale(br_matrix23* mat, br_scalar sx, br_scalar sy) { LOG_TRACE("(%p, %f, %f)", mat, sx, sy); - NOT_IMPLEMENTED(); + + M(0, 0) = sx; + M(0, 1) = BR_SCALAR(0.f); + M(1, 0) = BR_SCALAR(0.f); + M(1, 1) = sy; + M(2, 0) = BR_SCALAR(0.f); + M(2, 1) = BR_SCALAR(0.f); } // IDA: void __cdecl BrMatrix23ShearX(br_matrix23 *mat, br_scalar sy) void BrMatrix23ShearX(br_matrix23* mat, br_scalar sy) { LOG_TRACE("(%p, %f)", mat, sy); - NOT_IMPLEMENTED(); + + M(0, 0) = BR_SCALAR(1.f); + M(0, 1) = sy; + M(1, 0) = BR_SCALAR(0.f); + M(1, 1) = BR_SCALAR(1.f); + M(2, 0) = BR_SCALAR(0.f); + M(2, 1) = BR_SCALAR(0.f); } // IDA: void __cdecl BrMatrix23ShearY(br_matrix23 *mat, br_scalar sx) void BrMatrix23ShearY(br_matrix23* mat, br_scalar sx) { LOG_TRACE("(%p, %f)", mat, sx); - NOT_IMPLEMENTED(); + + M(0, 0) = BR_SCALAR(1.f); + M(0, 1) = BR_SCALAR(0.f); + M(1, 0) = sx; + M(1, 1) = BR_SCALAR(1.f); + M(2, 0) = BR_SCALAR(0.f); + M(2, 1) = BR_SCALAR(0.f); } // IDA: br_scalar __cdecl BrMatrix23Inverse(br_matrix23 *B, br_matrix23 *A) @@ -65,113 +121,199 @@ br_scalar BrMatrix23Inverse(br_matrix23* B, br_matrix23* A) { br_scalar pos; br_scalar neg; LOG_TRACE("(%p, %p)", B, A); - NOT_IMPLEMENTED(); + + idet = A(0, 0) * A(1, 1); + if (idet < BR_SCALAR(0.f)) { + pos = BR_SCALAR(0.f); + neg = idet; + } else { + pos = idet; + neg = BR_SCALAR(0.f); + } + idet = -A(0, 1) * A(1, 0); + if (idet < BR_SCALAR(0.f)) { + neg = neg + idet; + } else { + pos = pos + idet; + } + det = pos + neg; + + if (fabs(det) < 2.384186e-07f) { + return BR_SCALAR(0.f); + } + idet = 1 / det; + B(0, 0) = A(1, 1) * idet; + B(0, 1) = -A(0, 1) * idet; + B(1, 0) = -A(1, 0) * idet; + B(1, 1) = A(0, 0) * idet; + B(2, 0) = BR_MAC2(A(1, 0), A(2, 1), -A(2, 0), A(1, 1)) * idet; + B(2, 1) = BR_MAC2(-A(2, 1), A(0, 0), A(0, 1), A(2, 0)) * idet; + return det; } // IDA: void __cdecl BrMatrix23LPInverse(br_matrix23 *B, br_matrix23 *A) void BrMatrix23LPInverse(br_matrix23* B, br_matrix23* A) { LOG_TRACE("(%p, %p)", B, A); - NOT_IMPLEMENTED(); + + B(0, 0) = A(1, 1); + B(0, 1) = -A(0, 1); + B(1, 0) = -A(1, 0); + B(1, 1) = A(0, 0); + B(2, 0) = BR_MAC2(A(1, 0), A(2, 1), -A(2, 0), A(1, 1)); + B(2, 1) = BR_MAC2(-A(2, 1), A(0, 0), A(0, 1), A(2, 0)); } // IDA: void __cdecl BrMatrix23LPNormalise(br_matrix23 *A, br_matrix23 *B) void BrMatrix23LPNormalise(br_matrix23* A, br_matrix23* B) { LOG_TRACE("(%p, %p)", A, B); - NOT_IMPLEMENTED(); + + br_scalar norm = sqrtf(BR_MAC2(B(1, 0), B(1, 0), B(1, 1), B(1,1))); + if (norm < 2.384186e-07f) { + A(1, 0) = BR_SCALAR(1.f); + A(1, 1) = BR_SCALAR(0.f); + } else { + A(1, 0) = B(1, 0) / norm; + A(1, 1) = B(1, 1) / norm; + } + A(0, 0) = A(1, 1); + A(0, 1) = -A(1, 0); + A(2, 0) = B(2, 0); + A(2, 1) = B(2, 1); } // IDA: void __cdecl BrMatrix23ApplyP(br_vector2 *A, br_vector2 *B, br_matrix23 *C) void BrMatrix23ApplyP(br_vector2* A, br_vector2* B, br_matrix23* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC2(B->v[0], C(0, 0), B->v[1], C(1, 0)) + C(2, 0); + A->v[1] = BR_MAC2(B->v[0], C(0, 1), B->v[1], C(1, 1)) + C(2, 1); } // IDA: void __cdecl BrMatrix23ApplyV(br_vector2 *A, br_vector2 *B, br_matrix23 *C) void BrMatrix23ApplyV(br_vector2* A, br_vector2* B, br_matrix23* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC2(B->v[0], C(0, 0), B->v[1], C(1, 0)); + A->v[1] = BR_MAC2(B->v[0], C(0, 1), B->v[1], C(1, 1)); } // IDA: void __cdecl BrMatrix23TApplyP(br_vector2 *A, br_vector2 *B, br_matrix23 *C) void BrMatrix23TApplyP(br_vector2* A, br_vector2* B, br_matrix23* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC2(B->v[0], C(0, 0), B->v[1], C(0, 1)); + A->v[1] = BR_MAC2(B->v[0], C(1, 0), B->v[1], C(1, 1)); } // IDA: void __cdecl BrMatrix23TApplyV(br_vector2 *A, br_vector2 *B, br_matrix23 *C) void BrMatrix23TApplyV(br_vector2* A, br_vector2* B, br_matrix23* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC2(B->v[0], C(0, 0), B->v[1], C(0, 1)); + A->v[1] = BR_MAC2(B->v[0], C(1, 0), B->v[1], C(1, 1)); } // IDA: void __cdecl BrMatrix23Pre(br_matrix23 *mat, br_matrix23 *A) void BrMatrix23Pre(br_matrix23* mat, br_matrix23* A) { LOG_TRACE("(%p, %p)", mat, A); - NOT_IMPLEMENTED(); + + BrMatrix23Mul(&mattmp1_23, A, mat); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23Post(br_matrix23 *mat, br_matrix23 *A) void BrMatrix23Post(br_matrix23* mat, br_matrix23* A) { LOG_TRACE("(%p, %p)", mat, A); - NOT_IMPLEMENTED(); + + BrMatrix23Mul(&mattmp1_23, mat, A); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23PreRotate(br_matrix23 *mat, br_angle rz) void BrMatrix23PreRotate(br_matrix23* mat, br_angle rz) { LOG_TRACE("(%p, %d)", mat, rz); - NOT_IMPLEMENTED(); + + BrMatrix23Rotate(&mattmp2_23, rz); + BrMatrix23Mul(&mattmp1_23, &mattmp2_23, mat); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23PostRotate(br_matrix23 *mat, br_angle rz) void BrMatrix23PostRotate(br_matrix23* mat, br_angle rz) { LOG_TRACE("(%p, %d)", mat, rz); - NOT_IMPLEMENTED(); + + BrMatrix23Rotate(&mattmp2_23, rz); + BrMatrix23Mul(&mattmp1_23, mat, &mattmp2_23); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23PreTranslate(br_matrix23 *mat, br_scalar x, br_scalar y) void BrMatrix23PreTranslate(br_matrix23* mat, br_scalar x, br_scalar y) { LOG_TRACE("(%p, %f, %f)", mat, x, y); - NOT_IMPLEMENTED(); + + BrMatrix23Translate(&mattmp2_23, x, y); + BrMatrix23Mul(&mattmp1_23, &mattmp2_23, mat); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23PostTranslate(br_matrix23 *A, br_scalar x, br_scalar y) void BrMatrix23PostTranslate(br_matrix23* A, br_scalar x, br_scalar y) { LOG_TRACE("(%p, %f, %f)", A, x, y); - NOT_IMPLEMENTED(); + + A(2, 0) += x; + A(2, 1) += y; } // IDA: void __cdecl BrMatrix23PreScale(br_matrix23 *mat, br_scalar sx, br_scalar sy) void BrMatrix23PreScale(br_matrix23* mat, br_scalar sx, br_scalar sy) { LOG_TRACE("(%p, %f, %f)", mat, sx, sy); - NOT_IMPLEMENTED(); + + BrMatrix23Scale(&mattmp2_23, sx, sy); + BrMatrix23Mul(&mattmp1_23, &mattmp2_23, mat); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23PostScale(br_matrix23 *mat, br_scalar sx, br_scalar sy) void BrMatrix23PostScale(br_matrix23* mat, br_scalar sx, br_scalar sy) { LOG_TRACE("(%p, %f, %f)", mat, sx, sy); - NOT_IMPLEMENTED(); + + BrMatrix23Scale(&mattmp2_23, sx, sy); + BrMatrix23Mul(&mattmp1_23, mat, &mattmp2_23); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23PreShearX(br_matrix23 *mat, br_scalar sy) void BrMatrix23PreShearX(br_matrix23* mat, br_scalar sy) { LOG_TRACE("(%p, %f)", mat, sy); - NOT_IMPLEMENTED(); + + BrMatrix23ShearX(&mattmp2_23, sy); + BrMatrix23Mul(&mattmp1_23, &mattmp2_23, mat); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23PostShearX(br_matrix23 *mat, br_scalar sy) void BrMatrix23PostShearX(br_matrix23* mat, br_scalar sy) { LOG_TRACE("(%p, %f)", mat, sy); - NOT_IMPLEMENTED(); + + BrMatrix23ShearX(&mattmp2_23, sy); + BrMatrix23Mul(&mattmp1_23, mat, &mattmp2_23); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23PreShearY(br_matrix23 *mat, br_scalar sx) void BrMatrix23PreShearY(br_matrix23* mat, br_scalar sx) { LOG_TRACE("(%p, %f)", mat, sx); - NOT_IMPLEMENTED(); + + BrMatrix23ShearY(&mattmp2_23, sx); + BrMatrix23Mul(&mattmp1_23, &mattmp2_23, mat); + BrMatrix23Copy(mat, &mattmp1_23); } // IDA: void __cdecl BrMatrix23PostShearY(br_matrix23 *mat, br_scalar sx) void BrMatrix23PostShearY(br_matrix23* mat, br_scalar sx) { LOG_TRACE("(%p, %f)", mat, sx); - NOT_IMPLEMENTED(); + + BrMatrix23ShearY(&mattmp2_23, sx); + BrMatrix23Mul(&mattmp1_23, mat, &mattmp2_23); + BrMatrix23Copy(mat, &mattmp1_23); } diff --git a/src/BRSRC13/CORE/MATH/matrix34.c b/src/BRSRC13/CORE/MATH/matrix34.c index d9c45054..bd1ef0ba 100644 --- a/src/BRSRC13/CORE/MATH/matrix34.c +++ b/src/BRSRC13/CORE/MATH/matrix34.c @@ -11,6 +11,7 @@ br_matrix34 mattmp2; #define B(x, y) B->m[x][y] #define C(x, y) C->m[x][y] #define BR_MAC3(a, b, c, d, e, f) ((a) * (b) + (c) * (d) + (e) * (f)) +#define BR_MAC4(a, b, c, d, e, f, g, h) ((a) * (b) + (c) * (d) + (e) * (f) + (g) * (h)) // IDA: void __cdecl BrMatrix34Copy(br_matrix34 *A, br_matrix34 *B) void BrMatrix34Copy(br_matrix34* A, br_matrix34* B) { @@ -37,18 +38,18 @@ void BrMatrix34Copy(br_matrix34* A, br_matrix34* B) { void BrMatrix34Mul(br_matrix34* A, br_matrix34* B, br_matrix34* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - A->m[0][0] = C->m[0][0] * B->m[0][0] + C->m[2][0] * B->m[0][2] + B->m[0][1] * C->m[1][0]; - A->m[0][1] = B->m[0][0] * C->m[0][1] + B->m[0][2] * C->m[2][1] + B->m[0][1] * C->m[1][1]; - A->m[0][2] = B->m[0][0] * C->m[0][2] + C->m[1][2] * B->m[0][1] + B->m[0][2] * C->m[2][2]; - A->m[1][0] = C->m[0][0] * B->m[1][0] + B->m[1][2] * C->m[2][0] + C->m[1][0] * B->m[1][1]; - A->m[1][1] = B->m[1][2] * C->m[2][1] + C->m[0][1] * B->m[1][0] + C->m[1][1] * B->m[1][1]; - A->m[1][2] = B->m[1][2] * C->m[2][2] + C->m[1][2] * B->m[1][1] + C->m[0][2] * B->m[1][0]; - A->m[2][0] = C->m[0][0] * B->m[2][0] + C->m[2][0] * B->m[2][2] + B->m[2][1] * C->m[1][0]; - A->m[2][1] = C->m[0][1] * B->m[2][0] + C->m[2][1] * B->m[2][2] + B->m[2][1] * C->m[1][1]; - A->m[2][2] = C->m[1][2] * B->m[2][1] + B->m[2][0] * C->m[0][2] + C->m[2][2] * B->m[2][2]; - A->m[3][0] = C->m[0][0] * B->m[3][0] + C->m[2][0] * B->m[3][2] + B->m[3][1] * C->m[1][0] + C->m[3][0]; - A->m[3][1] = B->m[3][0] * C->m[0][1] + B->m[3][1] * C->m[1][1] + B->m[3][2] * C->m[2][1] + C->m[3][1]; - A->m[3][2] = C->m[1][2] * B->m[3][1] + B->m[3][2] * C->m[2][2] + B->m[3][0] * C->m[0][2] + C->m[3][2]; + A(0, 0) = BR_MAC3(B(0, 0), C(0, 0), B(0, 1), C(1, 0), B(0, 2), C(2, 0)); + A(0, 1) = BR_MAC3(B(0, 0), C(0, 1), B(0, 1), C(1, 1), B(0, 2), C(2, 1)); + A(0, 2) = BR_MAC3(B(0, 0), C(0, 2), B(0, 1), C(1, 2), B(0, 2), C(2, 2)); + A(1, 0) = BR_MAC3(B(1, 0), C(0, 0), B(1, 1), C(1, 0), B(1, 2), C(2, 0)); + A(1, 1) = BR_MAC3(B(1, 0), C(0, 1), B(1, 1), C(1, 1), B(1, 2), C(2, 1)); + A(1, 2) = BR_MAC3(B(1, 0), C(0, 2), B(1, 1), C(1, 2), B(1, 2), C(2, 2)); + A(2, 0) = BR_MAC3(B(2, 0), C(0, 0), B(2, 1), C(1, 0), B(2, 2), C(2, 0)); + A(2, 1) = BR_MAC3(B(2, 0), C(0, 1), B(2, 1), C(1, 1), B(2, 2), C(2, 1)); + A(2, 2) = BR_MAC3(B(2, 0), C(0, 2), B(2, 1), C(1, 2), B(2, 2), C(2, 2)); + A(3, 0) = BR_MAC3(B(3, 0), C(0, 0), B(3, 1), C(1, 0), B(3, 2), C(2, 0)) + C(3, 0); + A(3, 1) = BR_MAC3(B(3, 0), C(0, 1), B(3, 1), C(1, 1), B(3, 2), C(2, 1)) + C(3, 1); + A(3, 2) = BR_MAC3(B(3, 0), C(0, 2), B(3, 1), C(1, 2), B(3, 2), C(2, 2)) + C(3, 2); } // IDA: void __cdecl BrMatrix34Identity(br_matrix34 *mat) @@ -57,21 +58,21 @@ void BrMatrix34Identity(br_matrix34* mat) { // { 0, 1, 0}, // { 0, 0, 1} // ( 0, 0, 0 } - mat->m[0][0] = 1.0; - mat->m[0][1] = 0; - mat->m[0][2] = 0; + M(0, 0) = 1.f; + M(0, 1) = 0.f; + M(0, 2) = 0.f; - mat->m[1][0] = 0; - mat->m[1][1] = 1.0; - mat->m[1][2] = 0; + M(1, 0) = 0.f; + M(1, 1) = 1.f; + M(1, 2) = 0.f; - mat->m[2][0] = 0; - mat->m[2][1] = 0; - mat->m[2][2] = 1.0; + M(2, 0) = 0.f; + M(2, 1) = 0.f; + M(2, 2) = 1.f; - mat->m[3][0] = 0; - mat->m[3][1] = 0; - mat->m[3][2] = 0; + M(3, 0) = 0.f; + M(3, 1) = 0.f; + M(3, 2) = 0.f; } // IDA: void __cdecl BrMatrix34RotateX(br_matrix34 *mat, br_angle rx) @@ -125,7 +126,22 @@ void BrMatrix34RotateZ(br_matrix34* mat, br_angle rz) { br_scalar s; br_scalar c; LOG_TRACE("(%p, %d)", mat, rz); - NOT_IMPLEMENTED(); + + s = BR_SIN(rz); + c = BR_COS(rz); + + M(0, 0) = c; + M(0, 1) = s; + M(0, 2) = 0; + M(1, 0) = -s; + M(1, 1) = c; + M(1, 2) = 0; + M(2, 0) = 0; + M(2, 1) = 0; + M(2, 2) = 1; + M(3, 0) = 0; + M(3, 1) = 0; + M(3, 2) = 0; } // IDA: void __cdecl BrMatrix34Rotate(br_matrix34 *mat, br_angle r, br_vector3 *a) @@ -171,15 +187,15 @@ void BrMatrix34Rotate(br_matrix34* mat, br_angle r, br_vector3* a) { void BrMatrix34Translate(br_matrix34* mat, br_scalar dx, br_scalar dy, br_scalar dz) { LOG_TRACE("(%p, %f, %f, %f)", mat, dx, dy, dz); - M(0, 0) = 1; - M(0, 1) = 0; - M(0, 2) = 0; - M(1, 0) = 0; - M(1, 1) = 1; - M(1, 2) = 0; - M(2, 0) = 0; - M(2, 1) = 0; - M(2, 2) = 1; + M(0, 0) = 1.f; + M(0, 1) = 0.f; + M(0, 2) = 0.f; + M(1, 0) = 0.f; + M(1, 1) = 1.f; + M(1, 2) = 0.f; + M(2, 0) = 0.f; + M(2, 1) = 0.f; + M(2, 2) = 1.f; M(3, 0) = dx; M(3, 1) = dy; M(3, 2) = dz; @@ -190,71 +206,71 @@ void BrMatrix34Scale(br_matrix34* mat, br_scalar sx, br_scalar sy, br_scalar sz) LOG_TRACE("(%p, %f, %f, %f)", mat, sx, sy, sz); M(0, 0) = sx; - M(0, 1) = 0; - M(0, 2) = 0; - M(1, 0) = 0; + M(0, 1) = 0.f; + M(0, 2) = 0.f; + M(1, 0) = 0.f; M(1, 1) = sy; - M(1, 2) = 0; - M(2, 0) = 0; - M(2, 1) = 0; + M(1, 2) = 0.f; + M(2, 0) = 0.f; + M(2, 1) = 0.f; M(2, 2) = sz; - M(3, 0) = 0; - M(3, 1) = 0; - M(3, 2) = 0; + M(3, 0) = 0.f; + M(3, 1) = 0.f; + M(3, 2) = 0.f; } // IDA: void __cdecl BrMatrix34ShearX(br_matrix34 *mat, br_scalar sy, br_scalar sz) void BrMatrix34ShearX(br_matrix34* mat, br_scalar sy, br_scalar sz) { LOG_TRACE("(%p, %f, %f)", mat, sy, sz); - M(0, 0) = 1; + M(0, 0) = 1.f; M(0, 1) = sy; M(0, 2) = sz; - M(1, 0) = 0; - M(1, 1) = 1; - M(1, 2) = 0; - M(2, 0) = 0; - M(2, 1) = 0; - M(2, 2) = 1; - M(3, 0) = 0; - M(3, 1) = 0; - M(3, 2) = 0; + M(1, 0) = 0.f; + M(1, 1) = 1.f; + M(1, 2) = 0.f; + M(2, 0) = 0.f; + M(2, 1) = 0.f; + M(2, 2) = 1.f; + M(3, 0) = 0.f; + M(3, 1) = 0.f; + M(3, 2) = 0.f; } // IDA: void __cdecl BrMatrix34ShearY(br_matrix34 *mat, br_scalar sx, br_scalar sz) void BrMatrix34ShearY(br_matrix34* mat, br_scalar sx, br_scalar sz) { LOG_TRACE("(%p, %f, %f)", mat, sx, sz); - M(0, 0) = 1; - M(0, 1) = 0; - M(0, 2) = 0; + M(0, 0) = 1.f; + M(0, 1) = 0.f; + M(0, 2) = 0.f; M(1, 0) = sx; - M(1, 1) = 1; + M(1, 1) = 1.f; M(1, 2) = sz; - M(2, 0) = 0; - M(2, 1) = 0; - M(2, 2) = 1; - M(3, 0) = 0; - M(3, 1) = 0; - M(3, 2) = 0; + M(2, 0) = 0.f; + M(2, 1) = 0.f; + M(2, 2) = 1.f; + M(3, 0) = 0.f; + M(3, 1) = 0.f; + M(3, 2) = 0.f; } // IDA: void __cdecl BrMatrix34ShearZ(br_matrix34 *mat, br_scalar sx, br_scalar sy) void BrMatrix34ShearZ(br_matrix34* mat, br_scalar sx, br_scalar sy) { LOG_TRACE("(%p, %f, %f)", mat, sx, sy); - M(0, 0) = 1; - M(0, 1) = 0; - M(0, 2) = 0; - M(1, 0) = 0; - M(1, 1) = 1; - M(1, 2) = 0; + M(0, 0) = 1.f; + M(0, 1) = 0.f; + M(0, 2) = 0.f; + M(1, 0) = 0.f; + M(1, 1) = 1.f; + M(1, 2) = 0.f; M(2, 0) = sx; M(2, 1) = sy; - M(2, 2) = 1; - M(3, 0) = 0; - M(3, 1) = 0; - M(3, 2) = 0; + M(2, 2) = 1.f; + M(3, 0) = 0.f; + M(3, 1) = 0.f; + M(3, 2) = 0.f; } // IDA: br_scalar __cdecl BrMatrix34Inverse(br_matrix34 *B, br_matrix34 *A) @@ -273,7 +289,7 @@ br_scalar BrMatrix34Inverse(br_matrix34* B, br_matrix34* A) { #define BF(x, y) (BF[x][y]) #define ACCUMULATE \ - if (temp >= 0.0) \ + if (temp >= 0.f) \ pos += temp; \ else \ neg += temp; @@ -302,13 +318,13 @@ br_scalar BrMatrix34Inverse(br_matrix34* B, br_matrix34* A) { det = pos + neg; if (ABS(det) <= PRECISION_LIMIT) - return 0; + return 0.f; if ((ABS(det / (pos - neg)) < PRECISION_LIMIT)) { - return 0; + return 0.f; } - idet = 1.0F / det; + idet = 1.f / det; BF(0, 0) = (AF(1, 1) * AF(2, 2) - AF(1, 2) * AF(2, 1)) * idet; BF(1, 0) = -(AF(1, 0) * AF(2, 2) - AF(1, 2) * AF(2, 0)) * idet; @@ -377,7 +393,37 @@ void BrMatrix34RollingBall(br_matrix34* mat, int dx, int dy, int radius) { br_scalar dr; br_scalar h; LOG_TRACE("(%p, %d, %d, %d)", mat, dx, dy, radius); - NOT_IMPLEMENTED(); + + // The rolling ball, Graphics Gems III (1993), pages 51-60, Academic Press + + dr = sqrtf(dx * dx + dy * dy); + if (dr == BR_SCALAR(.0f)) { + BrMatrix34Identity(mat); + return; + } + h = sqrtf(dr * dr + radius * radius); + ca = radius / h; + sa = dr / h; + nx = -dy / dr; + ny = dx / dr; + // nz = 0; + + h = (1 - ca); + + M(0, 0) = nx * nx * h + ca; + M(0, 1) = nx * ny * h; + M(0, 2) = ny * sa; + M(1, 1) = ca + ny * ny * h; + M(1, 2) = -nx * sa; + M(2, 2) = ca; + + M(1, 0) = M(0, 1); + M(2, 0) = -M(0, 2); + M(2, 1) = -M(1, 2); + + M(3, 0) = BR_SCALAR(0.f); + M(3, 1) = BR_SCALAR(0.f); + M(3, 2) = BR_SCALAR(0.f); } // IDA: br_matrix34* __cdecl BrBoundsToMatrix34(br_matrix34 *mat, br_bounds *bounds) @@ -386,25 +432,67 @@ br_matrix34* BrBoundsToMatrix34(br_matrix34* mat, br_bounds* bounds) { br_vector3 tr; br_vector3 sc; LOG_TRACE("(%p, %p)", mat, bounds); - NOT_IMPLEMENTED(); + + for (i = 0; i < 3; ++i) { + tr.v[i] = 0.5f * bounds->min.v[i] + 0.5f * bounds->max.v[i]; + if (bounds->min.v[i] == bounds->max.v[i]) { + sc.v[i] = 1.f; + } else { + sc.v[i] = 0.5f * bounds->max.v[i] - 0.5f * bounds->min.v[i]; + } + } + + M(0, 0) = sc.v[0]; + M(0, 1) = 0.f; + M(0, 2) = 0.f; + M(1, 0) = 0.f; + M(1, 1) = sc.v[1]; + M(1, 2) = 0.f; + M(2, 0) = 0.f; + M(2, 1) = 0.f; + M(2, 2) = sc.v[2]; + M(3, 0) = tr.v[0]; + M(3, 1) = tr.v[1]; + M(3, 2) = tr.v[2]; } // IDA: void __cdecl BrMatrix34Copy4(br_matrix34 *A, br_matrix4 *B) void BrMatrix34Copy4(br_matrix34* A, br_matrix4* B) { LOG_TRACE("(%p, %p)", A, B); - NOT_IMPLEMENTED(); + + A(0, 0) = B(0, 0); + A(0, 1) = B(0, 1); + A(0, 2) = B(0, 2); + + A(1, 0) = B(1, 0); + A(1, 1) = B(1, 1); + A(1, 2) = B(1, 2); + + A(2, 0) = B(2, 0); + A(2, 1) = B(2, 1); + A(2, 2) = B(2, 2); + + A(3, 0) = B(3, 0); + A(3, 1) = B(3, 1); + A(3, 2) = B(3, 2); } // IDA: void __usercall BrMatrix34TApplyFV(br_vector3 *A@, br_fvector3 *B@, br_matrix34 *C@) void BrMatrix34TApplyFV(br_vector3* A, br_fvector3* B, br_matrix34* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC3(B->v[0], C(0, 0), B->v[1], C(0, 1), B->v[2], C(0, 2)); + A->v[1] = BR_MAC3(B->v[0], C(1, 0), B->v[1], C(1, 1), B->v[2], C(1, 2)); + A->v[2] = BR_MAC3(B->v[0], C(2, 0), B->v[1], C(2, 1), B->v[2], C(2, 2)); } // IDA: void __cdecl BrMatrix34Apply(br_vector3 *A, br_vector4 *B, br_matrix34 *C) void BrMatrix34Apply(br_vector3* A, br_vector4* B, br_matrix34* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC4(B->v[0], C(0, 0), B->v[1], C(1, 0), B->v[2], C(2, 0), B->v[3], C(3, 0)); + A->v[1] = BR_MAC4(B->v[0], C(0, 1), B->v[1], C(1, 1), B->v[2], C(2, 1), B->v[3], C(3, 1)); + A->v[2] = BR_MAC4(B->v[0], C(0, 2), B->v[1], C(1, 2), B->v[2], C(2, 2), B->v[3], C(3, 2)); } // IDA: void __cdecl BrMatrix34ApplyP(br_vector3 *A, br_vector3 *B, br_matrix34 *C) @@ -427,20 +515,29 @@ void BrMatrix34ApplyV(br_vector3* A, br_vector3* B, br_matrix34* C) { // IDA: void __cdecl BrMatrix34TApply(br_vector4 *A, br_vector4 *B, br_matrix34 *C) void BrMatrix34TApply(br_vector4* A, br_vector4* B, br_matrix34* C) { - LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + LOG_TRACE("(%p, %p, %p)", A, B, C) + + A->v[0] = BR_MAC3(B->v[0], C(0, 0), B->v[1], C(0, 1), B->v[2], C(0, 2)); + A->v[1] = BR_MAC3(B->v[0], C(1, 0), B->v[1], C(1, 1), B->v[2], C(1, 2)); + A->v[2] = BR_MAC3(B->v[0], C(2, 0), B->v[1], C(2, 1), B->v[2], C(2, 2)); + A->v[3] = BR_MAC3(B->v[0], C(3, 0), B->v[1], C(3, 1), B->v[2], C(3, 2)) + B->v[3]; } // IDA: void __cdecl BrMatrix34TApplyP(br_vector3 *A, br_vector3 *B, br_matrix34 *C) void BrMatrix34TApplyP(br_vector3* A, br_vector3* B, br_matrix34* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + // translation elements are presumed zero or irrelevant + A->v[0] = BR_MAC3(B->v[0], C(0, 0), B->v[1], C(0, 1), B->v[2], C(0, 2)); + A->v[1] = BR_MAC3(B->v[0], C(1, 0), B->v[1], C(1, 1), B->v[2], C(1, 2)); + A->v[2] = BR_MAC3(B->v[0], C(2, 0), B->v[1], C(2, 1), B->v[2], C(2, 2)); } // IDA: void __cdecl BrMatrix34TApplyV(br_vector3 *A, br_vector3 *B, br_matrix34 *C) void BrMatrix34TApplyV(br_vector3* A, br_vector3* B, br_matrix34* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); + // translation elements are presumed zero or irrelevant A->v[0] = BR_MAC3(B->v[0], C(0, 0), B->v[1], C(0, 1), B->v[2], C(0, 2)); A->v[1] = BR_MAC3(B->v[0], C(1, 0), B->v[1], C(1, 1), B->v[2], C(1, 2)); A->v[2] = BR_MAC3(B->v[0], C(2, 0), B->v[1], C(2, 1), B->v[2], C(2, 2)); @@ -448,101 +545,117 @@ void BrMatrix34TApplyV(br_vector3* A, br_vector3* B, br_matrix34* C) { // IDA: void __cdecl BrMatrix34Pre(br_matrix34 *mat, br_matrix34 *A) void BrMatrix34Pre(br_matrix34* mat, br_matrix34* A) { - br_matrix34 mattmp; LOG_TRACE("(%p, %p)", mat, A); - BrMatrix34Mul(&mattmp, A, mat); - BrMatrix34Copy(mat, &mattmp); + BrMatrix34Mul(&mattmp1, A, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34Post(br_matrix34 *mat, br_matrix34 *A) void BrMatrix34Post(br_matrix34* mat, br_matrix34* A) { - br_matrix34 mattmp; LOG_TRACE("(%p, %p)", mat, A); - BrMatrix34Mul(&mattmp, mat, A); - BrMatrix34Copy(mat, &mattmp); + BrMatrix34Mul(&mattmp1, mat, A); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PreRotateX(br_matrix34 *mat, br_angle rx) void BrMatrix34PreRotateX(br_matrix34* mat, br_angle rx) { LOG_TRACE("(%p, %d)", mat, rx); - BrMatrix34RotateX(&mattmp1, rx); - BrMatrix34Mul(&mattmp2, &mattmp1, mat); - BrMatrix34Copy(mat, &mattmp2); + BrMatrix34RotateX(&mattmp2, rx); + BrMatrix34Mul(&mattmp1, &mattmp2, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PostRotateX(br_matrix34 *mat, br_angle rx) void BrMatrix34PostRotateX(br_matrix34* mat, br_angle rx) { LOG_TRACE("(%p, %d)", mat, rx); - NOT_IMPLEMENTED(); + + BrMatrix34RotateX(&mattmp2, rx); + BrMatrix34Mul(&mattmp1, mat, &mattmp2); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PreRotateY(br_matrix34 *mat, br_angle ry) void BrMatrix34PreRotateY(br_matrix34* mat, br_angle ry) { LOG_TRACE("(%p, %d)", mat, ry); - BrMatrix34RotateY(&mattmp1, ry); - BrMatrix34Mul(&mattmp2, &mattmp1, mat); - BrMatrix34Copy(mat, &mattmp2); + BrMatrix34RotateY(&mattmp2, ry); + BrMatrix34Mul(&mattmp1, &mattmp2, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PostRotateY(br_matrix34 *mat, br_angle ry) void BrMatrix34PostRotateY(br_matrix34* mat, br_angle ry) { LOG_TRACE("(%p, %d)", mat, ry); - NOT_IMPLEMENTED(); + + BrMatrix34RotateY(&mattmp2, ry); + BrMatrix34Mul(&mattmp1, mat, &mattmp2); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PreRotateZ(br_matrix34 *mat, br_angle rz) void BrMatrix34PreRotateZ(br_matrix34* mat, br_angle rz) { LOG_TRACE("(%p, %d)", mat, rz); - NOT_IMPLEMENTED(); + + BrMatrix34RotateZ(&mattmp2, rz); + BrMatrix34Mul(&mattmp1, &mattmp2, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PostRotateZ(br_matrix34 *mat, br_angle rz) void BrMatrix34PostRotateZ(br_matrix34* mat, br_angle rz) { LOG_TRACE("(%p, %d)", mat, rz); - NOT_IMPLEMENTED(); + + BrMatrix34RotateZ(&mattmp2, rz); + BrMatrix34Mul(&mattmp1, mat, &mattmp2); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PreRotate(br_matrix34 *mat, br_angle r, br_vector3 *axis) void BrMatrix34PreRotate(br_matrix34* mat, br_angle r, br_vector3* axis) { LOG_TRACE("(%p, %d, %p)", mat, r, axis); - BrMatrix34Rotate(&mattmp1, r, axis); - BrMatrix34Mul(&mattmp2, &mattmp1, mat); - BrMatrix34Copy(mat, &mattmp2); + BrMatrix34Rotate(&mattmp2, r, axis); + BrMatrix34Mul(&mattmp1, &mattmp2, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PostRotate(br_matrix34 *mat, br_angle r, br_vector3 *axis) void BrMatrix34PostRotate(br_matrix34* mat, br_angle r, br_vector3* axis) { LOG_TRACE("(%p, %d, %p)", mat, r, axis); - NOT_IMPLEMENTED(); + + BrMatrix34Rotate(&mattmp2, r, axis); + BrMatrix34Mul(&mattmp1, mat, &mattmp2); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PreTranslate(br_matrix34 *mat, br_scalar x, br_scalar y, br_scalar z) void BrMatrix34PreTranslate(br_matrix34* mat, br_scalar x, br_scalar y, br_scalar z) { LOG_TRACE("(%p, %f, %f, %f)", mat, x, y, z); - BrMatrix34Translate(&mattmp1, x, y, z); - BrMatrix34Mul(&mattmp2, &mattmp1, mat); - BrMatrix34Copy(mat, &mattmp2); + BrMatrix34Translate(&mattmp2, x, y, z); + BrMatrix34Mul(&mattmp1, &mattmp2, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PostTranslate(br_matrix34 *mat, br_scalar x, br_scalar y, br_scalar z) void BrMatrix34PostTranslate(br_matrix34* mat, br_scalar x, br_scalar y, br_scalar z) { LOG_TRACE("(%p, %f, %f, %f)", mat, x, y, z); - BrMatrix34Translate(&mattmp1, x, y, z); - BrMatrix34Mul(&mattmp2, mat, &mattmp1); - BrMatrix34Copy(mat, &mattmp2); + M(3, 0) += x; + M(3, 1) += y; + M(3, 2) += z; } // IDA: void __cdecl BrMatrix34PreScale(br_matrix34 *mat, br_scalar sx, br_scalar sy, br_scalar sz) void BrMatrix34PreScale(br_matrix34* mat, br_scalar sx, br_scalar sy, br_scalar sz) { LOG_TRACE("(%p, %f, %f, %f)", mat, sx, sy, sz); - NOT_IMPLEMENTED(); + + BrMatrix34Scale(&mattmp2, sx, sy, sz); + BrMatrix34Mul(&mattmp1, &mattmp2, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PostScale(br_matrix34 *mat, br_scalar sx, br_scalar sy, br_scalar sz) @@ -557,44 +670,53 @@ void BrMatrix34PostScale(br_matrix34* mat, br_scalar sx, br_scalar sy, br_scalar // IDA: void __cdecl BrMatrix34PreShearX(br_matrix34 *mat, br_scalar sy, br_scalar sz) void BrMatrix34PreShearX(br_matrix34* mat, br_scalar sy, br_scalar sz) { LOG_TRACE("(%p, %f, %f)", mat, sy, sz); - NOT_IMPLEMENTED(); + + BrMatrix34ShearX(&mattmp2, sy, sz); + BrMatrix34Mul(&mattmp1, &mattmp2, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PostShearX(br_matrix34 *mat, br_scalar sy, br_scalar sz) void BrMatrix34PostShearX(br_matrix34* mat, br_scalar sy, br_scalar sz) { LOG_TRACE("(%p, %f, %f)", mat, sy, sz); - BrMatrix34ShearX(&mattmp1, sy, sz); - BrMatrix34Mul(&mattmp2, mat, &mattmp1); - BrMatrix34Copy(mat, &mattmp2); + BrMatrix34ShearX(&mattmp2, sy, sz); + BrMatrix34Mul(&mattmp1, mat, &mattmp2); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PreShearY(br_matrix34 *mat, br_scalar sx, br_scalar sz) void BrMatrix34PreShearY(br_matrix34* mat, br_scalar sx, br_scalar sz) { LOG_TRACE("(%p, %f, %f)", mat, sx, sz); - NOT_IMPLEMENTED(); + + BrMatrix34ShearY(&mattmp2, sx, sz); + BrMatrix34Mul(&mattmp1, &mattmp2, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PostShearY(br_matrix34 *mat, br_scalar sx, br_scalar sz) void BrMatrix34PostShearY(br_matrix34* mat, br_scalar sx, br_scalar sz) { LOG_TRACE("(%p, %f, %f)", mat, sx, sz); - BrMatrix34ShearY(&mattmp1, sx, sz); - BrMatrix34Mul(&mattmp2, mat, &mattmp1); - BrMatrix34Copy(mat, &mattmp2); + BrMatrix34ShearY(&mattmp2, sx, sz); + BrMatrix34Mul(&mattmp1, mat, &mattmp2); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PreShearZ(br_matrix34 *mat, br_scalar sx, br_scalar sy) void BrMatrix34PreShearZ(br_matrix34* mat, br_scalar sx, br_scalar sy) { LOG_TRACE("(%p, %f, %f)", mat, sx, sy); - NOT_IMPLEMENTED(); + + BrMatrix34ShearZ(&mattmp2, sx, sy); + BrMatrix34Mul(&mattmp1, &mattmp2, mat); + BrMatrix34Copy(mat, &mattmp1); } // IDA: void __cdecl BrMatrix34PostShearZ(br_matrix34 *mat, br_scalar sx, br_scalar sy) void BrMatrix34PostShearZ(br_matrix34* mat, br_scalar sx, br_scalar sy) { LOG_TRACE("(%p, %f, %f)", mat, sx, sy); - BrMatrix34ShearZ(&mattmp1, sx, sy); - BrMatrix34Mul(&mattmp2, mat, &mattmp1); - BrMatrix34Copy(mat, &mattmp2); + BrMatrix34ShearZ(&mattmp2, sx, sy); + BrMatrix34Mul(&mattmp1, mat, &mattmp2); + BrMatrix34Copy(mat, &mattmp1); } diff --git a/src/BRSRC13/CORE/MATH/matrix4.c b/src/BRSRC13/CORE/MATH/matrix4.c index 171a5c54..93d81242 100644 --- a/src/BRSRC13/CORE/MATH/matrix4.c +++ b/src/BRSRC13/CORE/MATH/matrix4.c @@ -1,6 +1,7 @@ #include "matrix4.h" #include "harness/trace.h" #include +#include #define A(x, y) A->m[x][y] #define B(x, y) B->m[x][y] @@ -36,19 +37,67 @@ void BrMatrix4Copy(br_matrix4* A, br_matrix4* B) { // IDA: void __cdecl BrMatrix4Mul(br_matrix4 *A, br_matrix4 *B, br_matrix4 *C) void BrMatrix4Mul(br_matrix4* A, br_matrix4* B, br_matrix4* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A(0, 0) = BR_MAC4(B(0, 0), C(0, 0), B(0, 1), C(1, 0), B(0, 2), C(2, 0), B(0, 3), C(3, 0)); + A(0, 1) = BR_MAC4(B(0, 0), C(0, 1), B(0, 1), C(1, 1), B(0, 2), C(2, 1), B(0, 3), C(3, 1)); + A(0, 2) = BR_MAC4(B(0, 0), C(0, 2), B(0, 1), C(1, 2), B(0, 2), C(2, 2), B(0, 3), C(3, 2)); + A(0, 3) = BR_MAC4(B(0, 0), C(0, 3), B(0, 1), C(1, 3), B(0, 2), C(2, 3), B(0, 3), C(3, 3)); + A(1, 0) = BR_MAC4(B(1, 0), C(0, 0), B(1, 1), C(1, 0), B(1, 2), C(2, 0), B(1, 3), C(3, 0)); + A(1, 1) = BR_MAC4(B(1, 0), C(0, 1), B(1, 1), C(1, 1), B(1, 2), C(2, 1), B(1, 3), C(3, 1)); + A(1, 2) = BR_MAC4(B(1, 0), C(0, 2), B(1, 1), C(1, 2), B(1, 2), C(2, 2), B(1, 3), C(3, 2)); + A(1, 3) = BR_MAC4(B(1, 0), C(0, 3), B(1, 1), C(1, 3), B(1, 2), C(2, 3), B(1, 3), C(3, 3)); + A(2, 0) = BR_MAC4(B(2, 0), C(0, 0), B(2, 1), C(1, 0), B(2, 2), C(2, 0), B(2, 3), C(3, 0)); + A(2, 1) = BR_MAC4(B(2, 0), C(0, 1), B(2, 1), C(1, 1), B(2, 2), C(2, 1), B(2, 3), C(3, 1)); + A(2, 2) = BR_MAC4(B(2, 0), C(0, 2), B(2, 1), C(1, 2), B(2, 2), C(2, 2), B(2, 3), C(3, 2)); + A(2, 3) = BR_MAC4(B(2, 0), C(0, 3), B(2, 1), C(1, 3), B(2, 2), C(2, 3), B(2, 3), C(3, 3)); + A(3, 0) = BR_MAC4(B(3, 0), C(0, 0), B(3, 1), C(1, 0), B(3, 2), C(2, 0), B(3, 3), C(3, 0)); + A(3, 1) = BR_MAC4(B(3, 0), C(0, 1), B(3, 1), C(1, 1), B(3, 2), C(2, 1), B(3, 3), C(3, 1)); + A(3, 2) = BR_MAC4(B(3, 0), C(0, 2), B(3, 1), C(1, 2), B(3, 2), C(2, 2), B(3, 3), C(3, 2)); + A(3, 3) = BR_MAC4(B(3, 0), C(0, 3), B(3, 1), C(1, 3), B(3, 2), C(2, 3), B(3, 3), C(3, 3)); } // IDA: void __cdecl BrMatrix4Identity(br_matrix4 *mat) void BrMatrix4Identity(br_matrix4* mat) { LOG_TRACE("(%p)", mat); - NOT_IMPLEMENTED(); + + M(0, 0) = BR_SCALAR(1.f); + M(0, 1) = BR_SCALAR(0.f); + M(0, 2) = BR_SCALAR(0.f); + M(0, 3) = BR_SCALAR(0.f); + M(1, 0) = BR_SCALAR(0.f); + M(1, 1) = BR_SCALAR(1.f); + M(1, 2) = BR_SCALAR(0.f); + M(1, 3) = BR_SCALAR(0.f); + M(2, 0) = BR_SCALAR(0.f); + M(2, 1) = BR_SCALAR(0.f); + M(2, 2) = BR_SCALAR(1.f); + M(2, 3) = BR_SCALAR(0.f); + M(3, 0) = BR_SCALAR(0.f); + M(3, 1) = BR_SCALAR(0.f); + M(3, 2) = BR_SCALAR(0.f); + M(3, 3) = BR_SCALAR(1.f); } // IDA: void __cdecl BrMatrix4Scale(br_matrix4 *mat, br_scalar sx, br_scalar sy, br_scalar sz) void BrMatrix4Scale(br_matrix4* mat, br_scalar sx, br_scalar sy, br_scalar sz) { LOG_TRACE("(%p, %f, %f, %f)", mat, sx, sy, sz); - NOT_IMPLEMENTED(); + + M(0, 0) = sx; + M(1, 1) = sy; + M(2, 2) = sz; + M(0, 1) = BR_SCALAR(0.f); + M(0, 2) = BR_SCALAR(0.f); + M(0, 3) = BR_SCALAR(0.f); + M(1, 0) = BR_SCALAR(0.f); + M(1, 2) = BR_SCALAR(0.f); + M(1, 3) = BR_SCALAR(0.f); + M(2, 0) = BR_SCALAR(0.f); + M(2, 1) = BR_SCALAR(0.f); + M(2, 3) = BR_SCALAR(0.f); + M(3, 0) = BR_SCALAR(0.f); + M(3, 1) = BR_SCALAR(0.f); + M(3, 2) = BR_SCALAR(0.f); + M(3, 3) = BR_SCALAR(1.f); } // IDA: br_scalar __cdecl BrMatrix4Inverse(br_matrix4 *A, br_matrix4 *B) @@ -197,25 +246,56 @@ void BrMatrix4Adjoint(br_matrix4* A, br_matrix4* B) { void BrMatrix4Perspective(br_matrix4* mat, br_angle field_of_view, br_scalar aspect, br_scalar hither, br_scalar yon) { br_scalar scale; LOG_TRACE("(%p, %d, %f, %f, %f)", mat, field_of_view, aspect, hither, yon); - NOT_IMPLEMENTED(); + + scale = BR_COS(field_of_view / 2) / BR_SIN(field_of_view / 2); + + M(0, 0) = scale / aspect; + M(1, 1) = scale; + M(2, 2) = (yon + hither) / (yon - hither); + M(3, 2) = BR_SCALAR(-2.f) * ((yon * hither) / (yon - hither)); + + M(0, 1) = BR_SCALAR(0.f); + M(0, 2) = BR_SCALAR(0.f); + M(0, 3) = BR_SCALAR(0.f); + M(1, 0) = BR_SCALAR(0.f); + M(1, 2) = BR_SCALAR(0.f); + M(1, 3) = BR_SCALAR(0.f); + M(2, 0) = BR_SCALAR(0.f); + M(2, 1) = BR_SCALAR(0.f); + M(2, 3) = BR_SCALAR(-1.f); + M(3, 0) = BR_SCALAR(0.f); + M(3, 1) = BR_SCALAR(0.f); + M(3, 3) = BR_SCALAR(0.f); } // IDA: void __cdecl BrMatrix4Apply(br_vector4 *A, br_vector4 *B, br_matrix4 *C) void BrMatrix4Apply(br_vector4* A, br_vector4* B, br_matrix4* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC4(B->v[0], C(0, 0), B->v[1], C(1, 0), B->v[2], C(2, 0), B->v[3], C(3, 0)); + A->v[1] = BR_MAC4(B->v[0], C(0, 1), B->v[1], C(1, 1), B->v[2], C(2, 1), B->v[3], C(3, 1)); + A->v[2] = BR_MAC4(B->v[0], C(0, 2), B->v[1], C(1, 2), B->v[2], C(2, 2), B->v[3], C(3, 2)); + A->v[3] = BR_MAC4(B->v[0], C(0, 3), B->v[1], C(1, 3), B->v[2], C(2, 3), B->v[3], C(3, 3)); } // IDA: void __cdecl BrMatrix4ApplyP(br_vector4 *A, br_vector3 *B, br_matrix4 *C) void BrMatrix4ApplyP(br_vector4* A, br_vector3* B, br_matrix4* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC3(B->v[0], C(0, 0), B->v[1], C(1, 0), B->v[2], C(2, 0)) + C(3, 0); + A->v[1] = BR_MAC3(B->v[0], C(0, 1), B->v[1], C(1, 1), B->v[2], C(2, 1)) + C(3, 1); + A->v[2] = BR_MAC3(B->v[0], C(0, 2), B->v[1], C(1, 2), B->v[2], C(2, 2)) + C(3, 2); + A->v[3] = BR_MAC3(B->v[0], C(0, 3), B->v[1], C(1, 3), B->v[2], C(2, 3)) + C(3, 3); } // IDA: void __cdecl BrMatrix4ApplyV(br_vector4 *A, br_vector3 *B, br_matrix4 *C) void BrMatrix4ApplyV(br_vector4* A, br_vector3* B, br_matrix4* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC3(B->v[0], C(0, 0), B->v[1], C(1, 0), B->v[2], C(2, 0)); + A->v[1] = BR_MAC3(B->v[0], C(0, 1), B->v[1], C(1, 1), B->v[2], C(2, 1)); + A->v[2] = BR_MAC3(B->v[0], C(0, 2), B->v[1], C(1, 2), B->v[2], C(2, 2)); + A->v[3] = BR_MAC3(B->v[0], C(0, 3), B->v[1], C(1, 3), B->v[2], C(2, 3)); } // IDA: void __cdecl BrMatrix4TApply(br_vector4 *A, br_vector4 *B, br_matrix4 *C) @@ -231,36 +311,94 @@ void BrMatrix4TApply(br_vector4* A, br_vector4* B, br_matrix4* C) { // IDA: void __cdecl BrMatrix4TApplyP(br_vector4 *A, br_vector3 *B, br_matrix4 *C) void BrMatrix4TApplyP(br_vector4* A, br_vector3* B, br_matrix4* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC3(B->v[0], C(0, 0), B->v[1], C(0, 1), B->v[2], C(0, 2)) + C(0, 3); + A->v[1] = BR_MAC3(B->v[0], C(1, 0), B->v[1], C(1, 1), B->v[2], C(1, 2)) + C(1, 3); + A->v[2] = BR_MAC3(B->v[0], C(2, 0), B->v[1], C(2, 1), B->v[2], C(2, 2)) + C(2, 3); + A->v[3] = BR_MAC3(B->v[0], C(3, 0), B->v[1], C(3, 1), B->v[2], C(3, 2)) + C(3, 3); } // IDA: void __cdecl BrMatrix4TApplyV(br_vector4 *A, br_vector3 *B, br_matrix4 *C) void BrMatrix4TApplyV(br_vector4* A, br_vector3* B, br_matrix4* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A->v[0] = BR_MAC3(B->v[0], C(0, 0), B->v[1], C(0, 1), B->v[2], C(0, 2)); + A->v[1] = BR_MAC3(B->v[0], C(1, 0), B->v[1], C(1, 1), B->v[2], C(1, 2)); + A->v[2] = BR_MAC3(B->v[0], C(2, 0), B->v[1], C(2, 1), B->v[2], C(2, 2)); + A->v[3] = BR_MAC3(B->v[0], C(3, 0), B->v[1], C(3, 1), B->v[2], C(3, 2)); } // IDA: void __cdecl BrMatrix4Copy34(br_matrix4 *A, br_matrix34 *B) void BrMatrix4Copy34(br_matrix4* A, br_matrix34* B) { LOG_TRACE("(%p, %p)", A, B); - NOT_IMPLEMENTED(); + + A(0, 0) = B(0, 0); + A(0, 1) = B(0, 1); + A(0, 2) = B(0, 2); + A(0, 3) = BR_SCALAR(0.f); + A(1, 0) = B(1, 0); + A(1, 1) = B(1, 1); + A(1, 2) = B(1, 2); + A(1, 3) = BR_SCALAR(0.f); + A(2, 0) = B(2, 0); + A(2, 1) = B(2, 1); + A(2, 2) = B(2, 2); + A(2, 3) = BR_SCALAR(0.f); + A(3, 0) = B(3, 0); + A(3, 1) = B(3, 1); + A(3, 2) = B(3, 2); + A(3, 3) = BR_SCALAR(1.f); } // IDA: void __cdecl BrMatrix4Mul34(br_matrix4 *A, br_matrix34 *B, br_matrix4 *C) void BrMatrix4Mul34(br_matrix4* A, br_matrix34* B, br_matrix4* C) { LOG_TRACE("(%p, %p, %p)", A, B, C); - NOT_IMPLEMENTED(); + + A(0, 0) = BR_MAC3(B(0, 0), C(0, 0), B(0, 1), C(1, 0), B(0, 2), C(2, 0)); + A(0, 1) = BR_MAC3(B(0, 0), C(0, 1), B(0, 1), C(1, 1), B(0, 2), C(2, 1)); + A(0, 2) = BR_MAC3(B(0, 0), C(0, 2), B(0, 1), C(1, 2), B(0, 2), C(2, 2)); + A(0, 3) = BR_MAC3(B(0, 0), C(0, 3), B(0, 1), C(1, 3), B(0, 2), C(2, 3)); + A(1, 0) = BR_MAC3(B(1, 0), C(0, 0), B(1, 1), C(1, 0), B(1, 2), C(2, 0)); + A(1, 1) = BR_MAC3(B(1, 0), C(0, 1), B(1, 1), C(1, 1), B(1, 2), C(2, 1)); + A(1, 2) = BR_MAC3(B(1, 0), C(0, 2), B(1, 1), C(1, 2), B(1, 2), C(2, 2)); + A(1, 3) = BR_MAC3(B(1, 0), C(0, 3), B(1, 1), C(1, 3), B(1, 2), C(2, 3)); + A(2, 0) = BR_MAC3(B(2, 0), C(0, 0), B(2, 1), C(1, 0), B(2, 2), C(2, 0)); + A(2, 1) = BR_MAC3(B(2, 0), C(0, 1), B(2, 1), C(1, 1), B(2, 2), C(2, 1)); + A(2, 2) = BR_MAC3(B(2, 0), C(0, 2), B(2, 1), C(1, 2), B(2, 2), C(2, 2)); + A(2, 3) = BR_MAC3(B(2, 0), C(0, 3), B(2, 1), C(1, 3), B(2, 2), C(2, 3)); + A(3, 0) = BR_MAC3(B(3, 0), C(0, 0), B(3, 1), C(1, 0), B(3, 2), C(2, 0)) + C(3, 0); + A(3, 1) = BR_MAC3(B(3, 0), C(0, 1), B(3, 1), C(1, 1), B(3, 2), C(2, 1)) + C(3, 1); + A(3, 2) = BR_MAC3(B(3, 0), C(0, 2), B(3, 1), C(1, 2), B(3, 2), C(2, 2)) + C(3, 2); + A(3, 3) = BR_MAC3(B(3, 0), C(0, 3), B(3, 1), C(1, 3), B(3, 2), C(2, 3)) + C(3, 3); } // IDA: void __cdecl BrMatrix4Pre34(br_matrix4 *A, br_matrix34 *B) void BrMatrix4Pre34(br_matrix4* A, br_matrix34* B) { br_matrix4 C; LOG_TRACE("(%p, %p)", A, B); - NOT_IMPLEMENTED(); + + memcpy(&C, A, sizeof(*A)); + BrMatrix4Mul34(A, B, &C); } // IDA: void __cdecl BrMatrix4ShearZ(br_matrix4 *mat, br_scalar sx, br_scalar sy) void BrMatrix4ShearZ(br_matrix4* mat, br_scalar sx, br_scalar sy) { LOG_TRACE("(%p, %f, %f)", mat, sx, sy); - NOT_IMPLEMENTED(); + + M(0, 0) = BR_SCALAR(1.f); + M(0, 1) = BR_SCALAR(0.f); + M(0, 2) = BR_SCALAR(0.f); + M(0, 3) = BR_SCALAR(0.f); + M(1, 0) = BR_SCALAR(0.f); + M(1, 1) = BR_SCALAR(1.f); + M(1, 2) = BR_SCALAR(0.f); + M(1, 3) = BR_SCALAR(0.f); + M(2, 0) = sx; + M(2, 1) = sy; + M(2, 2) = BR_SCALAR(1.f); + M(2, 3) = BR_SCALAR(0.f); + M(3, 0) = BR_SCALAR(0.f); + M(3, 1) = BR_SCALAR(0.f); + M(3, 2) = BR_SCALAR(0.f); + M(3, 3) = BR_SCALAR(1.f); } diff --git a/src/BRSRC13/CORE/MATH/quat.c b/src/BRSRC13/CORE/MATH/quat.c index c5510cea..fdd22493 100644 --- a/src/BRSRC13/CORE/MATH/quat.c +++ b/src/BRSRC13/CORE/MATH/quat.c @@ -1,5 +1,12 @@ #include "quat.h" +#include "matrix34.h" +#include "matrix4.h" #include "harness/trace.h" +#include + +#define M(x, y) mat->m[x][y] +#define M_DIAG(x) (((br_scalar*)mat->m)[4*(x)]) +#define Q_EL(e) (((br_scalar*)&(q->x))[(e)]) // IDA: br_quat* __cdecl BrQuatMul(br_quat *q, br_quat *l, br_quat *r) br_quat* BrQuatMul(br_quat* q, br_quat* l, br_quat* r) { @@ -14,20 +21,41 @@ br_quat* BrQuatMul(br_quat* q, br_quat* l, br_quat* r) { br_scalar s; br_scalar t; LOG_TRACE("(%p, %p, %p)", q, l, r); - NOT_IMPLEMENTED(); + + x1 = (l->x + l->z) * (r->y + r->x); + x2 = (l->w + l->y) * (r->w - r->z); + x3 = (l->w - l->y) * (r->w + r->z); + s = x1 + x2 + x3; + x4 = ((l->z - l->x) * (r->x - r->y) + s) * 0.5f; + q->x = x4 + (l->w + l->x) * (r->w + r->x) - s; + q->y = x4 + (l->w - l->x) * (r->z + r->y) - x3; + q->z = x4 + (l->y + l->z) * (r->w - r->x) - x2; + q->w = x4 + (l->z - l->y) * (r->y - r->z) - x1; + return q; } // IDA: br_quat* __cdecl BrQuatNormalise(br_quat *q, br_quat *qq) br_quat* BrQuatNormalise(br_quat* q, br_quat* qq) { br_scalar s; LOG_TRACE("(%p, %p)", q, qq); - NOT_IMPLEMENTED(); + + s = 1.f / sqrtf(qq->x * qq->x + qq->y * qq->y + qq->z * qq->z + qq->w * qq->w); + q->x = qq->x * s; + q->y = qq->y * s; + q->z = qq->z * s; + q->w = qq->w * s; + return q; } // IDA: br_quat* __cdecl BrQuatInvert(br_quat *q, br_quat *qq) br_quat* BrQuatInvert(br_quat* q, br_quat* qq) { LOG_TRACE("(%p, %p)", q, qq); - NOT_IMPLEMENTED(); + + q->x = -qq->x; + q->y = -qq->y; + q->z = -qq->z; + q->w = qq->w; + return q; } // IDA: br_quat* __cdecl BrQuatSlerp(br_quat *q, br_quat *l, br_quat *r, br_scalar a, br_int_16 spins) @@ -40,7 +68,37 @@ br_quat* BrQuatSlerp(br_quat* q, br_quat* l, br_quat* r, br_scalar a, br_int_16 br_scalar scale_r; br_quat t; LOG_TRACE("(%p, %p, %p, %f, %d)", q, l, r, a, spins); - NOT_IMPLEMENTED(); + + // Animating rotation with quaternion curves, Ken Shoemake,SIGGRAPH Computer Graphics, 19(3):245–254, 1985. doi:10.1145/325165.325242. + c_omega = l->x * r->x + l->y * r->y + l->z * r->z + l->w * r->w; + if (c_omega >= 0.f) { + t.x = r->x; + t.y = r->y; + t.z = r->z; + t.w = r->w; + } else { + t.x = -r->x; + t.y = -r->y; + t.z = -r->z; + t.w = -r->w; + c_omega = -c_omega; + } + + if ((1.f - c_omega) <= BR_SCALAR_EPSILON) { + scale_l = 1.f - a; + scale_r = a; + } else { + omega = BrRadianToAngle(acosf(c_omega)); + s_omega = sinf(BrAngleToRadian(omega)); + omega_spin = omega + spins * BR_ANGLE_RAD(PI); + scale_l = sinf(BrAngleToRadian(omega - a * omega_spin)) / s_omega; + scale_r = sinf(BrAngleToRadian(a * omega_spin)) / s_omega; + } + q->x = scale_l * l->x + scale_r * t.x; + q->y = scale_l * l->y + scale_r * t.y; + q->z = scale_l * l->z + scale_r * t.z; + q->w = scale_l * l->w + scale_r * t.w; + return q; } // IDA: br_matrix34* __cdecl BrQuatToMatrix34(br_matrix34 *mat, br_quat *q) @@ -58,7 +116,34 @@ br_matrix34* BrQuatToMatrix34(br_matrix34* mat, br_quat* q) { br_scalar yz; br_scalar zz; LOG_TRACE("(%p, %p)", mat, q); - NOT_IMPLEMENTED(); + + zz = q->z + q->z; + yy = q->y + q->y; + zs = q->z * zz; + xx = q->x + q->x; + wx = q->w * xx; + wy = q->w * yy; + wz = q->w * zz; + xy = q->x * yy; + xz = q->x * zz; + ys = q->y * yy; + xs = q->x * xx; + yz = q->y * zz; + + M(0, 0) = 1.f - (ys + zs); + M(1, 0) = xy - wz; + M(2, 0) = xz + wy; + M(0, 1) = xy + wz; + M(1, 1) = 1.f - (zs + xs); + M(2, 1) = yz - wx; + M(0, 2) = xz - wy; + M(1, 2) = yz + wx; + M(2, 2) = 1.f - (xs + ys); + M(3, 0) = 0.f; + M(3, 1) = 0.f; + M(3, 2) = 0.f; + + return mat; } // IDA: br_quat* __cdecl BrMatrix34ToQuat(br_quat *q, br_matrix34 *mat) @@ -69,19 +154,45 @@ br_quat* BrMatrix34ToQuat(br_quat* q, br_matrix34* mat) { int j; int k; LOG_TRACE("(%p, %p)", q, mat); - NOT_IMPLEMENTED(); + + tr = M(0, 0) + M(1, 1) + M(2, 2); + if (tr >= 0.f) { + tr = sqrtf(tr + 1.f); + s = 0.5f / tr; + q->w = tr * 0.5f; + q->x = (M(1, 2) - M(2, 1)) * s; + q->y = (M(2, 0) - M(0, 2)) * s; + q->z = (M(0, 1) - M(1, 0)) * s; + return q; + } + i = M_DIAG(1) >= M_DIAG(0) ? 1 : 0; + i = M_DIAG(2) >= M_DIAG(i) ? 2 : i; + j = (i + 1) % 3; + k = (j + 1) % 3; + tr = sqrtf(M_DIAG(i) - (M_DIAG(j) + M_DIAG(k)) + 1.f); + s = 0.5f / tr; + Q_EL(i) = 0.5f * tr; + Q_EL(j) = (M(i, j) + M(j, i)) * s; + Q_EL(k) = (M(i, k) + M(k, i)) * s; + q->w = (M(k, j) - M(j, k)) * s; + return q; } // IDA: br_matrix4* __cdecl BrQuatToMatrix4(br_matrix4 *mat, br_quat *q) br_matrix4* BrQuatToMatrix4(br_matrix4* mat, br_quat* q) { br_matrix34 tmp; LOG_TRACE("(%p, %p)", mat, q); - NOT_IMPLEMENTED(); + + BrQuatToMatrix34(&tmp, q); + BrMatrix4Copy34(mat, &tmp); + return mat; } // IDA: br_quat* __cdecl BrMatrix4ToQuat(br_quat *q, br_matrix4 *mat) br_quat* BrMatrix4ToQuat(br_quat* q, br_matrix4* mat) { br_matrix34 tmp; LOG_TRACE("(%p, %p)", q, mat); - NOT_IMPLEMENTED(); + + BrMatrix34Copy4(&tmp, mat); + return BrMatrix34ToQuat(q, &tmp); } diff --git a/test/BRSRC13/test_matrix23.c b/test/BRSRC13/test_matrix23.c new file mode 100644 index 00000000..4c470b2d --- /dev/null +++ b/test/BRSRC13/test_matrix23.c @@ -0,0 +1,430 @@ +#include "tests.h" + +#include "CORE/MATH/matrix23.h" +#include +#include + +void test_matrix23_BrMatrix23Copy() { + br_matrix23 matin = {{ + { 1.f, 2.f, }, + { 3.f, 4.f, }, + { 5.f, 6.f, }, + }}; + br_matrix23 matout; + BrMatrix23Copy(&matout, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(matin.m, matout.m, 6); +} + +void test_matrix23_BrMatrix23Identity() { + br_matrix23 mat; + br_matrix23 expected = {{ + { 1.f, 0.f, }, + { 0.f, 1.f, }, + { 0.f, 0.f, }, + }}; + BrMatrix23Identity(&mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(mat.m, expected.m, 6); +} + +void test_matrix23_BrMatrix23Rotate() { + br_matrix23 mat; + br_matrix23 expected = {{ + { .5f, .8660254037f, }, + { -.8660254037f, .5f, }, + { .0f, .0f, }, + }}; + BrMatrix23Rotate(&mat, BR_ANGLE_DEG(60)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-3, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23Translate() { + br_matrix23 mat; + br_matrix23 expected = {{ + { 1.f, 0.f, }, + { 0.f, 1.f, }, + { 3.f, 5.f, }, + }}; + BrMatrix23Translate(&mat, 3.f, 5.f);; + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23Scale() { + br_matrix23 mat; + br_matrix23 expected = {{ + { 3.f, 0.f, }, + { 0.f, 5.f, }, + { 0.f, 0.f, }, + }}; + BrMatrix23Scale(&mat, 3.f, 5.f); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23ShearX() { + br_matrix23 mat; + br_matrix23 expected = {{ + { 1.f, 5.f, }, + { 0.f, 1.f, }, + { 0.f, 0.f, }, + }}; + BrMatrix23ShearX(&mat, 5.f); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23ShearY() { + br_matrix23 mat; + br_matrix23 expected = {{ + { 1.f, 0.f, }, + { 5.f, 1.f, }, + { 0.f, 0.f, }, + }}; + BrMatrix23ShearY(&mat, 5.f); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23Inverse() { + { + br_matrix23 b; + br_matrix23 a = {{ + { 1.f, 2.f}, + { 3.f, 4.f}, + { 5.f, 6.f}, + }}; + br_matrix23 expected = {{ + { -2.f, 1.f, }, + { 1.5f, -0.5f, }, + { 1.f, -2.f, }, + }}; + br_scalar det = BrMatrix23Inverse(&b, &a); + TEST_ASSERT_FLOAT_WITHIN(1.e-5, -2.f, det); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, b.m, 6); + } + { + br_matrix23 b; + br_matrix23 a = {{ + { 1.f, 2.f, }, + { 2.f, 4.f, }, + { 2.f, 1.f, }, + }}; + br_scalar det = BrMatrix23Inverse(&b, &a); + TEST_ASSERT_FLOAT_WITHIN(1.e-5, 0.f, det); + } +} + +void test_matrix23_BrMatrix23LPInverse() { + br_matrix23 b; + br_matrix23 a = {{ + { 0.7071067f, -0.7071067f, }, + { 0.7071067f, 0.7071067f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 0.7071067f, 0.7071067f, }, + { -0.7071067f, 0.7071067f, }, + { 1.4142135f, -5.6568542f, }, + }}; + BrMatrix23LPInverse(&b, &a); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-5, expected.m, b.m, 6); +} + +void test_matrix23_BrMatrix23LPNormalise() { + br_matrix23 b; + br_matrix23 a = {{ + { 1.f, 1e-9f, }, + { 0.f, 1.f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 1.f, 0.f, }, + { 0.f, 1.f, }, + { 3.f, 5.f, }, + }}; + BrMatrix23LPNormalise(&b, &a); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, b.m, 6); +} + +void test_matrix23_BrMatrix23ApplyP() { + br_vector2 vout; + br_matrix23 mat = {{ + { 1.f, 2.f, }, + { 5.f, 6.f, }, + { 9.f, 10.f, }, + }}; + br_vector2 vin = { + { 1.f, 2.f, }, + }; + br_vector2 expected = { + { 20.f, 24.f, }, + }; + BrMatrix23ApplyP(&vout, &vin, &mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 2); +} + +void test_matrix23_BrMatrix23ApplyV() { + br_vector2 vout; + br_matrix23 mat = {{ + { 1.f, 2.f, }, + { 5.f, 6.f, }, + { 9.f, 10.f, }, + }}; + br_vector2 vin = { + { 1.f, 2.f, }, + }; + br_vector2 expected = { + { 11.f, 14.f, }, + }; + BrMatrix23ApplyV(&vout, &vin, &mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 2); +} + +void test_matrix23_BrMatrix23TApplyP() { + br_vector2 vout; + br_matrix23 mat = {{ + { 1.f, 2.f, }, + { 5.f, 6.f, }, + { 9.f, 10.f, }, + }}; + br_vector2 vin = { + { 1.f, 2.f, }, + }; + br_vector2 expected = { + { 5.f, 17.f, }, + }; + BrMatrix23TApplyP(&vout, &vin, &mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 2); +} + +void test_matrix23_BrMatrix23TApplyV() { + br_vector2 vout; + br_matrix23 mat = {{ + { 1.f, 2.f, }, + { 5.f, 6.f, }, + { 9.f, 10.f, }, + }}; + br_vector2 vin = { + { 1.f, 2.f, }, + }; + br_vector2 expected = { + { 5.f, 17.f, }, + }; + BrMatrix23TApplyV(&vout, &vin, &mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 2); +} + +void test_matrix23_BrMatrix23Pre() { + br_matrix23 mat = {{ + { 1.f, 2.f, }, + { 5.f, 6.f, }, + { 9.f, 10.f, }, + }}; + br_matrix23 a = {{ + { 17.f, 18.f, }, + { 21.f, 22.f, }, + { 25.f, 26.f, }, + }}; + br_matrix23 expected = {{ + { 107.f, 142.f, }, + { 131.f, 174.f, }, + { 164.f, 216.f, }, + }}; + BrMatrix23Pre(&mat, &a); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23Post() { + br_matrix23 mat = {{ + { 1.f, 2.f, }, + { 5.f, 6.f, }, + { 9.f, 10.f, }, + }}; + br_matrix23 a = {{ + { 17.f, 18.f, }, + { 21.f, 22.f, }, + { 25.f, 26.f, }, + }}; + br_matrix23 expected = {{ + { 59.f, 62.f, }, + { 211.f, 222.f, }, + { 388.f, 408.f, }, + }}; + BrMatrix23Post(&mat, &a); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PreRotate() { + br_matrix23 mat = {{ + { 0.70710678f, -0.70710678f, }, + { 0.70710678f, 0.70710678f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 1.f, 0.f, }, + { 0.f, 1.f, }, + { 3.f, 5.f, }, + }}; + BrMatrix23PreRotate(&mat, BR_ANGLE_DEG(45)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PostRotate() { + br_matrix23 mat = {{ + { 0.70710678f, -0.70710678f, }, + { 0.70710678f, 0.70710678f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 1.f, 0.f, }, + { 0.f, 1.f, }, + { -1.41421356f, 5.65685425f, }, + }}; + BrMatrix23PostRotate(&mat, BR_ANGLE_DEG(45)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PreTranslate() { + br_matrix23 mat = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 8.f, 2.f, }, + }}; + BrMatrix23PreTranslate(&mat, 3.f, 5.f); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PostTranslate() { + br_matrix23 mat = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 6.f, 10.f, }, + }}; + BrMatrix23PostTranslate(&mat, 3.f, 5.f); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PreScale() { + br_matrix23 mat = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 0.f, -3.f, }, + { 5.f, 0.f, }, + { 3.f, 5.f, }, + }}; + BrMatrix23PreScale(&mat, 3.f, 5.f); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PostScale() { + br_matrix23 mat = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 0.f, -5.f, }, + { 3.f, 0.f, }, + { 9.f, 25.f, }, + }}; + BrMatrix23PostScale(&mat, 3.f, 5.f); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PreShearX() { + br_matrix23 mat = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 5.f, -1.f, }, + { 1.f, 0.f, }, + { 3.f, 5.f, }, + }}; + BrMatrix23PreShearX(&mat, 5.f); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PostShearX() { + br_matrix23 mat = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 0.f, -1.f, }, + { 1.f, 5.f, }, + { 3.f, 20.f, }, + }}; + BrMatrix23PostShearX(&mat, 5.f); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PreShearY() { + br_matrix23 mat = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { 0.f, -1.f, }, + { 1.f, -5.f, }, + { 3.f, 5.f, }, + }}; + BrMatrix23PreShearY(&mat, 5.f); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_BrMatrix23PostShearY() { + br_matrix23 mat = {{ + { 0.f, -1.f, }, + { 1.f, 0.f, }, + { 3.f, 5.f, }, + }}; + br_matrix23 expected = {{ + { -5.f, -1.f, }, + { 1.f, 0.f, }, + { 28.f, 5.f, }, + }}; + BrMatrix23PostShearY(&mat, 5.f); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 6); +} + +void test_matrix23_suite() { + RUN_TEST(test_matrix23_BrMatrix23Copy); + RUN_TEST(test_matrix23_BrMatrix23Identity); + RUN_TEST(test_matrix23_BrMatrix23Rotate); + RUN_TEST(test_matrix23_BrMatrix23Translate); + RUN_TEST(test_matrix23_BrMatrix23Scale); + RUN_TEST(test_matrix23_BrMatrix23ShearX); + RUN_TEST(test_matrix23_BrMatrix23ShearY); + RUN_TEST(test_matrix23_BrMatrix23Inverse); + RUN_TEST(test_matrix23_BrMatrix23LPInverse); + RUN_TEST(test_matrix23_BrMatrix23LPNormalise); + RUN_TEST(test_matrix23_BrMatrix23ApplyP); + RUN_TEST(test_matrix23_BrMatrix23ApplyV); + RUN_TEST(test_matrix23_BrMatrix23TApplyP); + RUN_TEST(test_matrix23_BrMatrix23TApplyV); + RUN_TEST(test_matrix23_BrMatrix23Pre); + RUN_TEST(test_matrix23_BrMatrix23Post); + RUN_TEST(test_matrix23_BrMatrix23PreRotate); + RUN_TEST(test_matrix23_BrMatrix23PostRotate); + RUN_TEST(test_matrix23_BrMatrix23PreTranslate); + RUN_TEST(test_matrix23_BrMatrix23PostTranslate); + RUN_TEST(test_matrix23_BrMatrix23PreScale); + RUN_TEST(test_matrix23_BrMatrix23PostScale); + RUN_TEST(test_matrix23_BrMatrix23PreShearX); + RUN_TEST(test_matrix23_BrMatrix23PostShearX); + RUN_TEST(test_matrix23_BrMatrix23PreShearY); + RUN_TEST(test_matrix23_BrMatrix23PostShearY); +} diff --git a/test/BRSRC13/test_matrix34.c b/test/BRSRC13/test_matrix34.c new file mode 100644 index 00000000..9d283c89 --- /dev/null +++ b/test/BRSRC13/test_matrix34.c @@ -0,0 +1,583 @@ +#include "tests.h" + +#include "CORE/MATH/matrix34.h" +#include +#include + +static void test_matrix34_BrMatrix34Copy() { + br_matrix34 matin = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_matrix34 matout; + BrMatrix34Copy(&matout, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(matin.m, matout.m, 12); +} + +static void test_matrix34_BrMatrix34Mul() { + br_matrix34 a; + br_matrix34 b = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_matrix34 c = {{ + { 13.f, 14.f, 15.f, }, + { 16.f, 17.f, 18.f, }, + { 19.f, 20.f, 21.f, }, + { 22.f, 23.f, 24.f, }, + }}; + br_matrix34 expected = {{ + { 102.f, 108.f, 114.f, }, + { 246.f, 261.f, 276.f, }, + { 390.f, 414.f, 438.f, }, + { 556.f, 590.f, 624.f, }, + }}; + BrMatrix34Mul(&a, &b, &c); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, a.m, 12); +} + +static void test_matrix34_BrMatrix34Identity() { + br_matrix34 a; + br_matrix34 expected = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 0.f, 0.f, 0.f, }, + }}; + BrMatrix34Identity(&a); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, a.m, 12); +} + +static void test_matrix34_BrMatrix34RotateX() { + br_matrix34 a; + br_matrix34 expected = {{ + { 1.f, .0f, .0f, }, + { 0.f, .7071067f, .7071067f, }, + { 0.f, -0.7071067f, .7071067f, }, + { 0.f, .0f, .0f, }, + }}; + BrMatrix34RotateX(&a, BR_ANGLE_DEG(45)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, a.m, 12); +} + +static void test_matrix34_BrMatrix34RotateY() { + br_matrix34 a; + br_matrix34 expected = {{ + { .7071067f, 0.f, -.7071067f, }, + { .0f, 1.f, .0f, }, + { .7071067f, 0.f, .7071067f, }, + { .0f, 0.f, .0f, }, + }}; + BrMatrix34RotateY(&a, BR_ANGLE_DEG(45)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, a.m, 12); +} + +static void test_matrix34_BrMatrix34RotateZ() { + br_matrix34 a; + br_matrix34 expected = {{ + { .7071067f, .7071067f, 0.f, }, + { -.7071067f, .7071067f, 0.f, }, + { .0f, .0f, 1.f, }, + { .0f, .0f, 0.f, }, + }}; + BrMatrix34RotateZ(&a, BR_ANGLE_DEG(45)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, a.m, 12); +} + +static void test_matrix34_BrMatrix34Rotate() { + br_matrix34 mat; + // scipy.spatial.transform.Rotation.from_rotvec(numpy.matrix([1,1,1]) * -numpy.pi/4/math.sqrt(3)).as_matrix() + br_matrix34 expected = {{ + { .80473785f, .50587936f, -.31061722f, }, + { -.31061722f, .80473785f, .50587936f, }, + { .50587936f, -.31061722f, .80473785f, }, + { .0f, .0f, .0f, }, + }}; + br_vector3 a = {{ 0.577350269f, 0.577350269f, 0.577350269f, }}; + BrMatrix34Rotate(&mat, BR_ANGLE_DEG(45), &a); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34Translate() { + br_matrix34 mat; + br_matrix34 expected = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 3.f, 5.f, 7.f, }, + }}; + BrMatrix34Translate(&mat, 3.f, 5.f, 7.f); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34Scale() { + br_matrix34 mat; + br_matrix34 expected = {{ + { 3.f, 0.f, 0.f, }, + { 0.f, 5.f, 0.f, }, + { 0.f, 0.f, 7.f, }, + { 0.f, 0.f, 0.f, }, + }}; + BrMatrix34Scale(&mat, 3.f, 5.f, 7.f); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34ShearX() { + br_matrix34 mat; + br_matrix34 expected = {{ + { 1.f, 3.f, 5.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 0.f, 0.f, 0.f, }, + }}; + BrMatrix34ShearX(&mat, 3.f, 5.f); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34ShearY() { + br_matrix34 mat; + br_matrix34 expected = {{ + { 1.f, 0.f, 0.f, }, + { 3.f, 1.f, 5.f, }, + { 0.f, 0.f, 1.f, }, + { 0.f, 0.f, 0.f, }, + }}; + BrMatrix34ShearY(&mat, 3.f, 5.f); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34ShearZ() { + br_matrix34 mat; + br_matrix34 expected = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 3.f, 5.f, 1.f, }, + { 0.f, 0.f, 0.f, }, + }}; + BrMatrix34ShearZ(&mat, 3.f, 5.f); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34Inverse() { + br_matrix34 matout; + br_matrix34 matin = {{ + { 1.f, 4.f, 2.f, }, + { 3.f, 5.f, 1.f, }, + { 2.f, 5.f, 3.f, }, + { 4.f, 8.f, 2.f, }, + }}; + br_matrix34 expected = {{ + { -1.25f, 0.25f, 0.75f, }, + { 0.875f, 0.125f, -0.625f, }, + { -0.625f, -0.375f, 0.875f, }, + { -0.75f, -1.25f, 0.25f, }, + }}; + BrMatrix34Inverse(&matout, &matin); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-5, expected.m, matout.m, 12); +} + +static void test_matrix34_BrMatrix34LPInverse() { + br_matrix34 matout; + br_matrix34 matin = {{ + { 0.f, 1.f, 0.f, }, + { -1.f, 0.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 0.f, 0.f, 0.f, }, + }}; + br_matrix34 expected = {{ + { 0.f, -1.f, 0.f, }, + { 1.f, 0.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 0.f, 0.f, 0.f, }, + }}; + BrMatrix34Inverse(&matout, &matin); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-5, expected.m, matout.m, 12); +} + +static void test_matrix34_BrMatrix34LPNormalise() { + br_matrix34 matout; + br_matrix34 matin = {{ + { 1.f, 0.f, 0.f, }, + { 1.e-3f, 1.f, 2.e-3f, }, + { 0.f, 0.f, 1.f, }, + { 0.f, 0.f, 0.f, }, + }}; + br_matrix34 expected = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 0.f, 0.f, 0.f, }, + }}; + BrMatrix34LPNormalise(&matout, &matin); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-3, expected.m, matout.m, 12); +} + +static void test_matrix34_BrMatrix34RollingBall() { + { + br_matrix34 mat; + + br_matrix34 expected = {{ + { .9987616f, -.0024768f, .0496904f, }, + { -.0024768f, .99504639f, .0993808f, }, + { -.0496904f, -.0993808f, .99380799f, }, + { .0f, .0f, .0f, }, + }}; + BrMatrix34RollingBall(&mat, 5, 10, 100); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-3, expected.m, mat.m, 12); + } + { + br_matrix34 mat; + br_matrix34 expected; + BrMatrix34Identity(&expected); + BrMatrix34RollingBall(&mat, 0, 0, 100); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 12); + } +} + +static void test_matrix34_BrBoundsToMatrix34() { + br_matrix34 matout; + br_bounds bounds = { + { -2.f, -3.f, -4.f, }, + { 6.f, 9.f, 12.f, }, + }; + br_matrix34 expected = {{ + { 4.f, 0.f, 0.f, }, + { 0.f, 6.f, 0.f, }, + { 0.f, 0.f, 8.f, }, + { 2.f, 3.f, 4.f, }, + }}; + BrBoundsToMatrix34(&matout, &bounds); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, matout.m, 12); +} + +static void test_matrix34_BrMatrix34Copy4() { + br_matrix34 matout; + br_matrix4 matin = {{ + { 1.f, 2.f, 3.f, 0.f, }, + { 4.f, 5.f, 6.f, 0.f, }, + { 7.f, 8.f, 9.f, 0.f, }, + { 10.f, 11.f, 12.f, 1.f, }, + }}; + br_matrix34 expected = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + BrMatrix34Copy4(&matout, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, matout.m, 12); +} + +static void test_matrix34_BrMatrix34TApplyFV() { + br_vector3 vecout; + br_fvector3 vecin = { + { 1.f, 2.f, 3.f, }, + }; + br_matrix34 matin = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_vector3 expected = { + { 14.f, 32.f, 50.f, }, + }; + BrMatrix34TApplyFV(&vecout, &vecin, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vecout.v, 3); +} + +static void test_matrix34_BrMatrix34Apply() { + br_vector3 vecout; + br_vector4 vecin = { + { 1.f, 2.f, 3.f, 1.f, }, + }; + br_matrix34 matin = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_vector3 expected = { + { 40.f, 47.f, 54.f, }, + }; + BrMatrix34Apply(&vecout, &vecin, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vecout.v, 3); +} + +static void test_matrix34_BrMatrix34ApplyP() { + br_vector3 vecout; + br_vector3 vecin = { + { 1.f, 2.f, 3.f, }, + }; + br_matrix34 matin = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_vector3 expected = { + { 40.f, 47.f, 54.f, }, + }; + BrMatrix34ApplyP(&vecout, &vecin, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vecout.v, 3); +} + +static void test_matrix34_BrMatrix34ApplyV() { + br_vector3 vecout; + br_vector3 vecin = { + { 1.f, 2.f, 3.f, }, + }; + br_matrix34 matin = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_vector3 expected = { + { 30.f, 36.f, 42.f, }, + }; + BrMatrix34ApplyV(&vecout, &vecin, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vecout.v, 3); +} + +static void test_matrix34_BrMatrix34TApply() { + br_vector4 vecout; + br_vector4 vecin = { + { 1.f, 2.f, 3.f, 4.f, }, + }; + br_matrix34 matin = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_vector4 expected = { + { 14.f, 32.f, 50.f, 72.f, }, + }; + BrMatrix34TApply(&vecout, &vecin, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vecout.v, 4); +} + +static void test_matrix34_BrMatrix34TApplyP() { + br_vector3 vecout; + br_vector3 vecin = { + { 1.f, 2.f, 3.f, }, + }; + br_matrix34 matin = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_vector3 expected = { + { 14.f, 32.f, 50.f, }, + }; + BrMatrix34TApplyP(&vecout, &vecin, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vecout.v, 3); +} + +static void test_matrix34_BrMatrix34TApplyV() { + br_vector3 vecout; + br_vector3 vecin = { + { 1.f, 2.f, 3.f, }, + }; + br_matrix34 matin = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_vector3 expected = { + { 14.f, 32.f, 50.f, }, + }; + BrMatrix34TApplyV(&vecout, &vecin, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vecout.v, 3); +} + +static void test_matrix34_BrMatrix34Pre() { + br_matrix34 mat = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_matrix34 a = {{ + { 13.f, 14.f, 15.f, }, + { 16.f, 17.f, 18.f, }, + { 19.f, 20.f, 21.f, }, + { 22.f, 23.f, 24.f, }, + }}; + br_matrix34 expected = {{ + { 174.f, 216.f, 258.f, }, + { 210.f, 261.f, 312.f, }, + { 246.f, 306.f, 366.f, }, + { 292.f, 362.f, 432.f, }, + }}; + BrMatrix34Pre(&mat, &a); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34Post() { + br_matrix34 mat = {{ + { 1.f, 2.f, 3.f, }, + { 4.f, 5.f, 6.f, }, + { 7.f, 8.f, 9.f, }, + { 10.f, 11.f, 12.f, }, + }}; + br_matrix34 a = {{ + { 13.f, 14.f, 15.f, }, + { 16.f, 17.f, 18.f, }, + { 19.f, 20.f, 21.f, }, + { 22.f, 23.f, 24.f, }, + }}; + br_matrix34 expected = {{ + { 102.f, 108.f, 114.f, }, + { 246.f, 261.f, 276.f, }, + { 390.f, 414.f, 438.f, }, + { 556.f, 590.f, 624.f, }, + }}; + BrMatrix34Post(&mat, &a); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34PreRotateX() { + br_matrix34 mat = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 2.f, 3.f, 4.f, }, + }}; + br_matrix34 expected = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 0.f, -1.f, 0.f, }, + { 2.f, 3.f, 4.f, }, + }}; + BrMatrix34PreRotateX(&mat, BR_ANGLE_DEG(90)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-3, expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34PostRotateX() { + br_matrix34 mat = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 2.f, 3.f, 4.f, }, + }}; + br_matrix34 expected = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 0.f, -1.f, 0.f, }, + { 2.f, -4.f, 3.f, }, + }}; + BrMatrix34PostRotateX(&mat, BR_ANGLE_DEG(90)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34PreRotateY() { + br_matrix34 mat = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 3.f, 2.f, 4.f, }, + }}; + br_matrix34 expected = {{ + { 0.f, 0.f, -1.f, }, + { 0.f, 1.f, 0.f, }, + { 1.f, 0.f, 0.f, }, + { 3.f, 2.f, 4.f, }, + }}; + BrMatrix34PreRotateY(&mat, BR_ANGLE_DEG(90)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-3, expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34PostRotateY() { + br_matrix34 mat = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 3.f, 2.f, 4.f, }, + }}; + br_matrix34 expected = {{ + { 0.f, 0.f, -1.f, }, + { 0.f, 1.f, 0.f, }, + { 1.f, 0.f, 0.f, }, + { 4.f, 2.f, -3.f, }, + }}; + BrMatrix34PostRotateY(&mat, BR_ANGLE_DEG(90)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34PreRotateZ() { + br_matrix34 mat = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 3.f, 4.f, 2.f, }, + }}; + br_matrix34 expected = {{ + { 0.f, 1.f, 0.f, }, + { -1.f, 0.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 3.f, 4.f, 2.f, }, + }}; + BrMatrix34PreRotateZ(&mat, BR_ANGLE_DEG(90)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-3, expected.m, mat.m, 12); +} + +static void test_matrix34_BrMatrix34PostRotateZ() { + br_matrix34 mat = {{ + { 1.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { 3.f, 4.f, 2.f, }, + }}; + br_matrix34 expected = {{ + { 0.f, 1.f, 0.f, }, + { -1.f, 0.f, 0.f, }, + { 0.f, 0.f, 1.f, }, + { -4.f, 3.f, 2.f, }, + }}; + BrMatrix34PostRotateZ(&mat, BR_ANGLE_DEG(90)); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-2, expected.m, mat.m, 12); +} + +void test_matrix34_suite() { + RUN_TEST(test_matrix34_BrMatrix34Copy); + RUN_TEST(test_matrix34_BrMatrix34Mul); + RUN_TEST(test_matrix34_BrMatrix34Identity); + RUN_TEST(test_matrix34_BrMatrix34RotateX); + RUN_TEST(test_matrix34_BrMatrix34RotateY); + RUN_TEST(test_matrix34_BrMatrix34RotateZ); + RUN_TEST(test_matrix34_BrMatrix34Rotate); + RUN_TEST(test_matrix34_BrMatrix34Translate); + RUN_TEST(test_matrix34_BrMatrix34Scale); + RUN_TEST(test_matrix34_BrMatrix34ShearX); + RUN_TEST(test_matrix34_BrMatrix34ShearY); + RUN_TEST(test_matrix34_BrMatrix34ShearZ); + RUN_TEST(test_matrix34_BrMatrix34Inverse); + RUN_TEST(test_matrix34_BrMatrix34LPInverse); + RUN_TEST(test_matrix34_BrMatrix34LPNormalise); + RUN_TEST(test_matrix34_BrMatrix34RollingBall); + RUN_TEST(test_matrix34_BrBoundsToMatrix34); + RUN_TEST(test_matrix34_BrMatrix34Copy4); + RUN_TEST(test_matrix34_BrMatrix34TApplyFV); + RUN_TEST(test_matrix34_BrMatrix34Apply); + RUN_TEST(test_matrix34_BrMatrix34ApplyP); + RUN_TEST(test_matrix34_BrMatrix34ApplyV); + RUN_TEST(test_matrix34_BrMatrix34TApply); + RUN_TEST(test_matrix34_BrMatrix34TApplyP); + RUN_TEST(test_matrix34_BrMatrix34TApplyV); + RUN_TEST(test_matrix34_BrMatrix34Pre); + RUN_TEST(test_matrix34_BrMatrix34Post); + RUN_TEST(test_matrix34_BrMatrix34PreRotateX); + RUN_TEST(test_matrix34_BrMatrix34PostRotateX); + RUN_TEST(test_matrix34_BrMatrix34PreRotateY); + RUN_TEST(test_matrix34_BrMatrix34PostRotateY); + RUN_TEST(test_matrix34_BrMatrix34PreRotateZ); + RUN_TEST(test_matrix34_BrMatrix34PostRotateZ); +} diff --git a/test/BRSRC13/test_matrix4.c b/test/BRSRC13/test_matrix4.c new file mode 100644 index 00000000..16b4e048 --- /dev/null +++ b/test/BRSRC13/test_matrix4.c @@ -0,0 +1,342 @@ +#include "tests.h" + +#include "CORE/MATH/matrix4.h" +#include +#include + +void test_matrix4_BrMatrix4Copy() { + br_matrix4 matin = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 5.f, 6.f, 7.f, 8.f, }, + { 9.f, 10.f, 11.f, 12.f, }, + { 13.f, 14.f, 15.f, 16.f, }, + }}; + br_matrix4 matout; + BrMatrix4Copy(&matout, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(matin.m, matout.m, 16); +} + +void test_matrix4_BrMatrix4Mul() { + br_matrix4 a; + br_matrix4 identity; + // import numpy + // b = numpy.matrix([[1,2,3,4],[5,6,7,8],[9,10,11,12],[13,14,15,16]]) + // c = numpy.matrix([[17,18,19,20],[21,22,23,24],[25,26,27,28],[29,30,31,32]]) + // b * c + br_matrix4 b = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 5.f, 6.f, 7.f, 8.f, }, + { 9.f, 10.f, 11.f, 12.f, }, + { 13.f, 14.f, 15.f, 16.f, }, + }}; + br_matrix4 c = {{ + { 17.f, 18.f, 19.f, 20.f, }, + { 21.f, 22.f, 23.f, 24.f, }, + { 25.f, 26.f, 27.f, 28.f, }, + { 29.f, 30.f, 31.f, 32.f, }, + }}; + br_matrix4 expected = {{ + { 250.f, 260.f, 270.f, 280.f, }, + { 618.f, 644.f, 670.f, 696.f, }, + { 986.f, 1028.f, 1070.f, 1112.f, }, + { 1354.f, 1412.f, 1470.f, 1528.f, }, + }}; + BrMatrix4Identity(&identity); + + BrMatrix4Mul(&a, &b, &c); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, a.m, 16); + + BrMatrix4Mul(&a, &identity, &b); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(b.m, a.m, 16); +} + +void test_matrix4_BrMatrix4Identity() { + br_matrix4 mat; + br_matrix4 expected = {{ + { 1.f, 0.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, 0.f, }, + { 0.f, 0.f, 1.f, 0.f, }, + { 0.f, 0.f, 0.f, 1.f, }, + }}; + BrMatrix4Identity(&mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(mat.m, expected.m, 16); +} + +void test_matrix4_BrMatrix4Scale() { + br_matrix4 mat; + BrMatrix4Scale(&mat, 2.f, 3.f, 5.f); + br_matrix4 expected = {{ + { 2.f, 0.f, 0.f, 0.f, }, + { 0.f, 3.f, 0.f, 0.f, }, + { 0.f, 0.f, 5.f, 0.f, }, + { 0.f, 0.f, 0.f, 1.f, }, + }}; + TEST_ASSERT_EQUAL_FLOAT_ARRAY(mat.m, expected.m, 16); +} + +void test_matrix4_BrMatrix4Inverse() { + br_matrix4 matout; + br_matrix4 matin = {{ + { 0.f, -1.f, 0.f, 0.f, }, + { 1.f, 0.f, 0.f, 0.f, }, + { 0.f, 0.f, 0.f, -1.f, }, + { 0.f, 0.f, 1.f, 0.f, }, + }}; + br_matrix4 expected = {{ + { 0.f, 1.f, 0.f, 0.f, }, + { -1.f, 0.f, 0.f, 0.f, }, + { 0.f, 0.f, 0.f, 1.f, }, + { 0.f, 0.f, -1.f, 0.f, }, + }}; + BrMatrix4Inverse(&matout, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, matout.m, 16); +} + +void test_matrix4_BrMatrix4Determinant() { + br_matrix4 mat = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 2.f, 3.f, 4.f, 1.f, }, + { 3.f, 4.f, 1.f, 2.f, }, + { 4.f, 1.f, 2.f, 3.f, }, + }}; + float expected = 160.f; + float determinant = BrMatrix4Determinant(&mat); + TEST_ASSERT_FLOAT_WITHIN(1e-5f, expected, determinant); +} + +void test_matrix4_BrMatrix4Adjoint() { + br_matrix4 matout; + br_matrix4 matin = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 2.f, 3.f, 4.f, 1.f, }, + { 3.f, 4.f, 1.f, 2.f, }, + { 4.f, 1.f, 2.f, 3.f, }, + }}; + br_matrix4 expected = {{ + {-36.f, 4.f, 4.f, 44.f, }, + { 4.f, 4.f, 44.f, -36.f, }, + { 4.f, 44.f, -36.f, 4.f, }, + { 44.f, -36.f, 4.f, 4.f, }, + }}; + BrMatrix4Adjoint(&matout, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, matout.m, 16); +} + +void test_matrix4_BrMatrix4Perspective() { + br_matrix4 mat; + float aspect = 4.f / 3.f; + br_matrix4 expected = {{ + {0.75f, 0.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, 0.f, }, + { 0.f, 0.f, 3.f, -1.f, }, + { 0.f, 0.f, 4.f, 0.f, }, + }}; + BrMatrix4Perspective(&mat, BR_ANGLE_DEG(90), aspect, -1.f, -2.f); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-3, expected.m, mat.m, 16); +} + +void test_matrix4_BrMatrix4Apply() { + br_vector4 vout; + br_matrix4 mat = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 5.f, 6.f, 7.f, 8.f, }, + { 9.f, 10.f, 11.f, 12.f, }, + { 13.f, 14.f, 15.f, 16.f, }, + }}; + br_vector4 vin = { + { 1.f, 2.f, 3.f, 4.f, }, + }; + BrMatrix4Apply(&vout, &vin, &mat); + br_vector4 expected = { + { 90.f, 100.f, 110.f, 120.f, }, + }; + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 4); +} + +void test_matrix4_BrMatrix4ApplyP() { + br_vector4 vout; + br_matrix4 mat = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 5.f, 6.f, 7.f, 8.f, }, + { 9.f, 10.f, 11.f, 12.f, }, + { 13.f, 14.f, 15.f, 16.f, }, + }}; + br_vector3 vin = { + { 1.f, 2.f, 3.f, }, + }; + br_vector4 expected = { + { 51.f, 58.f, 65.f, 72.f, }, + }; + BrMatrix4ApplyP(&vout, &vin, &mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 4); +} + +void test_matrix4_BrMatrix4ApplyV() { + br_vector4 vout; + br_matrix4 mat = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 5.f, 6.f, 7.f, 8.f, }, + { 9.f, 10.f, 11.f, 12.f, }, + { 13.f, 14.f, 15.f, 16.f, }, + }}; + br_vector3 vin = { + { 1.f, 2.f, 3.f, }, + }; + br_vector4 expected = { + { 38.f, 44.f, 50.f, 56.f, }, + }; + BrMatrix4ApplyV(&vout, &vin, &mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 4); +} + +void test_matrix4_BrMatrix4TApply() { + br_vector4 vout; + br_matrix4 mat = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 5.f, 6.f, 7.f, 8.f, }, + { 9.f, 10.f, 11.f, 12.f, }, + { 13.f, 14.f, 15.f, 16.f, }, + }}; + br_vector4 vin = { + { 1.f, 2.f, 3.f, 4.f, }, + }; + br_vector4 expected = { + { 30.f, 70.f, 110.f, 150.f, }, + }; + BrMatrix4TApply(&vout, &vin, &mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 4); +} + +void test_matrix4_BrMatrix4TApplyP() { + br_vector4 vout; + br_matrix4 mat = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 5.f, 6.f, 7.f, 8.f, }, + { 9.f, 10.f, 11.f, 12.f, }, + { 13.f, 14.f, 15.f, 16.f, }, + }}; + br_vector3 vin = { + { 1.f, 2.f, 3.f, }, + }; + br_vector4 expected = { + { 18.f, 46.f, 74.f, 102.f, }, + }; + BrMatrix4TApplyP(&vout, &vin, &mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 4); +} + +void test_matrix4_BrMatrix4TApplyV() { + br_vector4 vout; + br_matrix4 mat = {{ + { 1.f, 2.f, 3.f, 4.f, }, + { 5.f, 6.f, 7.f, 8.f, }, + { 9.f, 10.f, 11.f, 12.f, }, + { 13.f, 14.f, 15.f, 16.f, }, + }}; + br_vector3 vin = { + { 1.f, 2.f, 3.f, }, + }; + br_vector4 expected = { + { 14.f, 38.f, 62.f, 86.f, }, + }; + BrMatrix4TApplyV(&vout, &vin, &mat); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.v, vout.v, 4); +} + +void test_matrix4_BrMatrix4Copy34() { + br_matrix4 matout; + br_matrix34 matin = {{ + { 1.f, 2.f, 3.f, }, + { 5.f, 6.f, 7.f, }, + { 9.f, 10.f, 11.f, }, + { 13.f, 14.f, 15.f, }, + }}; + br_matrix4 expected = {{ + { 1.f, 2.f, 3.f, 0.f, }, + { 5.f, 6.f, 7.f, 0.f, }, + { 9.f, 10.f, 11.f, 0.f, }, + { 13.f, 14.f, 15.f, 1.f, }, + }}; + BrMatrix4Copy34(&matout, &matin); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, matout.m, 16); +} + +void test_matrix4_BrMatrix4Mul34() { + br_matrix4 a; + br_matrix34 b = {{ + { 1.f, 2.f, 3.f, }, + { 5.f, 6.f, 7.f, }, + { 9.f, 10.f, 11.f, }, + { 13.f, 14.f, 15.f, }, + }}; + br_matrix4 c = {{ + { 17.f, 18.f, 19.f, 20.f, }, + { 21.f, 22.f, 23.f, 24.f, }, + { 25.f, 26.f, 27.f, 28.f, }, + { 29.f, 30.f, 31.f, 32.f, }, + }}; + br_matrix4 expected = {{ + { 134.f, 140.f, 146.f, 152.f, }, + { 386.f, 404.f, 422.f, 440.f, }, + { 638.f, 668.f, 698.f, 728.f, }, + { 919.f, 962.f, 1005.f, 1048.f, }, + }}; + BrMatrix4Mul34(&a, &b, &c); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, a.m, 16); +} + +void test_matrix4_BrMatrix4Pre34() { + br_matrix34 b = {{ + { 1.f, 2.f, 3.f, }, + { 5.f, 6.f, 7.f, }, + { 9.f, 10.f, 11.f, }, + { 13.f, 14.f, 15.f, }, + }}; + br_matrix4 a = {{ + { 17.f, 18.f, 19.f, 20.f, }, + { 21.f, 22.f, 23.f, 24.f, }, + { 25.f, 26.f, 27.f, 28.f, }, + { 29.f, 30.f, 31.f, 32.f, }, + }}; + br_matrix4 expected = {{ + { 134.f, 140.f, 146.f, 152.f, }, + { 386.f, 404.f, 422.f, 440.f, }, + { 638.f, 668.f, 698.f, 728.f, }, + { 919.f, 962.f, 1005.f, 1048.f, }, + }}; + BrMatrix4Pre34(&a, &b); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, a.m, 16); +} + +void test_matrix4_BrMatrix4ShearZ() { + br_matrix4 a; + br_matrix4 expected = {{ + { 1.f, 0.f, 0.f, 0.f, }, + { 0.f, 1.f, 0.f, 0.f, }, + { 3.f, 5.f, 1.f, 0.f, }, + { 0.f, 0.f, 0.f, 1.f, }, + }}; + BrMatrix4ShearZ(&a, 3.f, 5.f); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, a.m, 16); +} + +void test_matrix4_suite() { + RUN_TEST(test_matrix4_BrMatrix4Copy); + RUN_TEST(test_matrix4_BrMatrix4Mul); + RUN_TEST(test_matrix4_BrMatrix4Identity); + RUN_TEST(test_matrix4_BrMatrix4Scale); + RUN_TEST(test_matrix4_BrMatrix4Inverse); + RUN_TEST(test_matrix4_BrMatrix4Determinant); + RUN_TEST(test_matrix4_BrMatrix4Adjoint); + RUN_TEST(test_matrix4_BrMatrix4Perspective); + RUN_TEST(test_matrix4_BrMatrix4Apply); + RUN_TEST(test_matrix4_BrMatrix4ApplyP); + RUN_TEST(test_matrix4_BrMatrix4ApplyV); + RUN_TEST(test_matrix4_BrMatrix4TApply); + RUN_TEST(test_matrix4_BrMatrix4TApplyP); + RUN_TEST(test_matrix4_BrMatrix4TApplyV); + RUN_TEST(test_matrix4_BrMatrix4Copy34); + RUN_TEST(test_matrix4_BrMatrix4Mul34); + RUN_TEST(test_matrix4_BrMatrix4Pre34); + RUN_TEST(test_matrix4_BrMatrix4ShearZ); +} diff --git a/test/BRSRC13/test_quat.c b/test/BRSRC13/test_quat.c new file mode 100644 index 00000000..06fb905d --- /dev/null +++ b/test/BRSRC13/test_quat.c @@ -0,0 +1,368 @@ +#include "CORE/MATH/quat.h" +#include "CORE/MATH/matrix34.h" +#include "tests.h" +#include +#include + +static void test_quat_BrQuatMul() { + br_quat q; + br_quat l; + br_quat r; + br_quat *res; + + l.x = sqrtf(0.5f); + l.y = 0.f; + l.z = 0.f; + l.w = sqrt(0.5f); + + r.x = 0.f; + r.y = sqrt(0.5f); + r.z = 0.f; + r.w = sqrt(0.5f); + + res = BrQuatMul(&q, &l, &r); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(0.5f, q.x); + TEST_ASSERT_EQUAL_FLOAT(0.5f, q.y); + TEST_ASSERT_EQUAL_FLOAT(0.5f, q.z); + TEST_ASSERT_EQUAL_FLOAT(0.5f, q.w); + + l.x = 0.f; + l.y = 0.f; + l.z = 0.f; + l.w = 1.f; + + res = BrQuatMul(&q, &l, &r); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(r.x, q.x); + TEST_ASSERT_EQUAL_FLOAT(r.y, q.y); + TEST_ASSERT_EQUAL_FLOAT(r.z, q.z); + TEST_ASSERT_EQUAL_FLOAT(r.w, q.w); + + res = BrQuatMul(&q, &r, &l); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(r.x, q.x); + TEST_ASSERT_EQUAL_FLOAT(r.y, q.y); + TEST_ASSERT_EQUAL_FLOAT(r.z, q.z); + TEST_ASSERT_EQUAL_FLOAT(r.w, q.w); +} + +static void test_quat_BrQuatNormalise() { + br_quat q; + br_quat qq; + br_quat *res; + + qq.x = 1.f; + qq.y = 1.f; + qq.z = 1.f; + qq.w = 1.f; + + res = BrQuatNormalise(&q, &qq); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(0.5f, q.x); + TEST_ASSERT_EQUAL_FLOAT(0.5f, q.y); + TEST_ASSERT_EQUAL_FLOAT(0.5f, q.z); + TEST_ASSERT_EQUAL_FLOAT(0.5f, q.w); + + qq.x = 4.f; + qq.y = 3.f; + qq.z = 0.f; + qq.w = 0.f; + + res = BrQuatNormalise(&q, &qq); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(0.8f, q.x); + TEST_ASSERT_EQUAL_FLOAT(0.6f, q.y); + TEST_ASSERT_EQUAL_FLOAT(0.0f, q.z); + TEST_ASSERT_EQUAL_FLOAT(0.0f, q.w); +} + +static void test_quat_BrQuatInvert() { + br_quat q; + br_quat qq; + br_quat *res; + + qq.x = 1.f; + qq.y = 1.f; + qq.z = 1.f; + qq.w = 1.f; + + res = BrQuatInvert(&q, &qq); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(-1.f, q.x); + TEST_ASSERT_EQUAL_FLOAT(-1.f, q.y); + TEST_ASSERT_EQUAL_FLOAT(-1.f, q.z); + TEST_ASSERT_EQUAL_FLOAT(1.f, q.w); + + qq.x = 4.f; + qq.y = 3.f; + qq.z = 0.f; + qq.w = 0.f; + + res = BrQuatInvert(&q, &qq); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(-4.f, q.x); + TEST_ASSERT_EQUAL_FLOAT(-3.f, q.y); + TEST_ASSERT_EQUAL_FLOAT(-0.0f, q.z); + TEST_ASSERT_EQUAL_FLOAT(0.0f, q.w); + + qq.x = 0.f; + qq.y = 3.f; + qq.z = 0.f; + qq.w = 4.f; + + res = BrQuatInvert(&q, &qq); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(-0.f, q.x); + TEST_ASSERT_EQUAL_FLOAT(-3.f, q.y); + TEST_ASSERT_EQUAL_FLOAT(-0.0f, q.z); + TEST_ASSERT_EQUAL_FLOAT(4.0f, q.w); +} + +static void test_quat_BrQuatSlerp() { + br_quat q; + br_quat l; + br_quat r; + br_scalar a; + br_int_16 spins; + br_quat *res; + + l.x = sqrtf(0.5f); + l.y = 0.f; + l.z = 0.f; + l.w = sqrtf(0.5f); + + r.x = 0.f; + r.y = sqrtf(0.5f); + r.z = 0.f; + r.w = sqrtf(0.5f); + + a = 0.f; + spins = 0; + + res = BrQuatSlerp(&q, &l, &r, a, spins); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(sqrt(0.5f), q.x); + TEST_ASSERT_EQUAL_FLOAT(0.f, q.y); + TEST_ASSERT_EQUAL_FLOAT(0.f, q.z); + TEST_ASSERT_EQUAL_FLOAT(sqrt(0.5f), q.w); + + a = 1.f; + spins = 0; + + res = BrQuatSlerp(&q, &l, &r, a, spins); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_EQUAL_FLOAT(0.f, q.x); + TEST_ASSERT_EQUAL_FLOAT(sqrt(0.5f), q.y); + TEST_ASSERT_EQUAL_FLOAT(0.f, q.z); + TEST_ASSERT_EQUAL_FLOAT(sqrt(0.5f), q.w); + + a = 0.5f; + spins = 0; + + res = BrQuatSlerp(&q, &l, &r, a, spins); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_FLOAT_WITHIN(1e-4, 0.40824829f, q.x); + TEST_ASSERT_FLOAT_WITHIN(1e-4, 0.40824829f, q.y); + TEST_ASSERT_FLOAT_WITHIN(1e-4, 0.f, q.z); + TEST_ASSERT_FLOAT_WITHIN(1e-4, 0.81649658f, q.w); + + a = 0.5f; + spins = -1; + + res = BrQuatSlerp(&q, &l, &r, a, spins); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_FLOAT_WITHIN(1e-4, 0.707185f, q.x); + TEST_ASSERT_FLOAT_WITHIN(1e-4, -0.707185f, q.y); + TEST_ASSERT_FLOAT_WITHIN(1e-4, 0.f, q.z); + TEST_ASSERT_FLOAT_WITHIN(1e-4, 0.f, q.w); +} + +static void test_quat_BrQuatToMatrix34() { + br_matrix34 mat; + br_quat q; + br_matrix34 *res; + br_matrix34 expected; + + q.x = 0.f; + q.y = 0.f; + q.z = 0.f; + q.w = 1.f; + memset(&expected, 0, sizeof(expected)); + expected.m[0][0] = 1.f; + expected.m[1][1] = 1.f; + expected.m[2][2] = 1.f; + res = BrQuatToMatrix34(&mat, &q); + TEST_ASSERT_EQUAL_PTR(&mat, res); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 12); + + q.x = sqrtf(0.5f); + q.y = 0.f; + q.z = 0.f; + q.w = sqrtf(0.5f); + expected.m[0][0] = 1.f; + expected.m[0][1] = 0.f; + expected.m[0][2] = 0.f; + expected.m[1][0] = 0.f; + expected.m[1][1] = 0.f; + expected.m[1][2] = 1.f; + expected.m[2][0] = 0.f; + expected.m[2][1] = -1.f; + expected.m[2][2] = 0.f; + expected.m[3][0] = 0.f; + expected.m[3][1] = 0.f; + expected.m[3][2] = 0.f; + res = BrQuatToMatrix34(&mat, &q); + TEST_ASSERT_EQUAL_PTR(&mat, res); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-5, expected.m, mat.m, 12); +} + +static void test_quat_BrMatrix34ToQuat() { + br_quat q; + br_matrix34 mat; + br_quat *res; + br_quat expected; + + memset(&mat, 0, sizeof(mat)); + mat.m[0][0] = 1.f; + mat.m[1][1] = 1.f; + mat.m[2][2] = 1.f; + expected.x = 0.f; + expected.y = 0.f; + expected.z = 0.f; + expected.w = 1.f; + res = BrMatrix34ToQuat(&q, &mat); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.x, q.x); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.y, q.y); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.z, q.z); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.w, q.w); + + mat.m[0][0] = 1.f; + mat.m[0][1] = 0.f; + mat.m[0][2] = 0.f; + mat.m[1][0] = 0.f; + mat.m[1][1] = 0.f; + mat.m[1][2] = 1.f; + mat.m[2][0] = 0.f; + mat.m[2][1] = -1.f; + mat.m[2][2] = 0.f; + mat.m[3][0] = 0.f; + mat.m[3][1] = 0.f; + mat.m[3][2] = 0.f; + expected.x = sqrtf(0.5f); + expected.y = 0.f; + expected.z = 0.f; + expected.w = sqrtf(0.5f); + res = BrMatrix34ToQuat(&q, &mat); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.x, q.x); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.y, q.y); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.z, q.z); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.w, q.w); +} + +static void test_quat_BrQuatToMatrix4() { + br_matrix4 mat; + br_quat q; + br_matrix4 *res; + br_matrix4 expected; + + q.x = 0.f; + q.y = 0.f; + q.z = 0.f; + q.w = 1.f; + memset(&expected, 0, sizeof(expected)); + expected.m[0][0] = 1.f; + expected.m[1][1] = 1.f; + expected.m[2][2] = 1.f; + expected.m[3][3] = 1.f; + res = BrQuatToMatrix4(&mat, &q); + TEST_ASSERT_EQUAL_PTR(&mat, res); + TEST_ASSERT_EQUAL_FLOAT_ARRAY(expected.m, mat.m, 16); + + q.x = sqrtf(0.5f); + q.y = 0.f; + q.z = 0.f; + q.w = sqrtf(0.5f); + expected.m[0][0] = 1.f; + expected.m[0][1] = 0.f; + expected.m[0][2] = 0.f; + expected.m[0][3] = 0.f; + expected.m[1][0] = 0.f; + expected.m[1][1] = 0.f; + expected.m[1][2] = 1.f; + expected.m[1][3] = 0.f; + expected.m[2][0] = 0.f; + expected.m[2][1] = -1.f; + expected.m[2][2] = 0.f; + expected.m[2][3] = 0.f; + expected.m[3][0] = 0.f; + expected.m[3][1] = 0.f; + expected.m[3][2] = 0.f; + expected.m[3][3] = 1.f; + res = BrQuatToMatrix4(&mat, &q); + TEST_ASSERT_EQUAL_PTR(&mat, res); + TEST_ASSERT_FLOAT_ARRAY_WITHIN(1e-5, expected.m, mat.m, 16); +} + +static void test_quat_BrMatrix4ToQuat() { + br_quat q; + br_matrix4 mat; + br_quat *res; + br_quat expected; + + memset(&mat, 0, sizeof(mat)); + mat.m[0][0] = 1.f; + mat.m[1][1] = 1.f; + mat.m[2][2] = 1.f; + mat.m[3][3] = 1.f; + expected.x = 0.f; + expected.y = 0.f; + expected.z = 0.f; + expected.w = 1.f; + res = BrMatrix4ToQuat(&q, &mat); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.x, q.x); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.y, q.y); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.z, q.z); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.w, q.w); + + mat.m[0][0] = 1.f; + mat.m[0][1] = 0.f; + mat.m[0][2] = 0.f; + mat.m[0][3] = 0.f; + mat.m[1][0] = 0.f; + mat.m[1][1] = 0.f; + mat.m[1][2] = 1.f; + mat.m[1][3] = 0.f; + mat.m[2][0] = 0.f; + mat.m[2][1] = -1.f; + mat.m[2][2] = 0.f; + mat.m[2][3] = 0.f; + mat.m[3][0] = 0.f; + mat.m[3][1] = 0.f; + mat.m[3][2] = 0.f; + mat.m[3][3] = 1.f; + expected.x = sqrtf(0.5f); + expected.y = 0.f; + expected.z = 0.f; + expected.w = sqrtf(0.5f); + res = BrMatrix4ToQuat(&q, &mat); + TEST_ASSERT_EQUAL_PTR(&q, res); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.x, q.x); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.y, q.y); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.z, q.z); + TEST_ASSERT_FLOAT_WITHIN(1e-5, expected.w, q.w); +} + +void test_quat_suite() { + RUN_TEST(test_quat_BrQuatMul); + RUN_TEST(test_quat_BrQuatNormalise); + RUN_TEST(test_quat_BrQuatInvert); + RUN_TEST(test_quat_BrQuatSlerp); + RUN_TEST(test_quat_BrQuatToMatrix34); + RUN_TEST(test_quat_BrMatrix34ToQuat); + RUN_TEST(test_quat_BrQuatToMatrix4); + RUN_TEST(test_quat_BrMatrix4ToQuat); +} diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index b5045c07..3f5cd63a 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -32,6 +32,7 @@ target_sources(dethrace_test PRIVATE BRSRC13/test_genclip.c BRSRC13/test_pattern.c BRSRC13/test_pmfile.c + BRSRC13/test_quat.c BRSRC13/test_register.c BRSRC13/test_regsupt.c BRSRC13/test_resource.c @@ -53,4 +54,4 @@ target_sources(dethrace_test PRIVATE framework main.c tests.h -) \ No newline at end of file +) diff --git a/test/main.c b/test/main.c index 18c5195c..eded1441 100644 --- a/test/main.c +++ b/test/main.c @@ -45,6 +45,7 @@ extern void test_v1dbfile_suite(); extern void test_register_suite(); extern void test_pattern_suite(); extern void test_pmfile_suite(); +extern void test_quat_suite(); extern void test_graphics_suite(); extern void test_regsupt_suite(); extern void test_powerup_suite(); @@ -125,6 +126,7 @@ int main(int argc, char** argv) { test_pattern_suite(); test_pmfile_suite(); + test_quat_suite(); test_v1dbfile_suite(); test_regsupt_suite(); diff --git a/test/tests.h b/test/tests.h index c8b12a72..06325532 100644 --- a/test/tests.h +++ b/test/tests.h @@ -10,4 +10,12 @@ extern int has_data_directory(); if (!has_data_directory()) \ TEST_IGNORE(); -#endif \ No newline at end of file +#define TEST_ASSERT_FLOAT_ARRAY_WITHIN(delta, expected, actual, num_elements) { \ + float *priv_expected = (float*)(expected); \ + float *priv_actual = (float*)(actual); \ + for(int it = (num_elements); it != 0; --it, ++priv_expected, ++priv_actual) { \ + TEST_ASSERT_FLOAT_WITHIN((delta), *priv_expected, *priv_actual); \ + } \ +} + +#endif