vinihcrosaandClaude Sonnet 4.6
3e8e95d544
Implements the 6-DOF rigid body dynamics core: - Types (Vector6, Matrix6x6), Forces, BodyState value type - ForceModel abstract interface (non-const compute, D-09) - Body: mass matrix inversion at construction, friend CvodeIntegrator (D-11) - CvodeIntegrator: BDF integrator via SUNDIALS CVODE 7.x, static rhsCallback - Simulation: orchestrates Body + CvodeIntegrator + ForceModel registry - Math utilities: LinearAlgebra (invert6x6, matVecProduct), AngleUtils, Interpolation - 29 tests, 100% passing (zero-force, constant-force, SHO period, time accumulation) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
73 lines
2.0 KiB
C++
73 lines
2.0 KiB
C++
#include <catch2/catch_test_macros.hpp>
|
|
#include <catch2/matchers/catch_matchers_floating_point.hpp>
|
|
|
|
#include <ymir/core/Body.h>
|
|
#include <ymir/core/math/LinearAlgebra.h>
|
|
|
|
using Catch::Matchers::WithinAbs;
|
|
using namespace ymir;
|
|
|
|
static Matrix6x6 diagonalMass(double m, double Ixx, double Iyy, double Izz)
|
|
{
|
|
Matrix6x6 M{};
|
|
M[0][0] = m;
|
|
M[1][1] = m;
|
|
M[2][2] = m;
|
|
M[3][3] = Ixx;
|
|
M[4][4] = Iyy;
|
|
M[5][5] = Izz;
|
|
return M;
|
|
}
|
|
|
|
TEST_CASE("Body: invTotalMass * totalMass ≈ I", "[body]")
|
|
{
|
|
Matrix6x6 mass = diagonalMass(1000.0, 5e5, 4e5, 3e5);
|
|
Matrix6x6 added = diagonalMass(200.0, 1e5, 0.8e5, 0.6e5);
|
|
Vector6 q{}, qdot{};
|
|
|
|
Body body{mass, added, q, qdot};
|
|
|
|
auto result = math::matMul(body.totalMass(), body.invTotalMass());
|
|
|
|
for (int i = 0; i < 6; ++i)
|
|
for (int j = 0; j < 6; ++j)
|
|
REQUIRE_THAT(result[i][j], WithinAbs(i == j ? 1.0 : 0.0, 1e-10));
|
|
}
|
|
|
|
TEST_CASE("Body: computeAcceleration F/m for diagonal mass", "[body]")
|
|
{
|
|
Matrix6x6 mass = diagonalMass(1000.0, 1e5, 1e5, 1e5);
|
|
Matrix6x6 added{};
|
|
Vector6 q{}, qdot{};
|
|
|
|
Body body{mass, added, q, qdot};
|
|
|
|
Forces F;
|
|
F.f[0] = 500.0; // 500 N in surge
|
|
|
|
Vector6 acc = body.computeAcceleration(F);
|
|
|
|
// a = F/m = 500/1000 = 0.5 m/s²
|
|
REQUIRE_THAT(acc[0], WithinAbs(0.5, 1e-12));
|
|
for (int i = 1; i < 6; ++i)
|
|
REQUIRE_THAT(acc[i], WithinAbs(0.0, 1e-12));
|
|
}
|
|
|
|
TEST_CASE("Body: state() returns correct snapshot", "[body]")
|
|
{
|
|
Matrix6x6 mass = diagonalMass(1000.0, 1e5, 1e5, 1e5);
|
|
Matrix6x6 added{};
|
|
Vector6 q{1.0, 2.0, 3.0, 0.1, 0.2, 0.3};
|
|
Vector6 qdot{4.0, 5.0, 6.0, 0.4, 0.5, 0.6};
|
|
|
|
Body body{mass, added, q, qdot};
|
|
BodyState s = body.state(10.0, 0.1);
|
|
|
|
REQUIRE_THAT(s.x(), WithinAbs(1.0, 1e-15));
|
|
REQUIRE_THAT(s.y(), WithinAbs(2.0, 1e-15));
|
|
REQUIRE_THAT(s.yaw(), WithinAbs(0.3, 1e-15));
|
|
REQUIRE_THAT(s.u(), WithinAbs(4.0, 1e-15));
|
|
REQUIRE_THAT(s.r(), WithinAbs(0.6, 1e-15));
|
|
REQUIRE_THAT(s.time(), WithinAbs(10.0, 1e-15));
|
|
}
|