diff --git a/tests/general/unit_test_epa.cpp b/tests/general/unit_test_epa.cpp index 57da26c..3660d09 100644 --- a/tests/general/unit_test_epa.cpp +++ b/tests/general/unit_test_epa.cpp @@ -1,57 +1,75 @@ -// -// Created by Vlad on 11/13/2025. -// -#include "omath/collision/gjk_algorithm.hpp" -#include "omath/engines/source_engine/collider.hpp" #include -#include -#include +#include "omath/linear_algebra/vector3.hpp" +#include "omath/collision/simplex.hpp" +#include "omath/collision/epa_algorithm.hpp" // Epa + GjkAlgorithmWithSimplex +#include "omath/engines/source_engine/mesh.hpp" +#include "omath/engines/source_engine/collider.hpp" + +using Mesh = omath::source_engine::Mesh; +using Collider = omath::source_engine::MeshCollider; +using GJK = omath::collision::GjkAlgorithmWithSimplex; +using EPA = omath::collision::Epa; -namespace -{ - const omath::source_engine::Mesh mesh = {{{-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}}, - {}}; -} TEST(UnitTestEpa, TestCollisionTrue) { - std::vector> vbo = {{-1, -1, -1}, {-1, -1, 1}, {-1, 1, -1}, {-1, 1, 1}, - {1, 1, 1}, {1, 1, -1}, {1, -1, 1}, {1, -1, -1}}; - std::vector> vao; // not needed for GJK/EPA + // Unit cube [-1,1]^3 + std::vector> vbo = { + {-1,-1,-1}, {-1,-1, 1}, {-1, 1,-1}, {-1, 1, 1}, + { 1, 1, 1}, { 1, 1,-1}, { 1,-1, 1}, { 1,-1,-1} + }; + std::vector> vao; // not needed - omath::source_engine::Mesh a(vbo, vao, {1, 1, 1}); - omath::source_engine::Mesh b(vbo, vao, {1, 1, 1}); + Mesh a(vbo, vao, {1,1,1}); + Mesh b(vbo, vao, {1,1,1}); - a.set_origin({0, 0, 0}); - b.set_origin({0.5f, 0, 0}); // slight overlap + // Overlap along +X by 0.5 + a.set_origin({0,0,0}); + b.set_origin({0.5f,0,0}); - const omath::source_engine::MeshCollider collider_a(mesh); + Collider A(a), B(b); - omath::source_engine::MeshCollider A(a), B(b); + // GJK + auto gjk = GJK::collide(A, B); + ASSERT_TRUE(gjk.hit) << "GJK should report collision"; - // 1) GJK → final simplex - using Gjk = omath::collision::GjkAlgorithm; + // EPA + EPA::Params params; params.max_iterations = 64; params.tolerance = 1e-4f; + auto epa = EPA::solve(A, B, gjk.simplex, params); + ASSERT_TRUE(epa.success) << "EPA should converge"; - auto gjk = Gjk::is_collide_with_simplex_info(A, B); - if (!gjk.hit) - { - std::cout << "No collision\n"; - } - using Epa = omath::collision::Epa; - // 2) EPA → normal/depth - Epa::Params params; - params.max_iterations = 64; - params.tolerance = 1e-4f; - auto epa = Epa::solve(A, B, gjk.simplex, params); + // Normal is unit + EXPECT_NEAR(epa.normal.dot(epa.normal), 1.0f, 1e-5f); - if (!epa.success) - { - std::cout << "EPA failed\n"; - } + // For this setup, depth ≈ 1.5 (2 - 0.5) + EXPECT_NEAR(epa.depth, 1.5f, 1e-3f); + + // Normal axis sanity: near X axis + EXPECT_NEAR(std::abs(epa.normal.x), 1.0f, 1e-3f); + EXPECT_NEAR(epa.normal.y, 0.0f, 1e-3f); + EXPECT_NEAR(epa.normal.z, 0.0f, 1e-3f); + + // Try both signs with a tiny margin (avoid grazing contacts) + const float margin = 1.0f + 1e-3f; + const auto pen = epa.normal * epa.depth; + + Mesh b_plus = b; b_plus.set_origin(b_plus.get_origin() + pen * margin); + Mesh b_minus= b; b_minus.set_origin(b_minus.get_origin() - pen * margin); + + Collider B_plus(b_plus), B_minus(b_minus); + + const bool sep_plus = !GJK::collide(A, B_plus).hit; + const bool sep_minus = !GJK::collide(A, B_minus).hit; + + // Exactly one direction should separate + EXPECT_NE(sep_plus, sep_minus) << "Exactly one of ±penetration must separate"; + + // Optional: pick the resolving direction and assert round-trip + const auto resolve = sep_plus ? ( pen * margin) : (-pen * margin); + + Mesh b_resolved = b; b_resolved.set_origin(b_resolved.get_origin() + resolve); + EXPECT_FALSE(GJK::collide(A, Collider(b_resolved)).hit) << "Resolved position should be non-colliding"; + + // Moving the other way should still collide + Mesh b_wrong = b; b_wrong.set_origin(b_wrong.get_origin() - resolve); + EXPECT_TRUE(GJK::collide(A, Collider(b_wrong)).hit); } \ No newline at end of file