From 319cc72cde3052239b62cbb0f51a1f8bf8db252c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 18 Aug 2017 17:50:27 +0200 Subject: [PATCH] Fix issues in collision detection --- src/collision/CollisionCallback.cpp | 1 + src/collision/CollisionDetection.cpp | 33 ++-- src/collision/ContactManifoldSet.cpp | 4 + .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 167 ++++++++++++------ .../narrowphase/SAT/SATAlgorithm.cpp | 19 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 57 ++++-- .../narrowphase/SphereVsSphereAlgorithm.cpp | 28 ++- src/collision/shapes/ConvexMeshShape.h | 4 +- src/engine/OverlappingPair.h | 8 + 9 files changed, 227 insertions(+), 94 deletions(-) mode change 100644 => 100755 src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp mode change 100644 => 100755 src/collision/narrowphase/SphereVsSphereAlgorithm.cpp diff --git a/src/collision/CollisionCallback.cpp b/src/collision/CollisionCallback.cpp index f470763d..db820db4 100644 --- a/src/collision/CollisionCallback.cpp +++ b/src/collision/CollisionCallback.cpp @@ -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); diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 57360706..0abd82a3 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -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,9 +845,12 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body // Process the potential contacts processPotentialContacts(&pair); - // Report the contacts to the user - CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); - collisionCallback->notifyContact(collisionInfo); + if (pair.hasContacts()) { + + // Report the contacts to the user + CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator); + collisionCallback->notifyContact(collisionInfo); + } } // Go to the next proxy shape @@ -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); + } } } } diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index b9b7f3e8..23602dda 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -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()); } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp old mode 100644 new mode 100755 index c98329bf..b15cb186 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -63,68 +63,88 @@ 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); - // 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); + // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner + // segment of capsule 1 + decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); + decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); - // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner - // segment of capsule 1 - decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); - decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); + // If the segments were overlapping (the clip segment is valid) + if (t1 > decimal(0.0) && t2 > decimal(0.0)) { - // If the segments were overlapping (the clip segment is valid) - if (t1 > decimal(0.0) && t2 > decimal(0.0)) { + if (reportContacts) { - if (reportContacts) { + // 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; - // 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; + // Project point capsule2SegA onto line of innner segment of capsule 1 + const Vector3 seg1Normalized = seg1.getUnit(); + Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized; - // Project point capsule2SegA onto line of innner segment of capsule 1 - const Vector3 seg1Normalized = seg1.getUnit(); - Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized; + Vector3 normalCapsule2SpaceNormalized; + Vector3 segment1ToSegment2; - // Compute a perpendicular vector from segment 1 to segment 2 - Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1); - Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); + // If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping) + if (segmentsPerpendicularDistance > MACHINE_EPSILON) { - 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 + segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1); + normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit(); + } + else { // If the capsule inner segments are overlapping (degenerate case) - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * segment1ToSegment2Normalized; + // 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. - decimal penetrationDepth = sumRadius - segmentsDistance; + Vector3 vec1(1, 0, 0); + Vector3 vec2(0, 1, 0); - // Create the contact info object - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local); - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local); + 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)) - return true; - } + 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 + 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(); + + decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance; + + 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; } } @@ -132,31 +152,72 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, Vector3 closestPointCapsule1Seg; Vector3 closestPointCapsule2Seg; computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB, - closestPointCapsule1Seg, closestPointCapsule2Seg); + closestPointCapsule1Seg, closestPointCapsule2Seg); // Compute the distance between the sphere center and the closest point on the segment Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg); const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare(); // If the collision shapes overlap - if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) { + if (closestPointsDistanceSquare < sumRadius * sumRadius) { + + if (reportContacts) { - if (reportContacts) { + // If the distance between the inner segments is not zero + if (closestPointsDistanceSquare > MACHINE_EPSILON) { - decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); - closestPointsSeg1ToSeg2 /= closestPointsDistance; + decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare); + closestPointsSeg1ToSeg2 /= closestPointsDistance; - const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); - const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); + const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); + const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius(); - const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; + const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2; - decimal penetrationDepth = sumRadius - closestPointsDistance; + decimal penetrationDepth = sumRadius - closestPointsDistance; - // Create the contact info object - narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); + // 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; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 98686311..cebec9a6 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -838,17 +838,20 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // Get the adjacent edge HalfEdgeStructure::Edge edge = referencePolyhedron->getHalfEdge(currentEdgeIndex); - // Get the twin edge - HalfEdgeStructure::Edge twinEdge = referencePolyhedron->getHalfEdge(edge.twinEdgeIndex); + // 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; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index 52016f2f..a5d7a6ac 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -69,28 +69,61 @@ 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) { - decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); - sphereCenterToSegment /= sphereSegmentDistance; + // If the sphere center is not on the capsule inner segment + if (sphereSegmentDistanceSquare > MACHINE_EPSILON) { - 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; + contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); + contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); - decimal penetrationDepth = sumRadius - sphereSegmentDistance; + normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; - if (!isSphereShape1) { - normalWorld = -normalWorld; - } + 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, - isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, - isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); + isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, + isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); } return true; diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp old mode 100644 new mode 100755 index 1335c911..0b520213 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -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; - // Create the contact info object - narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, - intersectionOnBody1, intersectionOnBody2); + // 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(normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2); } return true; diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index 28ba9c86..dd2b7021 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -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; } } diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index a55df081..906cddb7 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -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;