diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index f98e4d20..c98329bf 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -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; } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 88bd9ace..be9cc05f 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -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(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); - const ConvexPolyhedronShape* polyhedron = static_cast(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(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(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; diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 035bd8e5..6a8746c6 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -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; }; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 4974d367..98686311 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -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 polygonVertices; // Vertices to clip of the incident face - std::vector planesNormals; // Normals of the clipping planes - std::vector 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::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 polygonVertices; // Vertices to clip of the incident face + std::vector planesNormals; // Normals of the clipping planes + std::vector 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::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 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::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 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::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; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index e291fd83..52016f2f 100755 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -71,24 +71,27 @@ 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(); - - Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; - - decimal penetrationDepth = sumRadius - sphereSegmentDistance; + decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare); + sphereCenterToSegment /= sphereSegmentDistance; - if (!isSphereShape1) { - normalWorld = -normalWorld; + const Vector3 contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius()); + const Vector3 contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius(); + + Vector3 normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment; + + 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; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index 94211ac0..1335c911 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -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; }