diff --git a/Makefile b/Makefile index db29693f..9b0de036 100644 --- a/Makefile +++ b/Makefile @@ -114,7 +114,7 @@ CFLAGS_COMMON = -MMD -W -Wall -Wcast-align -Wcast-qual -Wno-unused-parameter -fn CPPFLAGS_COMMON = LDFLAGS_COMMON = LIBS_COMMON = -CXXFLAGS_COMMON = -Wno-non-virtual-dtor -Wreorder -fno-exceptions -fno-rtti +CXXFLAGS_COMMON = -std=c++11 -Wno-non-virtual-dtor -Wreorder -fno-exceptions -fno-rtti ifeq ($(BUILD),debug) ifeq ($(findstring $(CFLAGS),-g),) @@ -743,6 +743,7 @@ $(INSTALLDIR)/radiant.$(EXE): \ libl_net.$(A) \ libmathlib.$(A) \ libprofile.$(A) \ + libquickhull.$(A) \ libxmllib.$(A) \ $(if $(findstring $(OS),Win32),icons/radiant.o,) \ @@ -793,6 +794,10 @@ libxmllib.$(A): \ libs/xml/xmltextags.o \ libs/xml/xmlwriter.o \ +libquickhull.$(A): CPPFLAGS_EXTRA := -Ilibs +libquickhull.$(A): \ + libs/quickhull/QuickHull.o \ + $(INSTALLDIR)/modules/archivezip.$(DLL): LIBS_EXTRA := $(LIBS_ZLIB) $(INSTALLDIR)/modules/archivezip.$(DLL): CPPFLAGS_EXTRA := $(CPPFLAGS_ZLIB) -Ilibs -Iinclude $(INSTALLDIR)/modules/archivezip.$(DLL): \ diff --git a/libs/quickhull/ConvexHull.hpp b/libs/quickhull/ConvexHull.hpp new file mode 100644 index 00000000..1abc22f4 --- /dev/null +++ b/libs/quickhull/ConvexHull.hpp @@ -0,0 +1,179 @@ +#ifndef CONVEXHULL_HPP_ +#define CONVEXHULL_HPP_ + +#include "Structs/Vector3.hpp" +#include "Structs/Mesh.hpp" +#include "Structs/VertexDataSource.hpp" +#include +#include +#include +#include + +namespace quickhull { + + template + class ConvexHull { + std::unique_ptr>> m_optimizedVertexBuffer; + VertexDataSource m_vertices; + std::vector m_indices; + public: + ConvexHull() {} + + // Copy constructor + ConvexHull(const ConvexHull& o) { + m_indices = o.m_indices; + if (o.m_optimizedVertexBuffer) { + m_optimizedVertexBuffer.reset(new std::vector>(*o.m_optimizedVertexBuffer)); + m_vertices = VertexDataSource(*m_optimizedVertexBuffer); + } + else { + m_vertices = o.m_vertices; + } + } + + ConvexHull& operator=(const ConvexHull& o) { + if (&o == this) { + return *this; + } + m_indices = o.m_indices; + if (o.m_optimizedVertexBuffer) { + m_optimizedVertexBuffer.reset(new std::vector>(*o.m_optimizedVertexBuffer)); + m_vertices = VertexDataSource(*m_optimizedVertexBuffer); + } + else { + m_vertices = o.m_vertices; + } + return *this; + } + + ConvexHull(ConvexHull&& o) { + m_indices = std::move(o.m_indices); + if (o.m_optimizedVertexBuffer) { + m_optimizedVertexBuffer = std::move(o.m_optimizedVertexBuffer); + o.m_vertices = VertexDataSource(); + m_vertices = VertexDataSource(*m_optimizedVertexBuffer); + } + else { + m_vertices = o.m_vertices; + } + } + + ConvexHull& operator=(ConvexHull&& o) { + if (&o == this) { + return *this; + } + m_indices = std::move(o.m_indices); + if (o.m_optimizedVertexBuffer) { + m_optimizedVertexBuffer = std::move(o.m_optimizedVertexBuffer); + o.m_vertices = VertexDataSource(); + m_vertices = VertexDataSource(*m_optimizedVertexBuffer); + } + else { + m_vertices = o.m_vertices; + } + return *this; + } + + // Construct vertex and index buffers from half edge mesh and pointcloud + ConvexHull(const MeshBuilder& mesh, const VertexDataSource& pointCloud, bool CCW, bool useOriginalIndices) { + if (!useOriginalIndices) { + m_optimizedVertexBuffer.reset(new std::vector>()); + } + + std::vector faceProcessed(mesh.m_faces.size(),false); + std::vector faceStack; + std::unordered_map vertexIndexMapping; // Map vertex indices from original point cloud to the new mesh vertex indices + for (size_t i = 0;ipush_back(pointCloud[v]); + vertexIndexMapping[v] = m_optimizedVertexBuffer->size()-1; + v = m_optimizedVertexBuffer->size()-1; + } + else { + v = it->second; + } + } + } + m_indices.push_back(vertices[0]); + if (CCW) { + m_indices.push_back(vertices[2]); + m_indices.push_back(vertices[1]); + } + else { + m_indices.push_back(vertices[1]); + m_indices.push_back(vertices[2]); + } + } + } + + if (!useOriginalIndices) { + m_vertices = VertexDataSource(*m_optimizedVertexBuffer); + } + else { + m_vertices = pointCloud; + } + } + + std::vector& getIndexBuffer() { + return m_indices; + } + + VertexDataSource& getVertexBuffer() { + return m_vertices; + } + + // Export the mesh to a Waveform OBJ file + void writeWaveformOBJ(const std::string& filename, const std::string& objectName = "quickhull") + { + std::ofstream objFile; + objFile.open (filename); + objFile << "o " << objectName << "\n"; + for (const auto& v : getVertexBuffer()) { + objFile << "v " << v.x << " " << v.y << " " << v.z << "\n"; + } + const auto& indBuf = getIndexBuffer(); + size_t triangleCount = indBuf.size()/3; + for (size_t i=0;i + class HalfEdgeMesh { + public: + + struct HalfEdge { + IndexType m_endVertex; + IndexType m_opp; + IndexType m_face; + IndexType m_next; + }; + + struct Face { + IndexType m_halfEdgeIndex; // Index of one of the half edges of this face + }; + + std::vector> m_vertices; + std::vector m_faces; + std::vector m_halfEdges; + + HalfEdgeMesh(const MeshBuilder& builderObject, const VertexDataSource& vertexData ) + { + std::unordered_map faceMapping; + std::unordered_map halfEdgeMapping; + std::unordered_map vertexMapping; + + size_t i=0; + for (const auto& face : builderObject.m_faces) { + if (!face.isDisabled()) { + m_faces.push_back({static_cast(face.m_he)}); + faceMapping[i] = m_faces.size()-1; + + const auto heIndices = builderObject.getHalfEdgeIndicesOfFace(face); + for (const auto heIndex : heIndices) { + const IndexType vertexIndex = builderObject.m_halfEdges[heIndex].m_endVertex; + if (vertexMapping.count(vertexIndex)==0) { + m_vertices.push_back(vertexData[vertexIndex]); + vertexMapping[vertexIndex] = m_vertices.size()-1; + } + } + } + i++; + } + + i=0; + for (const auto& halfEdge : builderObject.m_halfEdges) { + if (!halfEdge.isDisabled()) { + m_halfEdges.push_back({static_cast(halfEdge.m_endVertex),static_cast(halfEdge.m_opp),static_cast(halfEdge.m_face),static_cast(halfEdge.m_next)}); + halfEdgeMapping[i] = m_halfEdges.size()-1; + } + i++; + } + + for (auto& face : m_faces) { + assert(halfEdgeMapping.count(face.m_halfEdgeIndex) == 1); + face.m_halfEdgeIndex = halfEdgeMapping[face.m_halfEdgeIndex]; + } + + for (auto& he : m_halfEdges) { + he.m_face = faceMapping[he.m_face]; + he.m_opp = halfEdgeMapping[he.m_opp]; + he.m_next = halfEdgeMapping[he.m_next]; + he.m_endVertex = vertexMapping[he.m_endVertex]; + } + } + + }; +} + + +#endif /* HalfEdgeMesh_h */ diff --git a/libs/quickhull/MathUtils.hpp b/libs/quickhull/MathUtils.hpp new file mode 100644 index 00000000..b87b3b93 --- /dev/null +++ b/libs/quickhull/MathUtils.hpp @@ -0,0 +1,46 @@ + +#ifndef QuickHull_MathUtils_hpp +#define QuickHull_MathUtils_hpp + +#include "Structs/Vector3.hpp" +#include "Structs/Ray.hpp" + +namespace quickhull { + + namespace mathutils { + + template + inline T getSquaredDistanceBetweenPointAndRay(const Vector3& p, const Ray& r) { + const Vector3 s = p-r.m_S; + T t = s.dotProduct(r.m_V); + return s.getLengthSquared() - t*t*r.m_VInvLengthSquared; + } + + // Note that the unit of distance returned is relative to plane's normal's length (divide by N.getNormalized() if needed to get the "real" distance). + template + inline T getSignedDistanceToPlane(const Vector3& v, const Plane& p) { + return p.m_N.dotProduct(v) + p.m_D; + } + + template + inline Vector3 getTriangleNormal(const Vector3& a,const Vector3& b,const Vector3& c) { + // We want to get (a-c).crossProduct(b-c) without constructing temp vectors + T x = a.x - c.x; + T y = a.y - c.y; + T z = a.z - c.z; + T rhsx = b.x - c.x; + T rhsy = b.y - c.y; + T rhsz = b.z - c.z; + T px = y * rhsz - z * rhsy ; + T py = z * rhsx - x * rhsz ; + T pz = x * rhsy - y * rhsx ; + return Vector3(px,py,pz); + } + + + } + +} + + +#endif diff --git a/libs/quickhull/QuickHull.cpp b/libs/quickhull/QuickHull.cpp new file mode 100644 index 00000000..b8b16f04 --- /dev/null +++ b/libs/quickhull/QuickHull.cpp @@ -0,0 +1,502 @@ +#include "QuickHull.hpp" +#include "MathUtils.hpp" +#include +#include +#include +#include +#include +#include +#include "Structs/Mesh.hpp" + +namespace quickhull { + + template<> + const float QuickHull::Epsilon = 0.0001f; + + template<> + const double QuickHull::Epsilon = 0.0000001; + + /* + * Implementation of the algorithm + */ + + template + ConvexHull QuickHull::getConvexHull(const std::vector>& pointCloud, bool CCW, bool useOriginalIndices, T epsilon) { + VertexDataSource vertexDataSource(pointCloud); + return getConvexHull(vertexDataSource,CCW,useOriginalIndices,epsilon); + } + + template + ConvexHull QuickHull::getConvexHull(const Vector3* vertexData, size_t vertexCount, bool CCW, bool useOriginalIndices, T epsilon) { + VertexDataSource vertexDataSource(vertexData,vertexCount); + return getConvexHull(vertexDataSource,CCW,useOriginalIndices,epsilon); + } + + template + ConvexHull QuickHull::getConvexHull(const T* vertexData, size_t vertexCount, bool CCW, bool useOriginalIndices, T epsilon) { + VertexDataSource vertexDataSource((const vec3*)vertexData,vertexCount); + return getConvexHull(vertexDataSource,CCW,useOriginalIndices,epsilon); + } + + template + HalfEdgeMesh QuickHull::getConvexHullAsMesh(const FloatType* vertexData, size_t vertexCount, bool CCW, FloatType epsilon) { + VertexDataSource vertexDataSource((const vec3*)vertexData,vertexCount); + buildMesh(vertexDataSource, CCW, false, epsilon); + return HalfEdgeMesh(m_mesh, m_vertexData); + } + + template + void QuickHull::buildMesh(const VertexDataSource& pointCloud, bool CCW, bool useOriginalIndices, T epsilon) { + if (pointCloud.size()==0) { + m_mesh = MeshBuilder(); + 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(); + + m_planar = false; // The planar case happens when all the points appear to lie on a two dimensional subspace of R^3. + 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 + ConvexHull QuickHull::getConvexHull(const VertexDataSource& pointCloud, bool CCW, bool useOriginalIndices, T epsilon) { + buildMesh(pointCloud,CCW,useOriginalIndices,epsilon); + return ConvexHull(m_mesh,m_vertexData, CCW, useOriginalIndices); + } + + template + void QuickHull::createConvexHalfEdgeMesh() { + // Temporary variables used during iteration + std::vector visibleFaces; + std::vector horizonEdges; + struct FaceData { + IndexType m_faceIndex; + IndexType m_enteredFromHalfEdge; // If the face turns out not to be visible, this half edge will be marked as horizon edge + FaceData(IndexType fi, IndexType he) : m_faceIndex(fi),m_enteredFromHalfEdge(he) {} + }; + std::vector possiblyVisibleFaces; + + // Compute base tetrahedron + m_mesh = getInitialTetrahedron(); + assert(m_mesh.m_faces.size()==4); + + // Init face stack with those faces that have points assigned to them + std::deque faceList; + for (size_t i=0;i < 4;i++) { + auto& f = m_mesh.m_faces[i]; + if (f.m_pointsOnPositiveSide && f.m_pointsOnPositiveSide->size()>0) { + faceList.push_back(i); + f.m_inFaceStack = 1; + } + } + + // Process faces until the face list is empty. + size_t iter = 0; + while (!faceList.empty()) { + iter++; + if (iter == std::numeric_limits::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 IndexType topFaceIndex = faceList.front(); + 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. + horizonEdges.clear(); + possiblyVisibleFaces.clear(); + visibleFaces.clear(); + possiblyVisibleFaces.emplace_back(topFaceIndex,std::numeric_limits::max()); + while (possiblyVisibleFaces.size()) { + const auto faceData = possiblyVisibleFaces.back(); + 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& 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; + visibleFaces.push_back(faceData.m_faceIndex); + for (auto heIndex : m_mesh.getHalfEdgeIndicesOfFace(pvf)) { + if (m_mesh.m_halfEdges[heIndex].m_opp != faceData.m_enteredFromHalfEdge) { + 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 came from is part of the horizon edge. + pvf.m_isVisibleFaceOnCurrentIteration = 0; + 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<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 : 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<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 planeNormal = mathutils::getTriangleNormal(m_vertexData[A],m_vertexData[B],activePoint); + newFace.m_P = Plane(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;jsize()>0); + if (!newFace.m_inFaceStack) { + faceList.push_back(newFaceIndex); + newFace.m_inFaceStack = 1; + } + } + } + } + + // Cleanup + m_indexVectorPool.clear(); + } + + /* + * Private helper functions + */ + + template + std::array QuickHull::getExtremeValues() { + std::array 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& pos = m_vertexData[i]; + if (pos.x>extremeVals[0]) { + extremeVals[0]=pos.x; + outIndices[0]=(IndexType)i; + } + else if (pos.xextremeVals[2]) { + extremeVals[2]=pos.y; + outIndices[2]=(IndexType)i; + } + else if (pos.yextremeVals[4]) { + extremeVals[4]=pos.z; + outIndices[4]=(IndexType)i; + } + else if (pos.z + bool QuickHull::reorderHorizonEdges(std::vector& horizonEdges) { + const size_t horizonEdgeCount = horizonEdges.size(); + for (size_t i=0;i + T QuickHull::getScale(const std::array& 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 + MeshBuilder QuickHull::getInitialTetrahedron() { + const size_t vertexCount = m_vertexData.size(); + + // If we have at most 4 points, just return a degenerate tetrahedron: + if (vertexCount <= 4) { + IndexType 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 N = mathutils::getTriangleNormal(m_vertexData[v[0]],m_vertexData[v[1]],m_vertexData[v[2]]); + const Plane trianglePlane(N,m_vertexData[v[0]]); + if (trianglePlane.isPointOnPositiveSide(m_vertexData[v[3]])) { + std::swap(v[0],v[1]); + } + return MeshBuilder(v[0],v[1],v[2],v[3]); + } + + // Find two most distant extreme points. + T maxD = m_epsilonSquared; + std::pair 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 MeshBuilder(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 r(m_vertexData[selectedPoints.first], (m_vertexData[selectedPoints.second] - m_vertexData[selectedPoints.first])); + maxD = m_epsilonSquared; + size_t maxI=std::numeric_limits::max(); + const size_t vCount = m_vertexData.size(); + for (size_t i=0;i 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 IndexType 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 IndexType fourthPoint = (it == m_vertexData.end()) ? selectedPoints.first : std::distance(m_vertexData.begin(),it); + return MeshBuilder(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 baseTriangle{selectedPoints.first, selectedPoints.second, maxI}; + const Vector3 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 N = mathutils::getTriangleNormal(baseTriangleVertices[0],baseTriangleVertices[1],baseTriangleVertices[2]); + Plane trianglePlane(N,baseTriangleVertices[0]); + for (size_t i=0;imaxD) { + 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 N = 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 = N + m_vertexData[0]; + m_planarPointCloudTemp.push_back(extraPoint); + maxI = m_planarPointCloudTemp.size()-1; + m_vertexData = VertexDataSource(m_planarPointCloudTemp); + } + + // Enforce CCW orientation (if user prefers clockwise orientation, swap two vertices in each triangle when final mesh is created) + const Plane 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 + MeshBuilder mesh(baseTriangle[0],baseTriangle[1],baseTriangle[2],maxI); + for (auto& f : mesh.m_faces) { + auto v = mesh.getVertexIndicesOfFace(f); + const Vector3& va = m_vertexData[v[0]]; + const Vector3& vb = m_vertexData[v[1]]; + const Vector3& vc = m_vertexData[v[2]]; + const Vector3 N = mathutils::getTriangleNormal(va, vb, vc); + const Plane trianglePlane(N,va); + f.m_P = trianglePlane; + } + + // 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; + template class QuickHull; +} + diff --git a/libs/quickhull/QuickHull.hpp b/libs/quickhull/QuickHull.hpp new file mode 100644 index 00000000..ce950ede --- /dev/null +++ b/libs/quickhull/QuickHull.hpp @@ -0,0 +1,200 @@ +#ifndef QUICKHULL_HPP_ +#define QUICKHULL_HPP_ + +#include +#include +#include +#include "Structs/Vector3.hpp" +#include "Structs/Plane.hpp" +#include "Structs/Pool.hpp" +#include "Structs/Mesh.hpp" +#include "ConvexHull.hpp" +#include "HalfEdgeMesh.hpp" +#include "MathUtils.hpp" + + +/* + * Implementation of the 3D QuickHull algorithm by Antti Kuukka + * + * No copyrights. What follows is 100% Public Domain. + * + * + * + * INPUT: a list of points in 3D space (for example, vertices of a 3D mesh) + * + * OUTPUT: a ConvexHull object which provides vertex and index buffers of the generated convex hull as a triangle mesh. + * + * + * + * The implementation is thread-safe if each thread is using its own QuickHull object. + * + * + * SUMMARY OF THE ALGORITHM: + * - Create initial simplex (tetrahedron) using extreme points. We have four faces now and they form a convex mesh M. + * - For each point, assign them to the first face for which they are on the positive side of (so each point is assigned to at most + * one face). Points inside the initial tetrahedron are left behind now and no longer affect the calculations. + * - Add all faces that have points assigned to them to Face Stack. + * - Iterate until Face Stack is empty: + * - Pop topmost face F from the stack + * - From the points assigned to F, pick the point P that is farthest away from the plane defined by F. + * - Find all faces of M that have P on their positive side. Let us call these the "visible faces". + * - Because of the way M is constructed, these faces are connected. Solve their horizon edge loop. + * - "Extrude to P": Create new faces by connecting P with the points belonging to the horizon edge. Add the new faces to M and remove the visible + * faces from M. + * - Each point that was assigned to visible faces is now assigned to at most one of the newly created faces. + * - Those new faces that have points assigned to them are added to the top of Face Stack. + * - M is now the convex hull. + * + * TO DO: + * - Implement a proper 2D QuickHull and use that to solve the degenerate 2D case (when all the points lie on the same plane in 3D space). + * */ + +namespace quickhull { + + struct DiagnosticsData { + size_t m_failedHorizonEdges; // How many times QuickHull failed to solve the horizon edge. Failures lead to degenerated convex hulls. + + DiagnosticsData() : m_failedHorizonEdges(0) { } + }; + + template + class QuickHull { + using vec3 = Vector3; + + static const FloatType Epsilon; + + FloatType m_epsilon, m_epsilonSquared, m_scale; + bool m_planar; + std::vector m_planarPointCloudTemp; + VertexDataSource m_vertexData; + MeshBuilder m_mesh; + std::array m_extremeValues; + DiagnosticsData m_diagnostics; + + // Temporary variables used during iteration process + std::vector m_newFaceIndices; + std::vector m_newHalfEdgeIndices; + std::vector< std::unique_ptr> > m_disabledFacePointVectors; + + // Create a half edge mesh representing the base tetrahedron from which the QuickHull iteration proceeds. m_extremeValues must be properly set up when this is called. + MeshBuilder getInitialTetrahedron(); + + // Given a list of half edges, try to rearrange them so that they form a loop. Return true on success. + bool reorderHorizonEdges(std::vector& horizonEdges); + + // Find indices of extreme values (max x, min x, max y, min y, max z, min z) for the given point cloud + std::array getExtremeValues(); + + // Compute scale of the vertex data. + FloatType getScale(const std::array& extremeValues); + + // Each face contains a unique pointer to a vector of indices. However, many - often most - faces do not have any points on the positive + // side of them especially at the the end of the iteration. When a face is removed from the mesh, its associated point vector, if such + // exists, is moved to the index vector pool, and when we need to add new faces with points on the positive side to the mesh, + // we reuse these vectors. This reduces the amount of std::vectors we have to deal with, and impact on performance is remarkable. + Pool> m_indexVectorPool; + inline std::unique_ptr> getIndexVectorFromPool(); + inline void reclaimToIndexVectorPool(std::unique_ptr>& ptr); + + // Associates a point with a face if the point resides on the positive side of the plane. Returns true if the points was on the positive side. + inline bool addPointToFace(typename MeshBuilder::Face& f, IndexType pointIndex); + + // This will update m_mesh from which we create the ConvexHull object that getConvexHull function returns + void createConvexHalfEdgeMesh(); + + // Constructs the convex hull into a MeshBuilder object which can be converted to a ConvexHull or Mesh object + void buildMesh(const VertexDataSource& pointCloud, bool CCW, bool useOriginalIndices, FloatType eps); + + // The public getConvexHull functions will setup a VertexDataSource object and call this + ConvexHull getConvexHull(const VertexDataSource& pointCloud, bool CCW, bool useOriginalIndices, FloatType eps); + public: + // Computes convex hull for a given point cloud. + // Params: + // pointCloud: a vector of of 3D points + // CCW: whether the output mesh triangles should have CCW orientation + // useOriginalIndices: should the output mesh use same vertex indices as the original point cloud. If this is false, + // then we generate a new vertex buffer which contains only the vertices that are part of the convex hull. + // eps: minimum distance to a plane to consider a point being on positive of it (for a point cloud with scale 1) + ConvexHull getConvexHull(const std::vector>& pointCloud, bool CCW, bool useOriginalIndices, FloatType eps = Epsilon); + + // Computes convex hull for a given point cloud. + // Params: + // vertexData: pointer to the first 3D point of the point cloud + // vertexCount: number of vertices in the point cloud + // CCW: whether the output mesh triangles should have CCW orientation + // useOriginalIndices: should the output mesh use same vertex indices as the original point cloud. If this is false, + // then we generate a new vertex buffer which contains only the vertices that are part of the convex hull. + // eps: minimum distance to a plane to consider a point being on positive of it (for a point cloud with scale 1) + ConvexHull getConvexHull(const Vector3* vertexData, size_t vertexCount, bool CCW, bool useOriginalIndices, FloatType eps = Epsilon); + + // Computes convex hull for a given point cloud. This function assumes that the vertex data resides in memory + // in the following format: x_0,y_0,z_0,x_1,y_1,z_1,... + // Params: + // vertexData: pointer to the X component of the first point of the point cloud. + // vertexCount: number of vertices in the point cloud + // CCW: whether the output mesh triangles should have CCW orientation + // useOriginalIndices: should the output mesh use same vertex indices as the original point cloud. If this is false, + // then we generate a new vertex buffer which contains only the vertices that are part of the convex hull. + // eps: minimum distance to a plane to consider a point being on positive of it (for a point cloud with scale 1) + ConvexHull getConvexHull(const FloatType* vertexData, size_t vertexCount, bool CCW, bool useOriginalIndices, FloatType eps = Epsilon); + + // Computes convex hull for a given point cloud. This function assumes that the vertex data resides in memory + // in the following format: x_0,y_0,z_0,x_1,y_1,z_1,... + // Params: + // vertexData: pointer to the X component of the first point of the point cloud. + // vertexCount: number of vertices in the point cloud + // CCW: whether the output mesh triangles should have CCW orientation + // eps: minimum distance to a plane to consider a point being on positive of it (for a point cloud with scale 1) + // Returns: + // Convex hull of the point cloud as a mesh object with half edge structure. + HalfEdgeMesh getConvexHullAsMesh(const FloatType* vertexData, size_t vertexCount, bool CCW, FloatType eps = Epsilon); + + // Get diagnostics about last generated convex hull + const DiagnosticsData& getDiagnostics() { + return m_diagnostics; + } + }; + + /* + * Inline function definitions + */ + + template + std::unique_ptr> QuickHull::getIndexVectorFromPool() { + auto r = std::move(m_indexVectorPool.get()); + r->clear(); + return r; + } + + template + void QuickHull::reclaimToIndexVectorPool(std::unique_ptr>& ptr) { + const size_t oldSize = ptr->size(); + if ((oldSize+1)*128 < ptr->capacity()) { + // Reduce memory usage! Huge vectors are needed at the beginning of iteration when faces have many points on their positive side. Later on, smaller vectors will suffice. + ptr.reset(nullptr); + return; + } + m_indexVectorPool.reclaim(ptr); + } + + template + bool QuickHull::addPointToFace(typename MeshBuilder::Face& f, IndexType pointIndex) { + const T D = mathutils::getSignedDistanceToPlane(m_vertexData[ pointIndex ],f.m_P); + if (D>0 && D*D > m_epsilonSquared*f.m_P.m_sqrNLength) { + if (!f.m_pointsOnPositiveSide) { + f.m_pointsOnPositiveSide = std::move(getIndexVectorFromPool()); + } + f.m_pointsOnPositiveSide->push_back( pointIndex ); + if (D > f.m_mostDistantPointDist) { + f.m_mostDistantPointDist = D; + f.m_mostDistantPoint = pointIndex; + } + return true; + } + return false; + } + +} + + +#endif /* QUICKHULL_HPP_ */ diff --git a/libs/quickhull/README.md b/libs/quickhull/README.md new file mode 100644 index 00000000..6c608ec5 --- /dev/null +++ b/libs/quickhull/README.md @@ -0,0 +1,29 @@ +This implementation is 100% Public Domain. + +Feel free to use. + +C++11 is needed to compile it. + +Basic usage: + + #include "quickhull/quickhull.hpp" + + using namespace quickhull; + QuickHull qh; // Could be double as well + std::vector> pointCloud; + // Add points to point cloud + ... + auto hull = qh.getConvexHull(pointCloud, true, false); + auto indexBuffer = hull.getIndexBuffer(); + auto vertexBuffer = hull.getVertexBuffer(); + // Do what you want with the convex triangle mesh + +Vertex data can be passed as a pointer to float/double as long as the data is in X_0,Y_0,Z_0,X_1,Y_1,Z_1,...,X_N,Y_N_Z_N format: + + auto hull = qh.getConvexHull(&pointCloud[0].x, pointCloud.size(), true, false); + +The first boolean parameter of getConvexHull specifies whether the output mesh should have its triangles in CCW orientation. The second boolean parameter specifies whether the output mesh should use vertex indices of the original point cloud. If it is false, a new vertex buffer is generated which consists only of those vertices that are part of the convex hull. + +This implementation is fast, because the convex hull is internally built using a half edge mesh representation which provides quick access to adjacent faces. It is also possible to get the output convex hull as a half edge mesh: + + auto mesh = qh.getConvexHullAsMesh(&pointCloud[0].x, pointCloud.size(), true); diff --git a/libs/quickhull/Structs/Mesh.hpp b/libs/quickhull/Structs/Mesh.hpp new file mode 100644 index 00000000..219eda1c --- /dev/null +++ b/libs/quickhull/Structs/Mesh.hpp @@ -0,0 +1,248 @@ +#ifndef MESH_HPP_ +#define MESH_HPP_ + +#include +#include "Vector3.hpp" +#include "Plane.hpp" +#include "Pool.hpp" +#include "../Types.hpp" +#include +#include +#include +#include +#include +#include "VertexDataSource.hpp" +#include + +namespace quickhull { + + template + class MeshBuilder { + public: + struct HalfEdge { + IndexType m_endVertex; + IndexType m_opp; + IndexType m_face; + IndexType m_next; + + void disable() { + m_endVertex = std::numeric_limits::max(); + } + + bool isDisabled() const { + return m_endVertex == std::numeric_limits::max(); + } + }; + + struct Face { + IndexType m_he; + Plane m_P; + T m_mostDistantPointDist; + IndexType m_mostDistantPoint; + size_t m_visibilityCheckedOnIteration; + std::uint8_t m_isVisibleFaceOnCurrentIteration : 1; + std::uint8_t m_inFaceStack : 1; + std::uint8_t m_horizonEdgesOnCurrentIteration : 3; // Bit for each half edge assigned to this face, each being 0 or 1 depending on whether the edge belongs to horizon edge + std::unique_ptr> m_pointsOnPositiveSide; + + Face() : m_he(std::numeric_limits::max()), + m_mostDistantPointDist(0), + m_mostDistantPoint(0), + m_visibilityCheckedOnIteration(0), + m_isVisibleFaceOnCurrentIteration(0), + m_inFaceStack(0), + m_horizonEdgesOnCurrentIteration(0) + { + + } + + void disable() { + m_he = std::numeric_limits::max(); + } + + bool isDisabled() const { + return m_he == std::numeric_limits::max(); + } + }; + + // Mesh data + std::vector m_faces; + std::vector m_halfEdges; + + // When the mesh is modified and faces and half edges are removed from it, we do not actually remove them from the container vectors. + // Insted, they are marked as disabled which means that the indices can be reused when we need to add new faces and half edges to the mesh. + // We store the free indices in the following vectors. + std::vector m_disabledFaces,m_disabledHalfEdges; + + IndexType addFace() { + if (m_disabledFaces.size()) { + IndexType index = m_disabledFaces.back(); + auto& f = m_faces[index]; + assert(f.isDisabled()); + assert(!f.m_pointsOnPositiveSide); + f.m_mostDistantPointDist = 0; + m_disabledFaces.pop_back(); + return index; + } + m_faces.emplace_back(); + return m_faces.size()-1; + } + + IndexType addHalfEdge() { + if (m_disabledHalfEdges.size()) { + const IndexType index = m_disabledHalfEdges.back(); + m_disabledHalfEdges.pop_back(); + return index; + } + m_halfEdges.emplace_back(); + return m_halfEdges.size()-1; + } + + // Mark a face as disabled and return a pointer to the points that were on the positive of it. + std::unique_ptr> disableFace(IndexType faceIndex) { + auto& f = m_faces[faceIndex]; + f.disable(); + m_disabledFaces.push_back(faceIndex); + return std::move(f.m_pointsOnPositiveSide); + } + + void disableHalfEdge(IndexType heIndex) { + auto& he = m_halfEdges[heIndex]; + he.disable(); + m_disabledHalfEdges.push_back(heIndex); + } + + MeshBuilder() = default; + + // Create a mesh with initial tetrahedron ABCD. Dot product of AB with the normal of triangle ABC should be negative. + MeshBuilder(IndexType a, IndexType b, IndexType c, IndexType d) { + // Create halfedges + HalfEdge AB; + AB.m_endVertex = b; + AB.m_opp = 6; + AB.m_face = 0; + AB.m_next = 1; + m_halfEdges.push_back(AB); + + HalfEdge BC; + BC.m_endVertex = c; + BC.m_opp = 9; + BC.m_face = 0; + BC.m_next = 2; + m_halfEdges.push_back(BC); + + HalfEdge CA; + CA.m_endVertex = a; + CA.m_opp = 3; + CA.m_face = 0; + CA.m_next = 0; + m_halfEdges.push_back(CA); + + HalfEdge AC; + AC.m_endVertex = c; + AC.m_opp = 2; + AC.m_face = 1; + AC.m_next = 4; + m_halfEdges.push_back(AC); + + HalfEdge CD; + CD.m_endVertex = d; + CD.m_opp = 11; + CD.m_face = 1; + CD.m_next = 5; + m_halfEdges.push_back(CD); + + HalfEdge DA; + DA.m_endVertex = a; + DA.m_opp = 7; + DA.m_face = 1; + DA.m_next = 3; + m_halfEdges.push_back(DA); + + HalfEdge BA; + BA.m_endVertex = a; + BA.m_opp = 0; + BA.m_face = 2; + BA.m_next = 7; + m_halfEdges.push_back(BA); + + HalfEdge AD; + AD.m_endVertex = d; + AD.m_opp = 5; + AD.m_face = 2; + AD.m_next = 8; + m_halfEdges.push_back(AD); + + HalfEdge DB; + DB.m_endVertex = b; + DB.m_opp = 10; + DB.m_face = 2; + DB.m_next = 6; + m_halfEdges.push_back(DB); + + HalfEdge CB; + CB.m_endVertex = b; + CB.m_opp = 1; + CB.m_face = 3; + CB.m_next = 10; + m_halfEdges.push_back(CB); + + HalfEdge BD; + BD.m_endVertex = d; + BD.m_opp = 8; + BD.m_face = 3; + BD.m_next = 11; + m_halfEdges.push_back(BD); + + HalfEdge DC; + DC.m_endVertex = c; + DC.m_opp = 4; + DC.m_face = 3; + DC.m_next = 9; + m_halfEdges.push_back(DC); + + // Create faces + Face ABC; + ABC.m_he = 0; + m_faces.push_back(std::move(ABC)); + + Face ACD; + ACD.m_he = 3; + m_faces.push_back(std::move(ACD)); + + Face BAD; + BAD.m_he = 6; + m_faces.push_back(std::move(BAD)); + + Face CBD; + CBD.m_he = 9; + m_faces.push_back(std::move(CBD)); + } + + std::array getVertexIndicesOfFace(const Face& f) const { + std::array v; + const HalfEdge* he = &m_halfEdges[f.m_he]; + v[0] = he->m_endVertex; + he = &m_halfEdges[he->m_next]; + v[1] = he->m_endVertex; + he = &m_halfEdges[he->m_next]; + v[2] = he->m_endVertex; + return v; + } + + std::array getVertexIndicesOfHalfEdge(const HalfEdge& he) const { + return {m_halfEdges[he.m_opp].m_endVertex,he.m_endVertex}; + } + + std::array getHalfEdgeIndicesOfFace(const Face& f) const { + return {f.m_he,m_halfEdges[f.m_he].m_next,m_halfEdges[m_halfEdges[f.m_he].m_next].m_next}; + } + }; + + + +} + + + +#endif diff --git a/libs/quickhull/Structs/Plane.hpp b/libs/quickhull/Structs/Plane.hpp new file mode 100644 index 00000000..903a7efe --- /dev/null +++ b/libs/quickhull/Structs/Plane.hpp @@ -0,0 +1,36 @@ +#ifndef QHPLANE_HPP_ +#define QHPLANE_HPP_ + +#include "Vector3.hpp" + +namespace quickhull { + + template + class Plane { + public: + Vector3 m_N; + + // Signed distance (if normal is of length 1) to the plane from origin + T m_D; + + // Normal length squared + T m_sqrNLength; + + bool isPointOnPositiveSide(const Vector3& Q) const { + T d = m_N.dotProduct(Q)+m_D; + if (d>=0) return true; + return false; + } + + Plane() = default; + + // Construct a plane using normal N and any point P on the plane + Plane(const Vector3& N, const Vector3& P) : m_N(N), m_D(-N.dotProduct(P)), m_sqrNLength(m_N.x*m_N.x+m_N.y*m_N.y+m_N.z*m_N.z) { + + } + }; + +} + + +#endif /* PLANE_HPP_ */ diff --git a/libs/quickhull/Structs/Pool.hpp b/libs/quickhull/Structs/Pool.hpp new file mode 100644 index 00000000..4216b4a1 --- /dev/null +++ b/libs/quickhull/Structs/Pool.hpp @@ -0,0 +1,35 @@ +#ifndef Pool_h +#define Pool_h + +#include +#include + +namespace quickhull { + + template + class Pool { + std::vector> m_data; + public: + void clear() { + m_data.clear(); + } + + void reclaim(std::unique_ptr& ptr) { + m_data.push_back(std::move(ptr)); + } + + std::unique_ptr get() { + if (m_data.size()==0) { + return std::unique_ptr(new T()); + } + auto it = m_data.end()-1; + std::unique_ptr r = std::move(*it); + m_data.erase(it); + return r; + } + + }; + +} + +#endif /* Pool_h */ diff --git a/libs/quickhull/Structs/Ray.hpp b/libs/quickhull/Structs/Ray.hpp new file mode 100644 index 00000000..19b60728 --- /dev/null +++ b/libs/quickhull/Structs/Ray.hpp @@ -0,0 +1,21 @@ +#ifndef QuickHull_Ray_hpp +#define QuickHull_Ray_hpp + +#include "Vector3.hpp" + +namespace quickhull { + + template + struct Ray { + const Vector3 m_S; + const Vector3 m_V; + const T m_VInvLengthSquared; + + Ray(const Vector3& S,const Vector3& V) : m_S(S), m_V(V), m_VInvLengthSquared(1/m_V.getLengthSquared()) { + } + }; + +} + + +#endif diff --git a/libs/quickhull/Structs/Vector3.hpp b/libs/quickhull/Structs/Vector3.hpp new file mode 100644 index 00000000..47a5f746 --- /dev/null +++ b/libs/quickhull/Structs/Vector3.hpp @@ -0,0 +1,140 @@ +#ifndef QuickHull_Vector3_hpp +#define QuickHull_Vector3_hpp + +#include +#include + +namespace quickhull { + + template + class Vector3 + { + public: + Vector3() = default; + + Vector3(T x, T y, T z) : x(x), y(y), z(z) { + + } + + T x,y,z; + + T dotProduct(const Vector3& other) const { + return x*other.x+y*other.y+z*other.z; + } + + void normalize() { + const T len = getLength(); + x/=len; + y/=len; + z/=len; + } + + Vector3 getNormalized() const { + const T len = getLength(); + return Vector3(x/len,y/len,z/len); + } + + T getLength() const { + return std::sqrt(x*x+y*y+z*z); + } + + Vector3 operator-(const Vector3& other) const { + return Vector3(x-other.x,y-other.y,z-other.z); + } + + Vector3 operator+(const Vector3& other) const { + return Vector3(x+other.x,y+other.y,z+other.z); + } + + Vector3& operator+=(const Vector3& other) { + x+=other.x; + y+=other.y; + z+=other.z; + return *this; + } + Vector3& operator-=(const Vector3& other) { + x-=other.x; + y-=other.y; + z-=other.z; + return *this; + } + Vector3& operator*=(T c) { + x*=c; + y*=c; + z*=c; + return *this; + } + + Vector3& operator/=(T c) { + x/=c; + y/=c; + z/=c; + return *this; + } + + Vector3 operator-() const { + return Vector3(-x,-y,-z); + } + + template + Vector3 operator*(S c) const { + return Vector3(x*c,y*c,z*c); + } + + template + Vector3 operator/(S c) const { + return Vector3(x/c,y/c,z/c); + } + + T getLengthSquared() const { + return x*x + y*y + z*z; + } + + bool operator!=(const Vector3& o) const { + return x != o.x || y != o.y || z != o.z; + } + + // Projection onto another vector + Vector3 projection(const Vector3& o) const { + T C = dotProduct(o)/o.getLengthSquared(); + return o*C; + } + + Vector3 crossProduct (const Vector3& rhs ) { + T a = y * rhs.z - z * rhs.y ; + T b = z * rhs.x - x * rhs.z ; + T c = x * rhs.y - y * rhs.x ; + Vector3 product( a , b , c ) ; + return product ; + } + + T getDistanceTo(const Vector3& other) const { + Vector3 diff = *this - other; + return diff.getLength(); + } + + T getSquaredDistanceTo(const Vector3& other) const { + const T dx = x-other.x; + const T dy = y-other.y; + const T dz = z-other.z; + return dx*dx+dy*dy+dz*dz; + } + + }; + + // Overload also << operator for easy printing of debug data + template + inline std::ostream& operator<<(std::ostream& os, const Vector3& vec) { + os << "(" << vec.x << "," << vec.y << "," << vec.z << ")"; + return os; + } + + template + inline Vector3 operator*(T c, const Vector3& v) { + return Vector3(v.x*c,v.y*c,v.z*c); + } + +} + + +#endif diff --git a/libs/quickhull/Structs/VertexDataSource.hpp b/libs/quickhull/Structs/VertexDataSource.hpp new file mode 100644 index 00000000..162b643b --- /dev/null +++ b/libs/quickhull/Structs/VertexDataSource.hpp @@ -0,0 +1,48 @@ +#ifndef VertexDataSource_h +#define VertexDataSource_h + +#include "Vector3.hpp" + +namespace quickhull { + + template + class VertexDataSource { + const Vector3* m_ptr; + size_t m_count; + + public: + VertexDataSource(const Vector3* ptr, size_t count) : m_ptr(ptr), m_count(count) { + + } + + VertexDataSource(const std::vector>& vec) : m_ptr(&vec[0]), m_count(vec.size()) { + + } + + VertexDataSource() : m_ptr(nullptr), m_count(0) { + + } + + VertexDataSource& operator=(const VertexDataSource& other) = default; + + size_t size() const { + return m_count; + } + + const Vector3& operator[](size_t index) const { + return m_ptr[index]; + } + + const Vector3* begin() const { + return m_ptr; + } + + const Vector3* end() const { + return m_ptr + m_count; + } + }; + +} + + +#endif /* VertexDataSource_h */ diff --git a/libs/quickhull/Tests/QuickHullTests.cpp b/libs/quickhull/Tests/QuickHullTests.cpp new file mode 100644 index 00000000..758a4f60 --- /dev/null +++ b/libs/quickhull/Tests/QuickHullTests.cpp @@ -0,0 +1,279 @@ +#include "../QuickHull.hpp" +#include "../MathUtils.hpp" +#include +#include +#include + +namespace quickhull { + + namespace tests { + + using FloatType = float; + using vec3 = Vector3; + + // Setup RNG + static std::mt19937 rng; + static std::uniform_real_distribution<> dist(0,1); + + FloatType rnd(FloatType from, FloatType to) { + return from + (FloatType)dist(rng)*(to-from); + }; + + void assertSameValue(FloatType a, FloatType b) { + assert(std::abs(a-b)<0.0001f); + } + + void testVector3() { + typedef Vector3 vec3; + vec3 a(1,0,0); + vec3 b(1,0,0); + + vec3 c = a.projection(b); + assert( (c-a).getLength()<0.00001f); + + a = vec3(1,1,0); + b = vec3(1,3,0); + c = b.projection(a); + assert( (c-vec3(2,2,0)).getLength()<0.00001f); + } + + template + std::vector> createSphere(T radius, size_t M, Vector3 offset = Vector3(0,0,0)) { + std::vector> pc; + const T pi = 3.14159f; + for (int i=0;i<=M;i++) { + FloatType y = std::sin(pi/2 + (FloatType)i/(M)*pi); + FloatType r = std::cos(pi/2 + (FloatType)i/(M)*pi); + FloatType K = FloatType(1)-std::abs((FloatType)((FloatType)i-M/2.0f))/(FloatType)(M/2.0f); + const size_t pcount = (size_t)(1 + K*M + FloatType(1)/2); + for (size_t j=0;j1 ? r*std::cos((FloatType)j/pcount*pi*2) : 0; + FloatType z = pcount>1 ? r*std::sin((FloatType)j/pcount*pi*2) : 0; + pc.emplace_back(x+offset.x,y+offset.y,z+offset.z); + } + } + return pc; + } + + void sphereTest() { + QuickHull qh; + FloatType y = 1; + for (;;) { + auto pc = createSphere(1, 100, Vector3(0,y,0)); + auto hull = qh.getConvexHull(pc,true,false); + y *= 15; + y /= 10; + if (hull.getVertexBuffer().size()==4) { + break; + } + } + + // Test worst case scenario: more and more points on the unit sphere. All points should be part of the convex hull, as long as we can make epsilon smaller without + // running out of numerical accuracy. + size_t i = 1; + FloatType eps = 0.002f; + for (;;) { + auto pc = createSphere(1, i, Vector3(0,0,0)); + auto hull = qh.getConvexHull(pc,true,false,eps); + std::cout << i << ":" << pc.size() << " : " << hull.getVertexBuffer().size() << " at eps=" << eps << std::endl; + if (qh.getDiagnostics().m_failedHorizonEdges) { + // This should not happen + assert(false); + break; + } + if (pc.size() == hull.getVertexBuffer().size()) { + // Fine, all the points on unit sphere do belong to the convex mesh. + i += 1; + } + else { + eps *= 0.5f; + std::cout << "Epsilon to " << eps << std::endl; + } + + if (i == 500) { + break; + } + } + } + + void testPlanarCase() { + QuickHull qh; + std::vector pc; + pc.emplace_back(-3.000000f, -0.250000f, -0.800000f); + pc.emplace_back(-3.000000f, 0.250000f, -0.800000f); + pc.emplace_back(-3.125000f, 0.250000f, -0.750000); + pc.emplace_back(-3.125000f, -0.250000f, -0.750000); + auto hull = qh.getConvexHull(pc,true,false); + assert(hull.getIndexBuffer().size()==12); + assert(hull.getVertexBuffer().size()==4); + } + + void testHalfEdgeOutput() { + QuickHull qh; + + // 8 corner vertices of a cube + tons of vertices inside. Output should be a half edge mesh with 12 faces (6 cube faces with 2 triangles per face) and 36 half edges (3 half edges per face). + std::vector pc; + for (int h=0;h<1000;h++) { + pc.emplace_back(rnd(-1,1),rnd(-1,1),rnd(-1,1)); + } + for (int h=0;h<8;h++) { + pc.emplace_back(h&1?-2:2, h&2?-2:2, h&4?-2:2); + } + HalfEdgeMesh mesh = qh.getConvexHullAsMesh(&pc[0].x, pc.size(), true); + assert(mesh.m_faces.size() == 12); + assert(mesh.m_halfEdges.size() == 36); + assert(mesh.m_vertices.size() == 8); + } + + void testPlanes() { + Vector3 N(1,0,0); + Vector3 p(2,0,0); + Plane P(N,p); + auto dist = mathutils::getSignedDistanceToPlane(Vector3(3,0,0), P); + assertSameValue(dist,1); + dist = mathutils::getSignedDistanceToPlane(Vector3(1,0,0), P); + assertSameValue(dist,-1); + dist = mathutils::getSignedDistanceToPlane(Vector3(1,0,0), P); + assertSameValue(dist,-1); + N = Vector3(2,0,0); + P = Plane(N,p); + dist = mathutils::getSignedDistanceToPlane(Vector3(6,0,0), P); + assertSameValue(dist,8); + } + + void run() { + // Setup test env + const size_t N = 200; + std::vector pc; + QuickHull qh; + ConvexHull hull; + + // Seed RNG using Unix time + std::chrono::system_clock::time_point now = std::chrono::system_clock::now(); + auto seed = std::chrono::duration_cast(now.time_since_epoch()).count(); + rng.seed((unsigned int)seed); + + // Test 1 : Put N points inside unit cube. Result mesh must have exactly 8 vertices because the convex hull is the unit cube. + pc.clear(); + for (int i=0;i<8;i++) { + pc.emplace_back(i&1 ? -1 : 1,i&2 ? -1 : 1,i&4 ? -1 : 1); + } + for (size_t i=0;i(1, 50); + hull = qh.getConvexHull(pc,true,false); + assert(pc.size() == hull.getVertexBuffer().size()); + hull = qh.getConvexHull(pc,true,true); + // Add every vertex twice. This should not affect final mesh + auto s = pc.size(); + for (size_t i=0;i essentially we get a line + const FloatType mul = 2*2*2; + while (true) { + for (auto& p : pc) { + p.x *= mul; + } + hull = qh.getConvexHull(pc,true,false); + if (hull.getVertexBuffer().size() == 4) { + break; + } + } + + // Test 3: 0D + pc.clear(); + vec3 centerPoint(2,2,2); + pc.push_back(centerPoint); + for (size_t i=0;i<100;i++) { + auto newp = centerPoint + vec3(rnd(-0.000001f,0.000001f),rnd(-0.000001f,0.000001f),rnd(-0.000001f,0.000001f)); + pc.push_back(newp); + } + hull = qh.getConvexHull(pc,true,false); + assert(hull.getIndexBuffer().size()==12); + + // Test 4: 2d degenerate case + testPlanarCase(); + + // Test 5: first a planar circle, then make a cylinder out of it + pc.clear(); + for (size_t i=0;i quickhull; + std::vector> pointCloud; + pointCloud.reserve( mergeVertices.size() ); + for( std::size_t i = 0; i < mergeVertices.size(); ++i ){ + pointCloud.push_back( quickhull::Vector3( static_cast( mergeVertices[i].x() ), + static_cast( mergeVertices[i].y() ), + static_cast( mergeVertices[i].z() ) ) ); + } + auto hull = quickhull.getConvexHull( pointCloud, false, true, 0.0001 ); + const auto& indexBuffer = hull.getIndexBuffer(); + const size_t triangleCount = indexBuffer.size() / 3; + for( size_t i = 0; i < triangleCount; ++i ) { + Vector3 p[3]; + for( size_t j = 0; j < 3; ++j ){ + p[j] = mergeVertices[indexBuffer[i * 3 + j]]; + } + const Plane3 plane = plane3_for_points( p[0], p[1], p[2] ); + if( plane3_valid( plane ) ){ + mergePlanes.insert( MergePlane( plane, p[0], p[2], p[1] ) ); + } + } +#else if( mergeVertices.size() < 130 ){ // use reliable path, when possible, as convhull_3d.h is not too much. /* bruteforce new planes */ for( MergeVertices::const_iterator i = mergeVertices.begin() + 0; i != mergeVertices.end() - 2; ++i ) @@ -1134,6 +1158,7 @@ void CSG_build_hull( const MergeVertices& mergeVertices, MergePlanes& mergePlane free( vertices ); free( faceIndices ); } +#endif } void CSG_WrapMerge( const ClipperPoints& clipperPoints ){ diff --git a/radiant/patch.cpp b/radiant/patch.cpp index fc83c9ff..6aabaf00 100644 --- a/radiant/patch.cpp +++ b/radiant/patch.cpp @@ -19,6 +19,8 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */ +#define _USE_MATH_DEFINES + #include "patch.h" #include