diff --git a/MathUtils.hpp b/MathUtils.hpp index e7c30fe..4c4317b 100644 --- a/MathUtils.hpp +++ b/MathUtils.hpp @@ -21,7 +21,8 @@ namespace quickhull { // (divide by N.getNormalized() to get actual Euclidian distance). template inline T getSignedDistanceToPlane(const Vector3& v, const Plane& p) { - return p.m_N.dotProduct(v) + p.m_D; + return p.m_N.dotProduct(v) + p.m_D; + // return (p.m_N.getNormalized().dotProduct(v) + p.m_D); } template @@ -36,7 +37,9 @@ namespace quickhull { T px = y * rhsz - z * rhsy ; T py = z * rhsx - x * rhsz ; T pz = x * rhsy - y * rhsx ; - return Vector3(px,py,pz); + // errorNormal = Vector3(std::abs(y*rhsz)+std::abs(z*rhsy),std::abs(z*rhsx)+std::abs(x*rhsz),std::abs(x*rhsy)+std::abs(y*rhsx)); + // return Vector3(px,py,pz); + return Vector3(px,py,pz).getNormalized(); } diff --git a/QuickHull.cpp b/QuickHull.cpp index 44a59f7..06c3ad5 100644 --- a/QuickHull.cpp +++ b/QuickHull.cpp @@ -91,6 +91,31 @@ namespace quickhull { return ConvexHull(m_mesh,m_vertexData, CCW, useOriginalIndices); } + // Check if the mesh remains convex after adding new faces + template + bool QuickHull::isMeshConvex(const std::vector& newFaces) { + for (const size_t& faceIndex : newFaces) { + const auto& face = m_mesh.m_faces[faceIndex]; + bool positive = false; + bool negative = false; + for (auto i : tempPoints) { + Vector3 vertex = m_vertexData[i]; + double distance = face.m_P.m_N.dotProduct(vertex) + face.m_P.m_D; + if (distance > m_epsilon) { + positive = true; + } else if (distance < -m_epsilon) { + negative = true; + } + + // If we find both positive and negative distances, the polyhedron is not convex + if (positive && negative) { + return false; + } + } + } + return true; + } + template void QuickHull::createConvexHalfEdgeMesh() { m_visibleFaces.clear(); @@ -113,6 +138,7 @@ namespace quickhull { // Process faces until the face list is empty. size_t iter = 0; + int incorrect_count=0; while (!m_faceList.empty()) { iter++; if (iter == std::numeric_limits::max()) { @@ -135,8 +161,16 @@ namespace quickhull { } // Pick the most distant point to this triangle plane as the point to which we extrude + // int breakFlag=0; const vec3& activePoint = m_vertexData[tf.m_mostDistantPoint]; + // std::cout << "Active Point" << activePoint.x << " " << activePoint.y << " " << activePoint.z << std::endl; const size_t activePointIndex = tf.m_mostDistantPoint; + // std::cout << activePointIndex << std::endl; + // if (activePointIndex==0) + // { + // breakFlag=1; + // } + tempPoints.push_back(activePointIndex); // 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 @@ -161,6 +195,9 @@ namespace quickhull { pvf.m_visibilityCheckedOnIteration = iter; const T d = P.m_N.dotProduct(activePoint)+P.m_D; if (d>0) { + // if (d>m_epsilon && d*d > m_epsilonSquared*P.m_sqrNLength) { + // if (d>0 && d*d > m_epsilonSquared*P.m_sqrNLength) { + // if (d>m_epsilon) { pvf.m_isVisibleFaceOnCurrentIteration = 1; pvf.m_horizonEdgesOnCurrentIteration = 0; m_visibleFaces.push_back(faceData.m_faceIndex); @@ -250,7 +287,9 @@ namespace quickhull { A = horizonEdgeVertexIndices[0]; B = horizonEdgeVertexIndices[1]; C = activePointIndex; - + // std::cout << "PtA" << m_vertexData[A].x << " " << m_vertexData[A].y << " " << m_vertexData[A].z << std::endl; + // std::cout << "PtB" << m_vertexData[B].x << " " << m_vertexData[B].y << " " << m_vertexData[B].z << std::endl; + // std::cout << "PtC" << m_vertexData[C].x << " " << m_vertexData[C].y << " " << m_vertexData[C].z << std::endl; const size_t newFaceIndex = m_mesh.addFace(); m_newFaceIndices.push_back(newFaceIndex); @@ -272,6 +311,7 @@ namespace quickhull { const Vector3 planeNormal = mathutils::getTriangleNormal(m_vertexData[A],m_vertexData[B],activePoint); newFace.m_P = Plane(planeNormal,activePoint); + // std::cout << "N" << planeNormal.x << " " << planeNormal.y << " " << planeNormal.z << std::endl; newFace.m_he = AB; m_mesh.m_halfEdges[CA].m_opp = m_newHalfEdgeIndices[i>0 ? i*2-1 : 2*horizonEdgeCount-1]; @@ -292,6 +332,9 @@ namespace quickhull { } } // The points are no longer needed: we can move them to the vector pool for reuse. + // for (auto disabledPt : *disabledPoints) { + // std::cout << "Disabled Point" << m_vertexData[disabledPt].x << " " << m_vertexData[disabledPt].y << " " << m_vertexData[disabledPt].z << std::endl; + // } reclaimToIndexVectorPool(disabledPoints); } @@ -306,8 +349,27 @@ namespace quickhull { } } } + // if (isMeshConvex(m_newFaceIndices)) + // { + // // if (breakFlag==1) + // // { + // // m_indexVectorPool.clear(); + // // break; + // // } + // // std::cout << "YAY\n"; + // } + // else + // { + // incorrect_count+=1; + // // std::cout << "OOP\n"; + // // m_indexVectorPool.clear(); + // // break; + // } } - + if (!isMeshConvex(m_newFaceIndices)) + { + std::cout << "Invalid Output" << "\n"; + } // Cleanup m_indexVectorPool.clear(); } @@ -400,6 +462,10 @@ namespace quickhull { if (trianglePlane.isPointOnPositiveSide(m_vertexData[v[3]])) { std::swap(v[0],v[1]); } + tempPoints.push_back(v[0]); + tempPoints.push_back(v[1]); + tempPoints.push_back(v[2]); + tempPoints.push_back(v[3]); return m_mesh.setup(v[0],v[1],v[2],v[3]); } @@ -431,6 +497,7 @@ namespace quickhull { if (distToRay > maxD) { maxD=distToRay; maxI=i; + // Maybe here } } if (maxD == m_epsilonSquared) { @@ -483,7 +550,15 @@ namespace quickhull { } // Create a tetrahedron half edge mesh and compute planes defined by each triangle - m_mesh.setup(baseTriangle[0],baseTriangle[1],baseTriangle[2],maxI); + // std::cout << "Active Point Base" << m_vertexData[baseTriangle[0]].x << " " << m_vertexData[baseTriangle[0]].y << " " << m_vertexData[baseTriangle[0]].z << std::endl; + // std::cout << "Active Point Base" << m_vertexData[baseTriangle[1]].x << " " << m_vertexData[baseTriangle[1]].y << " " << m_vertexData[baseTriangle[1]].z << std::endl; + // std::cout << "Active Point Base" << m_vertexData[baseTriangle[2]].x << " " << m_vertexData[baseTriangle[2]].y << " " << m_vertexData[baseTriangle[2]].z << std::endl; + // std::cout << "Active Point Base" << m_vertexData[maxI].x << " " << m_vertexData[maxI].y << " " << m_vertexData[maxI].z << std::endl; + tempPoints.push_back(baseTriangle[0]); + tempPoints.push_back(baseTriangle[1]); + tempPoints.push_back(baseTriangle[2]); + tempPoints.push_back(maxI); + 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& va = m_vertexData[v[0]]; @@ -492,6 +567,10 @@ namespace quickhull { const Vector3 N1 = mathutils::getTriangleNormal(va, vb, vc); const Plane plane(N1,va); f.m_P = plane; + // std::cout << "Pt1" << va.x << " " << va.y << " " << va.z << std::endl; + // std::cout << "Pt2" << vb.x << " " << vb.y << " " << vb.z << std::endl; + // std::cout << "Pt3" << vc.x << " " << vc.y << " " << vc.z << std::endl; + // std::cout << "N" << N1.x << " " << N1.y << " " << N1.z << std::endl; } // Finally we assign a face for each vertex outside the tetrahedron (vertices inside the tetrahedron have no role anymore) diff --git a/QuickHull.hpp b/QuickHull.hpp index 09bbe93..6cbbc91 100644 --- a/QuickHull.hpp +++ b/QuickHull.hpp @@ -75,6 +75,7 @@ namespace quickhull { std::vector< std::unique_ptr> > m_disabledFacePointVectors; std::vector m_visibleFaces; std::vector m_horizonEdges; + std::vector tempPoints; struct FaceData { size_t m_faceIndex; size_t m_enteredFromHalfEdge; // If the face turns out not to be visible, @@ -125,6 +126,7 @@ namespace quickhull { // The public getConvexHull functions will setup a VertexDataSource object and call this ConvexHull getConvexHull(const VertexDataSource& pointCloud, bool CCW, bool useOriginalIndices, FloatType eps); + bool isMeshConvex(const std::vector& newFaces); public: // Computes convex hull for a given point cloud. // Params: @@ -194,11 +196,26 @@ namespace quickhull { template bool QuickHull::addPointToFace(typename MeshBuilder::Face& f, size_t pointIndex) { const T D = mathutils::getSignedDistanceToPlane(m_vertexData[ pointIndex ],f.m_P); + // if (D==0) + // { + // std::cout << "0point" << m_vertexData[pointIndex].x << " " << m_vertexData[pointIndex].y << " " << m_vertexData[pointIndex].z << std::endl; + // } + // if (D>m_epsilon && D*D > m_epsilonSquared*f.m_P.m_sqrNLength) { if (D>0 && D*D > m_epsilonSquared*f.m_P.m_sqrNLength) { + // if (D>m_epsilon) { + // std::cout << D << " " << m_epsilonSquared*f.m_P.m_sqrNLength << std::endl; + // std::cout << "Point" << m_vertexData[pointIndex].x << " " << m_vertexData[pointIndex].y << " " << m_vertexData[pointIndex].z << std::endl; if (!f.m_pointsOnPositiveSide) { f.m_pointsOnPositiveSide = std::move(getIndexVectorFromPool()); } f.m_pointsOnPositiveSide->push_back( pointIndex ); + // if (D == f.m_mostDistantPointDist) { + // std::cout << "Check"; + // std::cout << "Prior" << m_vertexData[f.m_mostDistantPoint].x << " " << m_vertexData[f.m_mostDistantPoint].y << " " << m_vertexData[f.m_mostDistantPoint].z << std::endl; + // std::cout << "New" << m_vertexData[pointIndex].x << " " << m_vertexData[pointIndex].y << " " << m_vertexData[pointIndex].z << std::endl; + // std::cout << "Normal" << f.m_P.m_N.x << " " << f.m_P.m_N.y << " " << f.m_P.m_N.z << std::endl; + + // } if (D > f.m_mostDistantPointDist) { f.m_mostDistantPointDist = D; f.m_mostDistantPoint = pointIndex; diff --git a/Structs/Plane.hpp b/Structs/Plane.hpp index 35ef352..e8b93a7 100644 --- a/Structs/Plane.hpp +++ b/Structs/Plane.hpp @@ -15,7 +15,7 @@ namespace quickhull { // Normal length squared T m_sqrNLength; - + Vector3 errorNormal; bool isPointOnPositiveSide(const Vector3& Q) const { T d = m_N.dotProduct(Q)+m_D; if (d>=0) return true;