adrien/netradiant-custom
1
Fork 0
mirror of https://github.com/Garux/netradiant-custom.git synced 2026-09-28 00:00:03 +02:00
Code Issues Packages Projects Releases Wiki Activity
Files
d9e1e02115823f4e97def8d0f84122a53acde602
netradiant-custom/libs/quickhull/QuickHull.cpp
T

515 lines
19 KiB
C++
Raw Blame History

#include "QuickHull.hpp"
#include "MathUtils.hpp"
#include <cmath>
#include <cassert>
#include <iostream>
#include <algorithm>
#include <limits>
#include "Structs/Mesh.hpp"
namespace quickhull {
template<>
float defaultEps() {
return 0.0001f;
}
template<>
double defaultEps() {
return 0.0000001;
}
/*
* Implementation of the algorithm
*/
template<typename T>
ConvexHull<T> QuickHull<T>::getConvexHull(const std::vector<Vector3<T>>& pointCloud, bool CCW, bool useOriginalIndices, T epsilon) {
VertexDataSource<T> vertexDataSource(pointCloud);
return getConvexHull(vertexDataSource,CCW,useOriginalIndices,epsilon);
}
template<typename T>
ConvexHull<T> QuickHull<T>::getConvexHull(const Vector3<T>* vertexData, size_t vertexCount, bool CCW, bool useOriginalIndices, T epsilon) {
VertexDataSource<T> vertexDataSource(vertexData,vertexCount);
return getConvexHull(vertexDataSource,CCW,useOriginalIndices,epsilon);
}
template<typename T>
ConvexHull<T> QuickHull<T>::getConvexHull(const T* vertexData, size_t vertexCount, bool CCW, bool useOriginalIndices, T epsilon) {
VertexDataSource<T> vertexDataSource((const vec3*)vertexData,vertexCount);
return getConvexHull(vertexDataSource,CCW,useOriginalIndices,epsilon);
}
template<typename FloatType>
HalfEdgeMesh<FloatType, size_t> QuickHull<FloatType>::getConvexHullAsMesh(const FloatType* vertexData, size_t vertexCount, bool CCW, FloatType epsilon) {
VertexDataSource<FloatType> vertexDataSource((const vec3*)vertexData,vertexCount);
buildMesh(vertexDataSource, epsilon);
return HalfEdgeMesh<FloatType, size_t>(m_mesh, m_vertexData);
}
template<typename T>
void QuickHull<T>::buildMesh(const VertexDataSource<T>& pointCloud, T epsilon)
{
if (pointCloud.size()==0) {
m_mesh = MeshBuilder<T>();
return;
}
m_vertexData = pointCloud;
// Very first: find extreme values and use them to compute the scale of the point cloud.
m_extremeValues = getExtremeValues();
m_scale = getScale(m_extremeValues);
// Epsilon we use depends on the scale
m_epsilon = epsilon*m_scale;
m_epsilonSquared = m_epsilon*m_epsilon;
// Reset diagnostics
m_diagnostics = DiagnosticsData();
// The planar case happens when all the points appear to lie on a two dimensional
// subspace of R^3.
m_planar = false;
createConvexHalfEdgeMesh();
if (m_planar) {
const size_t extraPointIndex = m_planarPointCloudTemp.size() - 1;
for (auto& he : m_mesh.m_halfEdges) {
if (he.m_endVertex == extraPointIndex) {
he.m_endVertex = 0;
}
}
m_vertexData = pointCloud;
m_planarPointCloudTemp.clear();
}
}
template<typename T>
ConvexHull<T> QuickHull<T>::getConvexHull(const VertexDataSource<T>& pointCloud, bool CCW, bool useOriginalIndices, T epsilon) {
buildMesh(pointCloud, epsilon);
return ConvexHull<T>(m_mesh,m_vertexData, CCW, useOriginalIndices);
}
template<typename T>
void QuickHull<T>::createConvexHalfEdgeMesh() {
m_visibleFaces.clear();
m_horizonEdges.clear();
m_possiblyVisibleFaces.clear();
// Compute base tetrahedron
setupInitialTetrahedron();
assert(m_mesh.m_faces.size() == 4);
// Init face stack with those faces that have points assigned to them
m_faceList.clear();
for (size_t i=0;i < 4;i++) {
auto& f = m_mesh.m_faces[i];
if (f.m_pointsOnPositiveSide && f.m_pointsOnPositiveSide->size()>0) {
m_faceList.push_back(i);
f.m_inFaceStack = 1;
}
}
// Process faces until the face list is empty.
size_t iter = 0;
while (!m_faceList.empty()) {
iter++;
if (iter == std::numeric_limits<size_t>::max()) {
// Visible face traversal marks visited faces with iteration counter
// (to mark that the face has been visited on this iteration) and the max
// value represents unvisited faces. At this point we have to reset iteration
// counter. This shouldn't be an issue on 64 bit machines.
iter = 0;
}
const size_t topFaceIndex = m_faceList.front();
m_faceList.pop_front();
auto& tf = m_mesh.m_faces[topFaceIndex];
tf.m_inFaceStack = 0;
assert(!tf.m_pointsOnPositiveSide || tf.m_pointsOnPositiveSide->size()>0);
if (!tf.m_pointsOnPositiveSide || tf.isDisabled()) {
continue;
}
// Pick the most distant point to this triangle plane as the point to which we extrude
const vec3& activePoint = m_vertexData[tf.m_mostDistantPoint];
const size_t activePointIndex = tf.m_mostDistantPoint;
// Find out the faces that have our active point on their positive side
// (these are the "visible faces"). The face on top of the stack of course is
// one of them. At the same time, we create a list of horizon edges.
m_horizonEdges.clear();
m_possiblyVisibleFaces.clear();
m_visibleFaces.clear();
m_possiblyVisibleFaces.emplace_back(topFaceIndex,std::numeric_limits<size_t>::max());
while (m_possiblyVisibleFaces.size()) {
const auto faceData = m_possiblyVisibleFaces.back();
m_possiblyVisibleFaces.pop_back();
auto& pvf = m_mesh.m_faces[faceData.m_faceIndex];
assert(!pvf.isDisabled());
if (pvf.m_visibilityCheckedOnIteration == iter) {
if (pvf.m_isVisibleFaceOnCurrentIteration) {
continue;
}
}
else {
const Plane<T>& P = pvf.m_P;
pvf.m_visibilityCheckedOnIteration = iter;
const T d = P.m_N.dotProduct(activePoint)+P.m_D;
if (d>0) {
pvf.m_isVisibleFaceOnCurrentIteration = 1;
pvf.m_horizonEdgesOnCurrentIteration = 0;
m_visibleFaces.push_back(faceData.m_faceIndex);
for (auto heIndex : m_mesh.getHalfEdgeIndicesOfFace(pvf)) {
if (m_mesh.m_halfEdges[heIndex].m_opp != faceData.m_enteredFromHalfEdge) {
m_possiblyVisibleFaces.emplace_back( m_mesh.m_halfEdges[m_mesh.m_halfEdges[heIndex].m_opp].m_face,heIndex );
}
}
continue;
}
assert(faceData.m_faceIndex != topFaceIndex);
}
// The face is not visible. Therefore, the halfedge we entered from
// is part of the horizon edge.
pvf.m_isVisibleFaceOnCurrentIteration = 0;
m_horizonEdges.push_back(faceData.m_enteredFromHalfEdge);
// Store which half edge is the horizon edge. The other half edges of the face
// will not be part of the final mesh so their data slots can by recycled.
const auto halfEdges = m_mesh.getHalfEdgeIndicesOfFace(m_mesh.m_faces[m_mesh.m_halfEdges[faceData.m_enteredFromHalfEdge].m_face]);
const std::int8_t ind = (halfEdges[0]==faceData.m_enteredFromHalfEdge) ? 0 : (halfEdges[1]==faceData.m_enteredFromHalfEdge ? 1 : 2);
m_mesh.m_faces[m_mesh.m_halfEdges[faceData.m_enteredFromHalfEdge].m_face].m_horizonEdgesOnCurrentIteration |= (1<<ind);
}
const size_t horizonEdgeCount = m_horizonEdges.size();
// Order horizon edges so that they form a loop. This may fail due to numerical
// inaccuracy in which case we give up trying to solve horizon edge for this point
// and accept a minor degeneration in the convex hull.
if (!reorderHorizonEdges(m_horizonEdges)) {
m_diagnostics.m_failedHorizonEdges++;
std::cerr << "Failed to solve horizon edge." << std::endl;
auto it = std::find(tf.m_pointsOnPositiveSide->begin(),
tf.m_pointsOnPositiveSide->end(),
activePointIndex);
tf.m_pointsOnPositiveSide->erase(it);
if (tf.m_pointsOnPositiveSide->size()==0) {
reclaimToIndexVectorPool(tf.m_pointsOnPositiveSide);
}
continue;
}
// Except for the horizon edges, all half edges of the visible faces can be marked as disabled. Their data slots will be reused.
// The faces will be disabled as well, but we need to remember the points that were on the positive side of them - therefore
// we save pointers to them.
m_newFaceIndices.clear();
m_newHalfEdgeIndices.clear();
m_disabledFacePointVectors.clear();
size_t disableCounter = 0;
for (auto faceIndex : m_visibleFaces) {
auto& disabledFace = m_mesh.m_faces[faceIndex];
auto halfEdges = m_mesh.getHalfEdgeIndicesOfFace(disabledFace);
for (size_t j=0;j<3;j++) {
if ((disabledFace.m_horizonEdgesOnCurrentIteration & (1<<j)) == 0) {
if (disableCounter < horizonEdgeCount*2) {
// Use on this iteration
m_newHalfEdgeIndices.push_back(halfEdges[j]);
disableCounter++;
}
else {
// Mark for reusal on later iteration step
m_mesh.disableHalfEdge(halfEdges[j]);
}
}
}
// Disable the face, but retain pointer to the points that were on the positive side of it. We need to assign those points
// to the new faces we create shortly.
auto t = m_mesh.disableFace(faceIndex);
if (t) {
assert(t->size()); // Because we should not assign point vectors to faces unless needed...
m_disabledFacePointVectors.push_back(std::move(t));
}
}
if (disableCounter < horizonEdgeCount*2) {
const size_t newHalfEdgesNeeded = horizonEdgeCount*2-disableCounter;
for (size_t i=0;i<newHalfEdgesNeeded;i++) {
m_newHalfEdgeIndices.push_back(m_mesh.addHalfEdge());
}
}
// Create new faces using the edgeloop
for (size_t i = 0; i < horizonEdgeCount; i++) {
const size_t AB = m_horizonEdges[i];
auto horizonEdgeVertexIndices = m_mesh.getVertexIndicesOfHalfEdge(m_mesh.m_halfEdges[AB]);
size_t A,B,C;
A = horizonEdgeVertexIndices[0];
B = horizonEdgeVertexIndices[1];
C = activePointIndex;
const size_t newFaceIndex = m_mesh.addFace();
m_newFaceIndices.push_back(newFaceIndex);
const size_t CA = m_newHalfEdgeIndices[2*i+0];
const size_t BC = m_newHalfEdgeIndices[2*i+1];
m_mesh.m_halfEdges[AB].m_next = BC;
m_mesh.m_halfEdges[BC].m_next = CA;
m_mesh.m_halfEdges[CA].m_next = AB;
m_mesh.m_halfEdges[BC].m_face = newFaceIndex;
m_mesh.m_halfEdges[CA].m_face = newFaceIndex;
m_mesh.m_halfEdges[AB].m_face = newFaceIndex;
m_mesh.m_halfEdges[CA].m_endVertex = A;
m_mesh.m_halfEdges[BC].m_endVertex = C;
auto& newFace = m_mesh.m_faces[newFaceIndex];
const Vector3<T> planeNormal = mathutils::getTriangleNormal(m_vertexData[A],m_vertexData[B],activePoint);
newFace.m_P = Plane<T>(planeNormal,activePoint);
newFace.m_he = AB;
m_mesh.m_halfEdges[CA].m_opp = m_newHalfEdgeIndices[i>0 ? i*2-1 : 2*horizonEdgeCount-1];
m_mesh.m_halfEdges[BC].m_opp = m_newHalfEdgeIndices[((i+1)*2) % (horizonEdgeCount*2)];
}
// Assign points that were on the positive side of the disabled faces to the new faces.
for (auto& disabledPoints : m_disabledFacePointVectors) {
assert(disabledPoints);
for (const auto& point : *(disabledPoints)) {
if (point == activePointIndex) {
continue;
}
for (size_t j=0;j<horizonEdgeCount;j++) {
if (addPointToFace(m_mesh.m_faces[m_newFaceIndices[j]], point)) {
break;
}
}
}
// The points are no longer needed: we can move them to the vector pool for reuse.
reclaimToIndexVectorPool(disabledPoints);
}
// Increase face stack size if needed
for (const auto newFaceIndex : m_newFaceIndices) {
auto& newFace = m_mesh.m_faces[newFaceIndex];
if (newFace.m_pointsOnPositiveSide) {
assert(newFace.m_pointsOnPositiveSide->size()>0);
if (!newFace.m_inFaceStack) {
m_faceList.push_back(newFaceIndex);
newFace.m_inFaceStack = 1;
}
}
}
}
// Cleanup
m_indexVectorPool.clear();
}
/*
* Private helper functions
*/
template <typename T>
std::array<size_t, 6> QuickHull<T>::getExtremeValues() {
std::array<size_t,6> outIndices{0, 0, 0, 0, 0, 0};
T extremeVals[6] = { m_vertexData[0].x, m_vertexData[0].x, m_vertexData[0].y,
m_vertexData[0].y, m_vertexData[0].z, m_vertexData[0].z };
const size_t vCount = m_vertexData.size();
for (size_t i = 1; i < vCount; i++) {
const Vector3<T>& pos = m_vertexData[i];
if (pos.x>extremeVals[0]) {
extremeVals[0]=pos.x;
outIndices[0]=i;
}
else if (pos.x<extremeVals[1]) {
extremeVals[1]=pos.x;
outIndices[1]=i;
}
if (pos.y>extremeVals[2]) {
extremeVals[2]=pos.y;
outIndices[2]=i;
}
else if (pos.y<extremeVals[3]) {
extremeVals[3]=pos.y;
outIndices[3]=i;
}
if (pos.z>extremeVals[4]) {
extremeVals[4]=pos.z;
outIndices[4]=i;
}
else if (pos.z<extremeVals[5]) {
extremeVals[5]=pos.z;
outIndices[5]=i;
}
}
return outIndices;
}
template<typename T>
bool QuickHull<T>::reorderHorizonEdges(std::vector<size_t>& horizonEdges) {
const size_t horizonEdgeCount = horizonEdges.size();
for (size_t i=0;i<horizonEdgeCount-1;i++) {
const size_t endVertex = m_mesh.m_halfEdges[ horizonEdges[i] ].m_endVertex;
bool foundNext = false;
for (size_t j=i+1;j<horizonEdgeCount;j++) {
const size_t beginVertex = m_mesh.m_halfEdges[ m_mesh.m_halfEdges[horizonEdges[j]].m_opp ].m_endVertex;
if (beginVertex == endVertex) {
std::swap(horizonEdges[i+1],horizonEdges[j]);
foundNext = true;
break;
}
}
if (!foundNext) {
return false;
}
}
assert(m_mesh.m_halfEdges[ horizonEdges[horizonEdges.size()-1] ].m_endVertex == m_mesh.m_halfEdges[ m_mesh.m_halfEdges[horizonEdges[0]].m_opp ].m_endVertex);
return true;
}
template <typename T>
T QuickHull<T>::getScale(const std::array<size_t,6>& extremeValues) {
T s = 0;
for (size_t i=0;i<6;i++) {
const T* v = (const T*)(&m_vertexData[extremeValues[i]]);
v += i/2;
auto a = std::abs(*v);
if (a>s) {
s = a;
}
}
return s;
}
template<typename T>
void QuickHull<T>::setupInitialTetrahedron() {
const size_t vertexCount = m_vertexData.size();
// If we have at most 4 points, just return a degenerate tetrahedron:
if (vertexCount <= 4) {
size_t v[4] = {0,std::min((size_t)1,vertexCount-1),std::min((size_t)2,vertexCount-1),std::min((size_t)3,vertexCount-1)};
const Vector3<T> N = mathutils::getTriangleNormal(m_vertexData[v[0]],m_vertexData[v[1]],m_vertexData[v[2]]);
const Plane<T> trianglePlane(N,m_vertexData[v[0]]);
if (trianglePlane.isPointOnPositiveSide(m_vertexData[v[3]])) {
std::swap(v[0],v[1]);
}
return m_mesh.setup(v[0],v[1],v[2],v[3]);
}
// Find two most distant extreme points.
T maxD = m_epsilonSquared;
std::pair<size_t,size_t> selectedPoints;
for (size_t i=0;i<6;i++) {
for (size_t j=i+1;j<6;j++) {
const T d = m_vertexData[ m_extremeValues[i] ].getSquaredDistanceTo( m_vertexData[ m_extremeValues[j] ] );
if (d > maxD) {
maxD=d;
selectedPoints={m_extremeValues[i],m_extremeValues[j]};
}
}
}
if (maxD == m_epsilonSquared) {
// A degenerate case: the point cloud seems to consists of a single point
return m_mesh.setup(0,std::min((size_t)1,vertexCount-1),std::min((size_t)2,vertexCount-1),std::min((size_t)3,vertexCount-1));
}
assert(selectedPoints.first != selectedPoints.second);
// Find the most distant point to the line between the two chosen extreme points.
const Ray<T> r(m_vertexData[selectedPoints.first], (m_vertexData[selectedPoints.second] - m_vertexData[selectedPoints.first]));
maxD = m_epsilonSquared;
size_t maxI=std::numeric_limits<size_t>::max();
const size_t vCount = m_vertexData.size();
for (size_t i=0;i<vCount;i++) {
const T distToRay = mathutils::getSquaredDistanceBetweenPointAndRay(m_vertexData[i],r);
if (distToRay > maxD) {
maxD=distToRay;
maxI=i;
}
}
if (maxD == m_epsilonSquared) {
// It appears that the point cloud belongs to a 1 dimensional subspace of R^3: convex hull has no volume => return a thin triangle
// Pick any point other than selectedPoints.first and selectedPoints.second as the third point of the triangle
auto it = std::find_if(m_vertexData.begin(),m_vertexData.end(),[&](const vec3& ve) {
return ve != m_vertexData[selectedPoints.first] && ve != m_vertexData[selectedPoints.second];
});
const size_t thirdPoint = (it == m_vertexData.end()) ? selectedPoints.first : std::distance(m_vertexData.begin(),it);
it = std::find_if(m_vertexData.begin(),m_vertexData.end(),[&](const vec3& ve) {
return ve != m_vertexData[selectedPoints.first] && ve != m_vertexData[selectedPoints.second] && ve != m_vertexData[thirdPoint];
});
const size_t fourthPoint = (it == m_vertexData.end()) ? selectedPoints.first : std::distance(m_vertexData.begin(),it);
return m_mesh.setup(selectedPoints.first,selectedPoints.second,thirdPoint,fourthPoint);
}
// These three points form the base triangle for our tetrahedron.
assert(selectedPoints.first != maxI && selectedPoints.second != maxI);
std::array<size_t,3> baseTriangle{selectedPoints.first, selectedPoints.second, maxI};
const Vector3<T> baseTriangleVertices[]={ m_vertexData[baseTriangle[0]], m_vertexData[baseTriangle[1]], m_vertexData[baseTriangle[2]] };
// Next step is to find the 4th vertex of the tetrahedron. We naturally choose the point farthest away from the triangle plane.
maxD=m_epsilon;
maxI=0;
const Vector3<T> N = mathutils::getTriangleNormal(baseTriangleVertices[0],baseTriangleVertices[1],baseTriangleVertices[2]);
Plane<T> trianglePlane(N,baseTriangleVertices[0]);
for (size_t i=0;i<vCount;i++) {
const T d = std::abs(mathutils::getSignedDistanceToPlane(m_vertexData[i],trianglePlane));
if (d>maxD) {
maxD=d;
maxI=i;
}
}
if (maxD == m_epsilon) {
// All the points seem to lie on a 2D subspace of R^3. How to handle this? Well, let's add one extra point to the point cloud so that the convex hull will have volume.
m_planar = true;
const vec3 N1 = mathutils::getTriangleNormal(baseTriangleVertices[1],baseTriangleVertices[2],baseTriangleVertices[0]);
m_planarPointCloudTemp.clear();
m_planarPointCloudTemp.insert(m_planarPointCloudTemp.begin(),m_vertexData.begin(),m_vertexData.end());
const vec3 extraPoint = N1 + m_vertexData[0];
m_planarPointCloudTemp.push_back(extraPoint);
maxI = m_planarPointCloudTemp.size()-1;
m_vertexData = VertexDataSource<T>(m_planarPointCloudTemp);
}
// Enforce CCW orientation (if user prefers clockwise orientation, swap two vertices in each triangle when final mesh is created)
const Plane<T> triPlane(N,baseTriangleVertices[0]);
if (triPlane.isPointOnPositiveSide(m_vertexData[maxI])) {
std::swap(baseTriangle[0],baseTriangle[1]);
}
// Create a tetrahedron half edge mesh and compute planes defined by each triangle
m_mesh.setup(baseTriangle[0],baseTriangle[1],baseTriangle[2],maxI);
for (auto& f : m_mesh.m_faces) {
auto v = m_mesh.getVertexIndicesOfFace(f);
const Vector3<T>& va = m_vertexData[v[0]];
const Vector3<T>& vb = m_vertexData[v[1]];
const Vector3<T>& vc = m_vertexData[v[2]];
const Vector3<T> N1 = mathutils::getTriangleNormal(va, vb, vc);
const Plane<T> plane(N1,va);
f.m_P = plane;
}
// Finally we assign a face for each vertex outside the tetrahedron (vertices inside the tetrahedron have no role anymore)
for (size_t i=0;i<vCount;i++) {
for (auto& face : m_mesh.m_faces) {
if (addPointToFace(face, i)) {
break;
}
}
}
}
/*
* Explicit template specifications for float and double
*/
template class QuickHull<float>;
template class QuickHull<double>;
}
Reference in New Issue View Git Blame Copy Permalink