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