Do not compute contacts if not necessary
This commit is contained in:
Daniel Chappuis
parent
8b82c4ac81
commit
b6ad69b278
|
|
@ -91,33 +91,37 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
|
|||
// If the segments were overlapping (the clip segment is valid)
|
||||
if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
|
||||
|
||||
// Clip the inner segment of capsule 2
|
||||
if (t1 > decimal(1.0)) t1 = decimal(1.0);
|
||||
const Vector3 clipPointA = capsule2SegB - t1 * seg2;
|
||||
if (t2 > decimal(1.0)) t2 = decimal(1.0);
|
||||
const Vector3 clipPointB = capsule2SegA + t2 * seg2;
|
||||
if (reportContacts) {
|
||||
|
||||
// Project point capsule2SegA onto line of innner segment of capsule 1
|
||||
const Vector3 seg1Normalized = seg1.getUnit();
|
||||
Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
|
||||
// Clip the inner segment of capsule 2
|
||||
if (t1 > decimal(1.0)) t1 = decimal(1.0);
|
||||
const Vector3 clipPointA = capsule2SegB - t1 * seg2;
|
||||
if (t2 > decimal(1.0)) t2 = decimal(1.0);
|
||||
const Vector3 clipPointB = capsule2SegA + t2 * seg2;
|
||||
|
||||
// Compute a perpendicular vector from segment 1 to segment 2
|
||||
Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
|
||||
Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
|
||||
// Project point capsule2SegA onto line of innner segment of capsule 1
|
||||
const Vector3 seg1Normalized = seg1.getUnit();
|
||||
Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
|
||||
|
||||
Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
|
||||
const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
|
||||
const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
|
||||
const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
|
||||
const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
|
||||
// Compute a perpendicular vector from segment 1 to segment 2
|
||||
Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
|
||||
Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
|
||||
|
||||
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
|
||||
Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
|
||||
const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
|
||||
const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
|
||||
const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius();
|
||||
const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius();
|
||||
|
||||
decimal penetrationDepth = sumRadius - segmentsDistance;
|
||||
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
|
||||
decimal penetrationDepth = sumRadius - segmentsDistance;
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
|
||||
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
@ -137,18 +141,22 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
|
|||
// If the collision shapes overlap
|
||||
if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
|
||||
|
||||
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
|
||||
closestPointsSeg1ToSeg2 /= closestPointsDistance;
|
||||
if (reportContacts) {
|
||||
|
||||
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
|
||||
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
|
||||
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
|
||||
closestPointsSeg1ToSeg2 /= closestPointsDistance;
|
||||
|
||||
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
|
||||
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
|
||||
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
|
||||
|
||||
decimal penetrationDepth = sumRadius - closestPointsDistance;
|
||||
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
|
||||
decimal penetrationDepth = sumRadius - closestPointsDistance;
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
|
||||
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -55,70 +55,74 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
|
|||
// If we have found a contact point inside the margins (shallow penetration)
|
||||
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
|
||||
|
||||
// GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the
|
||||
// capsule inner segment and parallel to the contact point normal, we would like to create
|
||||
// two contact points instead of a single one (as in the deep contact case with SAT algorithm)
|
||||
if (reportContacts) {
|
||||
|
||||
// Get the contact point created by GJK
|
||||
ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
|
||||
assert(contactPoint != nullptr);
|
||||
// GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the
|
||||
// capsule inner segment and parallel to the contact point normal, we would like to create
|
||||
// two contact points instead of a single one (as in the deep contact case with SAT algorithm)
|
||||
|
||||
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
|
||||
// Get the contact point created by GJK
|
||||
ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
|
||||
assert(contactPoint != nullptr);
|
||||
|
||||
// Get the collision shapes
|
||||
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
|
||||
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
|
||||
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
|
||||
|
||||
// For each face of the polyhedron
|
||||
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
|
||||
// Get the collision shapes
|
||||
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
|
||||
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
|
||||
|
||||
// Get the face
|
||||
HalfEdgeStructure::Face face = polyhedron->getFace(f);
|
||||
// For each face of the polyhedron
|
||||
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
|
||||
|
||||
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
|
||||
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
|
||||
|
||||
// Get the face normal
|
||||
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
|
||||
const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
|
||||
|
||||
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
|
||||
|
||||
bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
|
||||
bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
|
||||
|
||||
// If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
|
||||
// is in direction of the contact normal (from the polyhedron point of view).
|
||||
if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld)
|
||||
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
|
||||
|
||||
// Remove the previous contact point computed by GJK
|
||||
narrowPhaseInfo->resetContactPoints();
|
||||
// Get the face
|
||||
HalfEdgeStructure::Face face = polyhedron->getFace(f);
|
||||
|
||||
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
|
||||
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
|
||||
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
|
||||
|
||||
// Compute the end-points of the inner segment of the capsule
|
||||
// Get the face normal
|
||||
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
|
||||
const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
|
||||
|
||||
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
|
||||
|
||||
// Convert the inner capsule segment points into the polyhedron local-space
|
||||
const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
|
||||
const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
|
||||
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
|
||||
bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
|
||||
bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
|
||||
|
||||
const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
|
||||
// If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
|
||||
// is in direction of the contact normal (from the polyhedron point of view).
|
||||
if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld)
|
||||
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
|
||||
|
||||
// Compute and create two contact points
|
||||
satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
|
||||
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
|
||||
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
|
||||
narrowPhaseInfo, isCapsuleShape1);
|
||||
// Remove the previous contact point computed by GJK
|
||||
narrowPhaseInfo->resetContactPoints();
|
||||
|
||||
break;
|
||||
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
|
||||
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
|
||||
|
||||
// Compute the end-points of the inner segment of the capsule
|
||||
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
|
||||
|
||||
// Convert the inner capsule segment points into the polyhedron local-space
|
||||
const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
|
||||
const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
|
||||
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
|
||||
|
||||
const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
|
||||
|
||||
// Compute and create two contact points
|
||||
satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
|
||||
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
|
||||
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
|
||||
narrowPhaseInfo, isCapsuleShape1);
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
|
||||
|
|
|
|||
|
|
@ -83,8 +83,6 @@ class NarrowPhaseAlgorithm {
|
|||
/// Deleted assignment operator
|
||||
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
|
||||
|
||||
// TODO : Use the following reportContacts variable in all narrow-phase algorithms
|
||||
|
||||
/// Compute a contact info if the two bounding volume collide
|
||||
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts)=0;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -139,15 +139,18 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
|
|||
}
|
||||
}
|
||||
|
||||
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
|
||||
Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
|
||||
const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
|
||||
const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
|
||||
if (reportContacts) {
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
|
||||
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
|
||||
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
|
||||
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
|
||||
Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal);
|
||||
const Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius();
|
||||
const Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
|
||||
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
|
||||
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
|
||||
}
|
||||
|
||||
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
|
||||
|
||||
|
|
@ -384,34 +387,39 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
|
|||
// We need to clip the inner capsule segment with the adjacent faces of the separating face
|
||||
if (isMinPenetrationFaceNormal) {
|
||||
|
||||
computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
|
||||
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
|
||||
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
|
||||
narrowPhaseInfo, isCapsuleShape1);
|
||||
if (reportContacts) {
|
||||
|
||||
computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
|
||||
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
|
||||
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
|
||||
narrowPhaseInfo, isCapsuleShape1);
|
||||
}
|
||||
|
||||
lastFrameInfo.satIsAxisFacePolyhedron1 = true;
|
||||
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
|
||||
}
|
||||
else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
|
||||
|
||||
// Compute the closest points between the inner capsule segment and the
|
||||
// edge of the polyhedron in polyhedron local-space
|
||||
Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment;
|
||||
computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
|
||||
separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2,
|
||||
closestPointCapsuleInnerSegment, closestPointPolyhedronEdge);
|
||||
if (reportContacts) {
|
||||
|
||||
// Compute the closest points between the inner capsule segment and the
|
||||
// edge of the polyhedron in polyhedron local-space
|
||||
Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment;
|
||||
computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
|
||||
separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2,
|
||||
closestPointCapsuleInnerSegment, closestPointPolyhedronEdge);
|
||||
|
||||
// Project closest capsule inner segment point into the capsule bounds
|
||||
const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
|
||||
// Project closest capsule inner segment point into the capsule bounds
|
||||
const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
|
||||
|
||||
// Create the contact point
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
|
||||
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
|
||||
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
|
||||
// Create the contact point
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
|
||||
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
|
||||
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
|
||||
}
|
||||
|
||||
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
|
||||
lastFrameInfo.satMinEdge1Index = minEdgeIndex;
|
||||
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
|
||||
lastFrameInfo.satMinEdge1Index = minEdgeIndex;
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
@ -543,8 +551,7 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
|
|||
}
|
||||
|
||||
// Test collision between two convex polyhedrons
|
||||
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo,
|
||||
bool reportContacts) const {
|
||||
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const {
|
||||
|
||||
PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()");
|
||||
|
||||
|
|
@ -787,91 +794,93 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
|
|||
// If the minimum separating axis is a face normal
|
||||
if (isMinPenetrationFaceNormal) {
|
||||
|
||||
const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
|
||||
const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1;
|
||||
const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
|
||||
const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
|
||||
if (reportContacts) {
|
||||
|
||||
assert(minPenetrationDepth > decimal(0.0));
|
||||
const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
|
||||
const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1;
|
||||
const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
|
||||
const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
|
||||
|
||||
const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
|
||||
const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
|
||||
assert(minPenetrationDepth > decimal(0.0));
|
||||
|
||||
// Compute the world normal
|
||||
const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
|
||||
-(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
|
||||
const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
|
||||
const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
|
||||
|
||||
// Get the reference face
|
||||
HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex);
|
||||
// Compute the world normal
|
||||
const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfo->shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
|
||||
-(narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
|
||||
|
||||
// Find the incident face on the other polyhedron (most anti-parallel face)
|
||||
uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
|
||||
// Get the reference face
|
||||
HalfEdgeStructure::Face referenceFace = referencePolyhedron->getFace(minFaceIndex);
|
||||
|
||||
// Get the incident face
|
||||
HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
|
||||
// Find the incident face on the other polyhedron (most anti-parallel face)
|
||||
uint incidentFaceIndex = findMostAntiParallelFaceOnPolyhedron(incidentPolyhedron, axisIncidentSpace);
|
||||
|
||||
std::vector<Vector3> polygonVertices; // Vertices to clip of the incident face
|
||||
std::vector<Vector3> planesNormals; // Normals of the clipping planes
|
||||
std::vector<Vector3> planesPoints; // Points on the clipping planes
|
||||
// Get the incident face
|
||||
HalfEdgeStructure::Face incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
|
||||
|
||||
// Get all the vertices of the incident face (in the reference local-space)
|
||||
std::vector<uint>::const_iterator it;
|
||||
for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
|
||||
const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
|
||||
polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace);
|
||||
}
|
||||
std::vector<Vector3> polygonVertices; // Vertices to clip of the incident face
|
||||
std::vector<Vector3> planesNormals; // Normals of the clipping planes
|
||||
std::vector<Vector3> planesPoints; // Points on the clipping planes
|
||||
|
||||
// Get the reference face clipping planes
|
||||
uint currentEdgeIndex = referenceFace.edgeIndex;
|
||||
uint firstEdgeIndex = currentEdgeIndex;
|
||||
do {
|
||||
// Get all the vertices of the incident face (in the reference local-space)
|
||||
std::vector<uint>::const_iterator it;
|
||||
for (it = incidentFace.faceVertices.begin(); it != incidentFace.faceVertices.end(); ++it) {
|
||||
const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(*it);
|
||||
polygonVertices.push_back(incidentToReferenceTransform * faceVertexIncidentSpace);
|
||||
}
|
||||
|
||||
// Get the adjacent edge
|
||||
HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
|
||||
// Get the reference face clipping planes
|
||||
uint currentEdgeIndex = referenceFace.edgeIndex;
|
||||
uint firstEdgeIndex = currentEdgeIndex;
|
||||
do {
|
||||
|
||||
// Get the twin edge
|
||||
HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
|
||||
// Get the adjacent edge
|
||||
HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex);
|
||||
|
||||
// Get the adjacent face normal (and negate it to have a clipping plane)
|
||||
Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex);
|
||||
// Get the twin edge
|
||||
HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
|
||||
|
||||
// Get a vertex of the clipping plane (vertex of the adjacent edge)
|
||||
Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex);
|
||||
// Get the adjacent face normal (and negate it to have a clipping plane)
|
||||
Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex);
|
||||
|
||||
planesNormals.push_back(faceNormal);
|
||||
planesPoints.push_back(faceVertex);
|
||||
// Get a vertex of the clipping plane (vertex of the adjacent edge)
|
||||
Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex);
|
||||
|
||||
// Go to the next adjacent edge of the reference face
|
||||
currentEdgeIndex = edge.nextEdgeIndex;
|
||||
planesNormals.push_back(faceNormal);
|
||||
planesPoints.push_back(faceVertex);
|
||||
|
||||
} while (currentEdgeIndex != firstEdgeIndex);
|
||||
// Go to the next adjacent edge of the reference face
|
||||
currentEdgeIndex = edge.nextEdgeIndex;
|
||||
|
||||
assert(planesNormals.size() > 0);
|
||||
assert(planesNormals.size() == planesPoints.size());
|
||||
} while (currentEdgeIndex != firstEdgeIndex);
|
||||
|
||||
// Clip the reference faces with the adjacent planes of the reference face
|
||||
std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
|
||||
assert(clipPolygonVertices.size() > 0);
|
||||
assert(planesNormals.size() > 0);
|
||||
assert(planesNormals.size() == planesPoints.size());
|
||||
|
||||
// We only keep the clipped points that are below the reference face
|
||||
const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
|
||||
std::vector<Vector3>::const_iterator itPoints;
|
||||
for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
|
||||
// Clip the reference faces with the adjacent planes of the reference face
|
||||
std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
|
||||
assert(clipPolygonVertices.size() > 0);
|
||||
|
||||
// If the clip point is bellow the reference face
|
||||
if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0))
|
||||
{
|
||||
// We only keep the clipped points that are below the reference face
|
||||
const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
|
||||
std::vector<Vector3>::const_iterator itPoints;
|
||||
for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
|
||||
|
||||
// Convert the clip incident polyhedron vertex into the incident polyhedron local-space
|
||||
const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
|
||||
// If the clip point is bellow the reference face
|
||||
if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
|
||||
|
||||
// Project the contact point onto the reference face
|
||||
Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
|
||||
// Convert the clip incident polyhedron vertex into the incident polyhedron local-space
|
||||
const Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
|
||||
|
||||
// Create a new contact point
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
|
||||
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
|
||||
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
|
||||
// Project the contact point onto the reference face
|
||||
Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(*itPoints, axisReferenceSpace, referenceFaceVertex);
|
||||
|
||||
// Create a new contact point
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
|
||||
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
|
||||
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -881,20 +890,23 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
|
|||
}
|
||||
else { // If we have an edge vs edge contact
|
||||
|
||||
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
|
||||
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
|
||||
computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B,
|
||||
closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
|
||||
if (reportContacts) {
|
||||
|
||||
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
|
||||
const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
|
||||
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
|
||||
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
|
||||
computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B,
|
||||
closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
|
||||
|
||||
// Compute the world normal
|
||||
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
|
||||
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
|
||||
const Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
|
||||
|
||||
// Create the contact point
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
|
||||
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
|
||||
// Compute the world normal
|
||||
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
|
||||
|
||||
// Create the contact point
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
|
||||
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
|
||||
}
|
||||
|
||||
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
|
||||
lastFrameInfo.satIsAxisFacePolyhedron2 = false;
|
||||
|
|
|
|||
|
|
@ -71,25 +71,28 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b
|
|||
// If the collision shapes overlap
|
||||
if (sphereSegmentDistanceSquare < sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) {
|
||||
|
||||
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
|
||||
sphereCenterToSegment /= sphereSegmentDistance;
|
||||
if (reportContacts) {
|
||||
|
||||
const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
|
||||
const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
|
||||
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
|
||||
sphereCenterToSegment /= sphereSegmentDistance;
|
||||
|
||||
Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
|
||||
const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
|
||||
const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
|
||||
|
||||
decimal penetrationDepth = sumRadius - sphereSegmentDistance;
|
||||
Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
|
||||
|
||||
if (!isSphereShape1) {
|
||||
normalWorld = -normalWorld;
|
||||
decimal penetrationDepth = sumRadius - sphereSegmentDistance;
|
||||
|
||||
if (!isSphereShape1) {
|
||||
normalWorld = -normalWorld;
|
||||
}
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
|
||||
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
|
||||
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
|
||||
}
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
|
||||
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
|
||||
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -52,17 +52,21 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bo
|
|||
|
||||
// If the sphere collision shapes intersect
|
||||
if (squaredDistanceBetweenCenters < sumRadius * sumRadius) {
|
||||
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
|
||||
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
|
||||
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
|
||||
centerSphere2InBody1LocalSpace.getUnit();
|
||||
Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
|
||||
centerSphere1InBody2LocalSpace.getUnit();
|
||||
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
|
||||
intersectionOnBody1, intersectionOnBody2);
|
||||
if (reportContacts) {
|
||||
|
||||
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
|
||||
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
|
||||
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
|
||||
centerSphere2InBody1LocalSpace.getUnit();
|
||||
Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
|
||||
centerSphere1InBody2LocalSpace.getUnit();
|
||||
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
|
||||
|
||||
// Create the contact info object
|
||||
narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
|
||||
intersectionOnBody1, intersectionOnBody2);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user