claudes_game/tests/test_physics_utilities.cpp

#include <catch2/catch_test_macros.hpp>
#include <catch2/matchers/catch_matchers_floating_point.hpp>
#include "../src/physics.h"
#include "../src/test_utilities.h"
#include <cmath>

using Catch::Matchers::WithinAbs;

SCENARIO("Vector math utilities", "[physics][utilities][vector]") {
    const Vec3 a = {1.0, 2.0, 3.0};
    const Vec3 b = {4.0, 5.0, 6.0};

    SECTION("add") {
        const Vec3 sum = vec3_add(a, b);
        REQUIRE_THAT(sum.x, WithinAbs(5.0, R_TOL));
        REQUIRE_THAT(sum.y, WithinAbs(7.0, R_TOL));
        REQUIRE_THAT(sum.z, WithinAbs(9.0, R_TOL));
    }

    SECTION("sub") {
        const Vec3 diff = vec3_sub(b, a);
        REQUIRE_THAT(diff.x, WithinAbs(3.0, R_TOL));
        REQUIRE_THAT(diff.y, WithinAbs(3.0, R_TOL));
        REQUIRE_THAT(diff.z, WithinAbs(3.0, R_TOL));
    }

    SECTION("scale") {
        const Vec3 scaled = vec3_scale(a, 2.0);
        REQUIRE_THAT(scaled.x, WithinAbs(2.0, R_TOL));
        REQUIRE_THAT(scaled.y, WithinAbs(4.0, R_TOL));
        REQUIRE_THAT(scaled.z, WithinAbs(6.0, R_TOL));
    }

    SECTION("magnitude") {
        const double mag = vec3_magnitude(a);
        REQUIRE_THAT(mag, WithinAbs(sqrt(14.0), R_TOL));
    }

    SECTION("distance") {
        const double dist = vec3_distance(a, b);
        REQUIRE_THAT(dist, WithinAbs(sqrt(27.0), R_TOL));
    }

    SECTION("normalize") {
        const Vec3 unit = vec3_normalize(a);
        const double actual_mag = vec3_magnitude(unit);
        REQUIRE_THAT(actual_mag, WithinAbs(1.0, R_TOL));
    }

    SECTION("normalize zero vector") {
        const Vec3 zero = {0.0, 0.0, 0.0};
        const Vec3 result = vec3_normalize(zero);
        REQUIRE(compare_vec3(result, zero, R_TOL));
    }

    SECTION("dot product") {
        const double dot = vec3_dot(a, b);
        REQUIRE_THAT(dot, WithinAbs(32.0, R_TOL));
    }

    SECTION("cross product") {
        const Vec3 cross = vec3_cross(a, b);
        REQUIRE_THAT(cross.x, WithinAbs(-3.0, R_TOL));
        REQUIRE_THAT(cross.y, WithinAbs(6.0, R_TOL));
        REQUIRE_THAT(cross.z, WithinAbs(-3.0, R_TOL));
    }
}

SCENARIO("Acceleration calculation", "[physics][acceleration]") {
    SECTION("F = ma") {
        const Vec3 force = {10.0, 20.0, 30.0};
        const double mass = 5.0;
        const Vec3 accel = calculate_acceleration(force, mass);
        REQUIRE_THAT(accel.x, WithinAbs(2.0, R_TOL));
        REQUIRE_THAT(accel.y, WithinAbs(4.0, R_TOL));
        REQUIRE_THAT(accel.z, WithinAbs(6.0, R_TOL));
    }

    SECTION("zero mass returns zero acceleration") {
        const Vec3 force = {10.0, 20.0, 30.0};
        const Vec3 accel = calculate_acceleration(force, 0.0);
        REQUIRE(compare_vec3(accel, {0.0, 0.0, 0.0}, R_TOL));
    }
}

SCENARIO("Matrix operations", "[physics][utilities][matrix]") {
    const Mat3 A = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0};
    const Vec3 v = {1.0, 2.0, 3.0};

    SECTION("identity matrix") {
        const Mat3 I = mat3_identity();
        const Mat3 result = mat3_multiply(A, I);
        REQUIRE(compare_vec3(
            {result.m00, result.m11, result.m22},
            {A.m00, A.m11, A.m22}, R_TOL));
    }

    SECTION("multiply vector") {
        const Vec3 result = mat3_multiply_vec3(A, v);
        REQUIRE_THAT(result.x, WithinAbs(14.0, R_TOL));
        REQUIRE_THAT(result.y, WithinAbs(32.0, R_TOL));
        REQUIRE_THAT(result.z, WithinAbs(50.0, R_TOL));
    }

    SECTION("multiply two matrices") {
        const Mat3 B = {9.0, 8.0, 7.0, 6.0, 5.0, 4.0, 3.0, 2.0, 1.0};
        const Mat3 result = mat3_multiply(A, B);
        REQUIRE_THAT(result.m00, WithinAbs(30.0, R_TOL));
        REQUIRE_THAT(result.m01, WithinAbs(24.0, R_TOL));
        REQUIRE_THAT(result.m02, WithinAbs(18.0, R_TOL));
        REQUIRE_THAT(result.m10, WithinAbs(84.0, R_TOL));
        REQUIRE_THAT(result.m11, WithinAbs(69.0, R_TOL));
        REQUIRE_THAT(result.m12, WithinAbs(54.0, R_TOL));
        REQUIRE_THAT(result.m20, WithinAbs(138.0, R_TOL));
        REQUIRE_THAT(result.m21, WithinAbs(114.0, R_TOL));
        REQUIRE_THAT(result.m22, WithinAbs(90.0, R_TOL));
    }

    SECTION("transpose") {
        const Mat3 T = mat3_transpose(A);
        REQUIRE_THAT(T.m00, WithinAbs(1.0, R_TOL));
        REQUIRE_THAT(T.m01, WithinAbs(4.0, R_TOL));
        REQUIRE_THAT(T.m02, WithinAbs(7.0, R_TOL));
        REQUIRE_THAT(T.m10, WithinAbs(2.0, R_TOL));
        REQUIRE_THAT(T.m11, WithinAbs(5.0, R_TOL));
        REQUIRE_THAT(T.m12, WithinAbs(8.0, R_TOL));
        REQUIRE_THAT(T.m20, WithinAbs(3.0, R_TOL));
        REQUIRE_THAT(T.m21, WithinAbs(6.0, R_TOL));
        REQUIRE_THAT(T.m22, WithinAbs(9.0, R_TOL));
    }

    SECTION("transpose of transpose is original") {
        const Mat3 T2 = mat3_transpose(mat3_transpose(A));
        REQUIRE_THAT(T2.m00, WithinAbs(A.m00, R_TOL));
        REQUIRE_THAT(T2.m01, WithinAbs(A.m01, R_TOL));
        REQUIRE_THAT(T2.m02, WithinAbs(A.m02, R_TOL));
        REQUIRE_THAT(T2.m10, WithinAbs(A.m10, R_TOL));
        REQUIRE_THAT(T2.m11, WithinAbs(A.m11, R_TOL));
        REQUIRE_THAT(T2.m12, WithinAbs(A.m12, R_TOL));
        REQUIRE_THAT(T2.m20, WithinAbs(A.m20, R_TOL));
        REQUIRE_THAT(T2.m21, WithinAbs(A.m21, R_TOL));
        REQUIRE_THAT(T2.m22, WithinAbs(A.m22, R_TOL));
    }
}

