Working on capsule vs polyhedron narrow-phase algorithm

This commit is contained in:
Daniel Chappuis 2017-04-28 21:40:16 +02:00
parent f61fea8b8a
commit 7fb6f49149
20 changed files with 382 additions and 30 deletions

View File

@ -64,15 +64,8 @@ class ContactManifoldInfo {
/// Destructor
~ContactManifoldInfo() {
// Delete all the contact points in the linked list
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
ContactPointInfo* elementToDelete = element;
element = element->next;
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
// Remove all the contact points
reset();
}
/// Deleted copy-constructor
@ -96,6 +89,22 @@ class ContactManifoldInfo {
mContactPointsList = contactPointInfo;
}
/// Remove all the contact points
void reset() {
// Delete all the contact points in the linked list
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
ContactPointInfo* elementToDelete = element;
element = element->next;
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
mContactPointsList = nullptr;
}
/// Get the first contact point info of the linked list of contact points
ContactPointInfo* getFirstContactPointInfo() const {
return mContactPointsList;

View File

@ -42,6 +42,9 @@ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) {
// Compute the faces normals
computeFacesNormals();
// Compute the centroid
computeCentroid();
}
// Destructor
@ -126,3 +129,15 @@ void PolyhedronMesh::computeFacesNormals() {
mFacesNormals[f].normalize();
}
}
// Compute the centroid of the polyhedron
void PolyhedronMesh::computeCentroid() {
mCentroid.setToZero();
for (uint v=0; v < getNbVertices(); v++) {
mCentroid += getVertex(v);
}
mCentroid /= getNbVertices();
}

View File

@ -55,6 +55,9 @@ class PolyhedronMesh {
/// Array with the face normals
Vector3* mFacesNormals;
/// Centroid of the polyhedron
Vector3 mCentroid;
// -------------------- Methods -------------------- //
/// Create the half-edge structure of the mesh
@ -63,6 +66,9 @@ class PolyhedronMesh {
/// Compute the faces normals
void computeFacesNormals();
/// Compute the centroid of the polyhedron
void computeCentroid() ;
public:
// -------------------- Methods -------------------- //
@ -84,6 +90,9 @@ class PolyhedronMesh {
/// Return the half-edge structure of the mesh
const HalfEdgeStructure& getHalfEdgeStructure() const;
/// Return the centroid of the polyhedron
Vector3 getCentroid() const;
};
// Return the number of vertices
@ -102,6 +111,11 @@ inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const {
return mHalfEdgeStructure;
}
// Return the centroid of the polyhedron
inline Vector3 PolyhedronMesh::getCentroid() const {
return mCentroid;
}
}
#endif

View File

@ -175,7 +175,6 @@ class ProxyShape {
friend class CollisionDetection;
friend class CollisionWorld;
friend class DynamicsWorld;
friend class EPAAlgorithm;
friend class GJKAlgorithm;
friend class ConvexMeshShape;

View File

@ -30,13 +30,14 @@
// 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(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
const decimal parallelEpsilon = decimal(0.001);
// Get the capsule collision shapes
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
@ -62,7 +63,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
// If the two capsules are parallel (we create two contact points)
if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) {
if (areParallelVectors(seg1, seg2)) {
// 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);

View File

@ -60,7 +60,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
/// Compute the narrow-phase collision detection between two capsules
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};

View File

@ -27,24 +27,86 @@
#include "CapsuleVsConvexPolyhedronAlgorithm.h"
#include "SAT/SATAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/ConvexPolyhedronShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a capsule and a polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
SATAlgorithm satAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
// 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 contact normal is parallel to a face of the
// polyhedron mesh, we would like to create two contact points instead of a single one
// (as in the deep contact case with SAT algorithm)
// Get the contact point created by GJK
ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo();
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
// For each face of the polyhedron
// For each face of the convex mesh
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
// Get the face
HalfEdgeStructure::Face face = polyhedron->getFace(f);
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
// If the polyhedron face normal is parallel to the computed GJK contact normal
if (areParallelVectors(faceNormalWorld, contactPoint->normal)) {
// Remove the previous contact point computed by GJK
contactManifoldInfo.reset();
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,
contactManifoldInfo, isCapsuleShape1);
break;
}
}
// Return true
return true;
}
@ -53,7 +115,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
}

View File

@ -60,7 +60,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
/// Compute the narrow-phase collision detection between a capsule and a polyhedron
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};

View File

@ -27,7 +27,7 @@
#include "SATAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "collision/PolyhedronMesh.h"
#include "collision/shapes/ConvexPolyhedronShape.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/SphereShape.h"
#include "configuration.h"
#include "engine/Profiler.h"
@ -110,8 +110,220 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo*
// Test collision between a capsule and a convex mesh
bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
// Minimum penetration depth
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint minFaceIndex = 0;
uint minEdgeIndex = 0;
bool isMinPenetrationFaceNormal = false;
Vector3 separatingAxisCapsuleSpace;
Vector3 separatingPolyhedronEdgeVertex1;
Vector3 separatingPolyhedronEdgeVertex2;
// For each face of the convex mesh
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
// Get the face
HalfEdgeStructure::Face face = polyhedron->getFace(f);
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
// Compute the penetration depth (using the capsule support in the direction opposite to the face normal)
const Vector3 faceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-faceNormalCapsuleSpace, nullptr);
const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]);
const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint;
const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal);
// If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) {
return false;
}
// Check if we have found a new minimum penetration axis
if (penetrationDepth < minPenetrationDepth) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = f;
isMinPenetrationFaceNormal = true;
separatingAxisCapsuleSpace = faceNormalCapsuleSpace;
}
}
// 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);
const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA;
// For each direction that is the cross product of the capsule inner segment and
// an edge of the polyhedron
for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
// Get an edge from the polyhedron (convert it into the capsule local-space)
HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e);
const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex);
const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex);
const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1);
HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex);
const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex);
// Check using the Gauss Map if this edge cross product can be as separating axis
if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) {
// Compute the axis to test (cross product between capsule inner segment and polyhedron edge)
Vector3 axis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace);
// Skip separating axis test if polyhedron edge is parallel to the capsule inner segment
if (axis.lengthSquare() >= decimal(0.00001)) {
const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid();
const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1;
// Swap axis direction if necessary such that it points out of the polyhedron
if (axis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) {
axis = -axis;
}
axis.normalize();
// Compute the penetration depth
const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-axis, nullptr);
const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint;
const decimal penetrationDepth = capsuleSupportPointToEdgePoint.dot(axis);
// If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) {
return false;
}
// Check if we have found a new minimum penetration axis
if (penetrationDepth < minPenetrationDepth) {
minPenetrationDepth = penetrationDepth;
minEdgeIndex = e;
isMinPenetrationFaceNormal = false;
separatingAxisCapsuleSpace = axis;
separatingPolyhedronEdgeVertex1 = edgeVertex1;
separatingPolyhedronEdgeVertex2 = edgeVertex2;
}
}
}
}
// 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 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
const decimal capsuleRadius = capsuleShape->getRadius();
// If the separating axis is a face normal
// 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,
contactManifoldInfo, isCapsuleShape1);
}
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);
// Project closest capsule inner segment point into the capsule bounds
const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
// Create the contact point
contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
}
return true;
}
// Compute the two contact points between a polyhedron and a capsule when the separating
// axis is a face normal of the polyhedron
void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const {
HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex);
uint firstEdgeIndex = face.edgeIndex;
uint edgeIndex = firstEdgeIndex;
std::vector<Vector3> planesPoints;
std::vector<Vector3> planesNormals;
// For each adjacent edge of the separating face of the polyhedron
do {
HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex);
HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
// Construct a clippling plane for each adjacent edge of the separting face of the polyhedron
planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex));
planesNormals.push_back(polyhedron->getFaceNormal(twinEdge.faceIndex));
edgeIndex = edge.nextEdgeIndex;
} while(edgeIndex != firstEdgeIndex);
// First we clip the inner segment of the capsule with the four planes of the adjacent faces
std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
planesPoints, planesNormals);
// Project the two clipped points into the polyhedron face
const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
const Vector3 contactPoint1Polyhedron = clipSegment[0] + faceNormal * (penetrationDepth - capsuleRadius);
const Vector3 contactPoint2Polyhedron = clipSegment[1] + faceNormal * (penetrationDepth - capsuleRadius);
// Project the two clipped points into the capsule bounds
const Vector3 contactPoint1Capsule = (polyhedronToCapsuleTransform * clipSegment[0]) - separatingAxisCapsuleSpace * capsuleRadius;
const Vector3 contactPoint2Capsule = (polyhedronToCapsuleTransform * clipSegment[1]) - separatingAxisCapsuleSpace * capsuleRadius;
// Create the contact points
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
isCapsuleShape1 ? contactPoint1Capsule : contactPoint1Polyhedron,
isCapsuleShape1 ? contactPoint1Polyhedron : contactPoint1Capsule);
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth,
isCapsuleShape1 ? contactPoint2Capsule : contactPoint2Polyhedron,
isCapsuleShape1 ? contactPoint2Polyhedron : contactPoint2Capsule);
}
// This method returns true if an edge of a polyhedron and a capsule forms a
// face of the Minkowski Difference. This test is used to know if two edges
// (one edge of the polyhedron vs the inner segment of the capsule in this case)
// have to be test as a possible separating axis
bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
const Vector3& edgeAdjacentFace2Normal) const {
// Return true if the arc on the Gauss Map corresponding to the polyhedron edge
// intersect the unit circle plane corresponding to capsule Gauss Map
return capsuleSegment.dot(edgeAdjacentFace1Normal) * capsuleSegment.dot(edgeAdjacentFace2Normal) < decimal(0.0);
}
// Test collision between a triangle and a convex mesh

