diff --git a/libs/cg/include/psemek/cg/convex_hull_3d/quickhull.hpp b/libs/cg/include/psemek/cg/convex_hull_3d/quickhull.hpp new file mode 100644 index 00000000..12251022 --- /dev/null +++ b/libs/cg/include/psemek/cg/convex_hull_3d/quickhull.hpp @@ -0,0 +1,255 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace psemek::cg +{ + + template + std::vector> quickhull (std::vector> const & points) + { + struct face; + + using handle = typename std::list::iterator; + using edge = geom::segment; + using triangle = geom::triangle; + + using point = geom::point; + + struct face + { + triangle tri; + handle neigh[3]; + bool valid; + std::vector out; + }; + + std::list hull; + std::list deleted; + + handle const null = hull.end(); + + std::queue face_queue; + + { + // lowest 'x' + Index i0 = std::min_element(points.begin(), points.end(), [](point const & p1, point const & p2){ + return p1[0] < p2[0]; + }) - points.begin(); + + // furthest from i0 + Index i1 = std::max_element(points.begin(), points.end(), [&](point const & p1, point const & p2){ + return distance(points[i0], p1) < distance(points[i0], p2); + }) - points.begin(); + + auto n01 = geom::normalized(points[i1] - points[i0]); + auto dist01 = [&](point const & p){ + auto l = p - points[i0]; + return geom::length_sqr(l) - geom::sqr(geom::dot(l, n01)); + }; + + // furthest from i0-i1 line + Index i2 = std::max_element(points.begin(), points.end(), [&](point const & p1, point const & p2){ + return dist01(p1) < dist01(p2); + }) - points.begin(); + + auto dist012 = [&](point const & p){ + return std::abs(geom::volume(p, points[i0], points[i1], points[i2])); + }; + + // highest 'z' + Index i3 = std::max_element(points.begin(), points.end(), [&](point const & p1, point const & p2){ + return dist012(p1) < dist012(p2); + }) - points.begin(); + + if (geom::orientation(points[i0], points[i1], points[i2], points[i3]) == geom::sign_t::negative) + std::swap(i2, i3); + + triangle t0 { i0, i1, i2 }; + triangle t1 { i0, i3, i1 }; + triangle t2 { i0, i2, i3 }; + triangle t3 { i1, i3, i2 }; + + std::vector out0, out1, out2, out3; + + for (Index i = 0; i < points.size(); ++i) + { + if (i == i0) continue; + if (i == i1) continue; + if (i == i2) continue; + if (i == i3) continue; + + if (geom::orientation(points[t0[0]], points[t0[1]], points[t0[2]], points[i]) == geom::sign_t::negative) + out0.push_back(i); + else if (geom::orientation(points[t1[0]], points[t1[1]], points[t1[2]], points[i]) == geom::sign_t::negative) + out1.push_back(i); + else if (geom::orientation(points[t2[0]], points[t2[1]], points[t2[2]], points[i]) == geom::sign_t::negative) + out2.push_back(i); + else if (geom::orientation(points[t3[0]], points[t3[1]], points[t3[2]], points[i]) == geom::sign_t::negative) + out3.push_back(i); + } + + auto it0 = hull.insert(hull.end(), {t0, {}, true, std::move(out0)}); + auto it1 = hull.insert(hull.end(), {t1, {}, true, std::move(out1)}); + auto it2 = hull.insert(hull.end(), {t2, {}, true, std::move(out2)}); + auto it3 = hull.insert(hull.end(), {t3, {}, true, std::move(out3)}); + + it0->neigh[0] = it1; + it0->neigh[1] = it3; + it0->neigh[2] = it2; + + it1->neigh[0] = it2; + it1->neigh[1] = it3; + it1->neigh[2] = it0; + + it2->neigh[0] = it0; + it2->neigh[1] = it3; + it2->neigh[2] = it1; + + it3->neigh[0] = it1; + it3->neigh[1] = it2; + it3->neigh[2] = it0; + + face_queue.push(it0); + face_queue.push(it1); + face_queue.push(it2); + face_queue.push(it3); + } + + while (!face_queue.empty()) + { + handle h = face_queue.front(); + face_queue.pop(); + + if (!h->valid) + continue; + + std::vector const & out_set = h->out; + + if (out_set.empty()) + continue; + + triangle const t = h->tri; + + float dist = std::numeric_limits::infinity(); + Index max_i = out_set[0]; + + for (auto i : out_set) + { + float const d = geom::volume(points[t[0]], points[t[1]], points[t[2]], points[i]); + + if (d < dist) + { + dist = d; + max_i = i; + } + } + + std::vector visible; + + std::queue visible_queue; + visible_queue.push(h); + h->valid = false; + + while (!visible_queue.empty()) + { + auto h = visible_queue.front(); + visible_queue.pop(); + visible.push_back(h); + + for (auto i : {0, 1, 2}) + { + handle it = h->neigh[i]; + triangle const & t = it->tri; + if (it->valid && (geom::orientation(points[t[0]], points[t[1]], points[t[2]], points[max_i]) == geom::sign_t::negative)) + { + it->valid = false; + visible_queue.push(it); + } + } + } + + std::vector unassigned; + std::vector> boundary; + + for (auto h : visible) + { + auto & out = h->out; + unassigned.insert(unassigned.end(), out.begin(), out.end()); + + for (auto i : {0, 1, 2}) + { + handle n = h->neigh[i]; + if (n->valid) + { +// bool found = false; + + for (auto j : {0, 1, 2}) + { + if (n->neigh[j] == h) + { + boundary.push_back({n, j}); +// found = true; + break; + } + } + } + } + + deleted.splice(deleted.end(), hull, h); + } + + auto unassigned_end = std::prev(unassigned.end()); + + std::iter_swap(std::find(unassigned.begin(), unassigned.end(), max_i), unassigned_end); + + std::map> edge_map; + + for (auto p : boundary) + { + handle h = p.first; + std::size_t i = p.second; + triangle const t { h->tri[(i+1)%3], h->tri[i], max_i }; + + auto it = std::partition(unassigned.begin(), unassigned_end, [&](Index i){ + return geom::orientation(points[t[0]], points[t[1]], points[t[2]], points[i]) == geom::sign_t::positive; + }); + + std::vector out_set(it, unassigned_end); + unassigned_end = it; + + hull.push_back({ t, { h, null, null }, true, std::move(out_set) }); + handle n = std::prev(hull.end()); + h->neigh[i] = n; + face_queue.push(n); + + edge_map[{t[1], t[2]}] = {n, 1}; + edge_map[{t[2], t[0]}] = {n, 2}; + } + + for (auto const & p : edge_map) + { + p.second.first->neigh[p.second.second] = edge_map.at({p.first[1], p.first[0]}).first; + } + } + + std::vector result; + result.reserve(hull.size()); + + for (auto const & f : hull) + result.push_back(f.tri); + + return result; + } + +}