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

fixed formating

Browse Source
This commit is contained in:
Orange
2025-11-13 16:01:42 +03:00
parent 06b597f37c
commit 2699053102
1 changed files with 90 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,12 +1,12 @@
#pragma once #pragma once
#include <vector> #include "simplex.hpp"
#include <algorithm> // find_if
#include <array> #include <array>
#include <limits>
#include <queue>
#include <cmath> #include <cmath>
#include <cstdint> #include <cstdint>
#include <algorithm> // find_if #include <limits>
#include "simplex.hpp" #include <queue>
#include <vector>
namespace omath::collision namespace omath::collision
{ {
@@ -16,7 +16,7 @@ namespace omath::collision
{ 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>;
{ -a } -> std::same_as<V>; { -a } -> std::same_as<V>;
{ a * s } -> std::same_as<V>; { a* s } -> std::same_as<V>;
{ a / s } -> std::same_as<V>; { a / s } -> std::same_as<V>;
}; };
@@ -29,25 +29,23 @@ namespace omath::collision
struct Result struct Result
{ {
bool success{false}; bool success{false};
Vertex normal{}; // outward normal (from B to A) Vertex normal{}; // outward normal (from B to A)
float depth{0.0f}; float depth{0.0f};
int iterations{0}; int iterations{0};
int num_vertices{0}; int num_vertices{0};
int num_faces{0}; int num_faces{0};
}; };
struct Params struct Params
{ {
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
}; };
// Precondition: simplex.size()==4 and contains the origin. // Precondition: simplex.size()==4 and contains the origin.
[[nodiscard]] [[nodiscard]]
static Result solve(const ColliderType& a, static Result solve(const ColliderType& a, const ColliderType& b, const Simplex<Vertex>& simplex,
const ColliderType& b,
const Simplex<Vertex>& simplex,
const Params params = {}) const Params params = {})
{ {
// --- Build initial polytope from simplex (4 points) --- // --- Build initial polytope from simplex (4 points) ---
@@ -59,10 +57,10 @@ namespace omath::collision
// 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.push_back(make_face(verts, 0,1,2)); 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, 2, 3));
faces.push_back(make_face(verts, 0,3,1)); faces.push_back(make_face(verts, 0, 3, 1));
faces.push_back(make_face(verts, 1,3,2)); faces.push_back(make_face(verts, 1, 3, 2));
auto heap = rebuild_heap(faces); auto heap = rebuild_heap(faces);
@@ -71,15 +69,16 @@ namespace omath::collision
for (int it = 0; it < params.max_iterations; ++it) for (int it = 0; it < params.max_iterations; ++it)
{ {
// If heap might be stale after face edits, rebuild lazily. // If heap might be stale after face edits, rebuild lazily.
if (heap.empty()) break; if (heap.empty())
break;
// Rebuild when the "closest" face changed (simple cheap guard) // Rebuild when the "closest" face changed (simple cheap guard)
// (We could keep face handles; this is fine for small Ns.) // (We could keep face handles; this is fine for small Ns.)
{
const auto top = heap.top(); if (const auto top = heap.top(); faces[top.idx].d != top.d)
if (faces[top.idx].d != top.d) heap = rebuild_heap(faces);
heap = rebuild_heap(faces);
} if (heap.empty())
if (heap.empty()) break; break;
const int fidx = heap.top().idx; const int fidx = heap.top().idx;
const Face f = faces[fidx]; const Face f = faces[fidx];
@@ -93,7 +92,7 @@ namespace omath::collision
{ {
out.success = true; out.success = true;
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>(verts.size());
out.num_faces = static_cast<int>(faces.size()); out.num_faces = static_cast<int>(faces.size());
@@ -106,11 +105,13 @@ namespace omath::collision
// 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);
std::vector<Edge> boundary; boundary.reserve(faces.size()*2); std::vector<Edge> boundary;
boundary.reserve(faces.size() * 2);
for (int i = 0; i < static_cast<int>(faces.size()); ++i) for (int i = 0; i < static_cast<int>(faces.size()); ++i)
{ {
if (to_delete[i]) continue; if (to_delete[i])
continue;
if (visible_from(faces[i], p)) if (visible_from(faces[i], p))
{ {
const auto& rf = faces[i]; const auto& rf = faces[i];
@@ -122,9 +123,11 @@ namespace omath::collision
} }
// Remove visible faces // Remove visible faces
std::vector<Face> new_faces; new_faces.reserve(faces.size() + boundary.size()); std::vector<Face> new_faces;
new_faces.reserve(faces.size() + boundary.size());
for (int i = 0; i < static_cast<int>(faces.size()); ++i) for (int i = 0; i < static_cast<int>(faces.size()); ++i)
if (!to_delete[i]) new_faces.push_back(faces[i]); if (!to_delete[i])
new_faces.push_back(faces[i]);
faces.swap(new_faces); faces.swap(new_faces);
// Stitch new faces around the horizon // Stitch new faces around the horizon
@@ -143,10 +146,12 @@ namespace omath::collision
if (!faces.empty()) if (!faces.empty())
{ {
auto best = faces[0]; auto best = faces[0];
for (const auto& f : faces) if (f.d < best.d) best = f; for (const auto& f : faces)
if (f.d < best.d)
best = f;
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>(verts.size());
out.num_faces = static_cast<int>(faces.size()); out.num_faces = static_cast<int>(faces.size());
} }
@@ -157,15 +162,24 @@ namespace omath::collision
struct Face struct Face
{ {
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 { int a, b; }; struct Edge
{
int a, b;
};
struct HeapItem { float d; int idx; }; struct HeapItem
struct HeapCmp { {
bool operator()(const HeapItem& lhs, const HeapItem& rhs) const noexcept { 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 return lhs.d > rhs.d; // min-heap by distance
} }
}; };
@@ -188,12 +202,12 @@ namespace omath::collision
static void add_edge_boundary(std::vector<Edge>& boundary, int a, int b) static void add_edge_boundary(std::vector<Edge>& boundary, int a, int b)
{ {
// Keep edges that appear only once; erase if opposite already present // Keep edges that appear only once; erase if opposite already present
auto itb = std::find_if(boundary.begin(), boundary.end(), auto itb =
[&](const Edge& e){ return e.a == b && e.b == a; }); std::find_if(boundary.begin(), boundary.end(), [&](const Edge& e) { return e.a == b && e.b == a; });
if (itb != boundary.end()) if (itb != boundary.end())
boundary.erase(itb); // internal edge cancels out boundary.erase(itb); // internal edge cancels out
else else
boundary.push_back({a,b}); // horizon edge (directed) boundary.push_back({a, b}); // horizon edge (directed)
} }
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)
@@ -202,20 +216,23 @@ namespace omath::collision
const Vertex& a1 = verts[i1]; const Vertex& a1 = verts[i1];
const Vertex& a2 = verts[i2]; const Vertex& a2 = verts[i2];
Vertex n = (a1 - a0).cross(a2 - a0); Vertex n = (a1 - a0).cross(a2 - a0);
if (n.dot(n) <= 1e-30f) { if (n.dot(n) <= 1e-30f)
{
n = any_perp_vec(a1 - a0); // degenerate guard n = any_perp_vec(a1 - a0); // degenerate guard
} }
// Ensure normal points outward (away from origin): require n·a0 >= 0 // Ensure normal points outward (away from origin): require n·a0 >= 0
if (n.dot(a0) < 0.0f) { std::swap(i1, i2); n = -n; } 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)); const float inv_len = 1.0f / std::sqrt(std::max(n.dot(n), 1e-30f));
n = n * inv_len; n = n * inv_len;
const float d = n.dot(a0); const float d = n.dot(a0);
return { i0, i1, i2, n, d }; return {i0, i1, i2, n, d};
} }
static Vertex support_point(const ColliderType& a, static Vertex support_point(const ColliderType& a, const ColliderType& b, const Vertex& dir)
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);
} }
@@ -229,9 +246,10 @@ namespace omath::collision
template<class V> template<class V>
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}})
if (const auto d = v.cross(dir); !near_zero_vec(d)) return d; if (const auto d = v.cross(dir); !near_zero_vec(d))
return V{1,0,0}; return d;
return V{1, 0, 0};
} }
}; };
@@ -239,14 +257,17 @@ namespace omath::collision
template<class ColliderType> template<class ColliderType>
class GjkAlgorithmWithSimplex final class GjkAlgorithmWithSimplex final
{ {
using Vertex = typename ColliderType::VertexType; using Vertex = ColliderType::VertexType;
public: public:
struct Hit { bool hit{false}; Simplex<Vertex> simplex; }; struct Hit
{
bool hit{false};
Simplex<Vertex> simplex;
};
[[nodiscard]] [[nodiscard]]
static Vertex find_support_vertex(const ColliderType& a, static Vertex find_support_vertex(const ColliderType& a, const ColliderType& b, const Vertex& dir)
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);
} }
@@ -254,21 +275,25 @@ namespace omath::collision
[[nodiscard]] [[nodiscard]]
static Hit collide(const ColliderType& a, const ColliderType& b) static Hit collide(const ColliderType& a, const ColliderType& b)
{ {
auto support = find_support_vertex(a, b, {1,0,0}); auto support = find_support_vertex(a, b, {1, 0, 0});
Simplex<Vertex> simplex; simplex.push_front(support); Simplex<Vertex> simplex;
simplex.push_front(support);
auto direction = -support; auto direction = -support;
for (;;) for (;;)
{ {
support = find_support_vertex(a, b, direction); support = find_support_vertex(a, b, direction);
if (support.dot(direction) <= 0.f) return {}; if (support.dot(direction) <= 0.f)
return {};
simplex.push_front(support); simplex.push_front(support);
if (simplex.handle(direction)) if (simplex.handle(direction))
{ {
if (simplex.size() == 4) return { true, simplex }; if (simplex.size() == 4)
return {true, simplex};
// rare degeneracy: reseed // rare degeneracy: reseed
support = find_support_vertex(a, b, {0,1,0}); support = find_support_vertex(a, b, {0, 1, 0});
simplex.clear(); simplex.push_front(support); simplex.clear();
simplex.push_front(support);
direction = -support; direction = -support;
} }
} }