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 // For each contact manifold in the set of manifolds in the pair
ContactManifold* contactManifold = manifoldSet.getContactManifolds(); ContactManifold* contactManifold = manifoldSet.getContactManifolds();
assert(contactManifold != nullptr);
while (contactManifold != nullptr) { while (contactManifold != nullptr) {
assert(contactManifold->getNbContactPoints() > 0); assert(contactManifold->getNbContactPoints() > 0);

33
src/collision/CollisionDetection.cpp
View File

@ -440,7 +440,7 @@ void CollisionDetection::reportAllContacts() {
for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) { for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
// If there is a user callback // If there is a user callback
if (mWorld->mEventListener != nullptr) { if (mWorld->mEventListener != nullptr && it->second->hasContacts()) {
CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator); CollisionCallback::CollisionCallbackInfo collisionInfo(it->second, mPoolAllocator);
@ -845,9 +845,12 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Process the potential contacts // Process the potential contacts
processPotentialContacts(&pair); processPotentialContacts(&pair);
// Report the contacts to the user if (pair.hasContacts()) {
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
collisionCallback->notifyContact(collisionInfo); // Report the contacts to the user
CollisionCallback::CollisionCallbackInfo collisionInfo(&pair, mPoolAllocator);
collisionCallback->notifyContact(collisionInfo);
}
} }
// Go to the next proxy shape // 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 // Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); 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 // Process the potential contacts
processPotentialContacts(&pair); 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 // Add the contact points as a potential contact manifold into the pair
narrowPhaseInfo->addContactPointsAsPotentialContactManifold(); 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 // Process the potential contacts
processPotentialContacts(&pair); 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) { if (previous != nullptr) {
previous->setNext(manifold->getNext()); previous->setNext(manifold->getNext());
} }
else {
mManifolds = next;
}
if (next != nullptr) { if (next != nullptr) {
next->setPrevious(manifold->getPrevious()); next->setPrevious(manifold->getPrevious());
} }

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

@ -63,68 +63,88 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo,
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius(); decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
// If the two capsules are parallel (we create two contact points) // 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) // 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); const decimal segmentsPerpendicularDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsDistance >= sumRadius) { if (segmentsPerpendicularDistance >= sumRadius) {
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius. // 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. // Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
return false; return false;
} }
// If the distance between the two segments is larger than zero (inner segments of capsules are not overlapping) // Compute the planes that goes through the extreme points of the inner segment of capsule 1
// If the inner segments are overlapping, we cannot compute a contact normal (unknown direction). In this case, decimal d1 = seg1.dot(capsule1SegA);
// we skip the parallel contact points calculation (there might still be contact in the spherical caps of the capsules) decimal d2 = -seg1.dot(capsule1SegB);
if (segmentsDistance > MACHINE_EPSILON) {
// Compute the planes that goes through the extreme points of the inner segment of capsule 1 // Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
decimal d1 = seg1.dot(capsule1SegA); // segment of capsule 1
decimal d2 = -seg1.dot(capsule1SegB); 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 // If the segments were overlapping (the clip segment is valid)
// segment of capsule 1 if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
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 (reportContacts) {
if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
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 // Project point capsule2SegA onto line of innner segment of capsule 1
if (t1 > decimal(1.0)) t1 = decimal(1.0); const Vector3 seg1Normalized = seg1.getUnit();
const Vector3 clipPointA = capsule2SegB - t1 * seg2; Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
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 Vector3 normalCapsule2SpaceNormalized;
const Vector3 seg1Normalized = seg1.getUnit(); Vector3 segment1ToSegment2;
Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
// Compute a perpendicular vector from segment 1 to segment 2 // If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping)
Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1); if (segmentsPerpendicularDistance > MACHINE_EPSILON) {
Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); // Compute a perpendicular vector from segment 1 to segment 2
const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius()); normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit();
const Vector3 contactPointACapsule2Local = clipPointA - segment1ToSegment2Normalized * capsuleShape2->getRadius(); }
const Vector3 contactPointBCapsule2Local = clipPointB - segment1ToSegment2Normalized * capsuleShape2->getRadius(); 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 Vector3 seg2Normalized = seg2.getUnit();
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
} // 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 closestPointCapsule1Seg;
Vector3 closestPointCapsule2Seg; Vector3 closestPointCapsule2Seg;
computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB, computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
closestPointCapsule1Seg, closestPointCapsule2Seg); closestPointCapsule1Seg, closestPointCapsule2Seg);
// Compute the distance between the sphere center and the closest point on the segment // Compute the distance between the sphere center and the closest point on the segment
Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg); Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg);
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare(); const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
// If the collision shapes overlap // 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); decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
closestPointsSeg1ToSeg2 /= closestPointsDistance; closestPointsSeg1ToSeg2 /= closestPointsDistance;
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius()); const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->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 // Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local); 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; return true;
} }

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

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

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

@ -69,28 +69,61 @@ bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, b
decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius(); decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
// If the collision shapes overlap // 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 (reportContacts) {
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); // If the sphere center is not on the capsule inner segment
sphereCenterToSegment /= sphereSegmentDistance; if (sphereSegmentDistanceSquare > MACHINE_EPSILON) {
const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); 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) { penetrationDepth = sumRadius - sphereSegmentDistance;
normalWorld = -normalWorld;
} 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 // Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal, isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal); isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
} }
return true; return true;

28
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 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.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); decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
Vector3 intersectionOnBody1;
Vector3 intersectionOnBody2;
Vector3 normal;
// Create the contact info object // If the two sphere centers are not at the same position
narrowPhaseInfo->addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth, if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
intersectionOnBody1, intersectionOnBody2);
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; 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 // Return the position of a given vertex
inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const { inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const {
assert(vertexIndex < getNbVertices()); assert(vertexIndex < getNbVertices());
return mPolyhedronMesh->getVertex(vertexIndex); return mPolyhedronMesh->getVertex(vertexIndex) * mScaling;
} }
// Return the normal vector of a given face of the polyhedron // 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 // Return the centroid of the polyhedron
inline Vector3 ConvexMeshShape::getCentroid() const { 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 /// Return true if one of the shapes of the pair is a concave shape
bool hasConcaveShape() const; 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 /// Return a pointer to the first potential contact manifold in the linked-list
ContactManifoldInfo* getPotentialContactManifolds(); ContactManifoldInfo* getPotentialContactManifolds();
@ -249,6 +252,11 @@ inline bool OverlappingPair::hasConcaveShape() const {
!getShape2()->getCollisionShape()->isConvex(); !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 // Return a pointer to the first potential contact manifold in the linked-list
inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() { inline ContactManifoldInfo* OverlappingPair::getPotentialContactManifolds() {
return mPotentialContactManifolds; return mPotentialContactManifolds;