View File

@ -29,6 +29,7 @@
// Libraries
#include "collision/ContactManifoldInfo.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/shapes/ConvexPolyhedronShape.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -68,6 +69,17 @@ class SATAlgorithm {
/// Test collision between a capsule and a convex mesh
bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
/// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const;
// This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference
bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
const Vector3& edgeAdjacentFace2Normal) const;
/// Test collision between a triangle and a convex mesh
bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;

View File

@ -31,6 +31,9 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a capsule
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;

View File

@ -60,7 +60,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
/// Compute the narrow-phase collision detection between a sphere and a capsule
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};

View File

@ -31,13 +31,12 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection a sphere and a convex polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);

View File

@ -60,7 +60,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
/// Compute the narrow-phase collision detection a sphere and a convex polyhedron
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};

View File

@ -128,6 +128,9 @@ class BoxShape : public ConvexPolyhedronShape {
/// Return the normal vector of a given face of the polyhedron
virtual Vector3 getFaceNormal(uint faceIndex) const override;
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const override;
};
// Return the extents of the box
@ -236,6 +239,11 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const {
}
}
// Return the centroid of the box
inline Vector3 BoxShape::getCentroid() const {
return Vector3::zero();
}
// Return the number of half-edges of the polyhedron
inline uint BoxShape::getNbHalfEdges() const {
return 24;

View File

@ -142,6 +142,9 @@ class ConvexMeshShape : public ConvexPolyhedronShape {
/// Return the normal vector of a given face of the polyhedron
virtual Vector3 getFaceNormal(uint faceIndex) const override;
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const override;
};
/// Set the scaling vector of the collision shape
@ -239,6 +242,11 @@ inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const {
return mPolyhedronMesh->getFaceNormal(faceIndex);
}
// Return the centroid of the polyhedron
inline Vector3 ConvexMeshShape::getCentroid() const {
return mPolyhedronMesh->getCentroid();
}
}
#endif

View File

@ -88,6 +88,9 @@ class ConvexPolyhedronShape : public ConvexShape {
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const override;
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const=0;
};
// Return true if the collision shape is a polyhedron

View File

@ -81,7 +81,7 @@ class ConvexShape : public CollisionShape {
// -------------------- Friendship -------------------- //
friend class GJKAlgorithm;
friend class EPAAlgorithm;
friend class SATAlgorithm;
};
// Return true if the collision shape is convex, false if it is concave

View File

@ -60,6 +60,11 @@ Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) {
return vector;
}
/// Return true if two vectors are parallel
bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) {
return vector1.cross(vector2).lengthSquare() < decimal(0.00001);
}
/// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"
Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) {

View File

@ -76,6 +76,9 @@ inline bool sameSign(decimal a, decimal b) {
return a * b >= decimal(0.0);
}
/// Return true if two vectors are parallel
bool areParallelVectors(const Vector3& vector1, const Vector3& vector2);
/// Clamp a vector such that it is no longer than a given maximum length
Vector3 clamp(const Vector3& vector, decimal maxLength);