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

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

@ -256,7 +256,6 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1);
}
}
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 {
HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
// Get the face normal
Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
uint firstEdgeIndex = face.edgeIndex;
uint edgeIndex = firstEdgeIndex;
@ -360,12 +363,22 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// For each adjacent edge of the separating face of the polyhedron
do {
HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex);
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));
planesNormals.push_back(-polyhedron->getFaceNormal(twinEdge.faceIndex));
planesNormals.push_back(clipPlaneNormal);
edgeIndex = edge.nextEdgeIndex;
@ -375,8 +388,6 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
assert(clipSegment.size() == 2);
const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
// Project the two clipped points into the polyhedron face
const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);