admin/omath
1
0
Fork 0
mirror of https://github.com/orange-cpp/omath.git synced 2026-04-18 17:03:27 +00:00
Code Issues Packages Projects Releases Wiki Activity

optimized

Browse Source
This commit is contained in:
orange
2026-03-02 19:40:45 +03:00
parent ea2c7c3d7f
commit a79ad6948c
2 changed files with 72 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -50,89 +50,102 @@ namespace omath::collision
int max_iterations{64};
FloatingType tolerance{1e-4}; // absolute tolerance on distance growth
};
// Precondition: simplex.size()==4 and contains the origin.
[[nodiscard]]
static std::optional<Result> solve(const ColliderInterfaceType& a, const ColliderInterfaceType& b,
const Simplex<VectorType>& simplex, const Params params = {},
std::pmr::memory_resource& mem_resource = *std::pmr::get_default_resource())
{
// --- Build initial polytope from simplex (4 points) ---
std::pmr::vector<VectorType> vertexes = build_initial_polytope_from_simplex(simplex, mem_resource);
// Initial tetra faces (windings corrected in make_face)
std::pmr::vector<Face> faces = create_initial_tetra_faces(mem_resource, vertexes);
auto heap = rebuild_heap(faces, mem_resource);
// Build initial min-heap by distance.
Heap heap = rebuild_heap(faces, mem_resource);
Result out{};
// Hoisted outside the loop to reuse the allocation across iterations.
std::pmr::vector<Edge> boundary{&mem_resource};
boundary.reserve(16);
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.)
if (const auto top = heap.top(); faces[top.idx].d != top.d)
heap = rebuild_heap(faces, mem_resource);
// Lazily discard stale (deleted or index-mismatched) heap entries.
while (!heap.empty())
{
const auto& top = heap.top();
if (!faces[top.idx].deleted && faces[top.idx].d == top.d)
break;
heap.pop();
}
if (heap.empty())
break;
// FIXME: STORE REF VALUE, DO NOT USE
// AFTER IF STATEMENT BLOCK
const Face& face = faces[heap.top().idx];
// Get the furthest point in face normal direction
const VectorType p = support_point(a, b, face.n);
const auto p_dist = face.n.dot(p);
// Converged if we can’t push the face closer than tolerance
// Converged: new support can't push the face closer than tolerance.
if (p_dist - face.d <= params.tolerance)
{
out.normal = face.n;
out.depth = face.d; // along unit normal
out.depth = face.d;
out.iterations = it + 1;
out.num_vertices = static_cast<int>(vertexes.size());
out.num_faces = static_cast<int>(faces.size());
out.penetration_vector = out.normal * out.depth;
return out;
}
// Add new vertex
const int new_idx = static_cast<int>(vertexes.size());
vertexes.emplace_back(p);
const auto [to_delete, boundary] = mark_visible_and_collect_horizon(faces, p);
// Tombstone visible faces and collect the horizon boundary.
// This avoids copying the faces array (O(n)) each iteration.
boundary.clear();
for (auto& f : faces)
{
if (!f.deleted && visible_from(f, p))
{
f.deleted = true;
add_edge_boundary(boundary, f.i0, f.i1);
add_edge_boundary(boundary, f.i1, f.i2);
add_edge_boundary(boundary, f.i2, f.i0);
}
}
erase_marked(faces, to_delete);
// Stitch new faces around the horizon
// Stitch new faces around the horizon and push them directly onto the
// heap — no full O(n log n) rebuild needed.
for (const auto& e : boundary)
{
const int fi = static_cast<int>(faces.size());
faces.emplace_back(make_face(vertexes, e.a, e.b, new_idx));
// Rebuild heap after topology change
heap = rebuild_heap(faces, mem_resource);
heap.emplace(faces.back().d, fi);
}
if (!std::isfinite(vertexes.back().dot(vertexes.back())))
break; // safety
out.iterations = it + 1;
}
if (faces.empty())
// Find the best surviving (non-deleted) face.
const Face* best = nullptr;
for (const auto& f : faces)
if (!f.deleted && (best == nullptr || f.d < best->d))
best = &f;
if (!best)
return std::nullopt;
const auto best = *std::ranges::min_element(faces, [](const auto& first, const auto& second)
{ return first.d < second.d; });
out.normal = best.n;
out.depth = best.d;
out.normal = best->n;
out.depth = best->d;
out.num_vertices = static_cast<int>(vertexes.size());
out.num_faces = static_cast<int>(faces.size());
out.penetration_vector = out.normal * out.depth;
return out;
}
@@ -140,8 +153,9 @@ namespace omath::collision
struct Face final
{
int i0, i1, i2;
VectorType n; // unit outward normal
FloatingType d; // n · v0 (>=0 ideally because origin is inside)
VectorType n; // unit outward normal
FloatingType d; // n · v0 (>= 0 ideally because origin is inside)
bool deleted{false}; // tombstone flag — avoids O(n) compaction per iteration
};
struct Edge final
@@ -154,6 +168,7 @@ namespace omath::collision
FloatingType d;
int idx;
};
struct HeapCmp final
{
[[nodiscard]]
@@ -169,31 +184,28 @@ namespace omath::collision
static Heap rebuild_heap(const std::pmr::vector<Face>& faces, auto& memory_resource)
{
std::pmr::vector<HeapItem> storage{&memory_resource};
storage.reserve(faces.size()); // optional but recommended
storage.reserve(faces.size());
Heap h{HeapCmp{}, std::move(storage)};
for (int i = 0; i < static_cast<int>(faces.size()); ++i)
h.emplace(faces[i].d, i);
return h; // allocator is preserved
if (!faces[i].deleted)
h.emplace(faces[i].d, i);
return h;
}
[[nodiscard]]
static bool visible_from(const Face& f, const VectorType& p)
{
// positive if p is in front of the face
return f.n.dot(p) - f.d > static_cast<FloatingType>(1e-7);
}
static void add_edge_boundary(std::pmr::vector<Edge>& boundary, int a, int b)
{
// Keep edges that appear only once; erase if opposite already present
// Keep edges that appear only once; cancel if opposite already present.
auto itb = std::ranges::find_if(boundary, [&](const Edge& e) { return e.a == b && e.b == a; });
if (itb != boundary.end())
boundary.erase(itb); // internal edge cancels out
boundary.erase(itb);
else
boundary.emplace_back(a, b); // horizon edge (directed)
boundary.emplace_back(a, b);
}
[[nodiscard]]
@@ -204,9 +216,7 @@ namespace omath::collision
const VectorType& a2 = vertexes[i2];
VectorType n = (a1 - a0).cross(a2 - a0);
if (n.dot(n) <= static_cast<FloatingType>(1e-30))
{
n = any_perp_vec(a1 - a0); // degenerate guard
}
// Ensure normal points outward (away from origin): require n·a0 >= 0
if (n.dot(a0) < static_cast<FloatingType>(0.0))
{
@@ -243,6 +253,7 @@ namespace omath::collision
return d;
return V{1, 0, 0};
}
[[nodiscard]]
static std::pmr::vector<Face> create_initial_tetra_faces(std::pmr::memory_resource& mem_resource,
const std::pmr::vector<VectorType>& vertexes)
@@ -262,48 +273,9 @@ namespace omath::collision
{
std::pmr::vector<VectorType> vertexes{&mem_resource};
vertexes.reserve(simplex.size());
for (std::size_t i = 0; i < simplex.size(); ++i)
vertexes.emplace_back(simplex[i]);
return vertexes;
}
static void erase_marked(std::pmr::vector<Face>& faces, const std::pmr::vector<bool>& to_delete)
{
auto* mr = faces.get_allocator().resource(); // keep same resource
std::pmr::vector<Face> kept{mr};
kept.reserve(faces.size());
for (std::size_t i = 0; i < faces.size(); ++i)
if (!to_delete[i])
kept.emplace_back(faces[i]);
faces.swap(kept);
}
struct Horizon
{
std::pmr::vector<bool> to_delete;
std::pmr::vector<Edge> boundary;
};
static Horizon mark_visible_and_collect_horizon(const std::pmr::vector<Face>& faces, const VectorType& p)
{
auto* mr = faces.get_allocator().resource();
Horizon horizon{std::pmr::vector<bool>(faces.size(), false, mr), std::pmr::vector<Edge>(mr)};
horizon.boundary.reserve(faces.size());
for (std::size_t i = 0; i < faces.size(); ++i)
if (visible_from(faces[i], p))
{
const auto& rf = faces[i];
horizon.to_delete[i] = true;
add_edge_boundary(horizon.boundary, rf.i0, rf.i1);
add_edge_boundary(horizon.boundary, rf.i1, rf.i2);
add_edge_boundary(horizon.boundary, rf.i2, rf.i0);
}
return horizon;
}
};
} // namespace omath::collision

15
include/omath/collision/gjk_algorithm.hpp
View File

@@ -43,7 +43,20 @@ namespace omath::collision
const ColliderInterfaceType& collider_b,
const GjkSettings& settings = {})
{
auto support = find_support_vertex(collider_a, collider_b, VectorType{1, 0, 0});
// Use centroid difference as initial direction — greatly reduces iterations for separated shapes.
VectorType initial_dir;
if constexpr (requires { collider_b.get_origin() - collider_a.get_origin(); })
{
initial_dir = collider_b.get_origin() - collider_a.get_origin();
if (initial_dir.dot(initial_dir) < settings.epsilon * settings.epsilon)
initial_dir = VectorType{1, 0, 0};
}
else
{
initial_dir = VectorType{1, 0, 0};
}
auto support = find_support_vertex(collider_a, collider_b, initial_dir);
Simplex<VectorType> simplex;
simplex.push_front(support);