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

105
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
{ {
@@ -45,9 +45,7 @@ namespace omath::collision
// 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) ---
@@ -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()) break; if (heap.empty())
break;
const int fidx = heap.top().idx; const int fidx = heap.top().idx;
const Face f = faces[fidx]; const Face f = faces[fidx];
@@ -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,7 +146,9 @@ 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;
@@ -161,11 +166,20 @@ namespace omath::collision
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,8 +202,8 @@ 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
@@ -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);
} }
@@ -230,7 +247,8 @@ namespace omath::collision
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 d;
return V{1, 0, 0}; 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);
} }
@@ -255,20 +276,24 @@ namespace omath::collision
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;
} }
} }