245 lines
15 KiB
C++
Executable File
245 lines
15 KiB
C++
Executable File
/********************************************************************************
|
|
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
|
|
* Copyright (c) 2010-2018 Daniel Chappuis *
|
|
*********************************************************************************
|
|
* *
|
|
* This software is provided 'as-is', without any express or implied warranty. *
|
|
* In no event will the authors be held liable for any damages arising from the *
|
|
* use of this software. *
|
|
* *
|
|
* Permission is granted to anyone to use this software for any purpose, *
|
|
* including commercial applications, and to alter it and redistribute it *
|
|
* freely, subject to the following restrictions: *
|
|
* *
|
|
* 1. The origin of this software must not be misrepresented; you must not claim *
|
|
* that you wrote the original software. If you use this software in a *
|
|
* product, an acknowledgment in the product documentation would be *
|
|
* appreciated but is not required. *
|
|
* *
|
|
* 2. Altered source versions must be plainly marked as such, and must not be *
|
|
* misrepresented as being the original software. *
|
|
* *
|
|
* 3. This notice may not be removed or altered from any source distribution. *
|
|
* *
|
|
********************************************************************************/
|
|
|
|
// Libraries
|
|
#include "CapsuleVsCapsuleAlgorithm.h"
|
|
#include "collision/shapes/CapsuleShape.h"
|
|
#include "collision/narrowphase/CapsuleVsCapsuleNarrowPhaseInfoBatch.h"
|
|
|
|
// We want to use the ReactPhysics3D namespace
|
|
using namespace reactphysics3d;
|
|
|
|
// Compute the narrow-phase collision detection between two capsules
|
|
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
|
|
// by Dirk Gregorius.
|
|
bool CapsuleVsCapsuleAlgorithm::testCollision(CapsuleVsCapsuleNarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint batchStartIndex, uint batchNbItems,
|
|
bool reportContacts, bool stopFirstContactFound, MemoryAllocator& memoryAllocator) {
|
|
|
|
bool isCollisionFound = false;
|
|
|
|
for (uint batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
|
|
|
|
assert(narrowPhaseInfoBatch.contactPoints[batchIndex].size() == 0);
|
|
|
|
assert(!narrowPhaseInfoBatch.isColliding[batchIndex]);
|
|
|
|
// Get the transform from capsule 1 local-space to capsule 2 local-space
|
|
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getInverse() *
|
|
narrowPhaseInfoBatch.shape1ToWorldTransforms[batchIndex];
|
|
|
|
// Compute the end-points of the inner segment of the first capsule
|
|
const decimal capsule1HalfHeight = narrowPhaseInfoBatch.capsule1Heights[batchIndex] * decimal(0.5);
|
|
Vector3 capsule1SegA(0, -capsule1HalfHeight, 0);
|
|
Vector3 capsule1SegB(0, capsule1HalfHeight, 0);
|
|
capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA;
|
|
capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB;
|
|
|
|
// Compute the end-points of the inner segment of the second capsule
|
|
const decimal capsule2HalfHeight = narrowPhaseInfoBatch.capsule2Heights[batchIndex] * decimal(0.5);
|
|
const Vector3 capsule2SegA(0, -capsule2HalfHeight, 0);
|
|
const Vector3 capsule2SegB(0, capsule2HalfHeight, 0);
|
|
|
|
// The two inner capsule segments
|
|
const Vector3 seg1 = capsule1SegB - capsule1SegA;
|
|
const Vector3 seg2 = capsule2SegB - capsule2SegA;
|
|
|
|
// Compute the sum of the radius of the two capsules (virtual spheres)
|
|
const decimal capsule1Radius = narrowPhaseInfoBatch.capsule1Radiuses[batchIndex];
|
|
const decimal capsule2Radius = narrowPhaseInfoBatch.capsule2Radiuses[batchIndex];
|
|
const decimal sumRadius = capsule1Radius + capsule2Radius;
|
|
|
|
// If the two capsules are parallel (we create two contact points)
|
|
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 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.
|
|
continue;
|
|
}
|
|
|
|
// 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);
|
|
|
|
// If the segments were overlapping (the clip segment is valid)
|
|
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;
|
|
|
|
// 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;
|
|
|
|
// If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping)
|
|
if (segmentsPerpendicularDistance > MACHINE_EPSILON) {
|
|
|
|
// Compute a perpendicular vector from segment 1 to segment 2
|
|
segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
|
|
normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit();
|
|
}
|
|
else { // If the capsule inner segments are overlapping (degenerate case)
|
|
|
|
// We cannot use the vector between segments as a contact normal. To generate a contact normal, we take
|
|
// any vector that is orthogonal to the inner capsule segments.
|
|
|
|
Vector3 vec1(1, 0, 0);
|
|
Vector3 vec2(0, 1, 0);
|
|
|
|
Vector3 seg2Normalized = seg2.getUnit();
|
|
|
|
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule 2 inner segment (smallest absolute dot product)
|
|
decimal cosA1 = std::abs(seg2Normalized.x); // abs(vec1.dot(seg2))
|
|
decimal cosA2 = std::abs(seg2Normalized.y); // abs(vec2.dot(seg2))
|
|
|
|
segment1ToSegment2.setToZero();
|
|
|
|
// We choose as a contact normal, any direction that is perpendicular to the inner capsules segments
|
|
normalCapsule2SpaceNormalized = cosA1 < cosA2 ? seg2Normalized.cross(vec1) : seg2Normalized.cross(vec2);
|
|
}
|
|
|
|
Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
|
|
const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsule1Radius);
|
|
const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsule1Radius);
|
|
const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsule2Radius;
|
|
const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsule2Radius;
|
|
|
|
decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
|
|
|
|
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsule2SpaceNormalized;
|
|
|
|
// Create the contact info object
|
|
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
|
|
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
|
|
}
|
|
|
|
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
|
|
isCollisionFound = true;
|
|
if (stopFirstContactFound) {
|
|
return isCollisionFound;
|
|
}
|
|
continue;
|
|
}
|
|
}
|
|
|
|
// Compute the closest points between the two inner capsule segments
|
|
Vector3 closestPointCapsule1Seg;
|
|
Vector3 closestPointCapsule2Seg;
|
|
computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
|
|
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) {
|
|
|
|
if (reportContacts) {
|
|
|
|
// If the distance between the inner segments is not zero
|
|
if (closestPointsDistanceSquare > MACHINE_EPSILON) {
|
|
|
|
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
|
|
closestPointsSeg1ToSeg2 /= closestPointsDistance;
|
|
|
|
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsule1Radius);
|
|
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsule2Radius;
|
|
|
|
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * closestPointsSeg1ToSeg2;
|
|
|
|
decimal penetrationDepth = sumRadius - closestPointsDistance;
|
|
|
|
// Create the contact info object
|
|
narrowPhaseInfoBatch.addContactPoint(batchIndex, 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 * capsule1Radius);
|
|
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius;
|
|
|
|
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
|
|
|
|
// Create the contact info object
|
|
narrowPhaseInfoBatch.addContactPoint(batchIndex, 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 * capsule1Radius);
|
|
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsule2Radius;
|
|
|
|
const Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normalCapsuleSpace2;
|
|
|
|
// Create the contact info object
|
|
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
|
|
}
|
|
}
|
|
}
|
|
|
|
narrowPhaseInfoBatch.isColliding[batchIndex] = true;
|
|
isCollisionFound = true;
|
|
if (stopFirstContactFound) {
|
|
return isCollisionFound;
|
|
}
|
|
}
|
|
}
|
|
|
|
return isCollisionFound;
|
|
}
|