diff --git a/tests/general/unit_test_epa_comprehensive.cpp b/tests/general/unit_test_epa_comprehensive.cpp new file mode 100644 index 0000000..c021164 --- /dev/null +++ b/tests/general/unit_test_epa_comprehensive.cpp @@ -0,0 +1,471 @@ +// +// Comprehensive EPA tests. +// Covers: all 3 axis directions, multiple depth levels, penetration-vector +// round-trips, depth monotonicity, symmetry, asymmetric sizes, memory +// resource variants, tolerance sensitivity, and iteration bookkeeping. +// +#include +#include +#include +#include +#include +#include +#include + +using Mesh = omath::source_engine::Mesh; +using Collider = omath::source_engine::MeshCollider; +using Gjk = omath::collision::GjkAlgorithm; +using Epa = omath::collision::Epa; +using Vec3 = omath::Vector3; + +namespace +{ + const std::vector> k_cube_vbo = { + { { -1.f, -1.f, -1.f }, {}, {} }, + { { -1.f, -1.f, 1.f }, {}, {} }, + { { -1.f, 1.f, -1.f }, {}, {} }, + { { -1.f, 1.f, 1.f }, {}, {} }, + { { 1.f, 1.f, 1.f }, {}, {} }, + { { 1.f, 1.f, -1.f }, {}, {} }, + { { 1.f, -1.f, 1.f }, {}, {} }, + { { 1.f, -1.f, -1.f }, {}, {} }, + }; + const std::vector> k_empty_ebo{}; + + constexpr Epa::Params k_default_params{ .max_iterations = 64, .tolerance = 1e-4f }; + + Collider make_cube(const Vec3& origin = {}, const Vec3& scale = { 1, 1, 1 }) + { + Mesh m{ k_cube_vbo, k_empty_ebo, scale }; + m.set_origin(origin); + return Collider{ m }; + } + + // Run GJK then EPA; asserts GJK hit and EPA converged. + Epa::Result solve(const Collider& a, const Collider& b, + const Epa::Params& params = k_default_params) + { + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + EXPECT_TRUE(hit) << "GJK must detect collision before EPA can run"; + auto result = Epa::solve(a, b, simplex, params); + EXPECT_TRUE(result.has_value()) << "EPA must converge"; + return *result; + } +} // namespace + +// --------------------------------------------------------------------------- +// Normal direction per axis +// --------------------------------------------------------------------------- + +// For two unit cubes (half-extent 1) with B offset by d along an axis: +// depth = 2 - d (distance from origin to nearest face of Minkowski diff) +// normal component along that axis ≈ ±1 + +TEST(EpaComprehensive, NormalAlongX_Positive) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })); + EXPECT_NEAR(std::abs(r.normal.x), 1.f, 1e-3f); + EXPECT_NEAR(r.normal.y, 0.f, 1e-3f); + EXPECT_NEAR(r.normal.z, 0.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongX_Negative) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ -0.5f, 0, 0 })); + EXPECT_NEAR(std::abs(r.normal.x), 1.f, 1e-3f); + EXPECT_NEAR(r.normal.y, 0.f, 1e-3f); + EXPECT_NEAR(r.normal.z, 0.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongY_Positive) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0.5f, 0 })); + EXPECT_NEAR(r.normal.x, 0.f, 1e-3f); + EXPECT_NEAR(std::abs(r.normal.y), 1.f, 1e-3f); + EXPECT_NEAR(r.normal.z, 0.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongY_Negative) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, -0.5f, 0 })); + EXPECT_NEAR(r.normal.x, 0.f, 1e-3f); + EXPECT_NEAR(std::abs(r.normal.y), 1.f, 1e-3f); + EXPECT_NEAR(r.normal.z, 0.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongZ_Positive) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, 0.5f })); + EXPECT_NEAR(r.normal.x, 0.f, 1e-3f); + EXPECT_NEAR(r.normal.y, 0.f, 1e-3f); + EXPECT_NEAR(std::abs(r.normal.z), 1.f, 1e-3f); +} + +TEST(EpaComprehensive, NormalAlongZ_Negative) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, -0.5f })); + EXPECT_NEAR(r.normal.x, 0.f, 1e-3f); + EXPECT_NEAR(r.normal.y, 0.f, 1e-3f); + EXPECT_NEAR(std::abs(r.normal.z), 1.f, 1e-3f); +} + +// --------------------------------------------------------------------------- +// Depth correctness (depth = 2 - offset for unit cubes) +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, Depth_ShallowOverlap) +{ + // offset 1.9 → depth 0.1 + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.9f, 0, 0 })); + EXPECT_NEAR(r.depth, 0.1f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_QuarterOverlap) +{ + // offset 1.5 → depth 0.5 + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.5f, 0, 0 })); + EXPECT_NEAR(r.depth, 0.5f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_HalfOverlap) +{ + // offset 1.0 → depth 1.0 + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.0f, 0, 0 })); + EXPECT_NEAR(r.depth, 1.0f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_ThreeQuarterOverlap) +{ + // offset 0.5 → depth 1.5 + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })); + EXPECT_NEAR(r.depth, 1.5f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_AlongY_HalfOverlap) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 1.0f, 0 })); + EXPECT_NEAR(r.depth, 1.0f, 1e-2f); +} + +TEST(EpaComprehensive, Depth_AlongZ_HalfOverlap) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, 1.0f })); + EXPECT_NEAR(r.depth, 1.0f, 1e-2f); +} + +// --------------------------------------------------------------------------- +// Depth monotonicity — deeper overlap → larger depth +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, DepthMonotonic_AlongX) +{ + const float d1 = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.9f, 0, 0 })).depth; // ~0.1 + const float d2 = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.5f, 0, 0 })).depth; // ~0.5 + const float d3 = solve(make_cube({ 0, 0, 0 }), make_cube({ 1.0f, 0, 0 })).depth; // ~1.0 + const float d4 = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })).depth; // ~1.5 + + EXPECT_LT(d1, d2); + EXPECT_LT(d2, d3); + EXPECT_LT(d3, d4); +} + +// --------------------------------------------------------------------------- +// Normal is a unit vector +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, NormalIsUnit_AlongX) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })); + EXPECT_NEAR(r.normal.dot(r.normal), 1.f, 1e-5f); +} + +TEST(EpaComprehensive, NormalIsUnit_AlongY) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 1.2f, 0 })); + EXPECT_NEAR(r.normal.dot(r.normal), 1.f, 1e-5f); +} + +TEST(EpaComprehensive, NormalIsUnit_AlongZ) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 0, 0.8f })); + EXPECT_NEAR(r.normal.dot(r.normal), 1.f, 1e-5f); +} + +// --------------------------------------------------------------------------- +// Penetration vector = normal * depth +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, PenetrationVectorLength_EqualsDepth) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0.5f, 0, 0 })); + const float pen_len = std::sqrt(r.penetration_vector.dot(r.penetration_vector)); + EXPECT_NEAR(pen_len, r.depth, 1e-5f); +} + +TEST(EpaComprehensive, PenetrationVectorDirection_ParallelToNormal) +{ + const auto r = solve(make_cube({ 0, 0, 0 }), make_cube({ 0, 1.0f, 0 })); + // penetration_vector = normal * depth → cross product must be ~zero + const auto cross = r.penetration_vector.cross(r.normal); + EXPECT_NEAR(cross.dot(cross), 0.f, 1e-8f); +} + +// --------------------------------------------------------------------------- +// Round-trip: applying penetration_vector separates the shapes +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, RoundTrip_AlongX) +{ + const auto a = make_cube({ 0, 0, 0 }); + Mesh mesh_b{ k_cube_vbo, k_empty_ebo }; + mesh_b.set_origin({ 0.5f, 0, 0 }); + const auto b = Collider{ mesh_b }; + + const auto r = solve(a, b); + constexpr float margin = 1.f + 1e-3f; + + // Move B along the penetration vector; it should separate from A + Mesh mesh_sep{ k_cube_vbo, k_empty_ebo }; + mesh_sep.set_origin(mesh_b.get_origin() + r.penetration_vector * margin); + EXPECT_FALSE(Gjk::is_collide(a, Collider{ mesh_sep })) << "Applying pen vector must separate"; + + // Moving the wrong way must still collide + Mesh mesh_wrong{ k_cube_vbo, k_empty_ebo }; + mesh_wrong.set_origin(mesh_b.get_origin() - r.penetration_vector * margin); + EXPECT_TRUE(Gjk::is_collide(a, Collider{ mesh_wrong })) << "Opposite direction must still collide"; +} + +TEST(EpaComprehensive, RoundTrip_AlongY) +{ + const auto a = make_cube({ 0, 0, 0 }); + Mesh mesh_b{ k_cube_vbo, k_empty_ebo }; + mesh_b.set_origin({ 0, 0.8f, 0 }); + const auto b = Collider{ mesh_b }; + + const auto r = solve(a, b); + constexpr float margin = 1.f + 1e-3f; + + Mesh mesh_sep{ k_cube_vbo, k_empty_ebo }; + mesh_sep.set_origin(mesh_b.get_origin() + r.penetration_vector * margin); + EXPECT_FALSE(Gjk::is_collide(a, Collider{ mesh_sep })); + + Mesh mesh_wrong{ k_cube_vbo, k_empty_ebo }; + mesh_wrong.set_origin(mesh_b.get_origin() - r.penetration_vector * margin); + EXPECT_TRUE(Gjk::is_collide(a, Collider{ mesh_wrong })); +} + +TEST(EpaComprehensive, RoundTrip_AlongZ) +{ + const auto a = make_cube({ 0, 0, 0 }); + Mesh mesh_b{ k_cube_vbo, k_empty_ebo }; + mesh_b.set_origin({ 0, 0, 1.2f }); + const auto b = Collider{ mesh_b }; + + const auto r = solve(a, b); + constexpr float margin = 1.f + 1e-3f; + + Mesh mesh_sep{ k_cube_vbo, k_empty_ebo }; + mesh_sep.set_origin(mesh_b.get_origin() + r.penetration_vector * margin); + EXPECT_FALSE(Gjk::is_collide(a, Collider{ mesh_sep })); +} + +// --------------------------------------------------------------------------- +// Symmetry — swapping A and B preserves depth +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, Symmetry_DepthIsIndependentOfOrder) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + + const float depth_ab = solve(a, b).depth; + const float depth_ba = solve(b, a).depth; + + EXPECT_NEAR(depth_ab, depth_ba, 1e-2f); +} + +TEST(EpaComprehensive, Symmetry_NormalsAreOpposite) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + + const Vec3 n_ab = solve(a, b).normal; + const Vec3 n_ba = solve(b, a).normal; + + // The normals should be anti-parallel: n_ab · n_ba ≈ -1 + EXPECT_NEAR(n_ab.dot(n_ba), -1.f, 1e-3f); +} + +// --------------------------------------------------------------------------- +// Asymmetric sizes +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, LargeVsSmall_DepthCorrect) +{ + // Big (half-ext 2) at origin, small (half-ext 0.5) at (2.0, 0, 0) + // Minkowski diff closest face in X at distance 0.5 + const auto r = solve(make_cube({ 0, 0, 0 }, { 2, 2, 2 }), make_cube({ 2.0f, 0, 0 }, { 0.5f, 0.5f, 0.5f })); + EXPECT_NEAR(r.depth, 0.5f, 1e-2f); + EXPECT_NEAR(std::abs(r.normal.x), 1.f, 1e-3f); +} + +TEST(EpaComprehensive, LargeVsSmall_RoundTrip) +{ + const auto a = make_cube({ 0, 0, 0 }, { 2, 2, 2 }); + + Mesh mesh_b{ k_cube_vbo, k_empty_ebo, { 0.5f, 0.5f, 0.5f } }; + mesh_b.set_origin({ 2.0f, 0, 0 }); + const auto b = Collider{ mesh_b }; + + const auto r = solve(a, b); + constexpr float margin = 1.f + 1e-3f; + + Mesh mesh_sep{ k_cube_vbo, k_empty_ebo, { 0.5f, 0.5f, 0.5f } }; + mesh_sep.set_origin(mesh_b.get_origin() + r.penetration_vector * margin); + EXPECT_FALSE(Gjk::is_collide(a, Collider{ mesh_sep })); +} + +// --------------------------------------------------------------------------- +// Memory resource variants +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, MonotonicBuffer_ConvergesCorrectly) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + constexpr std::size_t k_buf = 32768; + alignas(std::max_align_t) char buf[k_buf]; + std::pmr::monotonic_buffer_resource mr{ buf, k_buf, std::pmr::null_memory_resource() }; + + const auto r = Epa::solve(a, b, simplex, k_default_params, mr); + ASSERT_TRUE(r.has_value()); + EXPECT_NEAR(r->depth, 1.5f, 1e-2f); +} + +TEST(EpaComprehensive, MonotonicBuffer_MultipleReleaseCycles) +{ + // Verify mr.release() correctly resets the buffer across multiple calls + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + constexpr std::size_t k_buf = 32768; + alignas(std::max_align_t) char buf[k_buf]; + std::pmr::monotonic_buffer_resource mr{ buf, k_buf, std::pmr::null_memory_resource() }; + + float first_depth = 0.f; + for (int i = 0; i < 5; ++i) + { + mr.release(); + const auto r = Epa::solve(a, b, simplex, k_default_params, mr); + ASSERT_TRUE(r.has_value()) << "solve must converge on iteration " << i; + if (i == 0) + first_depth = r->depth; + else + EXPECT_NEAR(r->depth, first_depth, 1e-6f) << "depth must be deterministic"; + } +} + +TEST(EpaComprehensive, DefaultResource_ConvergesCorrectly) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 1.0f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + const auto r = Epa::solve(a, b, simplex); + ASSERT_TRUE(r.has_value()); + EXPECT_NEAR(r->depth, 1.0f, 1e-2f); +} + +// --------------------------------------------------------------------------- +// Tolerance sensitivity +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, TighterTolerance_MoreAccurateDepth) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 1.0f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + const Epa::Params loose{ .max_iterations = 64, .tolerance = 1e-2f }; + const Epa::Params tight{ .max_iterations = 64, .tolerance = 1e-5f }; + + const auto r_loose = Epa::solve(a, b, simplex, loose); + const auto r_tight = Epa::solve(a, b, simplex, tight); + ASSERT_TRUE(r_loose.has_value()); + ASSERT_TRUE(r_tight.has_value()); + + // Tighter tolerance must yield a result at least as accurate + EXPECT_LE(std::abs(r_tight->depth - 1.0f), std::abs(r_loose->depth - 1.0f) + 1e-4f); +} + +// --------------------------------------------------------------------------- +// Bookkeeping fields +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, Bookkeeping_IterationsInBounds) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto r = solve(a, b); + + EXPECT_GT(r.iterations, 0); + EXPECT_LE(r.iterations, k_default_params.max_iterations); +} + +TEST(EpaComprehensive, Bookkeeping_FacesAndVerticesGrow) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto r = solve(a, b); + + // Started with a tetrahedron (4 faces, 4 vertices); EPA must have expanded it + EXPECT_GE(r.num_faces, 4); + EXPECT_GE(r.num_vertices, 4); +} + +TEST(EpaComprehensive, Bookkeeping_MaxIterationsRespected) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.5f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + constexpr Epa::Params tight{ .max_iterations = 3, .tolerance = 1e-10f }; + const auto r = Epa::solve(a, b, simplex, tight); + + // Must return something (fallback best-face path) and respect the cap + if (r.has_value()) + EXPECT_LE(r->iterations, tight.max_iterations); +} + +// --------------------------------------------------------------------------- +// Determinism +// --------------------------------------------------------------------------- + +TEST(EpaComprehensive, Deterministic_SameResultOnRepeatedCalls) +{ + const auto a = make_cube({ 0, 0, 0 }); + const auto b = make_cube({ 0.7f, 0, 0 }); + const auto [hit, simplex] = Gjk::is_collide_with_simplex_info(a, b); + ASSERT_TRUE(hit); + + const auto first = Epa::solve(a, b, simplex); + ASSERT_TRUE(first.has_value()); + + for (int i = 0; i < 5; ++i) + { + const auto r = Epa::solve(a, b, simplex); + ASSERT_TRUE(r.has_value()); + EXPECT_NEAR(r->depth, first->depth, 1e-6f); + EXPECT_NEAR(r->normal.x, first->normal.x, 1e-6f); + EXPECT_NEAR(r->normal.y, first->normal.y, 1e-6f); + EXPECT_NEAR(r->normal.z, first->normal.z, 1e-6f); + } +}