Merge pull request #103 from orange-cpp/feature/epa_algorithm

Feature/epa algorithm

include/omath/3d_primitives/mesh.hpp

@@ -25,7 +25,7 @@ namespace omath::primitives
         Vao m_vertex_array_object;
 
         Mesh(Vbo vbo, Vao vao, const Vector3<NumericType> scale = {1, 1, 1,})
-            : m_vertex_buffer(std::move(vbo)), m_vertex_array_object(std::move(vao)), m_scale(scale)
+            : m_vertex_buffer(std::move(vbo)), m_vertex_array_object(std::move(vao)), m_scale(std::move(scale))
         {
         }
         void set_origin(const Vector3<NumericType>& new_origin)

include/omath/collision/epa_algorithm.hpp

@@ -0,0 +1,261 @@
+#pragma once
+#include "simplex.hpp"
+#include <algorithm> // find_if
+#include <array>
+#include <cmath>
+#include <cstdint>
+#include <limits>
+#include <queue>
+#include <vector>
+
+namespace omath::collision
+{
+    template<class V>
+    concept EpaVector = requires(const V& a, const V& b, float s) {
+        { a - b } -> std::same_as<V>;
+        { a.cross(b) } -> std::same_as<V>;
+        { a.dot(b) } -> std::same_as<float>;
+        { -a } -> std::same_as<V>;
+        { a* s } -> std::same_as<V>;
+        { a / s } -> std::same_as<V>;
+    };
+
+    template<class ColliderType>
+    class Epa final
+    {
+    public:
+        using Vertex = typename ColliderType::VertexType;
+        static_assert(EpaVector<Vertex>, "VertexType must satisfy EpaVector concept");
+
+        struct Result final
+        {
+            bool success{false};
+            Vertex normal{}; // outward normal (from B to A)
+            float depth{0.0f};
+            int iterations{0};
+            int num_vertices{0};
+            int num_faces{0};
+        };
+
+        struct Params final
+        {
+            int max_iterations{64};
+            float tolerance{1e-4f}; // absolute tolerance on distance growth
+        };
+
+        // Precondition: simplex.size()==4 and contains the origin.
+        [[nodiscard]]
+        static Result solve(const ColliderType& a, const ColliderType& b, const Simplex<Vertex>& simplex,
+                            const Params params = {})
+        {
+            // --- Build initial polytope from simplex (4 points) ---
+            std::vector<Vertex> vertexes;
+            vertexes.reserve(64);
+            for (std::size_t i = 0; i < simplex.size(); ++i)
+                vertexes.push_back(simplex[i]);
+
+            // Initial tetra faces (windings corrected in make_face)
+            std::vector<Face> faces;
+            faces.reserve(128);
+            faces.emplace_back(make_face(vertexes, 0, 1, 2));
+            faces.emplace_back(make_face(vertexes, 0, 2, 3));
+            faces.emplace_back(make_face(vertexes, 0, 3, 1));
+            faces.emplace_back(make_face(vertexes, 1, 3, 2));
+
+            auto heap = rebuild_heap(faces);
+
+            Result out{};
+
+            for (int it = 0; it < params.max_iterations; ++it)
+            {
+                // If heap might be stale after face edits, rebuild lazily.
+                if (heap.empty())
+                    break;
+                // Rebuild when the "closest" face changed (simple cheap guard)
+                // (We could keep face handles; this is fine for small Ns.)
+
+                if (const auto top = heap.top(); faces[top.idx].d != top.d)
+                    heap = rebuild_heap(faces);
+
+                if (heap.empty())
+                    break;
+
+                const int fidx = heap.top().idx;
+                const Face f = faces[fidx];
+
+                // Get farthest point in face normal direction
+                const Vertex p = support_point(a, b, f.n);
+                const float p_dist = f.n.dot(p);
+
+                // 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;
+                    out.num_vertices = static_cast<int>(vertexes.size());
+                    out.num_faces = static_cast<int>(faces.size());
+                    return out;
+                }
+
+                // Add new vertex
+                const int new_idx = static_cast<int>(vertexes.size());
+                vertexes.push_back(p);
+
+                // Mark faces visible from p and collect their horizon
+                std::vector<char> to_delete(faces.size(), 0);
+                std::vector<Edge> boundary;
+                boundary.reserve(faces.size() * 2);
+
+                for (int i = 0; i < static_cast<int>(faces.size()); ++i)
+                {
+                    if (to_delete[i])
+                        continue;
+                    if (visible_from(faces[i], p))
+                    {
+                        const auto& rf = faces[i];
+                        to_delete[i] = 1;
+                        add_edge_boundary(boundary, rf.i0, rf.i1);
+                        add_edge_boundary(boundary, rf.i1, rf.i2);
+                        add_edge_boundary(boundary, rf.i2, rf.i0);
+                    }
+                }
+
+                // Remove visible faces
+                std::vector<Face> new_faces;
+                new_faces.reserve(faces.size() + boundary.size());
+                for (int i = 0; i < static_cast<int>(faces.size()); ++i)
+                    if (!to_delete[i])
+                        new_faces.push_back(faces[i]);
+                faces.swap(new_faces);
+
+                // Stitch new faces around the horizon
+                for (const auto& e : boundary)
+                    faces.push_back(make_face(vertexes, e.a, e.b, new_idx));
+
+                // Rebuild heap after topology change
+                heap = rebuild_heap(faces);
+
+                if (!std::isfinite(vertexes.back().dot(vertexes.back())))
+                    break; // safety
+                out.iterations = it + 1;
+            }
+
+            // Fallback: pick closest face as best-effort answer
+            if (!faces.empty())
+            {
+                auto best = faces[0];
+                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<int>(vertexes.size());
+                out.num_faces = static_cast<int>(faces.size());
+            }
+            return out;
+        }
+
+    private:
+        struct Face final
+        {
+            int i0, i1, i2;
+            Vertex n; // unit outward normal
+            float d; // n · v0  (>=0 ideally because origin is inside)
+        };
+
+        struct Edge final
+        {
+            int a, b;
+        };
+
+        struct HeapItem final
+        {
+            float d;
+            int idx;
+        };
+        struct HeapCmp final
+        {
+            bool operator()(const HeapItem& lhs, const HeapItem& rhs) const noexcept
+            {
+                return lhs.d > rhs.d; // min-heap by distance
+            }
+        };
+        using Heap = std::priority_queue<HeapItem, std::vector<HeapItem>, HeapCmp>;
+
+        [[nodiscard]]
+        static Heap rebuild_heap(const std::vector<Face>& faces)
+        {
+            Heap h;
+            for (int i = 0; i < static_cast<int>(faces.size()); ++i)
+                h.push({faces[i].d, i});
+            return h;
+        }
+
+        [[nodiscard]]
+        static bool visible_from(const Face& f, const Vertex& p)
+        {
+            // positive if p is in front of the face
+            return (f.n.dot(p) - f.d) > 1e-7f;
+        }
+
+        static void add_edge_boundary(std::vector<Edge>& boundary, int a, int b)
+        {
+            // Keep edges that appear only once; erase if opposite already present
+            auto itb =
+                    std::find_if(boundary.begin(), boundary.end(), [&](const Edge& e) { return e.a == b && e.b == a; });
+            if (itb != boundary.end())
+                boundary.erase(itb); // internal edge cancels out
+            else
+                boundary.push_back({a, b}); // horizon edge (directed)
+        }
+
+        [[nodiscard]]
+        static Face make_face(const std::vector<Vertex>& vertexes, int i0, int i1, int i2)
+        {
+            const Vertex& a0 = vertexes[i0];
+            const Vertex& a1 = vertexes[i1];
+            const Vertex& a2 = vertexes[i2];
+            Vertex n = (a1 - a0).cross(a2 - a0);
+            if (n.dot(n) <= 1e-30f)
+            {
+                n = any_perp_vec(a1 - a0); // degenerate guard
+            }
+            // Ensure normal points outward (away from origin): require n·a0 >= 0
+            if (n.dot(a0) < 0.0f)
+            {
+                std::swap(i1, i2);
+                n = -n;
+            }
+            const float inv_len = 1.0f / std::sqrt(std::max(n.dot(n), 1e-30f));
+            n = n * inv_len;
+            const float d = n.dot(a0);
+            return {i0, i1, i2, n, d};
+        }
+
+        [[nodiscard]]
+        static Vertex support_point(const ColliderType& a, const ColliderType& b, const Vertex& dir)
+        {
+            return a.find_abs_furthest_vertex(dir) - b.find_abs_furthest_vertex(-dir);
+        }
+
+        template<class V>
+        [[nodiscard]]
+        static constexpr bool near_zero_vec(const V& v, const float eps = 1e-7f)
+        {
+            return v.dot(v) <= eps * eps;
+        }
+
+        template<class V>
+        [[nodiscard]]
+        static constexpr V any_perp_vec(const V& v)
+        {
+            for (const auto& dir : {V{1, 0, 0}, V{0, 1, 0}, V{0, 0, 1}})
+                if (const auto d = v.cross(dir); !near_zero_vec(d))
+                    return d;
+            return V{1, 0, 0};
+        }
+    };
+} // namespace omath::collision

