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

@@ -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;
         }

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;
     };