From d4be363e7c7728697c0d1a1ccc36c0665c2a2757 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 30 Sep 2020 00:22:16 +0200 Subject: [PATCH] Optimization of face vs face clipping in SAT algorithm --- .../mathematics/mathematics_functions.h | 111 +++++++----------- .../narrowphase/SAT/SATAlgorithm.cpp | 53 ++++++--- 2 files changed, 79 insertions(+), 85 deletions(-) diff --git a/include/reactphysics3d/mathematics/mathematics_functions.h b/include/reactphysics3d/mathematics/mathematics_functions.h index 4f0e45bb..a163712a 100755 --- a/include/reactphysics3d/mathematics/mathematics_functions.h +++ b/include/reactphysics3d/mathematics/mathematics_functions.h @@ -327,94 +327,69 @@ RP3D_FORCE_INLINE Array clipSegmentWithPlanes(const Vector3& segA, cons return outputVertices; } -// Clip a polygon against multiple planes and return the clipped polygon vertices -// This method implements the Sutherland–Hodgman clipping algorithm -RP3D_FORCE_INLINE void clipPolygonWithPlanes(const Array& polygonVertices, const Array& planesPoints, - const Array& planesNormals, Array& outClippedPolygonVertices, - MemoryAllocator& allocator) { +// Clip a polygon against a single plane and return the clipped polygon vertices +// This method implements the Sutherland–Hodgman polygon clipping algorithm +RP3D_FORCE_INLINE void clipPolygonWithPlane(const Array& polygonVertices, const Vector3& planePoint, + const Vector3& planeNormal, Array& outClippedPolygonVertices) { - assert(planesPoints.size() == planesNormals.size()); - - const uint32 nbPlanesPoints = planesPoints.size(); - - const uint32 nbMaxElements = polygonVertices.size() * 2 * nbPlanesPoints; - Array vertices(allocator, nbMaxElements * nbPlanesPoints); - - outClippedPolygonVertices.reserve(nbMaxElements); - - vertices.addRange(polygonVertices); - - uint32 currentPolygonStartIndex = 0; uint32 nbInputVertices = polygonVertices.size(); - // For each clipping plane - for (uint32 p=0; p < nbPlanesPoints; p++) { + assert(outClippedPolygonVertices.size() == 0); - uint32 nextNbInputVertices = 0; + uint32 vStartIndex = nbInputVertices - 1; - uint32 vStart = currentPolygonStartIndex + nbInputVertices - 1; + const decimal planeNormalDotPlanePoint = planeNormal.dot(planePoint); - const decimal planeNormalDotPlanePoint = planesNormals[p].dot(planesPoints[p]); + decimal vStartDotN = (polygonVertices[vStartIndex] - planePoint).dot(planeNormal); - // For each edge of the polygon - for (uint vEnd = currentPolygonStartIndex; vEnd < currentPolygonStartIndex + nbInputVertices; vEnd++) { + // For each edge of the polygon + for (uint vEndIndex = 0; vEndIndex < nbInputVertices; vEndIndex++) { - Vector3& v1 = vertices[vStart]; - Vector3& v2 = vertices[vEnd]; + const Vector3& vStart = polygonVertices[vStartIndex]; + const Vector3& vEnd = polygonVertices[vEndIndex]; - const decimal v1DotN = (v1 - planesPoints[p]).dot(planesNormals[p]); - const decimal v2DotN = (v2 - planesPoints[p]).dot(planesNormals[p]); + const decimal vEndDotN = (vEnd - planePoint).dot(planeNormal); - // If the second vertex is in front of the clippling plane - if (v2DotN >= decimal(0.0)) { + // If the second vertex is in front of the clippling plane + if (vEndDotN >= decimal(0.0)) { - // If the first vertex is not in front of the clippling plane - if (v1DotN < decimal(0.0)) { + // If the first vertex is not in front of the clippling plane + if (vStartDotN < decimal(0.0)) { - // The second point we keep is the intersection between the segment v1, v2 and the clipping plane - const decimal t = computePlaneSegmentIntersection(v1, v2, planeNormalDotPlanePoint, planesNormals[p]); + // The second point we keep is the intersection between the segment v1, v2 and the clipping plane + const decimal t = computePlaneSegmentIntersection(vStart, vEnd, planeNormalDotPlanePoint, planeNormal); - if (t >= decimal(0) && t <= decimal(1.0)) { - vertices.add(v1 + t * (v2 - v1)); - nextNbInputVertices++; - } - else { - vertices.add(v2); - nextNbInputVertices++; - } + if (t >= decimal(0) && t <= decimal(1.0)) { + outClippedPolygonVertices.add(vStart + t * (vEnd - vStart)); } - - // Add the second vertex - vertices.add(v2); - nextNbInputVertices++; - } - else { // If the second vertex is behind the clipping plane - - // If the first vertex is in front of the clippling plane - if (v1DotN >= decimal(0.0)) { - - // The first point we keep is the intersection between the segment v1, v2 and the clipping plane - const decimal t = computePlaneSegmentIntersection(v1, v2, -planeNormalDotPlanePoint, -planesNormals[p]); - - if (t >= decimal(0.0) && t <= decimal(1.0)) { - vertices.add(v1 + t * (v2 - v1)); - nextNbInputVertices++; - } - else { - vertices.add(v1); - nextNbInputVertices++; - } + else { + outClippedPolygonVertices.add(vEnd); } } - vStart = vEnd; + // Add the second vertex + outClippedPolygonVertices.add(vEnd); + } + else { // If the second vertex is behind the clipping plane + + // If the first vertex is in front of the clippling plane + if (vStartDotN >= decimal(0.0)) { + + // The first point we keep is the intersection between the segment v1, v2 and the clipping plane + const decimal t = computePlaneSegmentIntersection(vStart, vEnd, -planeNormalDotPlanePoint, -planeNormal); + + if (t >= decimal(0.0) && t <= decimal(1.0)) { + outClippedPolygonVertices.add(vStart + t * (vEnd - vStart)); + } + else { + outClippedPolygonVertices.add(vStart); + } + } } - currentPolygonStartIndex += nbInputVertices; - nbInputVertices = nextNbInputVertices; + vStartIndex = vEndIndex; + vStartDotN = vEndDotN; } - - outClippedPolygonVertices.addRange(vertices, currentPolygonStartIndex); } // Project a point onto a plane that is given by a point and its unit length normal diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index aa7797bb..bfc32951 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -925,21 +925,28 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex); const uint32 nbIncidentFaceVertices = incidentFace.faceVertices.size(); - Array polygonVertices(mMemoryAllocator, nbIncidentFaceVertices); // Vertices to clip of the incident face - Array planesNormals(mMemoryAllocator, nbIncidentFaceVertices); // Normals of the clipping planes - Array planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes + const uint32 nbMaxElements = nbIncidentFaceVertices * 2 * referenceFace.faceVertices.size(); + Array verticesTemp1(mMemoryAllocator, nbMaxElements); + Array verticesTemp2(mMemoryAllocator, nbMaxElements); // Get all the vertices of the incident face (in the reference local-space) for (uint32 i=0; i < nbIncidentFaceVertices; i++) { const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(incidentFace.faceVertices[i]); - polygonVertices.add(incidentToReferenceTransform * faceVertexIncidentSpace); + verticesTemp1.add(incidentToReferenceTransform * faceVertexIncidentSpace); } - // Get the reference face clipping planes + // For each edge of the reference we use it to clip the incident face polygon using Sutherland-Hodgman algorithm uint32 currentEdgeIndex = referenceFace.edgeIndex; uint32 firstEdgeIndex = currentEdgeIndex; + Vector3 planeNormal; + Vector3 planePoint; + bool areVertices1Input = false; + uint32 nbOutputVertices; do { + // Switch the input/output arrays of vertices + areVertices1Input = !areVertices1Input; + // Get the adjacent edge const HalfEdgeStructure::Edge& edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); @@ -955,30 +962,42 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene // The clipping plane is perpendicular to the edge direction and the reference face normal Vector3 clipPlaneNormal = axisReferenceSpace.cross(edgeDirection); - planesNormals.add(clipPlaneNormal); - planesPoints.add(edgeV1); + planeNormal = clipPlaneNormal; + planePoint = edgeV1; + + assert(areVertices1Input && verticesTemp1.size() > 0 || !areVertices1Input); + assert(!areVertices1Input && verticesTemp2.size() > 0 || areVertices1Input); + + // Clip the incident face with one adjacent plane (corresponding to one edge) of the reference face + clipPolygonWithPlane(areVertices1Input ? verticesTemp1 : verticesTemp2, planePoint, planeNormal, areVertices1Input ? verticesTemp2 : verticesTemp1); // Go to the next adjacent edge of the reference face currentEdgeIndex = edge.nextEdgeIndex; - } while (currentEdgeIndex != firstEdgeIndex); + // Clear the input array of vertices before the next loop + if (areVertices1Input) { + verticesTemp1.clear(); + nbOutputVertices = verticesTemp2.size(); + } + else { + verticesTemp2.clear(); + nbOutputVertices = verticesTemp1.size(); + } - assert(planesNormals.size() > 0); - assert(planesNormals.size() == planesPoints.size()); + } while (currentEdgeIndex != firstEdgeIndex && nbOutputVertices > 0); - // Clip the reference faces with the adjacent planes of the reference face - Array clipPolygonVertices(mMemoryAllocator); - clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, clipPolygonVertices, mMemoryAllocator); + // Reference to the output clipped polygon vertices + Array& clippedPolygonVertices = areVertices1Input ? verticesTemp2 : verticesTemp1; // We only keep the clipped points that are below the reference face const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex); bool contactPointsFound = false; - const uint32 nbClipPolygonVertices = clipPolygonVertices.size(); + const uint32 nbClipPolygonVertices = clippedPolygonVertices.size(); for (uint32 i=0; i < nbClipPolygonVertices; i++) { // Compute the penetration depth of this contact point (can be different from the minPenetration depth which is // the maximal penetration depth of any contact point for this separating axis - decimal penetrationDepth = (referenceFaceVertex - clipPolygonVertices[i]).dot(axisReferenceSpace); + decimal penetrationDepth = (referenceFaceVertex - clippedPolygonVertices[i]).dot(axisReferenceSpace); // If the clip point is below the reference face if (penetrationDepth > decimal(0.0)) { @@ -991,10 +1010,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene Vector3 outWorldNormal = normalWorld; // Convert the clip incident polyhedron vertex into the incident polyhedron local-space - Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clipPolygonVertices[i]; + Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clippedPolygonVertices[i]; // Project the contact point onto the reference face - Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clipPolygonVertices[i], axisReferenceSpace, referenceFaceVertex); + Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clippedPolygonVertices[i], axisReferenceSpace, referenceFaceVertex); // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,