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

Finish implementing capsule vs capsule narrow-phase algorithm

Browse Source
This commit is contained in:
Daniel Chappuis 2017-03-28 23:07:10 +02:00
parent 050e8b36dc
commit a9b3afae59
1 changed files with 37 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp
View File

@ -66,14 +66,19 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
// 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 = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsDistance >= sumRadius || segmentsDistance < MACHINE_EPSILON) { if (segmentsDistance >= 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;
} }
// Compute the planes that goes through the extrem points of the inner segment of capsule 1 // 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 d1 = seg1.dot(capsule1SegA);
decimal d2 = -seg1.dot(capsule1SegB); decimal d2 = -seg1.dot(capsule1SegB);
@ -82,27 +87,21 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1); decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1); decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
bool isClipValid = false; // True if the segments were overlapping (the clip segment is valid) // 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 // Clip the inner segment of capsule 2
Vector3 clipPointA, clipPointB;
if (t1 >= decimal(0.0)) {
if (t1 > decimal(1.0)) t1 = decimal(1.0); if (t1 > decimal(1.0)) t1 = decimal(1.0);
clipPointA = capsule2SegB - t1 * seg2; const Vector3 clipPointA = capsule2SegB - t1 * seg2;
isClipValid = true;
}
if (t2 >= decimal(0.0) && t2 <= decimal(1.0)) {
if (t2 > decimal(1.0)) t2 = decimal(1.0); if (t2 > decimal(1.0)) t2 = decimal(1.0);
clipPointB = capsule2SegA + t2 * seg2; const Vector3 clipPointB = capsule2SegA + t2 * seg2;
isClipValid = true;
}
// If we have a valid clip segment // Project point capsule2SegA onto line of innner segment of capsule 1
if (isClipValid) { const Vector3 seg1Normalized = seg1.getUnit();
Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA); // Compute a perpendicular vector from segment 1 to segment 2
Vector3 segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit(); Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse(); Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
@ -122,6 +121,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
return true; return true;
} }
} }
}
// Compute the closest points between the two inner capsule segments // Compute the closest points between the two inner capsule segments
Vector3 closestPointCapsule1Seg; Vector3 closestPointCapsule1Seg;