Working on Quickhull algorithm

DanielChappuis · DanielChappuis · commit 90062aeced21 · 2022-12-26T21:03:22.000+01:00
diff --git a/include/reactphysics3d/collision/ConvexMesh.h b/include/reactphysics3d/collision/ConvexMesh.h
@@ -87,8 +87,8 @@ class ConvexMesh {
         /// Compute the faces normals
         bool computeFacesNormals(std::vector<Error>& errors);
 
-        /// Compute and return the area of a face
-        decimal getFaceArea(uint32 faceIndex) const;
+        /// Compute and return the face normal (not normalized)
+        Vector3 computeFaceNormal(uint32 faceIndex) const;
 
         /// Static factory method to create a convex mesh
         static ConvexMesh* create(MemoryAllocator& allocator);
diff --git a/include/reactphysics3d/collision/HalfEdgeStructure.h b/include/reactphysics3d/collision/HalfEdgeStructure.h
@@ -59,7 +59,7 @@ class HalfEdgeStructure {
             Face(MemoryAllocator& allocator) : edgeIndex(0), faceVertices(allocator) {}
 
             /// Constructor
-            Face(const Array<uint32>& vertices) : edgeIndex(0), faceVertices(vertices) {}
+            Face(const Array<uint32>& vertices) : edgeIndex(0), faceVertices(vertices) {}    
         };
 
         /// Vertex
diff --git a/include/reactphysics3d/utils/quickhull/QHHalfEdgeStructure.h b/include/reactphysics3d/utils/quickhull/QHHalfEdgeStructure.h
@@ -96,14 +96,15 @@ class QHHalfEdgeStructure {
             Edge* edge;             // One half-edge of the face
             Vector3 normal;
             Vector3 centroid;         // Center of the face (average of the face vertices)
+            decimal area;             // Area of the face
             Array<uint32> conflictPoints;   // Array with some remaining points visible from this face that need to be processed
 
             // TODO : DELETE THIS
             Set<uint32> vertices;
 
             /// Constructor
-            Face(const Vector3& normal, MemoryAllocator& allocator)
-                : nextFace(nullptr), previousFace(nullptr), normal(normal), conflictPoints(allocator, 8), vertices(allocator) {
+            Face(MemoryAllocator& allocator)
+                : nextFace(nullptr), previousFace(nullptr), normal(0, 0, 0), area(0), conflictPoints(allocator, 8), vertices(allocator) {
 
             }
 
@@ -112,6 +113,47 @@ class QHHalfEdgeStructure {
                 return edge->startVertex;
             }
 
+            // Recalculate the face centroid and normal to better fit its new vertices (using Newell method)
+            void recalculateFace(const Array<Vector3>& points) {
+
+                centroid.setToZero();
+                normal.setToZero();
+                uint32 nbVertices = 0;
+
+                //std::cout << "Recalculate Face" << std::endl;
+                //std::cout << "Previous centroid:" << face->centroid.to_string() << std::endl;
+                //std::cout << "Previous normal:" << face->normal.to_string() << std::endl;
+
+                // For each vertex of the face
+                const QHHalfEdgeStructure::Edge* firstFaceEdge = edge;
+                const QHHalfEdgeStructure::Edge* faceEdge = firstFaceEdge;
+                do {
+
+                    const Vector3 v1 = points[faceEdge->startVertex->externalIndex];
+                    const Vector3 v2 = points[faceEdge->endVertex->externalIndex];
+                    centroid += v1;
+                    normal += Vector3((v1.y - v2.y) * (v1.z + v2.z),
+                                      (v1.z - v2.z) * (v1.x + v2.x),
+                                      (v1.x - v2.x) * (v1.y + v2.y));
+
+                    nbVertices++;
+
+                   faceEdge = faceEdge->nextFaceEdge;
+
+                } while(faceEdge != firstFaceEdge);
+
+                assert(nbVertices > 0);
+
+                centroid = centroid / nbVertices;
+                const decimal normalLength = normal.length();
+                assert(normalLength > 0);
+                normal = normal / normalLength;
+                area = normalLength * decimal(0.5);
+
+                //std::cout << "New centroid:" << face->centroid.to_string() << std::endl;
+                //std::cout << "New normal:" << face->normal.to_string() << std::endl;
+            }
+
             // Return a string with the vertices of the face
             std::string verticesString() const {
 
@@ -161,54 +203,6 @@ class QHHalfEdgeStructure {
                 return edge->nextFaceEdge->nextFaceEdge->nextFaceEdge == edge;
             }
 
-            // TODO: DELETE THIS
-            decimal computeArea(const Array<Vector3>& points) const {
-
-                decimal area = 0.0;
-
-                Vector3 v1 = points[edge->startVertex->externalIndex];
-
-                // For each vertex of the face
-                const QHHalfEdgeStructure::Edge* firstFaceEdge = edge;
-                const QHHalfEdgeStructure::Edge* faceEdge = firstFaceEdge->nextFaceEdge;
-                do {
-
-                   Vector3 v2 = points[faceEdge->startVertex->externalIndex];
-                   Vector3 v3 = points[faceEdge->endVertex->externalIndex];
-
-                   area += (v3 - v1).cross(v2 - v1).length();
-
-                   faceEdge = faceEdge->nextFaceEdge;
-
-                } while(faceEdge->nextFaceEdge != firstFaceEdge);
-
-                return area * decimal(0.5);
-            }
-
-            // Compute the centroid of a face (the average of face vertices)
-            void computeCentroid(const Array<Vector3>& points) {
-
-                decimal nbVertices = 0;
-
-                // For each vertex of the face
-                const QHHalfEdgeStructure::Edge* firstFaceEdge = edge;
-                const QHHalfEdgeStructure::Edge* faceEdge = firstFaceEdge;
-                do {
-
-                   QHHalfEdgeStructure::Vertex* vertex = faceEdge->startVertex;
-
-                   centroid += points[vertex->externalIndex];
-
-                   faceEdge = faceEdge->nextFaceEdge;
-
-                   nbVertices++;
-
-                } while(faceEdge != firstFaceEdge);
-
-                assert(nbVertices > 0);
-                centroid /= nbVertices;
-            }
-
             // Return true if the face structure is valid (for debugging purpose)
             bool isValid() {
                bool isValid = true;
@@ -313,7 +307,7 @@ class QHHalfEdgeStructure {
         Vertex* addVertex(uint32 externalIndex);
 
         /// Add a face
-        Face* addFace(const Array<Vertex*>& faceVertices, const Vector3& normal, const Array<Vector3>& points, MemoryAllocator& allocator);
+        Face* addFace(const Array<Vertex*>& faceVertices, const Array<Vector3>& points, MemoryAllocator& allocator);
 
         /// Remove a face
         void removeFace(Face* face);
diff --git a/include/reactphysics3d/utils/quickhull/QuickHull.h b/include/reactphysics3d/utils/quickhull/QuickHull.h
@@ -106,7 +106,11 @@ class QuickHull {
                                 Array<QHHalfEdgeStructure::Face*>& outVisibleFaces,
                                 decimal epsilon);
 
-        /// Iterate over all new faces and fix faces that are forming a concave shape in order to always keep the hull convex
+        /// Fix faces that are forming a concave or coplanar shape (by giving priority to large faces)
+        static void mergeLargeConcaveFaces(QHHalfEdgeStructure& convexHull, Array<QHHalfEdgeStructure::Face*>& newFaces,
+                                          const Array<Vector3>& points, decimal epsilon, Set<QHHalfEdgeStructure::Face*>& deletedFaces);
+
+        /// Fix faces that are forming a concave or coplanar shape
         static void mergeConcaveFaces(QHHalfEdgeStructure& convexHull, Array<QHHalfEdgeStructure::Face*>& newFaces,
                                       const Array<Vector3>& points, decimal epsilon,
                                       Set<QHHalfEdgeStructure::Face*>& deletedFaces);
@@ -124,9 +128,6 @@ class QuickHull {
                                               QHHalfEdgeStructure::Edge* inEdge, const Array<Vector3>& points,
                                               Set<QHHalfEdgeStructure::Face*>& deletedFaces);
 
-        /// Recalculate the face centroid and normal to better fit its new vertices (using Newell method)
-        static void recalculateFace(QHHalfEdgeStructure::Face* face, const Array<Vector3>& points);
-
         /// Remove duplicated vertices in the input array of points
         static void removeDuplicatedVertices(Array<Vector3>& points, MemoryAllocator& allocator);
 
@@ -161,10 +162,6 @@ class QuickHull {
                                                    Array<PolygonVertexArray::PolygonFace>& outFaces,
                                                    MemoryAllocator& allocator);
 
-        // TODO : Remove this
-        static std::string showMap(Map<const QHHalfEdgeStructure::Face*, Array<uint32>>& mapFaceIndexToRemainingClosestPoints);
-
-
     public:
 
         // -------------------- Methods -------------------- //
diff --git a/src/collision/ConvexMesh.cpp b/src/collision/ConvexMesh.cpp
@@ -82,15 +82,22 @@ bool ConvexMesh::copyVertices(const PolygonVertexArray& polygonVertexArray, std:
     bool isValid = true;
 
     mCentroid.setToZero();
-    mMinBounds = polygonVertexArray.getVertex(0);
-    mMaxBounds = polygonVertexArray.getVertex(0);
 
+    if (polygonVertexArray.getNbVertices() > 0) {
+        mMinBounds = polygonVertexArray.getVertex(0);
+        mMaxBounds = polygonVertexArray.getVertex(0);
+    }
+
+    // For each vertex
     for (uint32 i=0 ; i < polygonVertexArray.getNbVertices(); i++) {
 
         const Vector3 vertex = polygonVertexArray.getVertex(i);
         mVertices.add(vertex);
+
+        // Compute centroid
         mCentroid += vertex;
 
+        // Compute mesh bounds
         if (vertex.x > mMaxBounds.x) mMaxBounds.x = vertex.x;
         if (vertex.x < mMinBounds.x) mMinBounds.x = vertex.x;
 
@@ -199,14 +206,29 @@ bool ConvexMesh::computeFacesNormals(std::vector<Error>& errors) {
 
         if (face.faceVertices.size() >= 3) {
 
-            const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]);
-            const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]);
-            mFacesNormals.add(vec1.cross(vec2));
+            auto test0 = face.faceVertices[0];
+            auto test1 = face.faceVertices[1];
+            auto test2 = face.faceVertices[2];
+
+            auto v0 = getVertex(face.faceVertices[0]);
+            auto v1 = getVertex(face.faceVertices[1]);
+            auto v2 = getVertex(face.faceVertices[2]);
+            auto v3 = Vector3(0, 0, 0);
+            if (face.faceVertices.size() > 3) {
+
+                v3 = getVertex(face.faceVertices[3]);
+            }
+
+            mFacesNormals.add(computeFaceNormal(f));
+            decimal normalLength = mFacesNormals[f].length();
+
+            // TODO : DELETE THIS
+            const decimal abc = mFacesNormals[f].length();
 
             // TODO : Do not use MACHINE_EPSILON here
-            if (mFacesNormals[f].lengthSquare() > MACHINE_EPSILON) {
+            if (normalLength > MACHINE_EPSILON) {
 
-                mFacesNormals[f].normalize();
+                mFacesNormals[f] /= normalLength;
             }
             else {
                isValid = false;
@@ -218,30 +240,26 @@ bool ConvexMesh::computeFacesNormals(std::vector<Error>& errors) {
     return isValid;
 }
 
-// Compute and return the area of a face
-decimal ConvexMesh::getFaceArea(uint32 faceIndex) const {
+// Compute and return the face normal (not normalized)
+Vector3 ConvexMesh::computeFaceNormal(uint32 faceIndex) const {
 
-    Vector3 sumCrossProducts(0, 0, 0);
+    Vector3 normal(0, 0, 0);
 
     const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(faceIndex);
     assert(face.faceVertices.size() >= 3);
 
-    const Vector3& v1 = getVertex(face.faceVertices[0]);
-
-    // For each vertex of the face
-    const uint32 nbFaceVertices = static_cast<uint32>(face.faceVertices.size());
-    for (uint32 i=2; i < nbFaceVertices; i++) {
-
-        const Vector3& v2 = getVertex(face.faceVertices[i-1]);
-        const Vector3& v3 = getVertex(face.faceVertices[i]);
+    // Use Newell's method to compute the face normal
+    for (uint32 i = face.faceVertices.size() - 1, j = 0; j < face.faceVertices.size(); i = j, j++) {
 
-        const Vector3 v1v2 = v2 - v1;
-        const Vector3 v1v3 = v3 - v1;
+        const Vector3& v1 = getVertex(face.faceVertices[i]);
+        const Vector3& v2 = getVertex(face.faceVertices[j]);
 
-        sumCrossProducts +=  v1v2.cross(v1v3);
+        normal += Vector3((v1.y - v2.y) * (v1.z + v2.z),
+                          (v1.z - v2.z) * (v1.x + v2.x),
+                          (v1.x - v2.x) * (v1.y + v2.y));
     }
 
-    return decimal(0.5) * sumCrossProducts.length();
+    return normal;
 }
 
 // Compute and return the volume of the convex mesh
@@ -254,7 +272,7 @@ decimal ConvexMesh::getVolume() const {
     for (uint32 f=0; f < getNbFaces(); f++) {
 
         const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(f);
-        const decimal faceArea = getFaceArea(f);
+        const decimal faceArea = computeFaceNormal(f).length();
         const Vector3 faceNormal = mFacesNormals[f];
         const Vector3& faceVertex = getVertex(face.faceVertices[0]);
 
diff --git a/src/utils/quickhull/QHHalfEdgeStructure.cpp b/src/utils/quickhull/QHHalfEdgeStructure.cpp
@@ -96,13 +96,13 @@ QHHalfEdgeStructure::Vertex* QHHalfEdgeStructure::addVertex(uint32 externalIndex
  * @param faceVertices Array of the vertices in a face (ordered in CCW order as seen from outside
  *                     the polyhedron). The indices are the internal indices of the vertices inside the HalfEdgeStructure.
  */
-QHHalfEdgeStructure::Face* QHHalfEdgeStructure::addFace(const Array<Vertex*>& faceVertices, const Vector3& normal, const Array<Vector3>& points,
+QHHalfEdgeStructure::Face* QHHalfEdgeStructure::addFace(const Array<Vertex*>& faceVertices, const Array<Vector3>& points,
                                                         MemoryAllocator& allocator) {
 
     assert(faceVertices.size() >= 3);
 
     // Create a new face
-    Face* face = new (mAllocator.allocate(sizeof(Face))) Face(normal, allocator);
+    Face* face = new (mAllocator.allocate(sizeof(Face))) Face(allocator);
 
     Edge* prevFaceEdge = nullptr;
     Edge* firstFaceEdge = nullptr;
@@ -161,7 +161,8 @@ QHHalfEdgeStructure::Face* QHHalfEdgeStructure::addFace(const Array<Vertex*>& fa
 
     mNbFaces++;
 
-    face->computeCentroid(points);
+    // Compute the normal, area and centroid
+    face->recalculateFace(points);
 
     return face;
 }
@@ -329,6 +330,7 @@ bool QHHalfEdgeStructure::isValid() const {
        isValid &= (face->previousFace == nullptr || face->previousFace == previousFace);
        isValid &= (previousFace == nullptr || previousFace->nextFace == face);
        isValid &= face->isValid();
+       isValid &= face->area > 0.00001;
        nbFaces++;
     };
     isValid &= nbFaces > 0 || mFaces == nullptr;
diff --git a/src/utils/quickhull/QuickHull.cpp b/src/utils/quickhull/QuickHull.cpp
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp
diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.h b/testbed/scenes/collisionshapes/CollisionShapesScene.h
