Add 3D quickhull implementation (ported from older geom)
This commit is contained in:
parent
18fa286896
commit
891c3a0fbd
1 changed files with 255 additions and 0 deletions
255
libs/cg/include/psemek/cg/convex_hull_3d/quickhull.hpp
Normal file
255
libs/cg/include/psemek/cg/convex_hull_3d/quickhull.hpp
Normal file
|
|
@ -0,0 +1,255 @@
|
|||
#pragma once
|
||||
|
||||
#include <psemek/geom/vector.hpp>
|
||||
#include <psemek/geom/point.hpp>
|
||||
#include <psemek/geom/orientation.hpp>
|
||||
#include <psemek/geom/simplex.hpp>
|
||||
#include <psemek/geom/math.hpp>
|
||||
|
||||
#include <vector>
|
||||
#include <list>
|
||||
#include <queue>
|
||||
#include <map>
|
||||
#include <algorithm>
|
||||
|
||||
namespace psemek::cg
|
||||
{
|
||||
|
||||
template <typename T, typename Index = std::size_t>
|
||||
std::vector<geom::triangle<Index>> quickhull (std::vector<geom::point<T, 3ul>> const & points)
|
||||
{
|
||||
struct face;
|
||||
|
||||
using handle = typename std::list<face>::iterator;
|
||||
using edge = geom::segment<Index>;
|
||||
using triangle = geom::triangle<Index>;
|
||||
|
||||
using point = geom::point<T, 3ul>;
|
||||
|
||||
struct face
|
||||
{
|
||||
triangle tri;
|
||||
handle neigh[3];
|
||||
bool valid;
|
||||
std::vector<Index> out;
|
||||
};
|
||||
|
||||
std::list<face> hull;
|
||||
std::list<face> deleted;
|
||||
|
||||
handle const null = hull.end();
|
||||
|
||||
std::queue<handle> 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<Index> 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<Index> const & out_set = h->out;
|
||||
|
||||
if (out_set.empty())
|
||||
continue;
|
||||
|
||||
triangle const t = h->tri;
|
||||
|
||||
float dist = std::numeric_limits<float>::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<handle> visible;
|
||||
|
||||
std::queue<handle> 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<Index> unassigned;
|
||||
std::vector<std::pair<handle, Index>> 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, std::pair<handle, Index>> 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<Index> 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<triangle> result;
|
||||
result.reserve(hull.size());
|
||||
|
||||
for (auto const & f : hull)
|
||||
result.push_back(f.tri);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue