mirror of
https://github.com/orange-cpp/omath.git
synced 2026-02-13 07:03:25 +00:00
added epa
This commit is contained in:
277
include/omath/collision/epa_algorithm.hpp
Normal file
277
include/omath/collision/epa_algorithm.hpp
Normal file
@@ -0,0 +1,277 @@
|
||||
#pragma once
|
||||
#include <vector>
|
||||
#include <array>
|
||||
#include <limits>
|
||||
#include <queue>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <algorithm> // find_if
|
||||
#include "simplex.hpp"
|
||||
|
||||
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
|
||||
{
|
||||
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
|
||||
{
|
||||
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> verts;
|
||||
verts.reserve(64);
|
||||
for (std::size_t i = 0; i < simplex.size(); ++i)
|
||||
verts.push_back(simplex[i]);
|
||||
|
||||
// Initial tetra faces (windings corrected in make_face)
|
||||
std::vector<Face> faces;
|
||||
faces.reserve(128);
|
||||
faces.push_back(make_face(verts, 0,1,2));
|
||||
faces.push_back(make_face(verts, 0,2,3));
|
||||
faces.push_back(make_face(verts, 0,3,1));
|
||||
faces.push_back(make_face(verts, 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.)
|
||||
{
|
||||
const auto top = heap.top();
|
||||
if (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>(verts.size());
|
||||
out.num_faces = static_cast<int>(faces.size());
|
||||
return out;
|
||||
}
|
||||
|
||||
// Add new vertex
|
||||
const int new_idx = static_cast<int>(verts.size());
|
||||
verts.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(verts, e.a, e.b, new_idx));
|
||||
|
||||
// Rebuild heap after topology change
|
||||
heap = rebuild_heap(faces);
|
||||
|
||||
if (!std::isfinite(verts.back().dot(verts.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>(verts.size());
|
||||
out.num_faces = static_cast<int>(faces.size());
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
private:
|
||||
struct Face
|
||||
{
|
||||
int i0, i1, i2;
|
||||
Vertex n; // unit outward normal
|
||||
float d; // n · v0 (>=0 ideally because origin is inside)
|
||||
};
|
||||
|
||||
struct Edge { int a, b; };
|
||||
|
||||
struct HeapItem { float d; int idx; };
|
||||
struct HeapCmp {
|
||||
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>;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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)
|
||||
}
|
||||
|
||||
static Face make_face(const std::vector<Vertex>& verts, int i0, int i1, int i2)
|
||||
{
|
||||
const Vertex& a0 = verts[i0];
|
||||
const Vertex& a1 = verts[i1];
|
||||
const Vertex& a2 = verts[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 };
|
||||
}
|
||||
|
||||
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>
|
||||
static constexpr bool near_zero_vec(const V& v, const float eps = 1e-7f)
|
||||
{
|
||||
return v.dot(v) <= eps * eps;
|
||||
}
|
||||
|
||||
template<class V>
|
||||
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};
|
||||
}
|
||||
};
|
||||
|
||||
// Optional: the GJK that returns a simplex for EPA (unchanged)
|
||||
template<class ColliderType>
|
||||
class GjkAlgorithmWithSimplex final
|
||||
{
|
||||
using Vertex = typename ColliderType::VertexType;
|
||||
public:
|
||||
struct Hit { bool hit{false}; Simplex<Vertex> simplex; };
|
||||
|
||||
[[nodiscard]]
|
||||
static Vertex find_support_vertex(const ColliderType& a,
|
||||
const ColliderType& b,
|
||||
const Vertex& dir)
|
||||
{
|
||||
return a.find_abs_furthest_vertex(dir) - b.find_abs_furthest_vertex(-dir);
|
||||
}
|
||||
|
||||
[[nodiscard]]
|
||||
static Hit collide(const ColliderType& a, const ColliderType& b)
|
||||
{
|
||||
auto support = find_support_vertex(a, b, {1,0,0});
|
||||
Simplex<Vertex> simplex; simplex.push_front(support);
|
||||
auto direction = -support;
|
||||
|
||||
for (;;)
|
||||
{
|
||||
support = find_support_vertex(a, b, direction);
|
||||
if (support.dot(direction) <= 0.f) return {};
|
||||
simplex.push_front(support);
|
||||
if (simplex.handle(direction))
|
||||
{
|
||||
if (simplex.size() == 4) return { true, simplex };
|
||||
// rare degeneracy: reseed
|
||||
support = find_support_vertex(a, b, {0,1,0});
|
||||
simplex.clear(); simplex.push_front(support);
|
||||
direction = -support;
|
||||
}
|
||||
}
|
||||
}
|
||||
};
|
||||
} // namespace omath::collision
|
||||
@@ -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));
|
||||
}
|
||||
|
||||
57
tests/general/unit_test_epa.cpp
Normal file
57
tests/general/unit_test_epa.cpp
Normal file
@@ -0,0 +1,57 @@
|
||||
//
|
||||
// Created by Vlad on 11/13/2025.
|
||||
//
|
||||
#include "omath/collision/gjk_algorithm.hpp"
|
||||
#include "omath/engines/source_engine/collider.hpp"
|
||||
#include <gtest/gtest.h>
|
||||
#include <omath/collision/epa_algorithm.hpp>
|
||||
#include <omath/engines/source_engine/mesh.hpp>
|
||||
|
||||
namespace
|
||||
{
|
||||
const omath::source_engine::Mesh mesh = {{{-1.f, -1.f, -1.f},
|
||||
{-1.f, -1.f, 1.f},
|
||||
{-1.f, 1.f, -1.f},
|
||||
{-1.f, 1.f, 1.f},
|
||||
{1.f, 1.f, 1.f},
|
||||
{1.f, 1.f, -1.f},
|
||||
{1.f, -1.f, 1.f},
|
||||
{1.f, -1.f, -1.f}},
|
||||
{}};
|
||||
}
|
||||
TEST(UnitTestEpa, TestCollisionTrue)
|
||||
{
|
||||
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 for GJK/EPA
|
||||
|
||||
omath::source_engine::Mesh a(vbo, vao, {1, 1, 1});
|
||||
omath::source_engine::Mesh b(vbo, vao, {1, 1, 1});
|
||||
|
||||
a.set_origin({0, 0, 0});
|
||||
b.set_origin({0.5f, 0, 0}); // slight overlap
|
||||
|
||||
const omath::source_engine::MeshCollider collider_a(mesh);
|
||||
|
||||
omath::source_engine::MeshCollider A(a), B(b);
|
||||
|
||||
// 1) GJK → final simplex
|
||||
using Gjk = omath::collision::GjkAlgorithm<omath::source_engine::MeshCollider>;
|
||||
|
||||
auto gjk = Gjk::is_collide_with_simplex_info(A, B);
|
||||
if (!gjk.hit)
|
||||
{
|
||||
std::cout << "No collision\n";
|
||||
}
|
||||
using Epa = omath::collision::Epa<omath::source_engine::MeshCollider>;
|
||||
// 2) EPA → normal/depth
|
||||
Epa::Params params;
|
||||
params.max_iterations = 64;
|
||||
params.tolerance = 1e-4f;
|
||||
auto epa = Epa::solve(A, B, gjk.simplex, params);
|
||||
|
||||
if (!epa.success)
|
||||
{
|
||||
std::cout << "EPA failed\n";
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user