include/omath/collision/gjk_algorithm.hpp

@@ -7,6 +7,13 @@
 
 namespace omath::collision
 {
+    template<class VertexType>
+    struct GjkHitInfo final
+    {
+        bool hit{false};
+        Simplex<VertexType> simplex; // valid only if hit == true and size==4
+    };
+
     template<class ColliderType>
     class GjkAlgorithm final
     {
@@ -23,7 +30,13 @@ namespace omath::collision
         [[nodiscard]]
         static bool is_collide(const ColliderType& collider_a, const ColliderType& collider_b)
         {
-            // Get initial support point in any direction
+            return is_collide_with_simplex_info(collider_a, collider_b).hit;
+        }
+
+        [[nodiscard]]
+        static GjkHitInfo<VertexType> is_collide_with_simplex_info(const ColliderType& collider_a,
+                                                                   const ColliderType& collider_b)
+        {
             auto support = find_support_vertex(collider_a, collider_b, {1, 0, 0});
 
             Simplex<VertexType> simplex;
@@ -36,12 +49,12 @@ namespace omath::collision
                 support = find_support_vertex(collider_a, collider_b, direction);
 
                 if (support.dot(direction) <= 0.f)
-                    return false;
+                    return {false, simplex};
 
                 simplex.push_front(support);
 
                 if (simplex.handle(direction))
-                    return true;
+                    return {true, simplex};
             }
         }
     };

