From 85b881428608a2b6b7cbd36281f51216eeb52153 Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 12:25:45 +0700 Subject: [PATCH 01/10] refactor object constructors --- includes/physic/object.hpp | 9 ++++++--- tests/test_collision.cpp | 4 ++-- tests/test_collision_2.cpp | 2 +- 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/includes/physic/object.hpp b/includes/physic/object.hpp index cf57fd9..f1dffc1 100644 --- a/includes/physic/object.hpp +++ b/includes/physic/object.hpp @@ -12,11 +12,15 @@ class Geometry; class Object { private: + // Linear property double mass; - Quat4 rotation; Vec3 position; Vec3 velocity; + // Angular property + Quat4 rotation; + + // Overal property std::vector forces; std::unique_ptr geometry; @@ -25,13 +29,12 @@ class Object public: Object(double mass, std::unique_ptr geom) : mass(mass), position(Vec3()), rotation(Quat4()), velocity(Vec3()), geometry(std::move(geom)) { SetGeometryOwner(); } - Object(double mass, Vec3 position, Quat4 rotation, std::unique_ptr geom) - : mass(mass), position(position), rotation(rotation), geometry(std::move(geom)), velocity(Vec3()) { SetGeometryOwner(); } double GetMass() const { return mass; } const Vec3 &GetVelocity() const { return velocity; } const Vec3 &GetPosition() const { return position; } const Quat4 &GetRotation() const { return rotation; } + const Quat4 &GetAngularVelocity() const { return angular_velocity; } const std::vector &GetForces() const { return forces; } void SetMass(double m) diff --git a/tests/test_collision.cpp b/tests/test_collision.cpp index 527038b..70ca06b 100644 --- a/tests/test_collision.cpp +++ b/tests/test_collision.cpp @@ -3,8 +3,8 @@ #include "object.hpp" -Object obj1(1, Vec3(), Quat4(), nullptr); -Object obj2(1, Vec3(), Quat4(), nullptr); +Object obj1(1, nullptr); +Object obj2(1, nullptr); TEST(BoxBoxCollision, OverlappingBoxes) { Box b1(Vec3(1, 1, 1), &obj1); diff --git a/tests/test_collision_2.cpp b/tests/test_collision_2.cpp index 282a7bf..5d7d98c 100644 --- a/tests/test_collision_2.cpp +++ b/tests/test_collision_2.cpp @@ -3,7 +3,7 @@ #include "math/geometry.hpp" #include "physic/object.hpp" -Object obj(1, Vec3(), Quat4(), nullptr); +Object obj(1, nullptr); TEST(SphereCollisionTest, OverlappingSpheresCollide) { Sphere s1(2.0, &obj); From 8debba921879ef4f84d75cc86a73476e9cf35462 Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 12:43:19 +0700 Subject: [PATCH 02/10] implement rotational matrix with quaternion-conversion --- includes/math/matrices.hpp | 105 ++++++++++++++++++++++++++++++++++ includes/math/quaternions.hpp | 4 ++ sources/math/quaternions.cpp | 19 ++++++ tests/test_conversion.cpp | 72 +++++++++++++++++++++++ tests/test_matrix.cpp | 76 ++++++++++++++++++++++++ 5 files changed, 276 insertions(+) create mode 100644 includes/math/matrices.hpp create mode 100644 sources/math/quaternions.cpp create mode 100644 tests/test_conversion.cpp create mode 100644 tests/test_matrix.cpp diff --git a/includes/math/matrices.hpp b/includes/math/matrices.hpp new file mode 100644 index 0000000..f47e216 --- /dev/null +++ b/includes/math/matrices.hpp @@ -0,0 +1,105 @@ +#ifndef MATRICES_HPP +#define MATRICES_HPP + +#include "vectors.hpp" +#include "quaternions.hpp" + +struct Matrix3x3 +{ + double data[9]; + + Matrix3x3() + { + data[0] = 0.0; + data[1] = 0.0; + data[2] = 0.0; + data[3] = 0.0; + data[4] = 0.0; + data[5] = 0.0; + data[6] = 0.0; + data[7] = 0.0; + data[8] = 0.0; + }; + double &operator()(int row, int col) + { + return data[row * 3 + col]; + } + const double &operator()(int row, int col) const + { + return data[row * 3 + col]; + } + Matrix3x3 operator*(const Matrix3x3 &other) const + { + Matrix3x3 result; + for (int row = 0; row < 3; ++row) + { + for (int col = 0; col < 3; ++col) + { + result(row, col) = 0.0; + for (int k = 0; k < 3; ++k) + { + result(row, col) += (*this)(row, k) * other(k, col); + } + } + } + return result; + } + Vec3 operator*(const Vec3 &vec) const + { + return Vec3( + data[0] * vec.x + data[1] * vec.y + data[2] * vec.z, + data[3] * vec.x + data[4] * vec.y + data[5] * vec.z, + data[6] * vec.x + data[7] * vec.y + data[8] * vec.z); + } + + Quat4 ToQuaternion() const + { + // Convert the 3x3 matrix to a quaternion + double trace = data[0] + data[4] + data[8]; + double w, x, y, z; + if (trace > 0) + { + double s = sqrt(trace + 1.0) * 2; // S= 4 * w + w = 0.25 * s; + x = (data[7] - data[5]) / s; + y = (data[2] - data[6]) / s; + z = (data[3] - data[1]) / s; + } + else if ((data[0] > data[4]) && (data[0] > data[8])) + { + double s = sqrt(1.0 + data[0] - data[4] - data[8]) * 2; // S= 4 * x + w = (data[7] - data[5]) / s; + x = 0.25 * s; + y = (data[1] + data[3]) / s; + z = (data[2] + data[6]) / s; + } + else if (data[4] > data[8]) + { + double s = sqrt(1.0 + data[4] - data[0] - data[8]) * 2; // S= 4 * y + w = (data[2] - data[6]) / s; + x = (data[1] + data[3]) / s; + y = 0.25 * s; + z = (data[5] + data[7]) / s; + } + else + { + double s = sqrt(1.0 + data[8] - data[0] - data[4]) * 2; // S= 4 * z + w = (data[3] - data[1]) / s; + x = (data[2] + data[6]) / s; + y = (data[5] + data[7]) / s; + z = 0.25 * s; + } + return Quat4(w, x, y, z); + } + + static Matrix3x3 Identity() + { + Matrix3x3 identity; + identity(0, 0) = 1.0; + identity(1, 1) = 1.0; + identity(2, 2) = 1.0; + return identity; + } +}; + +#endif \ No newline at end of file diff --git a/includes/math/quaternions.hpp b/includes/math/quaternions.hpp index 4c399df..f4efb3f 100644 --- a/includes/math/quaternions.hpp +++ b/includes/math/quaternions.hpp @@ -3,6 +3,8 @@ #include "vectors.hpp" +struct Matrix3x3; + struct Quat4 { double w, x, y, z; @@ -61,6 +63,8 @@ struct Quat4 z = w1 * z2 + w2 * z1 + x1 * y2 - y1 * x2; return *this; } + + Matrix3x3 ToMatrix() const; }; inline Quat4 operator+(Quat4 l, const Quat4 &r) { return l += r; } diff --git a/sources/math/quaternions.cpp b/sources/math/quaternions.cpp new file mode 100644 index 0000000..f1e2c1c --- /dev/null +++ b/sources/math/quaternions.cpp @@ -0,0 +1,19 @@ +#include "quaternions.hpp" +#include "matrices.hpp" + +Matrix3x3 Quat4::ToMatrix() const +{ + { + Matrix3x3 M; + M(0, 0) = 1 - 2 * (y * y + z * z); + M(0, 1) = 2 * (x * y - w * z); + M(0, 2) = 2 * (x * z + w * y); + M(1, 0) = 2 * (x * y + w * z); + M(1, 1) = 1 - 2 * (x * x + z * z); + M(1, 2) = 2 * (y * z - w * x); + M(2, 0) = 2 * (x * z - w * y); + M(2, 1) = 2 * (y * z + w * x); + M(2, 2) = 1 - 2 * (x * x + y * y); + return M; + } +} \ No newline at end of file diff --git a/tests/test_conversion.cpp b/tests/test_conversion.cpp new file mode 100644 index 0000000..0a5203d --- /dev/null +++ b/tests/test_conversion.cpp @@ -0,0 +1,72 @@ +#include +#include "math/matrices.hpp" +#include "math/quaternions.hpp" + +TEST(QuaternionMatrixTest, QuaternionToMatrixAndBackIdentity) +{ + Quat4 q; // Identity quaternion + Matrix3x3 m = q.ToMatrix(); + Quat4 q2 = m.ToQuaternion(); + EXPECT_NEAR(q2.w, 1.0, 1e-12); + EXPECT_NEAR(q2.x, 0.0, 1e-12); + EXPECT_NEAR(q2.y, 0.0, 1e-12); + EXPECT_NEAR(q2.z, 0.0, 1e-12); +} + +TEST(QuaternionMatrixTest, QuaternionToMatrixAndBack90degX) +{ + double angle = M_PI / 2; + double s = sin(angle / 2); + double c = cos(angle / 2); + Quat4 q(c, s, 0, 0); // 90 deg around X + Matrix3x3 m = q.ToMatrix(); + Quat4 q2 = m.ToQuaternion(); + // q and q2 may differ in sign, so compare absolute values + EXPECT_NEAR(fabs(q2.w), fabs(q.w), 1e-12); + EXPECT_NEAR(fabs(q2.x), fabs(q.x), 1e-12); + EXPECT_NEAR(fabs(q2.y), fabs(q.y), 1e-12); + EXPECT_NEAR(fabs(q2.z), fabs(q.z), 1e-12); +} + +TEST(QuaternionMatrixTest, QuaternionToMatrixAndBack90degY) +{ + double angle = M_PI / 2; + double s = sin(angle / 2); + double c = cos(angle / 2); + Quat4 q(c, 0, s, 0); // 90 deg around Y + Matrix3x3 m = q.ToMatrix(); + Quat4 q2 = m.ToQuaternion(); + EXPECT_NEAR(fabs(q2.w), fabs(q.w), 1e-12); + EXPECT_NEAR(fabs(q2.x), fabs(q.x), 1e-12); + EXPECT_NEAR(fabs(q2.y), fabs(q.y), 1e-12); + EXPECT_NEAR(fabs(q2.z), fabs(q.z), 1e-12); +} + +TEST(QuaternionMatrixTest, QuaternionToMatrixAndBack90degZ) +{ + double angle = M_PI / 2; + double s = sin(angle / 2); + double c = cos(angle / 2); + Quat4 q(c, 0, 0, s); // 90 deg around Z + Matrix3x3 m = q.ToMatrix(); + Quat4 q2 = m.ToQuaternion(); + EXPECT_NEAR(fabs(q2.w), fabs(q.w), 1e-12); + EXPECT_NEAR(fabs(q2.x), fabs(q.x), 1e-12); + EXPECT_NEAR(fabs(q2.y), fabs(q.y), 1e-12); + EXPECT_NEAR(fabs(q2.z), fabs(q.z), 1e-12); +} + +TEST(QuaternionMatrixTest, MatrixRotatesVector) +{ + // 90 deg rotation around Z axis + double angle = M_PI / 2; + double s = sin(angle / 2); + double c = cos(angle / 2); + Quat4 q(c, 0, 0, s); + Matrix3x3 m = q.ToMatrix(); + Vec3 v(1, 0, 0); + Vec3 r = m * v; + EXPECT_NEAR(r.x, 0.0, 1e-12); + EXPECT_NEAR(r.y, 1.0, 1e-12); + EXPECT_NEAR(r.z, 0.0, 1e-12); +} diff --git a/tests/test_matrix.cpp b/tests/test_matrix.cpp new file mode 100644 index 0000000..1d63a96 --- /dev/null +++ b/tests/test_matrix.cpp @@ -0,0 +1,76 @@ +#include +#include "math/matrices.hpp" +#include "math/quaternions.hpp" +#include "math/vectors.hpp" + +TEST(Matrix3x3Test, IdentityMatrix) { + Matrix3x3 I = Matrix3x3::Identity(); + EXPECT_DOUBLE_EQ(I(0,0), 1.0); + EXPECT_DOUBLE_EQ(I(1,1), 1.0); + EXPECT_DOUBLE_EQ(I(2,2), 1.0); + EXPECT_DOUBLE_EQ(I(0,1), 0.0); + EXPECT_DOUBLE_EQ(I(0,2), 0.0); + EXPECT_DOUBLE_EQ(I(1,0), 0.0); + EXPECT_DOUBLE_EQ(I(1,2), 0.0); + EXPECT_DOUBLE_EQ(I(2,0), 0.0); + EXPECT_DOUBLE_EQ(I(2,1), 0.0); +} + +TEST(Matrix3x3Test, MatrixVectorMultiplication) { + Matrix3x3 M; + M(0,0) = 1; M(0,1) = 2; M(0,2) = 3; + M(1,0) = 4; M(1,1) = 5; M(1,2) = 6; + M(2,0) = 7; M(2,1) = 8; M(2,2) = 9; + Vec3 v(1, 2, 3); + Vec3 result = M * v; + EXPECT_DOUBLE_EQ(result.x, 1*1 + 2*2 + 3*3); + EXPECT_DOUBLE_EQ(result.y, 4*1 + 5*2 + 6*3); + EXPECT_DOUBLE_EQ(result.z, 7*1 + 8*2 + 9*3); +} + +TEST(Matrix3x3Test, MatrixMatrixMultiplication) { + Matrix3x3 A; + A(0,0) = 1; A(0,1) = 2; A(0,2) = 3; + A(1,0) = 4; A(1,1) = 5; A(1,2) = 6; + A(2,0) = 7; A(2,1) = 8; A(2,2) = 9; + + Matrix3x3 B; + B(0,0) = 9; B(0,1) = 8; B(0,2) = 7; + B(1,0) = 6; B(1,1) = 5; B(1,2) = 4; + B(2,0) = 3; B(2,1) = 2; B(2,2) = 1; + + Matrix3x3 C = A * B; + EXPECT_DOUBLE_EQ(C(0,0), 1*9 + 2*6 + 3*3); + EXPECT_DOUBLE_EQ(C(0,1), 1*8 + 2*5 + 3*2); + EXPECT_DOUBLE_EQ(C(0,2), 1*7 + 2*4 + 3*1); + EXPECT_DOUBLE_EQ(C(1,0), 4*9 + 5*6 + 6*3); + EXPECT_DOUBLE_EQ(C(1,1), 4*8 + 5*5 + 6*2); + EXPECT_DOUBLE_EQ(C(1,2), 4*7 + 5*4 + 6*1); + EXPECT_DOUBLE_EQ(C(2,0), 7*9 + 8*6 + 9*3); + EXPECT_DOUBLE_EQ(C(2,1), 7*8 + 8*5 + 9*2); + EXPECT_DOUBLE_EQ(C(2,2), 7*7 + 8*4 + 9*1); +} + +TEST(Matrix3x3Test, ZeroMatrix) { + Matrix3x3 Z; + for(int i=0;i<3;++i) + for(int j=0;j<3;++j) + EXPECT_DOUBLE_EQ(Z(i,j), 0.0); +} + +TEST(Matrix3x3Test, MatrixSelfMultiplication) { + Matrix3x3 M; + M(0,0) = 2; M(0,1) = 0; M(0,2) = 0; + M(1,0) = 0; M(1,1) = 3; M(1,2) = 0; + M(2,0) = 0; M(2,1) = 0; M(2,2) = 4; + Matrix3x3 R = M * M; + EXPECT_DOUBLE_EQ(R(0,0), 4); + EXPECT_DOUBLE_EQ(R(1,1), 9); + EXPECT_DOUBLE_EQ(R(2,2), 16); + EXPECT_DOUBLE_EQ(R(0,1), 0); + EXPECT_DOUBLE_EQ(R(0,2), 0); + EXPECT_DOUBLE_EQ(R(1,0), 0); + EXPECT_DOUBLE_EQ(R(1,2), 0); + EXPECT_DOUBLE_EQ(R(2,0), 0); + EXPECT_DOUBLE_EQ(R(2,1), 0); +} From 4a502630f9467f93ca350a65735122a26baa0456 Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 13:03:10 +0700 Subject: [PATCH 03/10] Add inertial matrix and quaternion velocity --- includes/physic/object.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/includes/physic/object.hpp b/includes/physic/object.hpp index f1dffc1..c0ac62d 100644 --- a/includes/physic/object.hpp +++ b/includes/physic/object.hpp @@ -6,6 +6,7 @@ #include "force.hpp" #include "vectors.hpp" #include "quaternions.hpp" +#include "matrices.hpp" class Geometry; @@ -18,7 +19,9 @@ class Object Vec3 velocity; // Angular property + Matrix3x3 inertia_matrix; Quat4 rotation; + Quat4 angular_velocity; // Overal property std::vector forces; @@ -33,6 +36,7 @@ class Object double GetMass() const { return mass; } const Vec3 &GetVelocity() const { return velocity; } const Vec3 &GetPosition() const { return position; } + const Matrix3x3 &GetInertiaMatrix() const { return inertia_matrix; } const Quat4 &GetRotation() const { return rotation; } const Quat4 &GetAngularVelocity() const { return angular_velocity; } const std::vector &GetForces() const { return forces; } From 00590a54eabe9a0820737c25e25c96f0c0182e4f Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 14:19:33 +0700 Subject: [PATCH 04/10] Add `matrix` transpose. --- includes/math/matrices.hpp | 24 +++++++++++++++++++++++- tests/test_matrix.cpp | 17 +++++++++++++++++ 2 files changed, 40 insertions(+), 1 deletion(-) diff --git a/includes/math/matrices.hpp b/includes/math/matrices.hpp index f47e216..b1c2aba 100644 --- a/includes/math/matrices.hpp +++ b/includes/math/matrices.hpp @@ -20,6 +20,21 @@ struct Matrix3x3 data[7] = 0.0; data[8] = 0.0; }; + + Matrix3x3(double d0, double d1, double d2, + double d3, double d4, double d5, + double d6, double d7, double d8) + { + data[0] = d0; + data[1] = d1; + data[2] = d2; + data[3] = d3; + data[4] = d4; + data[5] = d5; + data[6] = d6; + data[7] = d7; + data[8] = d8; + } double &operator()(int row, int col) { return data[row * 3 + col]; @@ -91,7 +106,14 @@ struct Matrix3x3 } return Quat4(w, x, y, z); } - + Matrix3x3 Transpose() const { + return Matrix3x3( + data[0], data[3], data[6], + data[1], data[4], data[7], + data[2], data[5], data[8] + ); + } + static Matrix3x3 Identity() { Matrix3x3 identity; diff --git a/tests/test_matrix.cpp b/tests/test_matrix.cpp index 1d63a96..0be7c6e 100644 --- a/tests/test_matrix.cpp +++ b/tests/test_matrix.cpp @@ -74,3 +74,20 @@ TEST(Matrix3x3Test, MatrixSelfMultiplication) { EXPECT_DOUBLE_EQ(R(2,0), 0); EXPECT_DOUBLE_EQ(R(2,1), 0); } + +TEST(Matrix3x3Test, TransposeMatrix) { + Matrix3x3 M; + M(0,0) = 1; M(0,1) = 2; M(0,2) = 3; + M(1,0) = 4; M(1,1) = 5; M(1,2) = 6; + M(2,0) = 7; M(2,1) = 8; M(2,2) = 9; + Matrix3x3 MT = M.Transpose(); + EXPECT_DOUBLE_EQ(MT(0, 0), 1); + EXPECT_DOUBLE_EQ(MT(0, 1), 4); + EXPECT_DOUBLE_EQ(MT(0, 2), 7); + EXPECT_DOUBLE_EQ(MT(1, 0), 2); + EXPECT_DOUBLE_EQ(MT(1, 1), 5); + EXPECT_DOUBLE_EQ(MT(1, 2), 8); + EXPECT_DOUBLE_EQ(MT(2, 0), 3); + EXPECT_DOUBLE_EQ(MT(2, 1), 6); + EXPECT_DOUBLE_EQ(MT(2, 2), 9); +} \ No newline at end of file From 0ec115851e1a7bc80e1b80b495bb0468eb981b30 Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 20:41:03 +0700 Subject: [PATCH 05/10] Additional mathematical concepts --- includes/math/matrices.hpp | 71 +++++++----------------------------- includes/math/vectors.hpp | 5 +++ sources/math/matrices.cpp | 58 +++++++++++++++++++++++++++++ sources/math/vectors.cpp | 10 +++++ tests/test_matrix.cpp | 14 +++++++ tests/test_vec3.cpp | 75 ++++++++++++++++++++++++++++++++------ 6 files changed, 164 insertions(+), 69 deletions(-) create mode 100644 sources/math/matrices.cpp create mode 100644 sources/math/vectors.cpp diff --git a/includes/math/matrices.hpp b/includes/math/matrices.hpp index b1c2aba..17354a2 100644 --- a/includes/math/matrices.hpp +++ b/includes/math/matrices.hpp @@ -43,22 +43,14 @@ struct Matrix3x3 { return data[row * 3 + col]; } - Matrix3x3 operator*(const Matrix3x3 &other) const - { - Matrix3x3 result; - for (int row = 0; row < 3; ++row) - { - for (int col = 0; col < 3; ++col) - { - result(row, col) = 0.0; - for (int k = 0; k < 3; ++k) - { - result(row, col) += (*this)(row, k) * other(k, col); - } - } - } - return result; + + Matrix3x3 &operator*=(double scalar) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + (*this)(i, j) *= scalar; + return *this; } + Matrix3x3 &operator*=(const Matrix3x3 &other); Vec3 operator*(const Vec3 &vec) const { return Vec3( @@ -66,54 +58,16 @@ struct Matrix3x3 data[3] * vec.x + data[4] * vec.y + data[5] * vec.z, data[6] * vec.x + data[7] * vec.y + data[8] * vec.z); } + + Quat4 ToQuaternion() const; - Quat4 ToQuaternion() const + Matrix3x3 Transpose() const { - // Convert the 3x3 matrix to a quaternion - double trace = data[0] + data[4] + data[8]; - double w, x, y, z; - if (trace > 0) - { - double s = sqrt(trace + 1.0) * 2; // S= 4 * w - w = 0.25 * s; - x = (data[7] - data[5]) / s; - y = (data[2] - data[6]) / s; - z = (data[3] - data[1]) / s; - } - else if ((data[0] > data[4]) && (data[0] > data[8])) - { - double s = sqrt(1.0 + data[0] - data[4] - data[8]) * 2; // S= 4 * x - w = (data[7] - data[5]) / s; - x = 0.25 * s; - y = (data[1] + data[3]) / s; - z = (data[2] + data[6]) / s; - } - else if (data[4] > data[8]) - { - double s = sqrt(1.0 + data[4] - data[0] - data[8]) * 2; // S= 4 * y - w = (data[2] - data[6]) / s; - x = (data[1] + data[3]) / s; - y = 0.25 * s; - z = (data[5] + data[7]) / s; - } - else - { - double s = sqrt(1.0 + data[8] - data[0] - data[4]) * 2; // S= 4 * z - w = (data[3] - data[1]) / s; - x = (data[2] + data[6]) / s; - y = (data[5] + data[7]) / s; - z = 0.25 * s; - } - return Quat4(w, x, y, z); - } - Matrix3x3 Transpose() const { return Matrix3x3( data[0], data[3], data[6], data[1], data[4], data[7], - data[2], data[5], data[8] - ); + data[2], data[5], data[8]); } - static Matrix3x3 Identity() { Matrix3x3 identity; @@ -124,4 +78,7 @@ struct Matrix3x3 } }; +inline Matrix3x3 operator*(Matrix3x3 &l, const Matrix3x3 &r) { return l *= r; } +inline Matrix3x3 operator*(Matrix3x3 &l, double scalar) { return l *= scalar; } + #endif \ No newline at end of file diff --git a/includes/math/vectors.hpp b/includes/math/vectors.hpp index 9cdb5c1..c984f52 100644 --- a/includes/math/vectors.hpp +++ b/includes/math/vectors.hpp @@ -4,6 +4,9 @@ #include #include "config.hpp" +// Forward declaraction +struct Matrix3x3; + // Vectors struct Vec3 { @@ -43,6 +46,8 @@ struct Vec3 return *this; } + Matrix3x3 SelfInverseOuter() const; + constexpr double Dot(const Vec3 &other) const { return x * other.x + y * other.y + z * other.z; diff --git a/sources/math/matrices.cpp b/sources/math/matrices.cpp new file mode 100644 index 0000000..62a9af6 --- /dev/null +++ b/sources/math/matrices.cpp @@ -0,0 +1,58 @@ +#include "matrices.hpp" + +Matrix3x3 &Matrix3x3::operator*=(const Matrix3x3 &other) +{ + Matrix3x3 result; + for (int row = 0; row < 3; ++row) + { + for (int col = 0; col < 3; ++col) + { + result(row, col) = 0.0; + for (int k = 0; k < 3; ++k) + { + result(row, col) += (*this)(row, k) * other(k, col); + } + } + } + *this = result; + return *this; +} +Quat4 Matrix3x3::ToQuaternion() const +{ + // Convert the 3x3 matrix to a quaternion + double trace = data[0] + data[4] + data[8]; + double w, x, y, z; + if (trace > 0) + { + double s = sqrt(trace + 1.0) * 2; // S= 4 * w + w = 0.25 * s; + x = (data[7] - data[5]) / s; + y = (data[2] - data[6]) / s; + z = (data[3] - data[1]) / s; + } + else if ((data[0] > data[4]) && (data[0] > data[8])) + { + double s = sqrt(1.0 + data[0] - data[4] - data[8]) * 2; // S= 4 * x + w = (data[7] - data[5]) / s; + x = 0.25 * s; + y = (data[1] + data[3]) / s; + z = (data[2] + data[6]) / s; + } + else if (data[4] > data[8]) + { + double s = sqrt(1.0 + data[4] - data[0] - data[8]) * 2; // S= 4 * y + w = (data[2] - data[6]) / s; + x = (data[1] + data[3]) / s; + y = 0.25 * s; + z = (data[5] + data[7]) / s; + } + else + { + double s = sqrt(1.0 + data[8] - data[0] - data[4]) * 2; // S= 4 * z + w = (data[3] - data[1]) / s; + x = (data[2] + data[6]) / s; + y = (data[5] + data[7]) / s; + z = 0.25 * s; + } + return Quat4(w, x, y, z); +} diff --git a/sources/math/vectors.cpp b/sources/math/vectors.cpp new file mode 100644 index 0000000..35b8900 --- /dev/null +++ b/sources/math/vectors.cpp @@ -0,0 +1,10 @@ +#include "vectors.hpp" +#include "matrices.hpp" + +Matrix3x3 Vec3::SelfInverseOuter() const +{ + return Matrix3x3( + x * x, x * y, x * z, + y * x, y * y, y * z, + z * x, z * y, z * z); +} \ No newline at end of file diff --git a/tests/test_matrix.cpp b/tests/test_matrix.cpp index 0be7c6e..89bcdee 100644 --- a/tests/test_matrix.cpp +++ b/tests/test_matrix.cpp @@ -51,6 +51,20 @@ TEST(Matrix3x3Test, MatrixMatrixMultiplication) { EXPECT_DOUBLE_EQ(C(2,2), 7*7 + 8*4 + 9*1); } +TEST(Matrix3x3Test, MatrixScalarMultiplication) { + Matrix3x3 A(1, 2, 3, 4, 5, 6, 7, 8, 9); + A *= 2; + EXPECT_DOUBLE_EQ(A(0, 0), 2); + EXPECT_DOUBLE_EQ(A(0, 1), 4); + EXPECT_DOUBLE_EQ(A(0, 2), 6); + EXPECT_DOUBLE_EQ(A(1, 0), 8); + EXPECT_DOUBLE_EQ(A(1, 1), 10); + EXPECT_DOUBLE_EQ(A(1, 2), 12); + EXPECT_DOUBLE_EQ(A(2, 0), 14); + EXPECT_DOUBLE_EQ(A(2, 1), 16); + EXPECT_DOUBLE_EQ(A(2, 2), 18); +} + TEST(Matrix3x3Test, ZeroMatrix) { Matrix3x3 Z; for(int i=0;i<3;++i) diff --git a/tests/test_vec3.cpp b/tests/test_vec3.cpp index 79a087f..6beacdd 100644 --- a/tests/test_vec3.cpp +++ b/tests/test_vec3.cpp @@ -1,21 +1,25 @@ #include #include "vectors.hpp" +#include "matrices.hpp" -TEST(Vec3Test, DefaultConstructor) { +TEST(Vec3Test, DefaultConstructor) +{ Vec3 v; EXPECT_DOUBLE_EQ(v.x, 0); EXPECT_DOUBLE_EQ(v.y, 0); EXPECT_DOUBLE_EQ(v.z, 0); } -TEST(Vec3Test, ParameterizedConstructor) { +TEST(Vec3Test, ParameterizedConstructor) +{ Vec3 v(1.0, 2.0, 3.0); EXPECT_DOUBLE_EQ(v.x, 1.0); EXPECT_DOUBLE_EQ(v.y, 2.0); EXPECT_DOUBLE_EQ(v.z, 3.0); } -TEST(Vec3Test, Addition) { +TEST(Vec3Test, Addition) +{ Vec3 v1(1, 2, 3); Vec3 v2(4, 5, 6); Vec3 v3 = v1 + v2; @@ -24,7 +28,8 @@ TEST(Vec3Test, Addition) { EXPECT_DOUBLE_EQ(v3.z, 9); } -TEST(Vec3Test, Subtraction) { +TEST(Vec3Test, Subtraction) +{ Vec3 v1(4, 5, 6); Vec3 v2(1, 2, 3); Vec3 v3 = v1 - v2; @@ -33,7 +38,8 @@ TEST(Vec3Test, Subtraction) { EXPECT_DOUBLE_EQ(v3.z, 3); } -TEST(Vec3Test, ScalarMultiplication) { +TEST(Vec3Test, ScalarMultiplication) +{ Vec3 v(1, 2, 3); Vec3 result = v * 2.0; EXPECT_DOUBLE_EQ(result.x, 2); @@ -41,7 +47,8 @@ TEST(Vec3Test, ScalarMultiplication) { EXPECT_DOUBLE_EQ(result.z, 6); } -TEST(Vec3Test, ScalarDivision) { +TEST(Vec3Test, ScalarDivision) +{ Vec3 v(2, 4, 6); Vec3 result = v / 2.0; EXPECT_DOUBLE_EQ(result.x, 1); @@ -49,14 +56,16 @@ TEST(Vec3Test, ScalarDivision) { EXPECT_DOUBLE_EQ(result.z, 3); } -TEST(Vec3Test, DotProduct) { +TEST(Vec3Test, DotProduct) +{ Vec3 v1(1, 2, 3); Vec3 v2(4, -5, 6); double dot = v1 * v2; EXPECT_DOUBLE_EQ(dot, 12); } -TEST(Vec3Test, CrossProduct) { +TEST(Vec3Test, CrossProduct) +{ Vec3 v1(1, 0, 0); Vec3 v2(0, 1, 0); Vec3 cross = v1 ^ v2; @@ -65,7 +74,8 @@ TEST(Vec3Test, CrossProduct) { EXPECT_DOUBLE_EQ(cross.z, 1); } -TEST(Vec3Test, Normalize) { +TEST(Vec3Test, Normalize) +{ Vec3 v(3, 0, 4); v.Normalize(); EXPECT_NEAR(v.x, 0.6, 1e-9); @@ -73,23 +83,64 @@ TEST(Vec3Test, Normalize) { EXPECT_NEAR(v.z, 0.8, 1e-9); } -TEST(Vec3Test, Equality) { +TEST(Vec3Test, Equality) +{ Vec3 v1(1, 2, 3); Vec3 v2(1, 2, 3); EXPECT_TRUE(v1 == v2); } -TEST(Vec3Test, Inequality) { +TEST(Vec3Test, Inequality) +{ Vec3 v1(1, 2, 3); Vec3 v2(3, 2, 1); EXPECT_FALSE(v1 == v2); } -TEST(Vec3Test, IsZero) { +TEST(Vec3Test, IsZero) +{ Vec3 v1(0, 0, 0); Vec3 v2(1, 0, 4); EXPECT_TRUE(v1.IsZero()); EXPECT_FALSE(v2.IsZero()); } +TEST(Vec3Test, SelfInverseOuterProduct) +{ + // Test with a non-zero vector + Vec3 v(1, 2, 3); + Matrix3x3 outer = v.SelfInverseOuter(); + // The outer product of v with itself (v * v^T) should be a 3x3 matrix: + // [1 2 3] + // [2 4 6] + // [3 6 9] + EXPECT_DOUBLE_EQ(outer(0, 0), 1); + EXPECT_DOUBLE_EQ(outer(0, 1), 2); + EXPECT_DOUBLE_EQ(outer(0, 2), 3); + EXPECT_DOUBLE_EQ(outer(1, 0), 2); + EXPECT_DOUBLE_EQ(outer(1, 1), 4); + EXPECT_DOUBLE_EQ(outer(1, 2), 6); + EXPECT_DOUBLE_EQ(outer(2, 0), 3); + EXPECT_DOUBLE_EQ(outer(2, 1), 6); + EXPECT_DOUBLE_EQ(outer(2, 2), 9); + + // Test with a zero vector + Vec3 zero(0, 0, 0); + Matrix3x3 zero_outer = zero.SelfInverseOuter(); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + EXPECT_DOUBLE_EQ(zero_outer(i, j), 0); + // Test with negative values + Vec3 neg(-1, -2, -3); + Matrix3x3 neg_outer = neg.SelfInverseOuter(); + EXPECT_DOUBLE_EQ(neg_outer(0, 0), 1); + EXPECT_DOUBLE_EQ(neg_outer(0, 1), 2); + EXPECT_DOUBLE_EQ(neg_outer(0, 2), 3); + EXPECT_DOUBLE_EQ(neg_outer(1, 0), 2); + EXPECT_DOUBLE_EQ(neg_outer(1, 1), 4); + EXPECT_DOUBLE_EQ(neg_outer(1, 2), 6); + EXPECT_DOUBLE_EQ(neg_outer(2, 0), 3); + EXPECT_DOUBLE_EQ(neg_outer(2, 1), 6); + EXPECT_DOUBLE_EQ(neg_outer(2, 2), 9); +} From 5c24257b959a9133cff03ab7536370b68337213e Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 20:43:05 +0700 Subject: [PATCH 06/10] fix some merging conflicts --- includes/math/matrices.hpp | 14 ++++++++++++++ includes/math/vectors.hpp | 3 +++ 2 files changed, 17 insertions(+) diff --git a/includes/math/matrices.hpp b/includes/math/matrices.hpp index 17354a2..48992ed 100644 --- a/includes/math/matrices.hpp +++ b/includes/math/matrices.hpp @@ -44,6 +44,14 @@ struct Matrix3x3 return data[row * 3 + col]; } + Matrix3x3 &operator*=(double scalar) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + (*this)(i, j) *= scalar; + return *this; + } + Matrix3x3 &operator*=(const Matrix3x3 &other); + Matrix3x3 &operator*=(double scalar) { for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) @@ -61,12 +69,18 @@ struct Matrix3x3 Quat4 ToQuaternion() const; + Matrix3x3 Transpose() const + { + + Quat4 ToQuaternion() const; + Matrix3x3 Transpose() const { return Matrix3x3( data[0], data[3], data[6], data[1], data[4], data[7], data[2], data[5], data[8]); + data[2], data[5], data[8]); } static Matrix3x3 Identity() { diff --git a/includes/math/vectors.hpp b/includes/math/vectors.hpp index c984f52..c408f79 100644 --- a/includes/math/vectors.hpp +++ b/includes/math/vectors.hpp @@ -7,6 +7,9 @@ // Forward declaraction struct Matrix3x3; +// Forward declaraction +struct Matrix3x3; + // Vectors struct Vec3 { From 5902df8107b919073df5a40ad2a487ee8ef99419 Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 20:57:25 +0700 Subject: [PATCH 07/10] fix more merging problem --- includes/math/matrices.hpp | 36 ++++++++++++------------------------ 1 file changed, 12 insertions(+), 24 deletions(-) diff --git a/includes/math/matrices.hpp b/includes/math/matrices.hpp index 48992ed..d0f2128 100644 --- a/includes/math/matrices.hpp +++ b/includes/math/matrices.hpp @@ -43,35 +43,16 @@ struct Matrix3x3 { return data[row * 3 + col]; } - - Matrix3x3 &operator*=(double scalar) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - (*this)(i, j) *= scalar; - return *this; - } - Matrix3x3 &operator*=(const Matrix3x3 &other); - - Matrix3x3 &operator*=(double scalar) { - for (int i = 0; i < 3; i++) + + Matrix3x3 &operator*=(double scalar) + { + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) (*this)(i, j) *= scalar; return *this; } Matrix3x3 &operator*=(const Matrix3x3 &other); - Vec3 operator*(const Vec3 &vec) const - { - return Vec3( - data[0] * vec.x + data[1] * vec.y + data[2] * vec.z, - data[3] * vec.x + data[4] * vec.y + data[5] * vec.z, - data[6] * vec.x + data[7] * vec.y + data[8] * vec.z); - } - - Quat4 ToQuaternion() const; - Matrix3x3 Transpose() const - { - Quat4 ToQuaternion() const; Matrix3x3 Transpose() const @@ -80,7 +61,6 @@ struct Matrix3x3 data[0], data[3], data[6], data[1], data[4], data[7], data[2], data[5], data[8]); - data[2], data[5], data[8]); } static Matrix3x3 Identity() { @@ -94,5 +74,13 @@ struct Matrix3x3 inline Matrix3x3 operator*(Matrix3x3 &l, const Matrix3x3 &r) { return l *= r; } inline Matrix3x3 operator*(Matrix3x3 &l, double scalar) { return l *= scalar; } +inline Vec3 operator*(const Vec3 &vec, const Matrix3x3 &matrix) +{ + return Vec3( + matrix(0, 0) * vec.x + matrix(0, 1) * vec.y + matrix(0, 2) * vec.z, + matrix(1, 0) * vec.x + matrix(1, 1) * vec.y + matrix(1, 2) * vec.z, + matrix(2, 0) * vec.x + matrix(2, 1) * vec.y + matrix(2 ,2) * vec.z); +} +inline Vec3 operator*(const Matrix3x3 &m, const Vec3 &vec) { return vec*m; } #endif \ No newline at end of file From eb3d785347257939899352158dceec012f326b5d Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 21:11:04 +0700 Subject: [PATCH 08/10] Add `matrix` addition and subtraction --- includes/math/matrices.hpp | 18 +++- tests/test_matrix.cpp | 184 +++++++++++++++++++++++++------------ 2 files changed, 142 insertions(+), 60 deletions(-) diff --git a/includes/math/matrices.hpp b/includes/math/matrices.hpp index d0f2128..c27d498 100644 --- a/includes/math/matrices.hpp +++ b/includes/math/matrices.hpp @@ -52,6 +52,18 @@ struct Matrix3x3 return *this; } Matrix3x3 &operator*=(const Matrix3x3 &other); + Matrix3x3 &operator+=(const Matrix3x3 &other) + { + for (int i = 0; i < 9; i++) + data[i] += other.data[i]; + return *this; + } + Matrix3x3 &operator-=(const Matrix3x3 &other) + { + for (int i = 0; i < 9; i++) + data[i] -= other.data[i]; + return *this; + } Quat4 ToQuaternion() const; @@ -79,8 +91,10 @@ inline Vec3 operator*(const Vec3 &vec, const Matrix3x3 &matrix) return Vec3( matrix(0, 0) * vec.x + matrix(0, 1) * vec.y + matrix(0, 2) * vec.z, matrix(1, 0) * vec.x + matrix(1, 1) * vec.y + matrix(1, 2) * vec.z, - matrix(2, 0) * vec.x + matrix(2, 1) * vec.y + matrix(2 ,2) * vec.z); + matrix(2, 0) * vec.x + matrix(2, 1) * vec.y + matrix(2, 2) * vec.z); } -inline Vec3 operator*(const Matrix3x3 &m, const Vec3 &vec) { return vec*m; } +inline Vec3 operator*(const Matrix3x3 &m, const Vec3 &vec) { return vec * m; } +inline Matrix3x3 operator+(Matrix3x3 &l, const Matrix3x3 &r) { return l += r; } +inline Matrix3x3 operator-(Matrix3x3 &l, const Matrix3x3 &r) { return l -= r; } #endif \ No newline at end of file diff --git a/tests/test_matrix.cpp b/tests/test_matrix.cpp index 89bcdee..390a3db 100644 --- a/tests/test_matrix.cpp +++ b/tests/test_matrix.cpp @@ -3,55 +3,77 @@ #include "math/quaternions.hpp" #include "math/vectors.hpp" -TEST(Matrix3x3Test, IdentityMatrix) { +TEST(Matrix3x3Test, IdentityMatrix) +{ Matrix3x3 I = Matrix3x3::Identity(); - EXPECT_DOUBLE_EQ(I(0,0), 1.0); - EXPECT_DOUBLE_EQ(I(1,1), 1.0); - EXPECT_DOUBLE_EQ(I(2,2), 1.0); - EXPECT_DOUBLE_EQ(I(0,1), 0.0); - EXPECT_DOUBLE_EQ(I(0,2), 0.0); - EXPECT_DOUBLE_EQ(I(1,0), 0.0); - EXPECT_DOUBLE_EQ(I(1,2), 0.0); - EXPECT_DOUBLE_EQ(I(2,0), 0.0); - EXPECT_DOUBLE_EQ(I(2,1), 0.0); + EXPECT_DOUBLE_EQ(I(0, 0), 1.0); + EXPECT_DOUBLE_EQ(I(1, 1), 1.0); + EXPECT_DOUBLE_EQ(I(2, 2), 1.0); + EXPECT_DOUBLE_EQ(I(0, 1), 0.0); + EXPECT_DOUBLE_EQ(I(0, 2), 0.0); + EXPECT_DOUBLE_EQ(I(1, 0), 0.0); + EXPECT_DOUBLE_EQ(I(1, 2), 0.0); + EXPECT_DOUBLE_EQ(I(2, 0), 0.0); + EXPECT_DOUBLE_EQ(I(2, 1), 0.0); } -TEST(Matrix3x3Test, MatrixVectorMultiplication) { +TEST(Matrix3x3Test, MatrixVectorMultiplication) +{ Matrix3x3 M; - M(0,0) = 1; M(0,1) = 2; M(0,2) = 3; - M(1,0) = 4; M(1,1) = 5; M(1,2) = 6; - M(2,0) = 7; M(2,1) = 8; M(2,2) = 9; + M(0, 0) = 1; + M(0, 1) = 2; + M(0, 2) = 3; + M(1, 0) = 4; + M(1, 1) = 5; + M(1, 2) = 6; + M(2, 0) = 7; + M(2, 1) = 8; + M(2, 2) = 9; Vec3 v(1, 2, 3); Vec3 result = M * v; - EXPECT_DOUBLE_EQ(result.x, 1*1 + 2*2 + 3*3); - EXPECT_DOUBLE_EQ(result.y, 4*1 + 5*2 + 6*3); - EXPECT_DOUBLE_EQ(result.z, 7*1 + 8*2 + 9*3); + EXPECT_DOUBLE_EQ(result.x, 1 * 1 + 2 * 2 + 3 * 3); + EXPECT_DOUBLE_EQ(result.y, 4 * 1 + 5 * 2 + 6 * 3); + EXPECT_DOUBLE_EQ(result.z, 7 * 1 + 8 * 2 + 9 * 3); } -TEST(Matrix3x3Test, MatrixMatrixMultiplication) { +TEST(Matrix3x3Test, MatrixMatrixMultiplication) +{ Matrix3x3 A; - A(0,0) = 1; A(0,1) = 2; A(0,2) = 3; - A(1,0) = 4; A(1,1) = 5; A(1,2) = 6; - A(2,0) = 7; A(2,1) = 8; A(2,2) = 9; + A(0, 0) = 1; + A(0, 1) = 2; + A(0, 2) = 3; + A(1, 0) = 4; + A(1, 1) = 5; + A(1, 2) = 6; + A(2, 0) = 7; + A(2, 1) = 8; + A(2, 2) = 9; Matrix3x3 B; - B(0,0) = 9; B(0,1) = 8; B(0,2) = 7; - B(1,0) = 6; B(1,1) = 5; B(1,2) = 4; - B(2,0) = 3; B(2,1) = 2; B(2,2) = 1; + B(0, 0) = 9; + B(0, 1) = 8; + B(0, 2) = 7; + B(1, 0) = 6; + B(1, 1) = 5; + B(1, 2) = 4; + B(2, 0) = 3; + B(2, 1) = 2; + B(2, 2) = 1; Matrix3x3 C = A * B; - EXPECT_DOUBLE_EQ(C(0,0), 1*9 + 2*6 + 3*3); - EXPECT_DOUBLE_EQ(C(0,1), 1*8 + 2*5 + 3*2); - EXPECT_DOUBLE_EQ(C(0,2), 1*7 + 2*4 + 3*1); - EXPECT_DOUBLE_EQ(C(1,0), 4*9 + 5*6 + 6*3); - EXPECT_DOUBLE_EQ(C(1,1), 4*8 + 5*5 + 6*2); - EXPECT_DOUBLE_EQ(C(1,2), 4*7 + 5*4 + 6*1); - EXPECT_DOUBLE_EQ(C(2,0), 7*9 + 8*6 + 9*3); - EXPECT_DOUBLE_EQ(C(2,1), 7*8 + 8*5 + 9*2); - EXPECT_DOUBLE_EQ(C(2,2), 7*7 + 8*4 + 9*1); + EXPECT_DOUBLE_EQ(C(0, 0), 1 * 9 + 2 * 6 + 3 * 3); + EXPECT_DOUBLE_EQ(C(0, 1), 1 * 8 + 2 * 5 + 3 * 2); + EXPECT_DOUBLE_EQ(C(0, 2), 1 * 7 + 2 * 4 + 3 * 1); + EXPECT_DOUBLE_EQ(C(1, 0), 4 * 9 + 5 * 6 + 6 * 3); + EXPECT_DOUBLE_EQ(C(1, 1), 4 * 8 + 5 * 5 + 6 * 2); + EXPECT_DOUBLE_EQ(C(1, 2), 4 * 7 + 5 * 4 + 6 * 1); + EXPECT_DOUBLE_EQ(C(2, 0), 7 * 9 + 8 * 6 + 9 * 3); + EXPECT_DOUBLE_EQ(C(2, 1), 7 * 8 + 8 * 5 + 9 * 2); + EXPECT_DOUBLE_EQ(C(2, 2), 7 * 7 + 8 * 4 + 9 * 1); } -TEST(Matrix3x3Test, MatrixScalarMultiplication) { +TEST(Matrix3x3Test, MatrixScalarMultiplication) +{ Matrix3x3 A(1, 2, 3, 4, 5, 6, 7, 8, 9); A *= 2; EXPECT_DOUBLE_EQ(A(0, 0), 2); @@ -65,35 +87,81 @@ TEST(Matrix3x3Test, MatrixScalarMultiplication) { EXPECT_DOUBLE_EQ(A(2, 2), 18); } -TEST(Matrix3x3Test, ZeroMatrix) { +TEST(Matrix3x3Test, ZeroMatrix) +{ Matrix3x3 Z; - for(int i=0;i<3;++i) - for(int j=0;j<3;++j) - EXPECT_DOUBLE_EQ(Z(i,j), 0.0); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + EXPECT_DOUBLE_EQ(Z(i, j), 0.0); } -TEST(Matrix3x3Test, MatrixSelfMultiplication) { +TEST(Matrix3x3Test, MatrixSelfMultiplication) +{ Matrix3x3 M; - M(0,0) = 2; M(0,1) = 0; M(0,2) = 0; - M(1,0) = 0; M(1,1) = 3; M(1,2) = 0; - M(2,0) = 0; M(2,1) = 0; M(2,2) = 4; + M(0, 0) = 2; + M(0, 1) = 0; + M(0, 2) = 0; + M(1, 0) = 0; + M(1, 1) = 3; + M(1, 2) = 0; + M(2, 0) = 0; + M(2, 1) = 0; + M(2, 2) = 4; Matrix3x3 R = M * M; - EXPECT_DOUBLE_EQ(R(0,0), 4); - EXPECT_DOUBLE_EQ(R(1,1), 9); - EXPECT_DOUBLE_EQ(R(2,2), 16); - EXPECT_DOUBLE_EQ(R(0,1), 0); - EXPECT_DOUBLE_EQ(R(0,2), 0); - EXPECT_DOUBLE_EQ(R(1,0), 0); - EXPECT_DOUBLE_EQ(R(1,2), 0); - EXPECT_DOUBLE_EQ(R(2,0), 0); - EXPECT_DOUBLE_EQ(R(2,1), 0); + EXPECT_DOUBLE_EQ(R(0, 0), 4); + EXPECT_DOUBLE_EQ(R(1, 1), 9); + EXPECT_DOUBLE_EQ(R(2, 2), 16); + EXPECT_DOUBLE_EQ(R(0, 1), 0); + EXPECT_DOUBLE_EQ(R(0, 2), 0); + EXPECT_DOUBLE_EQ(R(1, 0), 0); + EXPECT_DOUBLE_EQ(R(1, 2), 0); + EXPECT_DOUBLE_EQ(R(2, 0), 0); + EXPECT_DOUBLE_EQ(R(2, 1), 0); } -TEST(Matrix3x3Test, TransposeMatrix) { +TEST(Matrix3x3Test, MatrixAddition) +{ + Matrix3x3 A(1, 2, 3, 4, 5, 6, 7, 8, 9); + Matrix3x3 B(9, 8, 7, 6, 5, 4, 3, 2, 1); + A += B; + EXPECT_DOUBLE_EQ(A.data[0], 10); + EXPECT_DOUBLE_EQ(A.data[1], 10); + EXPECT_DOUBLE_EQ(A.data[2], 10); + EXPECT_DOUBLE_EQ(A.data[3], 10); + EXPECT_DOUBLE_EQ(A.data[4], 10); + EXPECT_DOUBLE_EQ(A.data[5], 10); + EXPECT_DOUBLE_EQ(A.data[6], 10); + EXPECT_DOUBLE_EQ(A.data[7], 10); + EXPECT_DOUBLE_EQ(A.data[8], 10); +} +TEST(Matrix3x3Test, MatrixSubtraction) +{ + Matrix3x3 A(1, 2, 3, 4, 5, 6, 7, 8, 9); + Matrix3x3 B(9, 8, 7, 6, 5, 4, 3, 2, 1); + B -= A; + EXPECT_DOUBLE_EQ(B.data[0], 8); + EXPECT_DOUBLE_EQ(B.data[1], 6); + EXPECT_DOUBLE_EQ(B.data[2], 4); + EXPECT_DOUBLE_EQ(B.data[3], 2); + EXPECT_DOUBLE_EQ(B.data[4], 0); + EXPECT_DOUBLE_EQ(B.data[5], -2); + EXPECT_DOUBLE_EQ(B.data[6], -4); + EXPECT_DOUBLE_EQ(B.data[7], -6); + EXPECT_DOUBLE_EQ(B.data[8], -8); +} + +TEST(Matrix3x3Test, TransposeMatrix) +{ Matrix3x3 M; - M(0,0) = 1; M(0,1) = 2; M(0,2) = 3; - M(1,0) = 4; M(1,1) = 5; M(1,2) = 6; - M(2,0) = 7; M(2,1) = 8; M(2,2) = 9; + M(0, 0) = 1; + M(0, 1) = 2; + M(0, 2) = 3; + M(1, 0) = 4; + M(1, 1) = 5; + M(1, 2) = 6; + M(2, 0) = 7; + M(2, 1) = 8; + M(2, 2) = 9; Matrix3x3 MT = M.Transpose(); EXPECT_DOUBLE_EQ(MT(0, 0), 1); EXPECT_DOUBLE_EQ(MT(0, 1), 4); @@ -101,7 +169,7 @@ TEST(Matrix3x3Test, TransposeMatrix) { EXPECT_DOUBLE_EQ(MT(1, 0), 2); EXPECT_DOUBLE_EQ(MT(1, 1), 5); EXPECT_DOUBLE_EQ(MT(1, 2), 8); - EXPECT_DOUBLE_EQ(MT(2, 0), 3); - EXPECT_DOUBLE_EQ(MT(2, 1), 6); - EXPECT_DOUBLE_EQ(MT(2, 2), 9); + EXPECT_DOUBLE_EQ(MT(2, 0), 3); + EXPECT_DOUBLE_EQ(MT(2, 1), 6); + EXPECT_DOUBLE_EQ(MT(2, 2), 9); } \ No newline at end of file From 8bf6b2bfc9dd7e6eb08b267cb45ffd0744b0dedc Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 21:17:55 +0700 Subject: [PATCH 09/10] fix matrices implementation for accepting l-rvalues --- includes/math/matrices.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/includes/math/matrices.hpp b/includes/math/matrices.hpp index c27d498..1b26c2b 100644 --- a/includes/math/matrices.hpp +++ b/includes/math/matrices.hpp @@ -84,8 +84,8 @@ struct Matrix3x3 } }; -inline Matrix3x3 operator*(Matrix3x3 &l, const Matrix3x3 &r) { return l *= r; } -inline Matrix3x3 operator*(Matrix3x3 &l, double scalar) { return l *= scalar; } +inline Matrix3x3 operator*(Matrix3x3 l, const Matrix3x3 &r) { return l *= r; } +inline Matrix3x3 operator*(Matrix3x3 l, double scalar) { return l *= scalar; } inline Vec3 operator*(const Vec3 &vec, const Matrix3x3 &matrix) { return Vec3( @@ -94,7 +94,7 @@ inline Vec3 operator*(const Vec3 &vec, const Matrix3x3 &matrix) matrix(2, 0) * vec.x + matrix(2, 1) * vec.y + matrix(2, 2) * vec.z); } inline Vec3 operator*(const Matrix3x3 &m, const Vec3 &vec) { return vec * m; } -inline Matrix3x3 operator+(Matrix3x3 &l, const Matrix3x3 &r) { return l += r; } -inline Matrix3x3 operator-(Matrix3x3 &l, const Matrix3x3 &r) { return l -= r; } +inline Matrix3x3 operator+(Matrix3x3 l, const Matrix3x3 &r) { return l += r; } +inline Matrix3x3 operator-(Matrix3x3 l, const Matrix3x3 &r) { return l -= r; } #endif \ No newline at end of file From 97c8fddaade19c2548c3cd42d302ed3df26673db Mon Sep 17 00:00:00 2001 From: mrunknown850 Date: Sun, 22 Jun 2025 21:40:00 +0700 Subject: [PATCH 10/10] Implement self-determining angular inertia for sphere and box base on Parallel Axis Theorem --- includes/math/geometry.hpp | 6 +++ sources/math/geometry.cpp | 33 +++++++++++++ sources/physic/object.cpp | 3 ++ tests/test_geometry_inertia.cpp | 84 +++++++++++++++++++++++++++++++++ 4 files changed, 126 insertions(+) create mode 100644 tests/test_geometry_inertia.cpp diff --git a/includes/math/geometry.hpp b/includes/math/geometry.hpp index 238eb0a..47f5b14 100644 --- a/includes/math/geometry.hpp +++ b/includes/math/geometry.hpp @@ -4,6 +4,7 @@ #include #include "vectors.hpp" #include "quaternions.hpp" +#include "matrices.hpp" class Object; @@ -27,10 +28,12 @@ class Geometry virtual GeometryType GetType() const = 0; virtual bool CollidesWith(const Geometry &other) const = 0; + virtual Matrix3x3 GetLocalInertiaMatrix() const = 0; const Vec3 &GetLocalOffset() const { return offset; } const Quat4 &GetLocalRotation() const { return rotation; } Vec3 GetWorldCenter() const; Quat4 GetWorldRotation() const; + void SetOwner(Object *obj) { object = obj; } @@ -82,6 +85,8 @@ class Box : public Geometry std::vector GetNormalAxis() const; + Matrix3x3 GetLocalInertiaMatrix() const override; + const Vec3 &GetHalfExtends() const { return halfExtends; } void SetHalfExtends(double _x, double _y, double _z) { @@ -103,6 +108,7 @@ class Sphere : public Geometry { GeometryType GetType() const override { return GeometryType::SPHERE; } bool CollidesWith(const Geometry &other) const override; + Matrix3x3 GetLocalInertiaMatrix() const override; double GetRadius() const { return radius; } void SetRadius(double _r) { this->radius = _r; } diff --git a/sources/math/geometry.cpp b/sources/math/geometry.cpp index 0613366..8063f59 100644 --- a/sources/math/geometry.cpp +++ b/sources/math/geometry.cpp @@ -148,6 +148,39 @@ bool Sphere::CollidesWith(const Geometry &other) const return false; } +// Box specifics +Matrix3x3 Box::GetLocalInertiaMatrix() const +{ + double mass = this->object->GetMass(); + Matrix3x3 inertiaMatrix; + const Vec3 fullExtends = this->GetHalfExtends() * 2.0; + const Vec3 offset = this->GetLocalOffset(); + + inertiaMatrix(0, 0) = 1.0 / 12.0 * mass * (fullExtends.y * fullExtends.y + fullExtends.z * fullExtends.z); + inertiaMatrix(1, 1) = 1.0 / 12.0 * mass * (fullExtends.x * fullExtends.x + fullExtends.z * fullExtends.z); + inertiaMatrix(2, 2) = 1.0 / 12.0 * mass * (fullExtends.y * fullExtends.y + fullExtends.x * fullExtends.x); + + Matrix3x3 rotationMatrix = this->GetWorldRotation().ToMatrix(); + Matrix3x3 rotatedMatrix = (rotationMatrix * inertiaMatrix) * rotationMatrix.Transpose(); + Matrix3x3 res = rotatedMatrix + (Matrix3x3::Identity() * offset.LengthSquared() - offset.SelfInverseOuter()) * mass; + + return res; +} + +Matrix3x3 Sphere::GetLocalInertiaMatrix() const +{ + double mass = this->object->GetMass(); + double radius = this->GetRadius(); + const Vec3 offset = this->GetLocalOffset(); + + double sphere_angular_inertia = 2.0 / 5.0 * mass * radius * radius; + Matrix3x3 inertiaLocal = Matrix3x3::Identity() * sphere_angular_inertia; + Matrix3x3 offsetOuter = offset.SelfInverseOuter(); + Matrix3x3 paralelAxis = (Matrix3x3::Identity() * offset.LengthSquared() - offsetOuter) * mass; + Matrix3x3 res = inertiaLocal + paralelAxis; + return res; +} + // General functions Vec3 Geometry::GetWorldCenter() const { return offset + object->GetPosition(); } Quat4 Geometry::GetWorldRotation() const diff --git a/sources/physic/object.cpp b/sources/physic/object.cpp index 900432f..9e1519c 100644 --- a/sources/physic/object.cpp +++ b/sources/physic/object.cpp @@ -4,6 +4,9 @@ void Object::SetGeometryOwner() { if (geometry) + { geometry->SetOwner(this); + inertia_matrix = geometry->GetLocalInertiaMatrix(); + } } Object::~Object() = default; diff --git a/tests/test_geometry_inertia.cpp b/tests/test_geometry_inertia.cpp new file mode 100644 index 0000000..66106e6 --- /dev/null +++ b/tests/test_geometry_inertia.cpp @@ -0,0 +1,84 @@ +// filepath: /home/MrUnknown850/Documents/cphysic/tests/test_geometry_inertia.cpp +#include +#include "math/geometry.hpp" +#include "physic/object.hpp" +#include +#include + +TEST(BoxInertiaMatrixTest, AxisAlignedBoxAtOrigin) +{ + // Box of size 2x4x6 (halfExtends = 1,2,3), mass = 12, at origin, no rotation, no offset + auto box = std::make_unique(Vec3{1, 2, 3}, nullptr); + Object obj(12.0, std::move(box)); + const Matrix3x3& inertia = obj.GetInertiaMatrix(); + + // Analytical inertia for box at origin, axis-aligned: + // Ixx = 1/12 * m * (h^2 + d^2) + // Iyy = 1/12 * m * (w^2 + d^2) + // Izz = 1/12 * m * (w^2 + h^2) + double w = 2.0, h = 4.0, d = 6.0, m = 12.0; + double Ixx = 1.0/12.0 * m * (h*h + d*d); + double Iyy = 1.0/12.0 * m * (w*w + d*d); + double Izz = 1.0/12.0 * m * (w*w + h*h); + + EXPECT_NEAR(inertia(0,0), Ixx, 1e-10); + EXPECT_NEAR(inertia(1,1), Iyy, 1e-10); + EXPECT_NEAR(inertia(2,2), Izz, 1e-10); +} + +TEST(BoxInertiaMatrixTest, OffsetBox) +{ + // Box of size 2x2x2 (halfExtends = 1,1,1), mass = 3, offset from parent at (5,0,0) + auto box = std::make_unique(Vec3{1, 1, 1}, nullptr); + box->SetLocalOffset(5, 0, 0); + Object obj(3.0, std::move(box)); + const Matrix3x3& inertia = obj.GetInertiaMatrix(); + + // Parallel axis theorem: I_new = I_cm + m*d^2 (for each axis) + double m = 3.0, a = 2.0; + double I_cm = 1.0/6.0 * m * a * a; // cube, axis-aligned + double d2 = 25.0; // offset^2 + double I_expected_xx = I_cm; // No offset contribution + double I_expected_yy = I_cm + m * 25; // Offset in y + double I_expected_zz = I_cm + m * 25; // Offset in z + + EXPECT_NEAR(inertia(0,0), I_expected_xx, 1e-10); + EXPECT_NEAR(inertia(1,1), I_expected_yy, 1e-10); + EXPECT_NEAR(inertia(2,2), I_expected_zz, 1e-10); +} + +TEST(BoxInertiaMatrixTest, RotatedBox) +{ + // Box of size 2x2x2 (halfExtends = 1,1,1), mass = 2, rotated 90 deg around Z + auto box = std::make_unique(Vec3{1, 1, 1}, nullptr); + Quat4 rotZ90(std::cos(M_PI/4), 0, 0, std::sin(M_PI/4)); // 90 deg about Z + box->SetLocalRotation(rotZ90); + Object obj(2.0, std::move(box)); + const Matrix3x3& inertia = obj.GetInertiaMatrix(); + + // For a cube, inertia is the same on all axes, so rotation should not change diagonal + double m = 2.0, a = 2.0; + double I_diag = 1.0/6.0 * m * a * a; + EXPECT_NEAR(inertia(0,0), I_diag, 1e-10); + EXPECT_NEAR(inertia(1,1), I_diag, 1e-10); + EXPECT_NEAR(inertia(2,2), I_diag, 1e-10); +} + +TEST(BoxInertiaMatrixTest, RotatedAndOffsetBox) +{ + // Box of size 2x4x6 (halfExtends = 1,2,3), mass = 10, offset (1,2,3), rotated 45 deg about Y + auto box = std::make_unique(Vec3{1, 2, 3}, nullptr); + box->SetLocalOffset(1, 2, 3); + Quat4 rotY45(std::cos(M_PI/8), 0, std::sin(M_PI/8), 0); // 45 deg about Y + box->SetLocalRotation(rotY45); + Object obj(10.0, std::move(box)); + const Matrix3x3& inertia = obj.GetInertiaMatrix(); + + // Just check inertia is symmetric and positive definite + EXPECT_NEAR(inertia(0,1), inertia(1,0), 1e-10); + EXPECT_NEAR(inertia(0,2), inertia(2,0), 1e-10); + EXPECT_NEAR(inertia(1,2), inertia(2,1), 1e-10); + EXPECT_GT(inertia(0,0), 0); + EXPECT_GT(inertia(1,1), 0); + EXPECT_GT(inertia(2,2), 0); +} \ No newline at end of file