From 281e4cecff387a40ab09151ecd6e723125389ed5 Mon Sep 17 00:00:00 2001 From: Stephen Seo Date: Sat, 27 Apr 2024 19:11:36 +0900 Subject: [PATCH] Matrix/Vector multiplication and changes Change Mat3 values to column-major (I think?) order. Add Matrix/Matrix multiplication and tweak Matrix/Vector multiplication. Fix Matrix rotation values since Mat3 representation changed. Added more UnitTest tests. --- src/sc_sacd.cpp | 116 ++++++++++++++++++++++++--------- src/sc_sacd.h | 15 ++++- src/test.cpp | 170 ++++++++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 266 insertions(+), 35 deletions(-) diff --git a/src/sc_sacd.cpp b/src/sc_sacd.cpp index c0695f9..c677511 100644 --- a/src/sc_sacd.cpp +++ b/src/sc_sacd.cpp @@ -27,6 +27,30 @@ SC_SACD_Vec3 operator/(const SC_SACD_Vec3 &a, float scalar) { return SC_SACD_Vec3{a.x / scalar, a.y / scalar, a.z / scalar}; } +SC_SACD_Mat3 operator*(const SC_SACD_Mat3 &a, const SC_SACD_Mat3 &b) { + SC_SACD_Mat3 mat; + + mat.x0 = b.x0 * a.x0 + b.y0 * a.x1 + b.z0 * a.x2; + mat.y0 = b.x0 * a.y0 + b.y0 * a.y1 + b.z0 * a.y2; + mat.z0 = b.x0 * a.z0 + b.y0 * a.z1 + b.z0 * a.z2; + + mat.x1 = b.x1 * a.x0 + b.y1 * a.x1 + b.z1 * a.x2; + mat.y1 = b.x1 * a.y0 + b.y1 * a.y1 + b.z1 * a.y2; + mat.z1 = b.x1 * a.z0 + b.y1 * a.z1 + b.z1 * a.z2; + + mat.x2 = b.x2 * a.x0 + b.y2 * a.x1 + b.z2 * a.x2; + mat.y2 = b.x2 * a.y0 + b.y2 * a.y1 + b.z2 * a.y2; + mat.z2 = b.x2 * a.z0 + b.y2 * a.z1 + b.z2 * a.z2; + + return mat; +} + +SC_SACD_Vec3 operator*(const SC_SACD_Mat3 &mat, const SC_SACD_Vec3 &vec) { + return SC_SACD_Vec3{vec.x * mat.x0 + vec.y * mat.x1 + vec.z * mat.x2, + vec.x * mat.y0 + vec.y * mat.y1 + vec.z * mat.y2, + vec.x * mat.z0 + vec.y * mat.z1 + vec.z * mat.z2}; +} + std::vector SC_SACD_Get_Box_Normals( const SC_SACD_Generic_Box *box) { std::vector normals; @@ -294,13 +318,17 @@ SC_SACD_Vec3 SC_SACD_Cross_Product(const SC_SACD_Vec3 a, const SC_SACD_Vec3 b) { a.x * b.y - a.y * b.x}; } +SC_SACD_Mat3 SC_SACD_Mat3_Identity(void) { + return SC_SACD_Mat3{1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}; +} + +SC_SACD_Mat3 SC_SACD_Mat3_Mult(const SC_SACD_Mat3 *a, const SC_SACD_Mat3 *b) { + return (*a) * (*b); +} + SC_SACD_Vec3 SC_SACD_Mat3_Vec3_Mult(const SC_SACD_Mat3 *mat, const SC_SACD_Vec3 vec) { - return SC_SACD_Vec3{ - vec.x * mat->x0 + vec.y * mat->y0 + vec.z * mat->z0, - vec.x * mat->x1 + vec.y * mat->y1 + vec.z * mat->z1, - vec.x * mat->x2 + vec.y * mat->y2 + vec.z * mat->z2, - }; + return (*mat) * vec; } SC_SACD_Vec3 SC_SACD_Vec3_Rotate(const SC_SACD_Vec3 vec, float x_axis, @@ -326,45 +354,69 @@ SC_SACD_Vec3 SC_SACD_Vec3_Rotate(const SC_SACD_Vec3 vec, float x_axis, SC_SACD_Vec3 result; // About x_axis. - mat.x0 = 1.0F; - mat.y0 = 0.0F; - mat.z0 = 0.0F; - mat.x1 = 0.0F; - mat.y1 = std::cos(x_axis); - mat.z1 = -std::sin(x_axis); - mat.x2 = 0.0F; - mat.y2 = -mat.z1; - mat.z2 = mat.y1; + mat = SC_SACD_Rotation_Mat3_XAxis(x_axis); result = SC_SACD_Mat3_Vec3_Mult(&mat, vec); // About y_axis. - mat.x0 = std::cos(y_axis); - mat.y0 = 0.0F; - mat.z0 = std::sin(y_axis); - mat.x1 = 0.0F; - mat.y1 = 1.0F; - mat.z1 = 0.0F; - mat.x2 = -mat.z0; - mat.y2 = 0.0F; - mat.z2 = mat.x0; + mat = SC_SACD_Rotation_Mat3_YAxis(y_axis); result = SC_SACD_Mat3_Vec3_Mult(&mat, result); // About z_axis. - mat.x0 = std::cos(z_axis); - mat.y0 = -std::sin(z_axis); - mat.z0 = 0.0F; - mat.x1 = -mat.y0; - mat.y1 = mat.x0; - mat.z1 = 0.0F; - mat.x2 = 0.0F; - mat.y2 = 0.0F; - mat.z2 = 1.0F; + mat = SC_SACD_Rotation_Mat3_ZAxis(z_axis); return SC_SACD_Mat3_Vec3_Mult(&mat, result); } +SC_SACD_Mat3 SC_SACD_Rotation_Mat3_XAxis(float x_radians) { + SC_SACD_Mat3 mat; + + mat.x0 = 1.0F; + mat.x1 = 0.0F; + mat.x2 = 0.0F; + mat.y0 = 0.0F; + mat.y1 = std::cos(x_radians); + mat.y2 = -std::sin(x_radians); + mat.z0 = 0.0F; + mat.z1 = -mat.y2; + mat.z2 = mat.y1; + + return mat; +} + +SC_SACD_Mat3 SC_SACD_Rotation_Mat3_YAxis(float y_radians) { + SC_SACD_Mat3 mat; + + mat.x0 = std::cos(y_radians); + mat.x1 = 0.0F; + mat.x2 = std::sin(y_radians); + mat.y0 = 0.0F; + mat.y1 = 1.0F; + mat.y2 = 0.0F; + mat.z0 = -mat.x2; + mat.z1 = 0.0F; + mat.z2 = mat.x0; + + return mat; +} + +SC_SACD_Mat3 SC_SACD_Rotation_Mat3_ZAxis(float z_radians) { + SC_SACD_Mat3 mat; + + mat.x0 = std::cos(z_radians); + mat.x1 = -std::sin(z_radians); + mat.x2 = 0.0F; + mat.y0 = -mat.x1; + mat.y1 = mat.x0; + mat.y2 = 0.0F; + mat.z0 = 0.0F; + mat.z1 = 0.0F; + mat.z2 = 1.0F; + + return mat; +} + SC_SACD_Vec3 SC_SACD_Closest_Point_Dir_Normalized(const SC_SACD_Vec3 *pos, const SC_SACD_Vec3 *dir, const SC_SACD_Vec3 *point) { diff --git a/src/sc_sacd.h b/src/sc_sacd.h index 38e1f0a..dc45db4 100644 --- a/src/sc_sacd.h +++ b/src/sc_sacd.h @@ -25,9 +25,9 @@ typedef struct SC_SACD_EXPORT SC_SACD_Vec3 { } SC_SACD_Vec3; typedef struct SC_SACD_EXPORT SC_SACD_Mat3 { - float x0, y0, z0; - float x1, y1, z1; - float x2, y2, z2; + float x0, x1, x2; + float y0, y1, y2; + float z0, z1, z2; } SC_SACD_Mat3; typedef struct SC_SACD_EXPORT SC_SACD_AABB_Box { @@ -94,6 +94,11 @@ SC_SACD_EXPORT float SC_SACD_Dot_Product(const SC_SACD_Vec3 a, SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Cross_Product(const SC_SACD_Vec3 a, const SC_SACD_Vec3 b); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Mat3_Identity(void); + +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Mat3_Mult(const SC_SACD_Mat3 *a, + const SC_SACD_Mat3 *b); + SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Mat3_Vec3_Mult(const SC_SACD_Mat3 *mat, const SC_SACD_Vec3 vec); @@ -101,6 +106,10 @@ SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Vec3_Rotate(const SC_SACD_Vec3 vec, float x_axis, float y_axis, float z_axis); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Rotation_Mat3_XAxis(float x_radians); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Rotation_Mat3_YAxis(float y_radians); +SC_SACD_EXPORT SC_SACD_Mat3 SC_SACD_Rotation_Mat3_ZAxis(float z_radians); + SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Closest_Point_Dir_Normalized( const SC_SACD_Vec3 *pos, const SC_SACD_Vec3 *dir, const SC_SACD_Vec3 *point); diff --git a/src/test.cpp b/src/test.cpp index 667341c..a1f3c9f 100644 --- a/src/test.cpp +++ b/src/test.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -25,6 +26,16 @@ static int checks_passed = 0; } \ } while (false); +#define CHECK_FLOAT(var, value) \ + do { \ + ++checks_checked; \ + if ((var) > (value)-0.0001F && (var) < (value) + 0.0001F) { \ + ++checks_passed; \ + } else { \ + std::cout << "CHECK_FLOAT at line " << __LINE__ << " failed!\n"; \ + } \ + } while (false); + #include "sc_sacd.h" int main() { @@ -358,6 +369,165 @@ int main() { CHECK_TRUE(SC_SACD_Sphere_Box_Collision(&sphere, &box)); } + // Test matrix/vector multiplication. + { + SC_SACD_Mat3 mat_a{1.0F, 2.0F, 3.0F, 4.0F, 5.0F, 6.0F, 7.0F, 8.0F, 9.0F}; + + SC_SACD_Mat3 mat_b{1.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 1.0F}; + + { + auto result = SC_SACD_Mat3_Mult(&mat_a, &mat_b); + CHECK_TRUE(mat_a.x0 == result.x0); + CHECK_TRUE(mat_a.x1 == result.x1); + CHECK_TRUE(mat_a.x2 == result.x2); + CHECK_TRUE(mat_a.y0 == result.y0); + CHECK_TRUE(mat_a.y1 == result.y1); + CHECK_TRUE(mat_a.y2 == result.y2); + CHECK_TRUE(mat_a.z0 == result.z0); + CHECK_TRUE(mat_a.z1 == result.z1); + CHECK_TRUE(mat_a.z2 == result.z2); + } + + mat_b.x0 = 2.0F; + mat_b.y1 = 0.0F; + mat_b.z2 = 0.0F; + { + auto result = SC_SACD_Mat3_Mult(&mat_a, &mat_b); + CHECK_FLOAT(result.x0, 2.0F); + CHECK_FLOAT(result.y0, 8.0F); + CHECK_FLOAT(result.z0, 14.0F); + CHECK_FLOAT(result.x1, 0.0F); + CHECK_FLOAT(result.y1, 0.0F); + CHECK_FLOAT(result.z1, 0.0F); + CHECK_FLOAT(result.x2, 0.0F); + CHECK_FLOAT(result.y2, 0.0F); + CHECK_FLOAT(result.z2, 0.0F); + } + + mat_b = SC_SACD_Mat3_Identity(); + SC_SACD_Vec3 vec_a{1.0F, 0.0F, 0.0F}; + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_b, vec_a); + CHECK_TRUE(result.x == vec_a.x); + CHECK_TRUE(result.y == vec_a.y); + CHECK_TRUE(result.z == vec_a.z); + } + + // Rotations about each axis. + mat_a = SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v / 2.0F); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F); + CHECK_TRUE(result.y < 1.0001F && result.y > 0.9999F); + CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F); + } + + mat_a = SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_TRUE(result.x < -0.9999F && result.x > -1.0001F); + CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F); + CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F); + } + + mat_a = + SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v * 3.0F / 2.0F); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F); + CHECK_TRUE(result.y < -0.9999F && result.y > -1.0001F); + CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F); + } + + mat_a = SC_SACD_Rotation_Mat3_XAxis(std::numbers::pi_v / 2.0F); + vec_a.x = 0.0F; + vec_a.y = 1.0F; + vec_a.z = 0.0F; + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F); + CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F); + CHECK_TRUE(result.z < 1.0001F && result.z > 0.9999F); + } + + mat_a = SC_SACD_Rotation_Mat3_XAxis(std::numbers::pi_v); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F); + CHECK_TRUE(result.y < -0.9999F && result.y > -1.0001F); + CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F); + } + + mat_a = + SC_SACD_Rotation_Mat3_XAxis(std::numbers::pi_v * 3.0F / 2.0F); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F); + CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F); + CHECK_TRUE(result.z < -0.9999F && result.z > -1.0001F); + } + + mat_a = SC_SACD_Rotation_Mat3_YAxis(std::numbers::pi_v / 2.0F); + vec_a.x = 0.0F; + vec_a.y = 0.0F; + vec_a.z = 1.0F; + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_TRUE(result.x < 1.0001F && result.x > 0.9999F); + CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F); + CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F); + } + + mat_a = SC_SACD_Rotation_Mat3_YAxis(std::numbers::pi_v); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_TRUE(result.x < 0.0001F && result.x > -0.0001F); + CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F); + CHECK_TRUE(result.z < -0.9999F && result.z > -1.0001F); + } + + mat_a = + SC_SACD_Rotation_Mat3_YAxis(std::numbers::pi_v * 3.0F / 2.0F); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_TRUE(result.x < -0.9999F && result.x > -1.0001F); + CHECK_TRUE(result.y < 0.0001F && result.y > -0.0001F); + CHECK_TRUE(result.z < 0.0001F && result.z > -0.0001F); + } + + // Combined axis rotation. + vec_a.x = 1.0F; + vec_a.y = 0.0F; + vec_a.z = 0.0F; + mat_a = SC_SACD_Rotation_Mat3_YAxis(std::numbers::pi_v / 4.0F); + mat_b = SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v / 4.0F); + // Apply mat_a, then mat_b. + mat_a = SC_SACD_Mat3_Mult(&mat_b, &mat_a); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_FLOAT(result.x, 0.5F); + CHECK_FLOAT(result.y, 0.5F); + CHECK_FLOAT(result.z, -std::sqrt(2.0F) / 2.0F); + } + // Apply another rotation on combined mat_a. + mat_b = SC_SACD_Rotation_Mat3_ZAxis(std::numbers::pi_v / 4.0F); + mat_a = SC_SACD_Mat3_Mult(&mat_b, &mat_a); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_FLOAT(result.x, 0.0F); + CHECK_FLOAT(result.y, std::sqrt(2.0F) / 2.0F); + CHECK_FLOAT(result.z, -std::sqrt(2.0F) / 2.0F); + } + // Apply another rotation on combined mat_a. + mat_b = SC_SACD_Rotation_Mat3_XAxis(std::numbers::pi_v / 2.0F); + mat_a = SC_SACD_Mat3_Mult(&mat_b, &mat_a); + { + auto result = SC_SACD_Mat3_Vec3_Mult(&mat_a, vec_a); + CHECK_FLOAT(result.x, 0.0F); + CHECK_FLOAT(result.y, std::sqrt(2.0F) / 2.0F); + CHECK_FLOAT(result.z, std::sqrt(2.0F) / 2.0F); + } + } std::cout << "Checks checked: " << checks_checked << '\n' << "Checks passed: " << checks_passed << '\n';