From 12f888b8d466f6adb97075e9e1a8ad4bcbc0062a Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 21:19:09 +0300 Subject: [PATCH 1/2] Calculates penetration vector for EPA algorithm Adds `penetration_vector` to the `epa_result` struct to represent the direction and magnitude of penetration. This allows for more accurate collision response calculations and simplifies access to penetration information. Updates both the early-exit and iterative EPA calculations within `epa_algorithm.hpp` to compute and store the penetration vector, factoring in the relative origin of the colliding meshes. --- include/omath/collision/epa_algorithm.hpp | 11 +++++++++++ include/omath/collision/mesh_collider.hpp | 5 +++++ 2 files changed, 16 insertions(+) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index 1b51396..d2f0f5f 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -31,6 +31,7 @@ namespace omath::collision { bool success{false}; Vertex normal{}; // outward normal (from B to A) + Vertex penetration_vector; float depth{0.0f}; int iterations{0}; int num_vertices{0}; @@ -96,6 +97,11 @@ namespace omath::collision out.iterations = it + 1; out.num_vertices = static_cast(vertexes.size()); out.num_faces = static_cast(faces.size()); + + const auto centers = b.get_origin() - a.get_origin(); + const auto sign = out.normal.dot(centers) >= 0 ? 1 : -1; + + out.penetration_vector = out.normal * out.depth * sign; return out; } @@ -154,6 +160,11 @@ namespace omath::collision out.depth = best.d; out.num_vertices = static_cast(vertexes.size()); out.num_faces = static_cast(faces.size()); + + const auto centers = b.get_origin() - a.get_origin(); + const auto sign = out.normal.dot(centers) >= 0 ? 1 : -1; + + out.penetration_vector = out.normal * out.depth * sign; } return out; } diff --git a/include/omath/collision/mesh_collider.hpp b/include/omath/collision/mesh_collider.hpp index 30951d8..247b652 100644 --- a/include/omath/collision/mesh_collider.hpp +++ b/include/omath/collision/mesh_collider.hpp @@ -31,6 +31,11 @@ namespace omath::collision return m_mesh.vertex_to_world_space(find_furthest_vertex(direction)); } + [[nodiscard]] + const VertexType& get_origin() const + { + return m_mesh.get_origin(); + } private: MeshType m_mesh; }; From 0283935918593c4bf1a0f7824c3cacdb98e8dc30 Mon Sep 17 00:00:00 2001 From: Orange Date: Thu, 13 Nov 2025 21:25:45 +0300 Subject: [PATCH 2/2] added std::optional --- include/omath/collision/epa_algorithm.hpp | 11 +++---- tests/general/unit_test_epa.cpp | 38 +++++++++++------------ 2 files changed, 24 insertions(+), 25 deletions(-) diff --git a/include/omath/collision/epa_algorithm.hpp b/include/omath/collision/epa_algorithm.hpp index d2f0f5f..f8bc3db 100644 --- a/include/omath/collision/epa_algorithm.hpp +++ b/include/omath/collision/epa_algorithm.hpp @@ -29,7 +29,6 @@ namespace omath::collision struct Result final { - bool success{false}; Vertex normal{}; // outward normal (from B to A) Vertex penetration_vector; float depth{0.0f}; @@ -46,8 +45,8 @@ namespace omath::collision // Precondition: simplex.size()==4 and contains the origin. [[nodiscard]] - static Result solve(const ColliderType& a, const ColliderType& b, const Simplex& simplex, - const Params params = {}) + static std::optional solve(const ColliderType& a, const ColliderType& b, const Simplex& simplex, + const Params params = {}) { // --- Build initial polytope from simplex (4 points) --- std::vector vertexes; @@ -91,7 +90,6 @@ namespace omath::collision // Converged if we can’t push the face closer than tolerance if (p_dist - f.d <= params.tolerance) { - out.success = true; out.normal = f.n; out.depth = f.d; // along unit normal out.iterations = it + 1; @@ -155,7 +153,6 @@ namespace omath::collision for (const auto& f : faces) if (f.d < best.d) best = f; - out.success = true; out.normal = best.n; out.depth = best.d; out.num_vertices = static_cast(vertexes.size()); @@ -165,8 +162,10 @@ namespace omath::collision const auto sign = out.normal.dot(centers) >= 0 ? 1 : -1; out.penetration_vector = out.normal * out.depth * sign; + + return out; } - return out; + return std::nullopt; } private: diff --git a/tests/general/unit_test_epa.cpp b/tests/general/unit_test_epa.cpp index adc468a..30f02ae 100644 --- a/tests/general/unit_test_epa.cpp +++ b/tests/general/unit_test_epa.cpp @@ -36,22 +36,22 @@ TEST(UnitTestEpa, TestCollisionTrue) params.max_iterations = 64; params.tolerance = 1e-4f; auto epa = EPA::solve(A, B, gjk.simplex, params); - ASSERT_TRUE(epa.success) << "EPA should converge"; + ASSERT_TRUE(epa.has_value()) << "EPA should converge"; // Normal is unit - EXPECT_NEAR(epa.normal.dot(epa.normal), 1.0f, 1e-5f); + EXPECT_NEAR(epa->normal.dot(epa->normal), 1.0f, 1e-5f); // For this setup, depth ≈ 1.5 (2 - 0.5) - EXPECT_NEAR(epa.depth, 1.5f, 1e-3f); + 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); + 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; + const auto pen = epa->normal * epa->depth; Mesh b_plus = b; b_plus.set_origin(b_plus.get_origin() + pen * margin); @@ -102,25 +102,25 @@ TEST(UnitTestEpa, TestCollisionTrue2) params.max_iterations = 64; params.tolerance = 1e-4f; auto epa = EPA::solve(A, B, gjk.simplex, params); - ASSERT_TRUE(epa.success) << "EPA should converge"; + ASSERT_TRUE(epa.has_value()) << "EPA should converge"; // Normal is unit-length - EXPECT_NEAR(epa.normal.dot(epa.normal), 1.0f, 1e-5f); + EXPECT_NEAR(epa->normal.dot(epa->normal), 1.0f, 1e-5f); // For centers at 0 and +0.5 and half-extent 1 -> depth ≈ 1.5 - EXPECT_NEAR(epa.depth, 1.5f, 1e-3f); + EXPECT_NEAR(epa->depth, 1.5f, 1e-3f); // Axis sanity: mostly X - 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); + 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); // Choose a deterministic sign: orient penetration from A toward B const auto centers = b.get_origin() - a.get_origin(); // (0.5, 0, 0) - float sign = (epa.normal.dot(centers) >= 0.0f) ? +1.0f : -1.0f; + float sign = (epa->normal.dot(centers) >= 0.0f) ? +1.0f : -1.0f; constexpr float margin = 1.0f + 1e-3f; // tiny slack to avoid grazing - const auto pen = epa.normal * epa.depth * sign; + const auto pen = epa->normal * epa->depth * sign; // Apply once: B + pen must separate; the opposite must still collide Mesh b_resolved = b; @@ -132,8 +132,8 @@ TEST(UnitTestEpa, TestCollisionTrue2) EXPECT_TRUE(GJK::is_collide(A, Collider(b_wrong))) << "Opposite direction should still intersect"; // Some book-keeping sanity - EXPECT_GT(epa.iterations, 0); - EXPECT_LT(epa.iterations, params.max_iterations); - EXPECT_GE(epa.num_faces, 4); - EXPECT_GT(epa.num_vertices, 4); + EXPECT_GT(epa->iterations, 0); + EXPECT_LT(epa->iterations, params.max_iterations); + EXPECT_GE(epa->num_faces, 4); + EXPECT_GT(epa->num_vertices, 4); }