SCENARIO("Rotation matrices", "[physics][utilities][rotation]") {
    SECTION("rotate about Z by 90 degrees") {
        const Mat3 Rz = mat3_rotation_z(M_PI / 2);
        const Vec3 v = {1.0, 0.0, 0.0};
        const Vec3 result = mat3_multiply_vec3(Rz, v);
        REQUIRE_THAT(result.x, WithinAbs(0.0, R_TOL));
        REQUIRE_THAT(result.y, WithinAbs(1.0, R_TOL));
        REQUIRE_THAT(result.z, WithinAbs(0.0, R_TOL));
    }

    SECTION("rotate about X by 90 degrees") {
        const Mat3 Rx = mat3_rotation_x(M_PI / 2);
        const Vec3 v = {0.0, 1.0, 0.0};
        const Vec3 result = mat3_multiply_vec3(Rx, v);
        REQUIRE_THAT(result.x, WithinAbs(0.0, R_TOL));
        REQUIRE_THAT(result.y, WithinAbs(0.0, R_TOL));
        REQUIRE_THAT(result.z, WithinAbs(1.0, R_TOL));
    }

    SECTION("180 degree rotation") {
        const Mat3 Rz180 = mat3_rotation_z(M_PI);
        const Vec3 v = {1.0, 0.0, 0.0};
        const Vec3 result = mat3_multiply_vec3(Rz180, v);
        REQUIRE_THAT(result.x, WithinAbs(-1.0, R_TOL));
        REQUIRE_THAT(result.y, WithinAbs(0.0, R_TOL));
        REQUIRE_THAT(result.z, WithinAbs(0.0, R_TOL));
    }

    SECTION("360 degree rotation equals identity") {
        const Mat3 Rz360 = mat3_rotation_z(2.0 * M_PI);
        const Mat3 I = mat3_identity();
        REQUIRE_THAT(Rz360.m00, WithinAbs(I.m00, R_TOL));
        REQUIRE_THAT(Rz360.m11, WithinAbs(I.m11, R_TOL));
        REQUIRE_THAT(Rz360.m22, WithinAbs(I.m22, R_TOL));
    }

    SECTION("negative angle equals equivalent positive rotation") {
        const Mat3 Rz_neg90 = mat3_rotation_z(-M_PI / 2);
        const Mat3 Rz_270 = mat3_rotation_z(3.0 * M_PI / 2);
        REQUIRE_THAT(Rz_neg90.m00, WithinAbs(Rz_270.m00, R_TOL));
        REQUIRE_THAT(Rz_neg90.m01, WithinAbs(Rz_270.m01, R_TOL));
        REQUIRE_THAT(Rz_neg90.m10, WithinAbs(Rz_270.m10, R_TOL));
        REQUIRE_THAT(Rz_neg90.m11, WithinAbs(Rz_270.m11, R_TOL));
    }

    SECTION("combined rotations that cancel") {
        const Mat3 Rz90 = mat3_rotation_z(M_PI / 2);
        const Mat3 Rz_neg90 = mat3_rotation_z(-M_PI / 2);
        const Mat3 combined = mat3_multiply(Rz_neg90, Rz90);
        const Mat3 I = mat3_identity();
        REQUIRE_THAT(combined.m00, WithinAbs(I.m00, R_TOL));
        REQUIRE_THAT(combined.m11, WithinAbs(I.m11, R_TOL));
        REQUIRE_THAT(combined.m22, WithinAbs(I.m22, R_TOL));
    }

    SECTION("rotation matrix orthogonality") {
        const double angle = M_PI / 4;
        const Mat3 Rz = mat3_rotation_z(angle);
        const Mat3 Rz_T = mat3_transpose(Rz);
        const Mat3 product = mat3_multiply(Rz, Rz_T);
        const Mat3 I = mat3_identity();
        REQUIRE_THAT(product.m00, WithinAbs(I.m00, R_TOL));
        REQUIRE_THAT(product.m01, WithinAbs(I.m01, R_TOL));
        REQUIRE_THAT(product.m02, WithinAbs(I.m02, R_TOL));
        REQUIRE_THAT(product.m10, WithinAbs(I.m10, R_TOL));
        REQUIRE_THAT(product.m11, WithinAbs(I.m11, R_TOL));
        REQUIRE_THAT(product.m12, WithinAbs(I.m12, R_TOL));
        REQUIRE_THAT(product.m20, WithinAbs(I.m20, R_TOL));
        REQUIRE_THAT(product.m21, WithinAbs(I.m21, R_TOL));
        REQUIRE_THAT(product.m22, WithinAbs(I.m22, R_TOL));
    }

    SECTION("orbital rotation matrix with 90 deg inclination") {
        const Mat3 R = mat3_rotation_orbital(0.0, M_PI / 2, 0.0);
        const Vec3 v = {1.0, 0.0, 0.0};
        const Vec3 result = mat3_multiply_vec3(R, v);
        REQUIRE_THAT(result.x, WithinAbs(1.0, R_TOL));
        REQUIRE_THAT(result.y, WithinAbs(0.0, R_TOL));
        REQUIRE_THAT(result.z, WithinAbs(0.0, R_TOL));
    }
}

SCENARIO("compare_vec3 utility", "[physics][utilities][compare]") {
    SECTION("equal vectors within tolerance") {
        const Vec3 a = {1.0, 2.0, 3.0};
        const Vec3 b = {1.0001, 2.0001, 3.0001};
        REQUIRE(compare_vec3(a, b, 1e-3));
    }

    SECTION("equal vectors exactly") {
        const Vec3 a = {1.0, 2.0, 3.0};
        const Vec3 b = {1.0, 2.0, 3.0};
        REQUIRE(compare_vec3(a, b, 0.0));
    }

    SECTION("different vectors outside tolerance") {
        const Vec3 a = {1.0, 2.0, 3.0};
        const Vec3 b = {2.0, 2.0, 3.0};
        REQUIRE(!compare_vec3(a, b, 0.5));
    }

    SECTION("zero vector comparison") {
        const Vec3 a = {0.0, 0.0, 0.0};
        const Vec3 b = {0.0, 0.0, 0.0};
        REQUIRE(compare_vec3(a, b, 0.0));
    }
}