mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Optimization of face vs face clipping in SAT algorithm

Browse Source
This commit is contained in:
Daniel Chappuis 2020-09-30 00:22:16 +02:00
parent 21f9b39bc4
commit d4be363e7c
2 changed files with 79 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
include/reactphysics3d/mathematics/mathematics_functions.h
View File

@ -327,94 +327,69 @@ RP3D_FORCE_INLINE Array<Vector3> 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<Vector3>& polygonVertices, const Array<Vector3>& planesPoints,
const Array<Vector3>& planesNormals, Array<Vector3>& 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<Vector3>& polygonVertices, const Vector3& planePoint,
const Vector3& planeNormal, Array<Vector3>& outClippedPolygonVertices) {
assert(planesPoints.size() == planesNormals.size());
const uint32 nbPlanesPoints = planesPoints.size();
const uint32 nbMaxElements = polygonVertices.size() * 2 * nbPlanesPoints;
Array<Vector3> 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

53
src/collision/narrowphase/SAT/SATAlgorithm.cpp
View File

@ -925,21 +925,28 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
const uint32 nbIncidentFaceVertices = incidentFace.faceVertices.size();
Array<Vector3> polygonVertices(mMemoryAllocator, nbIncidentFaceVertices); // Vertices to clip of the incident face
Array<Vector3> planesNormals(mMemoryAllocator, nbIncidentFaceVertices); // Normals of the clipping planes
Array<Vector3> planesPoints(mMemoryAllocator, nbIncidentFaceVertices); // Points on the clipping planes
const uint32 nbMaxElements = nbIncidentFaceVertices * 2 * referenceFace.faceVertices.size();
Array<Vector3> verticesTemp1(mMemoryAllocator, nbMaxElements);
Array<Vector3> 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<Vector3> clipPolygonVertices(mMemoryAllocator);
clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals, clipPolygonVertices, mMemoryAllocator);
// Reference to the output clipped polygon vertices
Array<Vector3>& 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,