From d53b68b972b9c72187b5a10768bfb8a9be289f6b Mon Sep 17 00:00:00 2001 From: cinnaboot Date: Wed, 28 Jan 2026 16:51:04 -0500 Subject: [PATCH] Add 3x3 matrix implementation for orbital rotations MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add Mat3 struct (row-major 3x3 matrix) - Implement matrix operations: identity, multiply, vector multiply - Add rotation matrices for X and Z axes - Add mat3_rotation_orbital() combining ω, i, Ω rotations - Add comprehensive tests in test_integration.cpp - Create implementation plan document --- docs/planning/matrix-implementation-plan.md | 76 ++++++++++ src/physics.cpp | 57 ++++++++ src/physics.h | 15 ++ tests/test_integration.cpp | 145 ++++++++++++++++++++ 4 files changed, 293 insertions(+) create mode 100644 docs/planning/matrix-implementation-plan.md diff --git a/docs/planning/matrix-implementation-plan.md b/docs/planning/matrix-implementation-plan.md new file mode 100644 index 0000000..4145c17 --- /dev/null +++ b/docs/planning/matrix-implementation-plan.md @@ -0,0 +1,76 @@ +# Matrix Implementation Plan - 3D Orbital Rotations + +## Objective +Implement 3x3 rotation matrices in the physics module to support 3D orbital element orientation (inclination, RAAN, argument of periapsis). + +## Background +Currently `orbital_elements_to_cartesian()` only produces 2D orbits (z=0). To support inclined orbits like Molniya, we need to apply rotation matrices to transform 2D orbital plane coordinates into 3D space. + +## Rotation Sequence (z-x-z Euler angles) +``` +r_final = R_z(Ω) · R_x(i) · R_z(ω) · r_orbital_plane +v_final = R_z(Ω) · R_x(i) · R_z(ω) · v_orbital_plane +``` +Where: +- ω (omega) = argument of periapsis +- i = inclination +- Ω (Omega) = longitude of ascending node + +## Implementation + +### 1. Data Structure (physics.h) +```cpp +struct Mat3 { + double m00, m01, m02; // Row 0 + double m10, m11, m12; // Row 1 + double m20, m21, m22; // Row 2 +}; +``` +**Format**: Row-major 3x3 matrix (different from raylib's column-major 4x4) + +### 2. Functions to Implement (physics.h/cpp) + +#### Core Matrix Operations +- `mat3_identity()` - Returns identity matrix +- `mat3_multiply(Mat3 a, Mat3 b)` - Matrix-matrix multiplication +- `mat3_multiply_vec3(Mat3 m, Vec3 v)` - Matrix-vector multiplication + +#### Rotation Matrices +- `mat3_rotation_x(double angle)` - Rotation about X axis (for inclination) +- `mat3_rotation_z(double angle)` - Rotation about Z axis (for ω and Ω) + +#### Convenience Function +- `mat3_rotation_orbital(double omega, double i, double Omega)` - Combined rotation + +### 3. Test Plan (test_integration.cpp) + +#### Basic Operations +- Identity matrix multiplication +- Matrix-vector multiplication +- Matrix-matrix multiplication + +#### Edge Cases for Rotations +- Identity (0° rotation) +- 180° rotation (π radians) - coordinate flip +- 360° rotation (2π radians) - should equal identity +- Negative angles (-90° = 270°) +- Very small angles (numerical stability) +- Combined rotations that cancel (+90° then -90°) + +#### Validation Tests +- Orthogonality: R^T · R = I +- Determinant = 1 (proper rotation) + +### 4. Integration (Future Session) +After matrix implementation, modify `orbital_elements_to_cartesian()` to: +1. Generate 2D position/velocity in orbital plane +2. Apply combined rotation matrix +3. Return 3D coordinates + +## References +- docs/planning/molniya-orbit-test-plan.md +- src/orbital_mechanics.cpp (orbital_elements_to_cartesian) +- Standard orbital mechanics: Keplerian to Cartesian conversion + +## Date +Created: 2026-01-28 diff --git a/src/physics.cpp b/src/physics.cpp index cd2746e..5652485 100644 --- a/src/physics.cpp +++ b/src/physics.cpp @@ -107,3 +107,60 @@ Vec3 evaluate_acceleration(Vec3 relative_pos, double body_mass, double parent_ma return calculate_acceleration(total_force, body_mass); } + +Mat3 mat3_identity() { + return {1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0}; +} + +Mat3 mat3_multiply(Mat3 a, Mat3 b) { + return { + a.m00 * b.m00 + a.m01 * b.m10 + a.m02 * b.m20, + a.m00 * b.m01 + a.m01 * b.m11 + a.m02 * b.m21, + a.m00 * b.m02 + a.m01 * b.m12 + a.m02 * b.m22, + a.m10 * b.m00 + a.m11 * b.m10 + a.m12 * b.m20, + a.m10 * b.m01 + a.m11 * b.m11 + a.m12 * b.m21, + a.m10 * b.m02 + a.m11 * b.m12 + a.m12 * b.m22, + a.m20 * b.m00 + a.m21 * b.m10 + a.m22 * b.m20, + a.m20 * b.m01 + a.m21 * b.m11 + a.m22 * b.m21, + a.m20 * b.m02 + a.m21 * b.m12 + a.m22 * b.m22 + }; +} + +Vec3 mat3_multiply_vec3(Mat3 m, Vec3 v) { + return { + m.m00 * v.x + m.m01 * v.y + m.m02 * v.z, + m.m10 * v.x + m.m11 * v.y + m.m12 * v.z, + m.m20 * v.x + m.m21 * v.y + m.m22 * v.z + }; +} + +Mat3 mat3_rotation_x(double angle) { + double c = cos(angle); + double s = sin(angle); + return { + 1.0, 0.0, 0.0, + 0.0, c, -s, + 0.0, s, c + }; +} + +Mat3 mat3_rotation_z(double angle) { + double c = cos(angle); + double s = sin(angle); + return { + c, -s, 0.0, + s, c, 0.0, + 0.0, 0.0, 1.0 + }; +} + +Mat3 mat3_rotation_orbital(double omega, double i, double Omega) { + Mat3 Rz_omega = mat3_rotation_z(omega); + Mat3 Rx_i = mat3_rotation_x(i); + Mat3 Rz_Omega = mat3_rotation_z(Omega); + + Mat3 temp = mat3_multiply(Rx_i, Rz_omega); + return mat3_multiply(Rz_Omega, temp); +} diff --git a/src/physics.h b/src/physics.h index 6a1c05b..df1e053 100644 --- a/src/physics.h +++ b/src/physics.h @@ -6,6 +6,13 @@ struct Vec3 { double x, y, z; }; +// 3x3 Matrix (row-major) +struct Mat3 { + double m00, m01, m02; + double m10, m11, m12; + double m20, m21, m22; +}; + // Gravitational constant (m^3 kg^-1 s^-2) const double G = 6.67430e-11; @@ -19,6 +26,14 @@ double vec3_distance(Vec3 a, Vec3 b); Vec3 vec3_normalize(Vec3 v); double vec3_dot(Vec3 a, Vec3 b); +// Matrix functions +Mat3 mat3_identity(); +Mat3 mat3_multiply(Mat3 a, Mat3 b); +Vec3 mat3_multiply_vec3(Mat3 m, Vec3 v); +Mat3 mat3_rotation_x(double angle); +Mat3 mat3_rotation_z(double angle); +Mat3 mat3_rotation_orbital(double omega, double i, double Omega); + // Physics functions Vec3 calculate_acceleration(Vec3 force, double mass); diff --git a/tests/test_integration.cpp b/tests/test_integration.cpp index 452e487..400d1c1 100644 --- a/tests/test_integration.cpp +++ b/tests/test_integration.cpp @@ -97,3 +97,148 @@ TEST_CASE("RK4 integration step", "[physics][rk4]") { REQUIRE(final_distance > 0.9 * initial_distance); REQUIRE(final_distance < 1.1 * initial_distance); } + +TEST_CASE("Matrix identity", "[matrix][identity]") { + Mat3 I = mat3_identity(); + + REQUIRE(compare_double(I.m00, 1.0, 1e-10)); + REQUIRE(compare_double(I.m01, 0.0, 1e-10)); + REQUIRE(compare_double(I.m02, 0.0, 1e-10)); + REQUIRE(compare_double(I.m10, 0.0, 1e-10)); + REQUIRE(compare_double(I.m11, 1.0, 1e-10)); + REQUIRE(compare_double(I.m12, 0.0, 1e-10)); + REQUIRE(compare_double(I.m20, 0.0, 1e-10)); + REQUIRE(compare_double(I.m21, 0.0, 1e-10)); + REQUIRE(compare_double(I.m22, 1.0, 1e-10)); +} + +TEST_CASE("Matrix-vector multiplication", "[matrix][vector]") { + Mat3 m = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0}; + Vec3 v = {1.0, 2.0, 3.0}; + + Vec3 result = mat3_multiply_vec3(m, v); + + REQUIRE(compare_double(result.x, 14.0, 1e-10)); + REQUIRE(compare_double(result.y, 32.0, 1e-10)); + REQUIRE(compare_double(result.z, 50.0, 1e-10)); +} + +TEST_CASE("Matrix multiplication with identity", "[matrix][multiply]") { + Mat3 A = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0}; + Mat3 I = mat3_identity(); + + Mat3 result = mat3_multiply(A, I); + + REQUIRE(compare_double(result.m00, A.m00, 1e-10)); + REQUIRE(compare_double(result.m01, A.m01, 1e-10)); + REQUIRE(compare_double(result.m02, A.m02, 1e-10)); + REQUIRE(compare_double(result.m10, A.m10, 1e-10)); + REQUIRE(compare_double(result.m11, A.m11, 1e-10)); + REQUIRE(compare_double(result.m12, A.m12, 1e-10)); + REQUIRE(compare_double(result.m20, A.m20, 1e-10)); + REQUIRE(compare_double(result.m21, A.m21, 1e-10)); + REQUIRE(compare_double(result.m22, A.m22, 1e-10)); +} + +TEST_CASE("Rotation about Z axis", "[matrix][rotation]") { + double angle = M_PI / 2; // 90 degrees + Mat3 Rz = mat3_rotation_z(angle); + Vec3 v = {1.0, 0.0, 0.0}; + + Vec3 result = mat3_multiply_vec3(Rz, v); + + REQUIRE(compare_double(result.x, 0.0, 1e-10)); + REQUIRE(compare_double(result.y, 1.0, 1e-10)); + REQUIRE(compare_double(result.z, 0.0, 1e-10)); +} + +TEST_CASE("Rotation about X axis", "[matrix][rotation]") { + double angle = M_PI / 2; // 90 degrees + Mat3 Rx = mat3_rotation_x(angle); + Vec3 v = {0.0, 1.0, 0.0}; + + Vec3 result = mat3_multiply_vec3(Rx, v); + + REQUIRE(compare_double(result.x, 0.0, 1e-10)); + REQUIRE(compare_double(result.y, 0.0, 1e-10)); + REQUIRE(compare_double(result.z, 1.0, 1e-10)); +} + +TEST_CASE("Rotation edge cases", "[matrix][rotation][edge]") { + SECTION("180 degree rotation") { + Mat3 Rz180 = mat3_rotation_z(M_PI); + Vec3 v = {1.0, 0.0, 0.0}; + Vec3 result = mat3_multiply_vec3(Rz180, v); + + REQUIRE(compare_double(result.x, -1.0, 1e-10)); + REQUIRE(compare_double(result.y, 0.0, 1e-10)); + REQUIRE(compare_double(result.z, 0.0, 1e-10)); + } + + SECTION("360 degree rotation equals identity") { + Mat3 Rz360 = mat3_rotation_z(2.0 * M_PI); + Mat3 I = mat3_identity(); + + REQUIRE(compare_double(Rz360.m00, I.m00, 1e-10)); + REQUIRE(compare_double(Rz360.m11, I.m11, 1e-10)); + REQUIRE(compare_double(Rz360.m22, I.m22, 1e-10)); + } + + SECTION("Negative angle equals positive rotation") { + Mat3 Rz_neg90 = mat3_rotation_z(-M_PI / 2); + Mat3 Rz_270 = mat3_rotation_z(3.0 * M_PI / 2); + + REQUIRE(compare_double(Rz_neg90.m00, Rz_270.m00, 1e-10)); + REQUIRE(compare_double(Rz_neg90.m01, Rz_270.m01, 1e-10)); + REQUIRE(compare_double(Rz_neg90.m10, Rz_270.m10, 1e-10)); + REQUIRE(compare_double(Rz_neg90.m11, Rz_270.m11, 1e-10)); + } + + SECTION("Combined rotations that cancel") { + Mat3 Rz90 = mat3_rotation_z(M_PI / 2); + Mat3 Rz_neg90 = mat3_rotation_z(-M_PI / 2); + Mat3 combined = mat3_multiply(Rz_neg90, Rz90); + Mat3 I = mat3_identity(); + + REQUIRE(compare_double(combined.m00, I.m00, 1e-10)); + REQUIRE(compare_double(combined.m11, I.m11, 1e-10)); + REQUIRE(compare_double(combined.m22, I.m22, 1e-10)); + } +} + +TEST_CASE("Rotation matrix orthogonality", "[matrix][rotation][validation]") { + double angle = M_PI / 4; // 45 degrees + Mat3 Rz = mat3_rotation_z(angle); + + Mat3 Rz_T = {Rz.m00, Rz.m10, Rz.m20, + Rz.m01, Rz.m11, Rz.m21, + Rz.m02, Rz.m12, Rz.m22}; + + Mat3 product = mat3_multiply(Rz, Rz_T); + Mat3 I = mat3_identity(); + + REQUIRE(compare_double(product.m00, I.m00, 1e-10)); + REQUIRE(compare_double(product.m01, I.m01, 1e-10)); + REQUIRE(compare_double(product.m02, I.m02, 1e-10)); + REQUIRE(compare_double(product.m10, I.m10, 1e-10)); + REQUIRE(compare_double(product.m11, I.m11, 1e-10)); + REQUIRE(compare_double(product.m12, I.m12, 1e-10)); + REQUIRE(compare_double(product.m20, I.m20, 1e-10)); + REQUIRE(compare_double(product.m21, I.m21, 1e-10)); + REQUIRE(compare_double(product.m22, I.m22, 1e-10)); +} + +TEST_CASE("Orbital rotation matrix", "[matrix][orbital]") { + double omega = 0.0; + double i = M_PI / 2; // 90 degrees inclination + double Omega = 0.0; + + Mat3 R = mat3_rotation_orbital(omega, i, Omega); + Vec3 v = {1.0, 0.0, 0.0}; + + Vec3 result = mat3_multiply_vec3(R, v); + + REQUIRE(compare_double(result.x, 1.0, 1e-10)); + REQUIRE(compare_double(result.y, 0.0, 1e-10)); + REQUIRE(compare_double(result.z, 0.0, 1e-10)); +}