include/omath/collision/mesh_collider.hpp

@@ -19,14 +19,14 @@ namespace omath::collision
         }
 
         [[nodiscard]]
-        const Vector3<float>& find_furthest_vertex(const Vector3<float>& direction) const
+        const VertexType& find_furthest_vertex(const VertexType& direction) const
         {
             return *std::ranges::max_element(m_mesh.m_vertex_buffer, [&direction](const auto& first, const auto& second)
                                              { return first.dot(direction) < second.dot(direction); });
         }
 
         [[nodiscard]]
-        Vector3<float> find_abs_furthest_vertex(const Vector3<float>& direction) const
+        VertexType find_abs_furthest_vertex(const VertexType& direction) const
         {
             return m_mesh.vertex_to_world_space(find_furthest_vertex(direction));
         }

include/omath/linear_algebra/triangle.hpp

@@ -26,12 +26,12 @@ namespace omath
         {
         }
 
-        Vector3<float> m_vertex1;
-        Vector3<float> m_vertex2;
-        Vector3<float> m_vertex3;
+        Vector m_vertex1;
+        Vector m_vertex2;
+        Vector m_vertex3;
 
         [[nodiscard]]
-        constexpr Vector3<float> calculate_normal() const
+        constexpr Vector calculate_normal() const
         {
             const auto b = side_b_vector();
             const auto a = side_a_vector();
@@ -40,25 +40,25 @@ namespace omath
         }
 
         [[nodiscard]]
-        float side_a_length() const
+        Vector::ContainedType side_a_length() const
         {
             return m_vertex1.distance_to(m_vertex2);
         }
 
         [[nodiscard]]
-        float side_b_length() const
+        Vector::ContainedType side_b_length() const
         {
             return m_vertex3.distance_to(m_vertex2);
         }
 
         [[nodiscard]]
-        constexpr Vector3<float> side_a_vector() const
+        constexpr Vector side_a_vector() const
         {
             return m_vertex1 - m_vertex2;
         }
 
         [[nodiscard]]
-        constexpr float hypot() const
+        constexpr Vector::ContainedType hypot() const
         {
             return m_vertex1.distance_to(m_vertex3);
         }
@@ -72,12 +72,12 @@ namespace omath
             return std::abs(side_a * side_a + side_b * side_b - hypot_value * hypot_value) <= 0.0001f;
         }
         [[nodiscard]]
-        constexpr Vector3<float> side_b_vector() const
+        constexpr Vector side_b_vector() const
         {
             return m_vertex3 - m_vertex2;
         }
         [[nodiscard]]
