From 12f888b8d466f6adb97075e9e1a8ad4bcbc0062a Mon Sep 17 00:00:00 2001
From: Orange <orange-cpp@yandex.ru>
Date: Thu, 13 Nov 2025 21:19:09 +0300
Subject: [PATCH] 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<int>(vertexes.size());
                     out.num_faces = static_cast<int>(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<int>(vertexes.size());
                 out.num_faces = static_cast<int>(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;
     };