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
#include <vector>
#include "simplex.hpp"
#include <algorithm> // find_if
#include <array>
#include <limits>
#include <queue>
#include <cmath>
#include <cstdint>
#include <algorithm> // find_if
#include "simplex.hpp"
#include <limits>
#include <queue>
#include <vector>
namespace omath::collision
{
@@ -16,7 +16,7 @@ namespace omath::collision
{ 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>;
{ a / s } -> std::same_as<V>;
};
@@ -29,25 +29,23 @@ namespace omath::collision
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};
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
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,
static Result solve(const ColliderType& a, const ColliderType& b, const Simplex<Vertex>& simplex,
const Params params = {})
{
// --- Build initial polytope from simplex (4 points) ---
@@ -59,10 +57,10 @@ namespace omath::collision
// 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));
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);
@@ -71,15 +69,16 @@ namespace omath::collision
for (int it = 0; it < params.max_iterations; ++it)
{
// 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)
// (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;
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];
@@ -93,7 +92,7 @@ namespace omath::collision
{
out.success = true;
out.normal = f.n;
out.depth = f.d; // along unit normal
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());
@@ -106,11 +105,13 @@ namespace omath::collision
// 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);
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 (to_delete[i])
continue;
if (visible_from(faces[i], p))
{
const auto& rf = faces[i];
@@ -122,9 +123,11 @@ namespace omath::collision
}
// 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)
if (!to_delete[i]) new_faces.push_back(faces[i]);
if (!to_delete[i])
new_faces.push_back(faces[i]);
faces.swap(new_faces);
// Stitch new faces around the horizon
@@ -143,10 +146,12 @@ namespace omath::collision
if (!faces.empty())
{
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.normal = best.n;
out.depth = best.d;
out.depth = best.d;
out.num_vertices = static_cast<int>(verts.size());
out.num_faces = static_cast<int>(faces.size());
}
@@ -157,15 +162,24 @@ namespace omath::collision
struct Face
{
int i0, i1, i2;
Vertex n; // unit outward normal
float d; // n · v0 (>=0 ideally because origin is inside)
Vertex n; // unit outward normal
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 HeapCmp {
bool operator()(const HeapItem& lhs, const HeapItem& rhs) const noexcept {
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
}
};
@@ -188,12 +202,12 @@ namespace omath::collision
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; });
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
boundary.erase(itb); // internal edge cancels out
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)
@@ -202,20 +216,23 @@ namespace omath::collision
const Vertex& a1 = verts[i1];
const Vertex& a2 = verts[i2];
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
}
// 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));
n = n * inv_len;
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,
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);
}
@@ -229,9 +246,10 @@ namespace omath::collision
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};
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};
}
};
@@ -239,14 +257,17 @@ namespace omath::collision
template<class ColliderType>
class GjkAlgorithmWithSimplex final
{
using Vertex = typename ColliderType::VertexType;
using Vertex = ColliderType::VertexType;
public:
struct Hit { bool hit{false}; Simplex<Vertex> simplex; };
struct Hit
{
bool hit{false};
Simplex<Vertex> simplex;
};
[[nodiscard]]
static Vertex find_support_vertex(const ColliderType& a,
const ColliderType& b,
const Vertex& dir)
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);
}
@@ -254,21 +275,25 @@ namespace omath::collision
[[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 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 {};
if (support.dot(direction) <= 0.f)
return {};
simplex.push_front(support);
if (simplex.handle(direction))
{
if (simplex.size() == 4) return { true, simplex };
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);
support = find_support_vertex(a, b, {0, 1, 0});
simplex.clear();
simplex.push_front(support);
direction = -support;
}
}