admin/omath
1
0
Fork 0
mirror of https://github.com/orange-cpp/omath.git synced 2026-02-14 07:23:26 +00:00
Code Issues Packages Projects Releases Wiki Activity

Refactor EPA algorithm for clarity

Browse Source 
Improves code organization and readability within the EPA algorithm implementation.

Changes include renaming variables for better semantic meaning (e.g., `verts` to `vertexes`), adding `final` specifiers to structs for clarity, and enhancing function signatures with `[[nodiscard]]` where appropriate.

These refactorings aim to enhance maintainability and understanding of the code without altering its core functionality.
This commit is contained in:
Orange
2025-11-13 16:15:45 +03:00
parent 2b21caf58f
commit 09fd92ccad
1 changed files with 26 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
include/omath/collision/epa_algorithm.hpp
View File

@@ -11,8 +11,7 @@
namespace omath::collision namespace omath::collision
{ {
template<class V> template<class V>
concept EpaVector = requires(const V& a, const V& b, float s) concept EpaVector = requires(const V& a, const V& b, float s) {
{
{ a - b } -> std::same_as<V>; { a - b } -> std::same_as<V>;
{ a.cross(b) } -> std::same_as<V>; { a.cross(b) } -> std::same_as<V>;
{ a.dot(b) } -> std::same_as<float>; { a.dot(b) } -> std::same_as<float>;
@@ -28,7 +27,7 @@ namespace omath::collision
using Vertex = typename ColliderType::VertexType; using Vertex = typename ColliderType::VertexType;
static_assert(EpaVector<Vertex>, "VertexType must satisfy EpaVector concept"); static_assert(EpaVector<Vertex>, "VertexType must satisfy EpaVector concept");
struct Result struct Result final
{ {
bool success{false}; bool success{false};
Vertex normal{}; // outward normal (from B to A) Vertex normal{}; // outward normal (from B to A)
@@ -38,7 +37,7 @@ namespace omath::collision
int num_faces{0}; int num_faces{0};
}; };
struct Params struct Params final
{ {
int max_iterations{64}; int max_iterations{64};
float tolerance{1e-4f}; // absolute tolerance on distance growth float tolerance{1e-4f}; // absolute tolerance on distance growth
@@ -50,18 +49,18 @@ namespace omath::collision
const Params params = {}) const Params params = {})
{ {
// --- Build initial polytope from simplex (4 points) --- // --- Build initial polytope from simplex (4 points) ---
std::vector<Vertex> verts; std::vector<Vertex> vertexes;
verts.reserve(64); vertexes.reserve(64);
for (std::size_t i = 0; i < simplex.size(); ++i) for (std::size_t i = 0; i < simplex.size(); ++i)
verts.push_back(simplex[i]); vertexes.push_back(simplex[i]);
// Initial tetra faces (windings corrected in make_face) // Initial tetra faces (windings corrected in make_face)
std::vector<Face> faces; std::vector<Face> faces;
faces.reserve(128); faces.reserve(128);
faces.emplace_back(make_face(verts, 0, 1, 2)); faces.emplace_back(make_face(vertexes, 0, 1, 2));
faces.emplace_back(make_face(verts, 0, 2, 3)); faces.emplace_back(make_face(vertexes, 0, 2, 3));
faces.emplace_back(make_face(verts, 0, 3, 1)); faces.emplace_back(make_face(vertexes, 0, 3, 1));
faces.emplace_back(make_face(verts, 1, 3, 2)); faces.emplace_back(make_face(vertexes, 1, 3, 2));
auto heap = rebuild_heap(faces); auto heap = rebuild_heap(faces);
@@ -95,14 +94,14 @@ namespace omath::collision
out.normal = f.n; out.normal = f.n;
out.depth = f.d; // along unit normal out.depth = f.d; // along unit normal
out.iterations = it + 1; out.iterations = it + 1;
out.num_vertices = static_cast<int>(verts.size()); out.num_vertices = static_cast<int>(vertexes.size());
out.num_faces = static_cast<int>(faces.size()); out.num_faces = static_cast<int>(faces.size());
return out; return out;
} }
// Add new vertex // Add new vertex
const int new_idx = static_cast<int>(verts.size()); const int new_idx = static_cast<int>(vertexes.size());
verts.push_back(p); vertexes.push_back(p);
// Mark faces visible from p and collect their horizon // Mark faces visible from p and collect their horizon
std::vector<char> to_delete(faces.size(), 0); std::vector<char> to_delete(faces.size(), 0);
@@ -133,12 +132,12 @@ namespace omath::collision
// Stitch new faces around the horizon // Stitch new faces around the horizon
for (const auto& e : boundary) for (const auto& e : boundary)
faces.push_back(make_face(verts, e.a, e.b, new_idx)); faces.push_back(make_face(vertexes, e.a, e.b, new_idx));
// Rebuild heap after topology change // Rebuild heap after topology change
heap = rebuild_heap(faces); heap = rebuild_heap(faces);
if (!std::isfinite(verts.back().dot(verts.back()))) if (!std::isfinite(vertexes.back().dot(vertexes.back())))
break; // safety break; // safety
out.iterations = it + 1; out.iterations = it + 1;
} }
@@ -153,31 +152,31 @@ namespace omath::collision
out.success = true; out.success = true;
out.normal = best.n; out.normal = best.n;
out.depth = best.d; out.depth = best.d;
out.num_vertices = static_cast<int>(verts.size()); out.num_vertices = static_cast<int>(vertexes.size());
out.num_faces = static_cast<int>(faces.size()); out.num_faces = static_cast<int>(faces.size());
} }
return out; return out;
} }
private: private:
struct Face struct Face final
{ {
int i0, i1, i2; int i0, i1, i2;
Vertex n; // unit outward normal Vertex n; // unit outward normal
float d; // n · v0 (>=0 ideally because origin is inside) float d; // n · v0 (>=0 ideally because origin is inside)
}; };
struct Edge struct Edge final
{ {
int a, b; int a, b;
}; };
struct HeapItem struct HeapItem final
{ {
float d; float d;
int idx; int idx;
}; };
struct HeapCmp struct HeapCmp final
{ {
bool operator()(const HeapItem& lhs, const HeapItem& rhs) const noexcept bool operator()(const HeapItem& lhs, const HeapItem& rhs) const noexcept
{ {
@@ -186,6 +185,7 @@ namespace omath::collision
}; };
using Heap = std::priority_queue<HeapItem, std::vector<HeapItem>, HeapCmp>; using Heap = std::priority_queue<HeapItem, std::vector<HeapItem>, HeapCmp>;
[[nodiscard]]
static Heap rebuild_heap(const std::vector<Face>& faces) static Heap rebuild_heap(const std::vector<Face>& faces)
{ {
Heap h; Heap h;
@@ -194,6 +194,7 @@ namespace omath::collision
return h; return h;
} }
[[nodiscard]]
static bool visible_from(const Face& f, const Vertex& p) static bool visible_from(const Face& f, const Vertex& p)
{ {
// positive if p is in front of the face // positive if p is in front of the face
@@ -211,6 +212,7 @@ namespace omath::collision
boundary.push_back({a, b}); // horizon edge (directed) boundary.push_back({a, b}); // horizon edge (directed)
} }
[[nodiscard]]
static Face make_face(const std::vector<Vertex>& verts, int i0, int i1, int i2) static Face make_face(const std::vector<Vertex>& verts, int i0, int i1, int i2)
{ {
const Vertex& a0 = verts[i0]; const Vertex& a0 = verts[i0];
@@ -233,18 +235,21 @@ namespace omath::collision
return {i0, i1, i2, n, d}; return {i0, i1, i2, n, d};
} }
[[nodiscard]]
static Vertex support_point(const ColliderType& a, const ColliderType& b, const Vertex& dir) 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); return a.find_abs_furthest_vertex(dir) - b.find_abs_furthest_vertex(-dir);
} }
template<class V> template<class V>
[[nodiscard]]
static constexpr bool near_zero_vec(const V& v, const float eps = 1e-7f) static constexpr bool near_zero_vec(const V& v, const float eps = 1e-7f)
{ {
return v.dot(v) <= eps * eps; return v.dot(v) <= eps * eps;
} }
template<class V> template<class V>
[[nodiscard]]
static constexpr V any_perp_vec(const V& 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}}) for (const auto& dir : {V{1, 0, 0}, V{0, 1, 0}, V{0, 0, 1}})