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

Fix issue when computing clipping planes in SAT algorithm

Browse Source
This commit is contained in:
Daniel Chappuis 2017-09-07 22:24:30 +02:00
parent 1b82a3e228
commit b890773053
1 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -256,7 +256,6 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1); narrowPhaseInfo, isCapsuleShape1);
} }
} }
else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
@ -352,6 +351,10 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const { NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const {
HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex); HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
// Get the face normal
Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
uint firstEdgeIndex = face.edgeIndex; uint firstEdgeIndex = face.edgeIndex;
uint edgeIndex = firstEdgeIndex; uint edgeIndex = firstEdgeIndex;
@ -360,12 +363,22 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// For each adjacent edge of the separating face of the polyhedron // For each adjacent edge of the separating face of the polyhedron
do { do {
HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex); HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex);
HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
// Construct a clippling plane for each adjacent edge of the separting face of the polyhedron // Compute the edge vertices and edge direction
Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex);
Vector3 edgeV2 = polyhedron->getVertexPosition(twinEdge.vertexIndex);
Vector3 edgeDirection = edgeV2 - edgeV1;
// Compute the normal of the clipping plane for this edge
// The clipping plane is perpendicular to the edge direction and the reference face normal
Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection);
// Construct a clipping plane for each adjacent edge of the separating face of the polyhedron
planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex)); planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex));
planesNormals.push_back(-polyhedron->getFaceNormal(twinEdge.faceIndex)); planesNormals.push_back(clipPlaneNormal);
edgeIndex = edge.nextEdgeIndex; edgeIndex = edge.nextEdgeIndex;
@ -373,9 +386,7 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// First we clip the inner segment of the capsule with the four planes of the adjacent faces // First we clip the inner segment of the capsule with the four planes of the adjacent faces
std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals); std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
assert(clipSegment.size() == 2); assert(clipSegment.size() == 2);
const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
// Project the two clipped points into the polyhedron face // Project the two clipped points into the polyhedron face
const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);