-        constexpr Vector3<float> mid_point() const
+        constexpr Vector mid_point() const
         {
             return (m_vertex1 + m_vertex2 + m_vertex3) / 3;
         }

include/omath/linear_algebra/vector2.hpp

@@ -19,6 +19,7 @@ namespace omath
     class Vector2
     {
     public:
+        using ContainedType = Type;
         Type x = static_cast<Type>(0);
         Type y = static_cast<Type>(0);
 

include/omath/linear_algebra/vector3.hpp

@@ -23,6 +23,7 @@ namespace omath
     class Vector3 : public Vector2<Type>
     {
     public:
+        using ContainedType = Type;
         Type z = static_cast<Type>(0);
         constexpr Vector3(const Type& x, const Type& y, const Type& z) noexcept: Vector2<Type>(x, y), z(z)
         {

include/omath/linear_algebra/vector4.hpp

@@ -13,6 +13,7 @@ namespace omath
     class Vector4 : public Vector3<Type>
     {
     public:
+        using ContainedType = Type;
         Type w;
 
         constexpr Vector4(const Type& x, const Type& y, const Type& z, const Type& w): Vector3<Type>(x, y, z), w(w)

tests/general/unit_test_epa.cpp

@@ -0,0 +1,139 @@
+#include "omath/collision/epa_algorithm.hpp" // Epa<Collider> + GjkAlgorithmWithSimplex<Collider>
+#include "omath/collision/gjk_algorithm.hpp"
+#include "omath/collision/simplex.hpp"
+#include "omath/engines/source_engine/collider.hpp"
+#include "omath/engines/source_engine/mesh.hpp"
+#include "omath/linear_algebra/vector3.hpp"
+#include <gtest/gtest.h>
+
+using Mesh = omath::source_engine::Mesh;
+using Collider = omath::source_engine::MeshCollider;
+using GJK = omath::collision::GjkAlgorithm<Collider>;
+using EPA = omath::collision::Epa<Collider>;
+
+TEST(UnitTestEpa, TestCollisionTrue)
+{
+    // Unit cube [-1,1]^3
+    std::vector<omath::Vector3<float>> 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<omath::Vector3<std::size_t>> vao; // not needed
+
+    Mesh a(vbo, vao, {1, 1, 1});
+    Mesh b(vbo, vao, {1, 1, 1});
+
+    // Overlap along +X by 0.5
+    a.set_origin({0, 0, 0});
+    b.set_origin({0.5f, 0, 0});
+
+    Collider A(a), B(b);
+
+    // GJK
+    auto gjk = GJK::is_collide_with_simplex_info(A, B);
+    ASSERT_TRUE(gjk.hit) << "GJK should report collision";
+
+    // 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";
+
+    // Normal is unit
+    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);
+
+    // 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::is_collide_with_simplex_info(A, B_plus).hit;
+    const bool sep_minus = !GJK::is_collide_with_simplex_info(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::is_collide(A, Collider(b_resolved))) << "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::is_collide(A, Collider(b_wrong)));
+}
+TEST(UnitTestEpa, TestCollisionTrue2)
+{
+    std::vector<omath::Vector3<float>> 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<omath::Vector3<std::size_t>> vao; // not needed
+
+    Mesh a(vbo, vao, {1, 1, 1});
+    Mesh b(vbo, vao, {1, 1, 1});
+
+    // Overlap along +X by 0.5
+    a.set_origin({0, 0, 0});
+    b.set_origin({0.5f, 0, 0});
+
+    Collider A(a), B(b);
+
+    // --- GJK must detect collision and provide simplex ---
+    auto gjk = GJK::is_collide_with_simplex_info(A, B);
+    ASSERT_TRUE(gjk.hit) << "GJK should report collision for overlapping cubes";
+
+    // --- EPA penetration ---
+    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";
+
+    // Normal is unit-length
+    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);
+
+    // 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);
+
+    // 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;
+
+    constexpr float margin = 1.0f + 1e-3f; // tiny slack to avoid grazing
+    const auto pen = epa.normal * epa.depth * sign;
+
+    // Apply once: B + pen must separate; the opposite must still collide
+    Mesh b_resolved = b;
+    b_resolved.set_origin(b_resolved.get_origin() + pen * margin);
+    EXPECT_FALSE(GJK::is_collide(A, Collider(b_resolved))) << "Applying penetration should separate";
+
+    Mesh b_wrong = b;
+    b_wrong.set_origin(b_wrong.get_origin() - pen * margin);
+    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);
+}