lisyarus/psemek
1
0
Fork 1
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add 3D quickhull implementation (ported from older geom)

Browse source
This commit is contained in:
Nikita Lisitsa 2021-01-23 21:34:21 +03:00
parent 18fa286896
commit 891c3a0fbd
1 changed files with 255 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

255
libs/cg/include/psemek/cg/convex_hull_3d/quickhull.hpp Normal file
View 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;
}
}