Fix issues with smooth triangle contact

This commit is contained in:
Daniel Chappuis 2017-09-21 22:44:42 +02:00
parent 63833621a0
commit 643c781fa0
3 changed files with 22 additions and 20 deletions

View File

@ -857,6 +857,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
contactPointsFound = true; contactPointsFound = true;
Vector3 outWorldNormal = normalWorld;
// Convert the clip incident polyhedron vertex into the incident polyhedron local-space // Convert the clip incident polyhedron vertex into the incident polyhedron local-space
Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
@ -868,10 +870,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
minPenetrationDepth, normalWorld); minPenetrationDepth, outWorldNormal);
// Create a new contact point // Create a new contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, narrowPhaseInfo->addContactPoint(outWorldNormal, minPenetrationDepth,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
} }

View File

@ -85,14 +85,9 @@ void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape
triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2, triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2,
isShape1Triangle ? shape1ToWorld : shape2ToWorld, isShape1Triangle ? shape1ToWorld : shape2ToWorld,
isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(), isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(),
penetrationDepth, penetrationDepth, isShape1Triangle,
isShape1Triangle ? localContactPointShape2 : localContactPointShape1, isShape1Triangle ? localContactPointShape2 : localContactPointShape1,
outSmoothVertexNormal); outSmoothVertexNormal);
// Make sure the direction of the contact normal is from shape1 to shape2
if (!isShape1Triangle) {
outSmoothVertexNormal = -outSmoothVertexNormal;
}
} }
} }
@ -104,25 +99,30 @@ void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape
// stay aligned with the new contact normal. This method will return the new smooth world contact // stay aligned with the new contact normal. This method will return the new smooth world contact
// normal of the triangle and the the local contact point on the other shape. // normal of the triangle and the the local contact point on the other shape.
void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
const Transform& worldToOtherShapeTransform, decimal penetrationDepth, const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1,
Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const { Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const {
// Get the smooth contact normal of the mesh at the contact point on the triangle // Get the smooth contact normal of the mesh at the contact point on the triangle
Vector3 localNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle); Vector3 triangleLocalNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle);
// Convert the local contact normal into world-space // Convert the local contact normal into world-space
outSmoothWorldContactTriangleNormal = triangleShapeToWorldTransform.getOrientation() * localNormal; Vector3 triangleWorldNormal = triangleShapeToWorldTransform.getOrientation() * triangleLocalNormal;
// Convert the contact normal into the local-space of the other shape // Penetration axis with direction from triangle to other shape
Vector3 normalOtherShape = worldToOtherShapeTransform.getOrientation() * outSmoothWorldContactTriangleNormal; Vector3 triangleToOtherShapePenAxis = isTriangleShape1 ? outSmoothWorldContactTriangleNormal : -outSmoothWorldContactTriangleNormal;
// Convert the local contact point of the triangle into the local-space of the other shape // The triangle normal should be the one in the direction out of the current colliding face of the triangle
Vector3 trianglePointOtherShape = worldToOtherShapeTransform * triangleShapeToWorldTransform * if (triangleWorldNormal.dot(triangleToOtherShapePenAxis) < decimal(0.0)) {
localContactPointTriangle; triangleWorldNormal = -triangleWorldNormal;
triangleLocalNormal = -triangleLocalNormal;
}
// Re-align the local contact point on the other shape such that it is aligned along // Compute the final contact normal from shape 1 to shape 2
// the new contact normal outSmoothWorldContactTriangleNormal = isTriangleShape1 ? triangleWorldNormal : -triangleWorldNormal;
Vector3 otherShapePoint = trianglePointOtherShape - normalOtherShape * penetrationDepth;
// Re-align the local contact point on the other shape such that it is aligned along the new contact normal
Vector3 otherShapePointTriangleSpace = localContactPointTriangle - triangleLocalNormal * penetrationDepth;
Vector3 otherShapePoint = worldToOtherShapeTransform * triangleShapeToWorldTransform * otherShapePointTriangleSpace;
outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z); outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z);
} }

View File

@ -99,7 +99,7 @@ class TriangleShape : public ConvexPolyhedronShape {
/// This method implements the technique described in Game Physics Pearl book /// This method implements the technique described in Game Physics Pearl book
void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
const Transform& worldToOtherShapeTransform, decimal penetrationDepth, const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1,
Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const; Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const;
public: public: