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

Fix issues in collision detection

Browse Source
This commit is contained in:
Daniel Chappuis 2017-08-18 17:50:27 +02:00
parent 2f60190942
commit 319cc72cde
9 changed files with 227 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/collision/CollisionCallback.cpp
View File

@ -45,6 +45,7 @@ CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair*
// For each contact manifold in the set of manifolds in the pair
ContactManifold* contactManifold = manifoldSet.getContactManifolds();
assert(contactManifold != nullptr);
while (contactManifold != nullptr) {
assert(contactManifold->getNbContactPoints() > 0);

27
src/collision/CollisionDetection.cpp
View File

@ -440,7 +440,7 @@ void CollisionDetection::reportAllContacts() {
for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
// If there is a user callback
if (mWorld->mEventListener != nullptr) {
if (mWorld->mEventListener != nullptr && it->second->hasContacts()) {
CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator);
@ -845,10 +845,13 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Process the potential contacts
processPotentialContacts(&pair);
if (pair.hasContacts()) {
// Report the contacts to the user
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
collisionCallback->notifyContact(collisionInfo);
}
}
// Go to the next proxy shape
body2ProxyShape = body2ProxyShape->getNext();
@ -916,10 +919,6 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
// Report the contacts to the user
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
callback->notifyContact(collisionInfo);
}
}
@ -935,6 +934,13 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Process the potential contacts
processPotentialContacts(&pair);
if (pair.hasContacts()) {
// Report the contacts to the user
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
callback->notifyContact(collisionInfo);
}
}
}
@ -996,10 +1002,6 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold();
// Report the contacts to the user
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
callback->notifyContact(collisionInfo);
}
}
@ -1015,6 +1017,13 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Process the potential contacts
processPotentialContacts(&pair);
if (pair.hasContacts()) {
// Report the contacts to the user
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
callback->notifyContact(collisionInfo);
}
}
}
}

4
src/collision/ContactManifoldSet.cpp
View File

@ -247,6 +247,10 @@ void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
if (previous != nullptr) {
previous->setNext(manifold->getNext());
}
else {
mManifolds = next;
}
if (next != nullptr) {
next->setPrevious(manifold->getPrevious());
}

99
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp Normal file → Executable file
View File

@ -63,22 +63,18 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
// If the two capsules are parallel (we create two contact points)
if (areParallelVectors(seg1, seg2)) {
bool areCapsuleInnerSegmentsParralel = areParallelVectors(seg1, seg2);
if (areCapsuleInnerSegmentsParralel) {
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
const decimal segmentsDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsDistance >= sumRadius) {
const decimal segmentsPerpendicularDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsPerpendicularDistance >= sumRadius) {
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
// Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
return false;
}
// If the distance between the two segments is larger than zero (inner segments of capsules are not overlapping)
// If the inner segments are overlapping, we cannot compute a contact normal (unknown direction). In this case,
// we skip the parallel contact points calculation (there might still be contact in the spherical caps of the capsules)
if (segmentsDistance > MACHINE_EPSILON) {
// Compute the planes that goes through the extreme points of the inner segment of capsule 1
decimal d1 = seg1.dot(capsule1SegA);
decimal d2 = -seg1.dot(capsule1SegB);
@ -103,30 +99,54 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
const Vector3 seg1Normalized = seg1.getUnit();
Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
Vector3 normalCapsule2SpaceNormalized;
Vector3 segment1ToSegment2;
// If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping)
if (segmentsPerpendicularDistance > MACHINE_EPSILON) {
// Compute a perpendicular vector from segment 1 to segment 2
Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit();
}
else { // If the capsule inner segments are overlapping (degenerate case)
// We cannot use the vector between segments as a contact normal. To generate a contact normal, we take
// any vector that is orthogonal to the inner capsule segments.
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
Vector3 seg2Normalized = seg2.getUnit();
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule 2 inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(seg2Normalized.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(seg2Normalized.y); // abs(vec2.dot(seg2))
segment1ToSegment2.setToZero();
// We choose as a contact normal, any direction that is perpendicular to the inner capsules segments
normalCapsule2SpaceNormalized = cosA1 < cosA2 ? seg2Normalized.cross(vec1) : seg2Normalized.cross(vec2);
}
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();
const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized;
decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
decimal penetrationDepth = sumRadius - segmentsDistance;
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
}
return true;
}
}
}
// Compute the closest points between the two inner capsule segments
Vector3 closestPointCapsule1Seg;
@ -139,10 +159,13 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
// If the collision shapes overlap
if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
if (closestPointsDistanceSquare < sumRadius * sumRadius) {
if (reportContacts) {
// If the distance between the inner segments is not zero
if (closestPointsDistanceSquare > MACHINE_EPSILON) {
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
closestPointsSeg1ToSeg2 /= closestPointsDistance;
@ -155,7 +178,45 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // The segment are overlapping (degenerate case)
// If the capsule segments are parralel
if (areCapsuleInnerSegmentsParralel) {
// The segment are parallel, not overlapping and their distance is zero.
// Therefore, the capsules are just touching at the top of their inner segments
decimal squareDistCapsule2PointToCapsuleSegA = (capsule1SegA - closestPointCapsule2Seg).lengthSquare();
Vector3 capsule1SegmentMostExtremePoint = squareDistCapsule2PointToCapsuleSegA > MACHINE_EPSILON ? capsule1SegA : capsule1SegB;
Vector3 normalCapsuleSpace2 = (closestPointCapsule2Seg - capsule1SegmentMostExtremePoint);
normalCapsuleSpace2.normalize();
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // If the capsules inner segments are not parallel
// We cannot use a vector between the segments as contact normal. We need to compute a new contact normal with the cross
// product between the two segments.
Vector3 normalCapsuleSpace2 = seg1.cross(seg2);
normalCapsuleSpace2.normalize();
// Compute the contact points on both shapes
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
}
}
return true;

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

@ -841,14 +841,17 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// Get the twin edge
HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex);
// Get the adjacent face normal (and negate it to have a clipping plane)
Vector3 faceNormal = -referencePolyhedron->getFaceNormal(twinEdge.faceIndex);
// Compute the edge vertices and edge direction
Vector3 edgeV1 = referencePolyhedron->getVertexPosition(edge.vertexIndex);
Vector3 edgeV2 = referencePolyhedron->getVertexPosition(twinEdge.vertexIndex);
Vector3 edgeDirection = edgeV2 - edgeV1;
// Get a vertex of the clipping plane (vertex of the adjacent edge)
Vector3 faceVertex = referencePolyhedron->getVertexPosition(edge.vertexIndex);
// 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 = axisReferenceSpace.cross(edgeDirection);
planesNormals.push_back(faceNormal);
planesPoints.push_back(faceVertex);
planesNormals.push_back(clipPlaneNormal);
planesPoints.push_back(edgeV1);
// Go to the next adjacent edge of the reference face
currentEdgeIndex = edge.nextEdgeIndex;

43
src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp
View File

@ -69,23 +69,56 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b
decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
// If the collision shapes overlap
if (sphereSegmentDistanceSquare < sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) {
if (sphereSegmentDistanceSquare < sumRadius * sumRadius) {
decimal penetrationDepth;
Vector3 normalWorld;
Vector3 contactPointSphereLocal;
Vector3 contactPointCapsuleLocal;
if (reportContacts) {
// If the sphere center is not on the capsule inner segment
if (sphereSegmentDistanceSquare > MACHINE_EPSILON) {
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
sphereCenterToSegment /= sphereSegmentDistance;
const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
decimal penetrationDepth = sumRadius - sphereSegmentDistance;
penetrationDepth = sumRadius - sphereSegmentDistance;
if (!isSphereShape1) {
normalWorld = -normalWorld;
}
}
else { // If the sphere center is on the capsule inner segment (degenerate case)
// We take any direction that is orthogonal to the inner capsule segment as a contact normal
// Capsule inner segment
Vector3 capsuleSegment = (capsuleSegB - capsuleSegA).getUnit();
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(capsuleSegment.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(capsuleSegment.y); // abs(vec2.dot(seg2))
penetrationDepth = sumRadius;
// We choose as a contact normal, any direction that is perpendicular to the inner capsule segment
Vector3 normalCapsuleSpace = cosA1 < cosA2 ? capsuleSegment.cross(vec1) : capsuleSegment.cross(vec2);
normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace;
// Compute the two local contact points
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereShape->getRadius());
contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius();
}
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,

26
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp Normal file → Executable file
View File

@ -57,15 +57,29 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bo
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);
Vector3 intersectionOnBody1;
Vector3 intersectionOnBody2;
Vector3 normal;
// If the two sphere centers are not at the same position
if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit();
intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.getUnit();
normal = vectorBetweenCenters.getUnit();
}
else { // If the sphere centers are at the same position (degenerate case)
// Take any contact normal direction
normal.setAllValues(0, 1, 0);
intersectionOnBody1 = sphereShape1->getRadius() * (transform1.getInverse().getOrientation() * normal);
intersectionOnBody2 = sphereShape2->getRadius() * (transform2.getInverse().getOrientation() * normal);
}
// Create the contact info object
narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
}
return true;

4
src/collision/shapes/ConvexMeshShape.h
View File

@ -233,7 +233,7 @@ inline HalfEdgeStructure::Edge ConvexMeshShape::getHalfEdge(uint edgeIndex) cons
// Return the position of a given vertex
inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const {
assert(vertexIndex < getNbVertices());
return mPolyhedronMesh->getVertex(vertexIndex);
return mPolyhedronMesh->getVertex(vertexIndex) * mScaling;
}
// Return the normal vector of a given face of the polyhedron
@ -244,7 +244,7 @@ inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const {
// Return the centroid of the polyhedron
inline Vector3 ConvexMeshShape::getCentroid() const {
return mPolyhedronMesh->getCentroid();
return mPolyhedronMesh->getCentroid() * mScaling;
}
}

8
src/engine/OverlappingPair.h
View File

@ -155,6 +155,9 @@ class OverlappingPair {
/// Return true if one of the shapes of the pair is a concave shape
bool hasConcaveShape() const;
/// Return true if the overlapping pair has contact manifolds with contacts
bool hasContacts() const;
/// Return a pointer to the first potential contact manifold in the linked-list
ContactManifoldInfo* getPotentialContactManifolds();
@ -249,6 +252,11 @@ inline bool OverlappingPair::hasConcaveShape() const {
!getShape2()->getCollisionShape()->isConvex();
}
// Return true if the overlapping pair has contact manifolds with contacts
inline bool OverlappingPair::hasContacts() const {
return mContactManifoldSet.getContactManifolds() != nullptr;
}
// Return a pointer to the first potential contact manifold in the linked-list
inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() {
return mPotentialContactManifolds;