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

Refactor contact manifold and contact point creation

Browse Source
This commit is contained in:
Daniel Chappuis 2017-02-26 13:48:50 +02:00
parent 6a01abfae8
commit b21a6bb59b
28 changed files with 543 additions and 641 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -60,6 +60,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/body/CollisionBody.cpp"
"src/body/RigidBody.h"
"src/body/RigidBody.cpp"
"src/collision/ContactPointInfo.h"
"src/collision/ContactManifoldInfo.h"
"src/collision/broadphase/BroadPhaseAlgorithm.h"
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.h"

216
src/collision/CollisionDetection.cpp
View File

@ -72,66 +72,6 @@ void CollisionDetection::computeCollisionDetection() {
mNarrowPhaseInfoList = nullptr;
}
// TODO : Remove this method
// Report collision between two sets of shapes
/*
void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) {
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ++it) {
OverlappingPair* pair = it->second;
const ProxyShape* shape1 = pair->getShape1();
const ProxyShape* shape2 = pair->getShape2();
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one
if (!shapes1.empty() && !shapes2.empty() &&
(shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
(shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
continue;
}
if (!shapes1.empty() && shapes2.empty() &&
shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
{
continue;
}
if (!shapes2.empty() && shapes1.empty() &&
shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
{
continue;
}
// For each contact manifold set of the overlapping pair
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
for (int j=0; j<manifoldSet.getNbContactManifolds(); j++) {
const ContactManifold* manifold = manifoldSet.getContactManifold(j);
// For each contact manifold of the manifold set
for (uint i=0; i<manifold->getNbContactPoints(); i++) {
ContactPoint* contactPoint = manifold->getContactPoint(i);
// Create the contact info object for the contact
ContactPointInfo contactInfo;
contactInfo.init(contactPoint->getNormal(), contactPoint->getPenetrationDepth(),
contactPoint->getLocalPointOnBody1(), contactPoint->getLocalPointOnBody2());
// Notify the collision callback about this new contact
if (callback != nullptr) callback->notifyContact(contactInfo);
}
}
}
}
*/
// Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() {
@ -191,9 +131,6 @@ void CollisionDetection::computeMiddlePhase() {
CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody();
// Update the contact cache of the overlapping pair
pair->update();
// Check that at least one body is awake and not static
bool isBody1Active = !body1->isSleeping() && body1->getType() != BodyType::STATIC;
bool isBody2Active = !body2->isSleeping() && body2->getType() != BodyType::STATIC;
@ -308,25 +245,21 @@ void CollisionDetection::computeNarrowPhase() {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
ContactPointInfo contactPointInfo;
if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactPointInfo)) {
ContactManifoldInfo contactManifoldInfo(mSingleFrameAllocator);
if (narrowPhaseAlgorithm->testCollision(currentNarrowPhaseInfo, contactManifoldInfo)) {
// Reduce the number of points in the contact manifold
contactManifoldInfo.reduce();
// If it is the first contact since the pairs are overlapping
if (currentNarrowPhaseInfo->overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event
if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactPointInfo);
if (mWorld->mEventListener != nullptr) mWorld->mEventListener->beginContact(contactManifoldInfo);
}
// Create a new contact
ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(contactPointInfo);
contact->updateWorldContactPoints(currentNarrowPhaseInfo->shape1ToWorldTransform,
currentNarrowPhaseInfo->shape2ToWorldTransform);
// Add the contact to the contact manifold set of the corresponding overlapping pair
currentNarrowPhaseInfo->overlappingPair->addContact(contact);
// Add the contact manifold to the corresponding overlapping pair
currentNarrowPhaseInfo->overlappingPair->addContactManifold(contactManifoldInfo);
// Add the overlapping pair into the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(currentNarrowPhaseInfo->overlappingPair->getShape1(),
@ -334,7 +267,7 @@ void CollisionDetection::computeNarrowPhase() {
mContactOverlappingPairs[pairId] = currentNarrowPhaseInfo->overlappingPair;
// Trigger a callback event for the new contact
if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactPointInfo);
if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo);
}
}
@ -345,111 +278,6 @@ void CollisionDetection::computeNarrowPhase() {
addAllContactManifoldsToBodies();
}
// TODO : Remove this method
// Compute the narrow-phase collision detection
/*
void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) {
mContactOverlappingPairs.clear();
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
OverlappingPair* pair = it->second;
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// If both shapes1 and shapes2 sets are non-empty, we check that
// shape1 is among on set and shape2 is among the other one
if (!shapes1.empty() && !shapes2.empty() &&
(shapes1.count(shape1->mBroadPhaseID) == 0 || shapes2.count(shape2->mBroadPhaseID) == 0) &&
(shapes1.count(shape2->mBroadPhaseID) == 0 || shapes2.count(shape1->mBroadPhaseID) == 0)) {
++it;
continue;
}
if (!shapes1.empty() && shapes2.empty() &&
shapes1.count(shape1->mBroadPhaseID) == 0 && shapes1.count(shape2->mBroadPhaseID) == 0)
{
++it;
continue;
}
if (!shapes2.empty() && shapes1.empty() &&
shapes2.count(shape1->mBroadPhaseID) == 0 && shapes2.count(shape2->mBroadPhaseID) == 0)
{
++it;
continue;
}
// Check if the collision filtering allows collision between the two shapes and
// that the two shapes are still overlapping. Otherwise, we destroy the
// overlapping pair
if (((shape1->getCollideWithMaskBits() & shape2->getCollisionCategoryBits()) == 0 ||
(shape1->getCollisionCategoryBits() & shape2->getCollideWithMaskBits()) == 0) ||
!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it;
// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
// Destroy the overlapping pair
itToRemove->second->~OverlappingPair();
mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
continue;
}
else {
++it;
}
CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody();
// Update the contact cache of the overlapping pair
pair->update();
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
if (body1->getType() != BodyType::DYNAMIC && body2->getType() != BodyType::DYNAMIC) continue;
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
// Check if the two bodies are sleeping, if so, we do no test collision between them
if (body1->isSleeping() && body2->isSleeping()) continue;
// Select the narrow phase algorithm to use according to the two collision shapes
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
const int shape1Index = static_cast<int>(shape1Type);
const int shape2Index = static_cast<int>(shape2Type);
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Index][shape2Index];
// If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == nullptr) continue;
// Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
pair, shape1->getCachedCollisionData());
CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
pair, shape2->getCachedCollisionData());
TestCollisionBetweenShapesCallback narrowPhaseCallback(callback);
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback);
}
// Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies();
}
*/
// Allow the broadphase to notify the collision detection about an overlapping pair.
/// This method is called by the broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
@ -698,8 +526,8 @@ bool CollisionDetection::testOverlap(CollisionBody* body1, CollisionBody* body2)
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
ContactPointInfo contactPointInfo;
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo);
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
}
}
@ -786,8 +614,8 @@ void CollisionDetection::testOverlap(CollisionBody* body, OverlapCallback* overl
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
ContactPointInfo contactPointInfo;
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo);
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
isColliding |= narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo);
}
}
@ -859,10 +687,10 @@ void CollisionDetection::testCollision(CollisionBody* body1, CollisionBody* body
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
ContactPointInfo contactPointInfo;
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body1, body2,
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body1, body2,
body1ProxyShape, body2ProxyShape);
// Report the contact to the user
@ -937,10 +765,10 @@ void CollisionDetection::testCollision(CollisionBody* body, CollisionCallback* c
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
ContactPointInfo contactPointInfo;
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, body,
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, body,
proxyShape->getBody(), bodyProxyShape,
proxyShape);
@ -1008,10 +836,10 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
ContactPointInfo contactPointInfo;
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactPointInfo)) {
ContactManifoldInfo contactManifoldInfo(mMemoryAllocator);
if (narrowPhaseAlgorithm->testCollision(narrowPhaseInfo, contactManifoldInfo)) {
CollisionCallback::CollisionCallbackInfo collisionInfo(contactPointInfo, shape1->getBody(),
CollisionCallback::CollisionCallbackInfo collisionInfo(contactManifoldInfo, shape1->getBody(),
shape2->getBody(), shape1, shape2);
// Report the contact to the user

254
src/collision/ContactManifold.cpp
View File

@ -30,13 +30,30 @@
using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
ContactManifold::ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
PoolAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) {
// For each contact point info in the manifold
const ContactPointInfo* pointInfo = manifoldInfo.getFirstContactPointInfo();
while(pointInfo != nullptr) {
// Create the new contact point
ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(pointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
// Add the new contact point into the manifold
mContactPoints[mNbContactPoints] = contact;
mNbContactPoints++;
pointInfo = pointInfo->next;
}
assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
assert(mNbContactPoints > 0);
}
// Destructor
@ -44,209 +61,6 @@ ContactManifold::~ContactManifold() {
clear();
}
// Add a contact point in the manifold
void ContactManifold::addContactPoint(ContactPoint* contact) {
// For contact already in the manifold
for (uint i=0; i<mNbContactPoints; i++) {
// Check if the new point point does not correspond to a same contact point
// already in the manifold.
decimal distance = (mContactPoints[i]->getWorldPointOnBody1() -
contact->getWorldPointOnBody1()).lengthSquare();
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
// Delete the new contact
contact->~ContactPoint();
mMemoryAllocator.release(contact, sizeof(ContactPoint));
assert(mNbContactPoints > 0);
return;
}
}
// If the contact manifold is full
if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContactPoint(indexToRemove);
}
// Add the new contact point in the manifold
mContactPoints[mNbContactPoints] = contact;
mNbContactPoints++;
assert(mNbContactPoints > 0);
}
// Remove a contact point from the manifold
void ContactManifold::removeContactPoint(uint index) {
assert(index < mNbContactPoints);
assert(mNbContactPoints > 0);
// Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free
mContactPoints[index]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
// If we don't remove the last index
if (index < mNbContactPoints - 1) {
mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
}
mNbContactPoints--;
}
// Update the contact manifold
/// First the world space coordinates of the current contacts in the manifold are recomputed from
/// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
/// the contacts with a too large distance between the contact points in the plane orthogonal to the
/// contact normal.
void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
if (mNbContactPoints == 0) return;
// Update the world coordinates and penetration depth of the contact points in the manifold
for (uint i=0; i<mNbContactPoints; i++) {
mContactPoints[i]->updateWorldContactPoints(transform1, transform2);
mContactPoints[i]->updatePenetrationDepth();
}
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD;
// Remove the contact points that don't represent very well the contact manifold
for (int i=static_cast<int>(mNbContactPoints)-1; i>=0; i--) {
assert(i < static_cast<int>(mNbContactPoints));
// Compute the distance between contact points in the normal direction
decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth();
// If the contacts points are too far from each other in the normal direction
if (distanceNormal > squarePersistentContactThreshold) {
removeContactPoint(i);
}
else {
// Compute the distance of the two contact points in the plane
// orthogonal to the contact normal
Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() +
mContactPoints[i]->getNormal() * distanceNormal;
Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance
// threshold, we remove the contact
if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
removeContactPoint(i);
}
}
}
}
// Return the index of the contact point with the larger penetration depth.
/// This corresponding contact will be kept in the cache. The method returns -1 is
/// the new contact is the deepest.
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
int indexMaxPenetrationDepth = -1;
decimal maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache
for (uint i=0; i<mNbContactPoints; i++) {
// If the current contact has a larger penetration depth
if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
indexMaxPenetrationDepth = i;
}
}
// Return the index of largest penetration depth
return indexMaxPenetrationDepth;
}
// Return the index that will be removed.
/// The index of the contact point with the larger penetration
/// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
/// the different area and we want to keep the contacts with the largest area. The new point is also
/// kept. In order to compute the area of a quadrilateral, we use the formula :
/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
/// when we compute this area, we do not calculate it exactly but we
/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
/// by Erwin Coumans (http://wwww.bulletphysics.org).
int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint
decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint
decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 1) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 2) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 3) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.lengthSquare();
}
// Return the index of the contact to remove
return getMaxArea(area0, area1, area2, area3);
}
// Return the index of maximum area
int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
if (area0 < area1) {
if (area1 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area1 < area3) return 3;
else return 1;
}
}
else {
if (area0 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area0 < area3) return 3;
else return 0;
}
}
}
// Clear the contact manifold
void ContactManifold::clear() {
for (uint i=0; i<mNbContactPoints; i++) {
@ -258,3 +72,35 @@ void ContactManifold::clear() {
}
mNbContactPoints = 0;
}
// Add a contact point
void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
assert(mNbContactPoints < MAX_CONTACT_POINTS_IN_MANIFOLD);
// Create the new contact point
ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(contactPointInfo, mShape1->getLocalToWorldTransform(), mShape2->getLocalToWorldTransform());
// Add the new contact point into the manifold
mContactPoints[mNbContactPoints] = contactPoint;
mNbContactPoints++;
}
// Remove a contact point
void ContactManifold::removeContactPoint(int index) {
assert(mNbContactPoints > 0);
assert(index >= 0 && index < mNbContactPoints);
// Delete the contact
mContactPoints[index]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
for (int i=index; (i+1) < mNbContactPoints; i++) {
mContactPoints[i] = mContactPoints[i+1];
}
mNbContactPoints--;
}

27
src/collision/ContactManifold.h
View File

@ -31,6 +31,7 @@
#include "body/CollisionBody.h"
#include "collision/ProxyShape.h"
#include "constraint/ContactPoint.h"
#include "collision/ContactManifoldInfo.h"
#include "memory/PoolAllocator.h"
/// ReactPhysics3D namespace
@ -130,18 +131,6 @@ class ContactManifold {
// -------------------- Methods -------------------- //
/// Return the index of maximum area
int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
/// Return the index of the contact with the larger penetration depth.
int getIndexOfDeepestPenetration(ContactPoint* newContact) const;
/// Return the index that will be removed.
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
/// Remove a contact point from the manifold
void removeContactPoint(uint index);
/// Return true if the contact manifold has already been added into an island
bool isAlreadyInIsland() const;
@ -150,7 +139,7 @@ class ContactManifold {
// -------------------- Methods -------------------- //
/// Constructor
ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
ContactManifold(const ContactManifoldInfo& manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
PoolAllocator& memoryAllocator, short int normalDirectionId);
/// Destructor
@ -177,12 +166,6 @@ class ContactManifold {
/// Return the normal direction Id
short int getNormalDirectionId() const;
/// Add a contact point to the manifold
void addContactPoint(ContactPoint* contact);
/// Update the contact manifold.
void update(const Transform& transform1, const Transform& transform2);
/// Clear the contact manifold
void clear();
@ -231,6 +214,12 @@ class ContactManifold {
/// Return the largest depth of all the contact points
decimal getLargestContactDepth() const;
/// Add a contact point
void addContactPoint(const ContactPointInfo* contactPointInfo);
/// Remove a contact point
void removeContactPoint(int index);
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;

130
src/collision/ContactManifoldInfo.h Normal file
View File

@ -0,0 +1,130 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 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. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
// Libraries
#include "collision/ContactPointInfo.h"
#include "memory/Allocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class ContactManifoldInfo
/**
* This class is used to collect the list of ContactPointInfo that come
* from a collision test between two shapes.
*/
class ContactManifoldInfo {
private:
// -------------------- Attributes -------------------- //
/// Linked list with all the contact points
ContactPointInfo* mContactPointsList;
/// Memory allocator used to allocate contact points
Allocator& mAllocator;
// -------------------- Methods -------------------- //
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifoldInfo(Allocator& allocator) : mContactPointsList(nullptr), mAllocator(allocator) {}
/// 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));
}
}
/// Deleted copy-constructor
ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete;
/// Deleted assignment operator
ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete;
/// Add a new contact point into the manifold
void addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
assert(penDepth > decimal(0.0));
// Create the contact point info
ContactPointInfo* contactPointInfo = new (mAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
// Add it into the linked list of contact points
contactPointInfo->next = mContactPointsList;
mContactPointsList = contactPointInfo;
}
/// Get the first contact point info of the linked list of contact points
ContactPointInfo* getFirstContactPointInfo() const {
return mContactPointsList;
}
/// Reduce the number of points in the contact manifold
void reduce() {
// TODO : Implement this (do not forget to deallocate removed points)
}
/// Return the largest penetration depth among the contact points
decimal getLargestPenetrationDepth() const {
decimal maxDepth = decimal(0.0);
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
if (element->penetrationDepth > maxDepth) {
maxDepth = element->penetrationDepth;
}
element = element->next;
}
return maxDepth;
}
};
}
#endif

217
src/collision/ContactManifoldSet.cpp
View File

@ -43,92 +43,147 @@ ContactManifoldSet::~ContactManifoldSet() {
clear();
}
// Add a contact point to the manifold set
void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
void ContactManifoldSet::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
// Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contact->getNormal());
assert(contactManifoldInfo.getFirstContactPointInfo() != nullptr);
// If there is no contact manifold yet
if (mNbManifolds == 0) {
createManifold(normalDirectionId);
mManifolds[0]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0);
// If the maximum number of manifold is 1
if (mNbMaxManifolds == 1) {
createManifold(contactManifoldInfo, 0);
}
else {
// Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
createManifold(contactManifoldInfo, normalDirectionId);
}
}
else { // If there is already at least one contact manifold in the set
// If the maximum number of manifold is 1
if (mNbMaxManifolds == 1) {
// Replace the old manifold with the new one
updateManifoldWithNewOne(0, contactManifoldInfo);
}
else {
// Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contactManifoldInfo.getFirstContactPointInfo()->normal);
// Select the manifold with the most similar normal (if exists)
int similarManifoldIndex = 0;
if (mNbMaxManifolds > 1) {
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
}
// If a similar manifold has been found
if (similarManifoldIndex != -1) {
// Replace the old manifold with the new one
updateManifoldWithNewOne(similarManifoldIndex, contactManifoldInfo);
}
else {
// If we have not reach the maximum number of manifolds
if (mNbManifolds < mNbMaxManifolds) {
// Create the new contact manifold
createManifold(contactManifoldInfo, normalDirectionId);
}
else {
decimal newManifoldPenDepth = contactManifoldInfo.getLargestPenetrationDepth();
// We have reached the maximum number of manifold, we do not
// want to keep the manifold with the smallest penetration detph
int smallestPenDepthManifoldIndex = getManifoldWithSmallestContactPenetrationDepth(newManifoldPenDepth);
// If the manifold with the smallest penetration depth is not the new one,
// we have to keep the new manifold and remove the one with the smallest depth
if (smallestPenDepthManifoldIndex >= 0) {
removeManifold(smallestPenDepthManifoldIndex);
createManifold(contactManifoldInfo, normalDirectionId);
}
}
}
}
}
}
// Update a previous similar manifold with a new one
void ContactManifoldSet::updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold) {
// For each contact point of the previous manifold
for (int i=0; i<mManifolds[oldManifoldIndex]->getNbContactPoints(); ) {
ContactPoint* contactPoint = mManifolds[oldManifoldIndex]->getContactPoint(i);
// For each contact point in the new manifold
ContactPointInfo* newPoint = newManifold.getFirstContactPointInfo();
bool needToRemovePoint = true;
while (newPoint != nullptr) {
// If the new contact point is similar (very close) to the old contact point
if (!newPoint->isUsed && contactPoint->isSimilarWithContactPoint(newPoint)) {
// Replace (update) the old contact point with the new one
contactPoint->update(newPoint, mManifolds[oldManifoldIndex]->getShape1()->getLocalToWorldTransform(),
mManifolds[oldManifoldIndex]->getShape2()->getLocalToWorldTransform());
needToRemovePoint = false;
newPoint->isUsed = true;
break;
}
newPoint = newPoint->next;
}
// If no new contact point is similar to the old one
if (needToRemovePoint) {
// Remove the old contact point
mManifolds[oldManifoldIndex]->removeContactPoint(i);
}
else {
i++;
}
}
// For each point of the new manifold that have not been used yet (to update
// an existing point in the previous manifold), add it into the previous manifold
const ContactPointInfo* newPointInfo = newManifold.getFirstContactPointInfo();
while (newPointInfo != nullptr) {
if (!newPointInfo->isUsed) {
mManifolds[oldManifoldIndex]->addContactPoint(newPointInfo);
}
return;
}
newPointInfo = newPointInfo->next;
}
}
// Select the manifold with the most similar normal (if exists)
int similarManifoldIndex = 0;
if (mNbMaxManifolds > 1) {
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
}
// If a similar manifold has been found
if (similarManifoldIndex != -1) {
// Add the contact point to that similar manifold
mManifolds[similarManifoldIndex]->addContactPoint(contact);
assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0);
return;
}
// If the maximum number of manifold has not been reached yet
if (mNbManifolds < mNbMaxManifolds) {
// Create a new manifold for the contact point
createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact);
for (int i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0);
}
return;
}
// Return the manifold with the smallest contact penetration depth
int ContactManifoldSet::getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const {
// The contact point will be in a new contact manifold, we now have too much
// manifolds condidates. We need to remove one. We choose to keep the manifolds
// with the largest contact depth among their points
int smallestDepthIndex = -1;
decimal minDepth = contact->getPenetrationDepth();
// manifolds condidates. We need to remove one. We choose to remove the manifold
// with the smallest contact depth among their points
int smallestDepthManifoldIndex = -1;
decimal minDepth = initDepth;
assert(mNbManifolds == mNbMaxManifolds);
for (int i=0; i<mNbManifolds; i++) {
decimal depth = mManifolds[i]->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
smallestDepthIndex = i;
smallestDepthManifoldIndex = i;
}
}
// If we do not want to keep to new manifold (not created yet) with the
// new contact point
if (smallestDepthIndex == -1) {
// Delete the new contact
contact->~ContactPoint();
mMemoryAllocator.release(contact, sizeof(ContactPoint));
return;
}
assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds);
// Here we need to replace an existing manifold with a new one (that contains
// the new contact point)
removeManifold(smallestDepthIndex);
createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0);
}
return;
return smallestDepthManifoldIndex;
}
// Return the index of the contact manifold with a similar average normal.
@ -154,7 +209,7 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
int faceNo;
decimal u, v;
decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
decimal max = max3(std::fabs(normal.x), std::fabs(normal.y), std::fabs(normal.z));
Vector3 normalScaled = normal / max;
if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
@ -173,8 +228,8 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
v = normalScaled.y;
}
int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int indexU = std::floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int indexV = std::floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
@ -182,22 +237,6 @@ short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) cons
return faceNo * 200 + indexU * nbSubDivInFace + indexV;
}
// Update the contact manifolds
void ContactManifoldSet::update() {
for (int i=mNbManifolds-1; i>=0; i--) {
// Update the contact manifold
mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform());
// Remove the contact manifold if has no contact points anymore
if (mManifolds[i]->getNbContactPoints() == 0) {
removeManifold(i);
}
}
}
// Clear the contact manifold set
void ContactManifoldSet::clear() {
@ -210,11 +249,11 @@ void ContactManifoldSet::clear() {
}
// Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold(short int normalDirectionId) {
void ContactManifoldSet::createManifold(const ContactManifoldInfo& manifoldInfo, short int normalDirectionId) {
assert(mNbManifolds < mNbMaxManifolds);
mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId);
ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator, normalDirectionId);
mNbManifolds++;
}

17
src/collision/ContactManifoldSet.h
View File

@ -69,7 +69,7 @@ class ContactManifoldSet {
// -------------------- Methods -------------------- //
/// Create a new contact manifold and add it to the set
void createManifold(short normalDirectionId);
void createManifold(const ContactManifoldInfo& manifoldInfo, short normalDirectionId);
/// Remove a contact manifold from the set
void removeManifold(int index);
@ -82,6 +82,12 @@ class ContactManifoldSet {
// normal vector into of the of the bucket and returns a unique Id for the bucket
short int computeCubemapNormalId(const Vector3& normal) const;
/// Return the manifold with the smallest contact penetration depth
int getManifoldWithSmallestContactPenetrationDepth(decimal initDepth) const;
/// Update a previous similar manifold with a new one
void updateManifoldWithNewOne(int oldManifoldIndex, const ContactManifoldInfo& newManifold);
public:
// -------------------- Methods -------------------- //
@ -93,18 +99,15 @@ class ContactManifoldSet {
/// Destructor
~ContactManifoldSet();
/// Add a contact manifold in the set
void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
/// Return the first proxy shape
ProxyShape* getShape1() const;
/// Return the second proxy shape
ProxyShape* getShape2() const;
/// Add a contact point to the manifold set
void addContactPoint(ContactPoint* contact);
/// Update the contact manifolds
void update();
/// Clear the contact manifold set
void clear();

88
src/collision/ContactPointInfo.h Normal file
View File

@ -0,0 +1,88 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 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. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONTACT_POINT_INFO_H
#define REACTPHYSICS3D_CONTACT_POINT_INFO_H
// Libraries
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure ContactPointInfo
/**
* This structure contains informations about a collision contact
* computed during the narrow-phase collision detection. Those
* informations are used to compute the contact set for a contact
* between two bodies.
*/
struct ContactPointInfo {
private:
// -------------------- Methods -------------------- //
public:
// -------------------- Attributes -------------------- //
/// Normalized normal vector of the collision contact in world space
Vector3 normal;
/// Penetration depth of the contact
decimal penetrationDepth;
/// Contact point of body 1 in local space of body 1
Vector3 localPoint1;
/// Contact point of body 2 in local space of body 2
Vector3 localPoint2;
/// Pointer to the next contact point info
ContactPointInfo* next;
/// True if the contact point has already been inserted into a manifold
bool isUsed;
// -------------------- Methods -------------------- //
/// Constructor
ContactPointInfo(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2)
: normal(contactNormal), penetrationDepth(penDepth),
localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) {
}
/// Destructor
~ContactPointInfo() = default;
};
}
#endif

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

@ -30,8 +30,11 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) {
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
@ -39,7 +42,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
// Get the transform from capsule 1 local-space to capsule 2 local-space
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse();
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform;
// Compute the end-points of the inner segment of the first capsule
Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
@ -63,7 +66,7 @@ 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)
const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsDistance > sumRadius || segmentsDistance < MACHINE_EPSILON) {
if (segmentsDistance >= sumRadius || segmentsDistance < MACHINE_EPSILON) {
// 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.
@ -102,8 +105,9 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
Vector3 segment1ToSegment2 = (capsule2SegA - capsule1SegA);
Vector3 segment1ToSegment2Normalized = segment1ToSegment2.getUnit();
const Vector3 contactPointACapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointA - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
const Vector3 contactPointBCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (clipPointB - segment1ToSegment2 + segment1ToSegment2Normalized * capsuleShape1->getRadius());
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();
@ -112,9 +116,10 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal penetrationDepth = sumRadius - segmentsDistance;
// Create the contact info object
// TODO : Make sure we create two contact points at the same time (same method here)
contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointBCapsule1Local);
contactPointInfo.init(normalWorld, penetrationDepth, contactPointACapsule2Local, contactPointBCapsule2Local);
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
return true;
}
}
@ -129,7 +134,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
// If the collision shapes overlap
if (closestPointsDistanceSquare <= sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
if (closestPointsDistanceSquare < sumRadius * sumRadius && closestPointsDistanceSquare > MACHINE_EPSILON) {
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
closestPointsSeg1ToSeg2 /= closestPointsDistance;
@ -142,7 +147,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase
decimal penetrationDepth = sumRadius - closestPointsDistance;
// Create the contact info object
contactPointInfo.init(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
return true;
}

2
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
View File

@ -62,7 +62,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo) override;
ContactManifoldInfo& contactManifoldInfo) override;
};
}

2
src/collision/narrowphase/DefaultCollisionDispatch.cpp
View File

@ -45,7 +45,7 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
return &mSphereVsSphereAlgorithm;
}
// Sphere vs Capsule algorithm
if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::SPHERE) {
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CAPSULE) {
return &mSphereVsCapsuleAlgorithm;
}
// Capsule vs Capsule algorithm

6
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -47,7 +47,7 @@ using namespace reactphysics3d;
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo) {
ContactManifoldInfo& contactManifoldInfo) {
PROFILE("GJKAlgorithm::testCollision()");
@ -201,8 +201,8 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
return GJKResult::INTERPENETRATE;
}
// Create the contact info object
contactPointInfo.init(normal, penetrationDepth, pA, pB);
// Add a new contact point
contactManifoldInfo.addContactPoint(normal, penetrationDepth, pA, pB);
return GJKResult::COLLIDE_IN_MARGIN;
}

5
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -27,7 +27,8 @@
#define REACTPHYSICS3D_GJK_ALGORITHM_H
// Libraries
#include "constraint/ContactPoint.h"
#include "collision/ContactManifoldInfo.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/shapes/ConvexShape.h"
#include "VoronoiSimplex.h"
@ -87,7 +88,7 @@ class GJKAlgorithm {
/// Compute a contact info if the two bounding volumes collide.
GJKResult testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo);
ContactManifoldInfo& contactManifoldInfo);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);

4
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "collision/ContactManifoldInfo.h"
#include "memory/PoolAllocator.h"
#include "engine/OverlappingPair.h"
#include "collision/NarrowPhaseInfo.h"
@ -84,7 +84,7 @@ class NarrowPhaseAlgorithm {
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo)=0;
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo)=0;
};
}

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

@ -37,7 +37,7 @@
using namespace reactphysics3d;
bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo) {
ContactManifoldInfo& contactManifoldInfo) {
}

5
src/collision/narrowphase/SAT/SATAlgorithm.h
View File

@ -27,7 +27,8 @@
#define REACTPHYSICS3D_SAT_ALGORITHM_H
// Libraries
#include "constraint/ContactPoint.h"
#include "collision/ContactManifoldInfo.h"
#include "collision/NarrowPhaseInfo.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -59,7 +60,7 @@ class SATAlgorithm {
/// Compute a contact info if the two bounding volumes collide.
bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo);
ContactManifoldInfo& contactManifoldInfo);
};
}

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

@ -31,14 +31,17 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactPointInfo& contactPointInfo) {
bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes
const SphereShape* sphereShape = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
// Get the transform from sphere local-space to capsule local-space
const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape1ToWorldTransform * narrowPhaseInfo->shape2ToWorldTransform.getInverse();
const Transform sphereToCapsuleSpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform;
// Transform the center of the sphere into the local-space of the capsule shape
const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition();
@ -58,7 +61,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
// If the collision shapes overlap
if (sphereSegmentDistanceSquare <= sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) {
if (sphereSegmentDistanceSquare < sumRadius * sumRadius && sphereSegmentDistanceSquare > MACHINE_EPSILON) {
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
sphereCenterToSegment /= sphereSegmentDistance;
@ -71,7 +74,7 @@ bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseI
decimal penetrationDepth = sumRadius - sphereSegmentDistance;
// Create the contact info object
contactPointInfo.init(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal);
contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, contactPointSphereLocal, contactPointCapsuleLocal);
return true;
}

2
src/collision/narrowphase/SphereVsCapsuleAlgorithm.h
View File

@ -62,7 +62,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo) override;
ContactManifoldInfo& contactManifoldInfo) override;
};
}

6
src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp
View File

@ -33,7 +33,7 @@
using namespace reactphysics3d;
bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo) {
ContactManifoldInfo& contactManifoldInfo) {
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
@ -41,7 +41,7 @@ bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPha
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo);
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) {
@ -55,7 +55,7 @@ bool SphereVsConvexMeshAlgorithm::testCollision(const NarrowPhaseInfo* narrowPha
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
return satAlgorithm.testCollision(narrowPhaseInfo, contactPointInfo);
return satAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
}
return false;

2
src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h
View File

@ -62,7 +62,7 @@ class SphereVsConvexMeshAlgorithm : public NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo) override;
ContactManifoldInfo& contactManifoldInfo) override;
};
}

11
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -31,8 +31,11 @@
using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo) {
ContactManifoldInfo& contactManifoldInfo) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
@ -49,7 +52,7 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
if (squaredDistanceBetweenCenters < sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
@ -59,8 +62,8 @@ bool SphereVsSphereAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseIn
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
contactPointInfo.init(vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
contactManifoldInfo.addContactPoint(vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
return true;
}

2
src/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -62,7 +62,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactPointInfo& contactPointInfo) override;
ContactManifoldInfo& contactManifoldInfo) override;
};
}

2
src/collision/shapes/CollisionShape.h
View File

@ -40,7 +40,7 @@
namespace reactphysics3d {
/// Type of the collision shape
enum class CollisionShapeType {TRIANGLE, BOX, CAPSULE, SPHERE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
enum class CollisionShapeType {TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9;
// Declarations

32
src/constraint/ContactPoint.cpp
View File

@ -31,13 +31,35 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
: mNormal(contactInfo.normal),
mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnBody1(contactInfo.localPoint1),
mLocalPointOnBody2(contactInfo.localPoint2),
ContactPoint::ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
const Transform& body2Transform)
: mNormal(contactInfo->normal),
mPenetrationDepth(contactInfo->penetrationDepth),
mLocalPointOnBody1(contactInfo->localPoint1),
mLocalPointOnBody2(contactInfo->localPoint2),
mIsRestingContact(false) {
assert(mPenetrationDepth > decimal(0.0));
// Compute the world position of the contact points
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
}
// Update the contact point with a new one that is similar (very close)
/// The idea is to keep the cache impulse (for warm starting the contact solver)
void ContactPoint::update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
const Transform& body2Transform) {
assert(isSimilarWithContactPoint(contactInfo));
assert(contactInfo->penetrationDepth > decimal(0.0));
mNormal = contactInfo->normal;
mPenetrationDepth = contactInfo->penetrationDepth;
mLocalPointOnBody1 = contactInfo->localPoint1;
mLocalPointOnBody2 = contactInfo->localPoint2;
// Compute the world position of the contact points
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
}

86
src/constraint/ContactPoint.h
View File

@ -29,60 +29,13 @@
// Libraries
#include "body/CollisionBody.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/ContactPointInfo.h"
#include "configuration.h"
#include "mathematics/mathematics.h"
#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure ContactPointInfo
/**
* This structure contains informations about a collision contact
* computed during the narrow-phase collision detection. Those
* informations are used to compute the contact set for a contact
* between two bodies.
*/
struct ContactPointInfo {
private:
// -------------------- Methods -------------------- //
public:
// -------------------- Attributes -------------------- //
/// Normalized normal vector of the collision contact in world space
Vector3 normal;
/// Penetration depth of the contact
decimal penetrationDepth;
/// Contact point of body 1 in local space of body 1
Vector3 localPoint1;
/// Contact point of body 2 in local space of body 2
Vector3 localPoint2;
// -------------------- Methods -------------------- //
/// Constructor
ContactPointInfo() = default;
/// Destructor
~ContactPointInfo() = default;
/// Initialize the contact point info
void init(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
normal = contactNormal;
penetrationDepth = penDepth;
localPoint1 = localPt1;
localPoint2 = localPt2;
}
};
// Class ContactPoint
/**
* This class represents a collision contact point between two
@ -95,16 +48,16 @@ class ContactPoint {
// -------------------- Attributes -------------------- //
/// Normalized normal vector of the contact (from body1 toward body2) in world space
const Vector3 mNormal;
Vector3 mNormal;
/// Penetration depth
decimal mPenetrationDepth;
/// Contact point on body 1 in local space of body 1
const Vector3 mLocalPointOnBody1;
Vector3 mLocalPointOnBody1;
/// Contact point on body 2 in local space of body 2
const Vector3 mLocalPointOnBody2;
Vector3 mLocalPointOnBody2;
/// Contact point on body 1 in world space
Vector3 mWorldPointOnBody1;
@ -123,7 +76,8 @@ class ContactPoint {
// -------------------- Methods -------------------- //
/// Constructor
ContactPoint(const ContactPointInfo& contactInfo);
ContactPoint(const ContactPointInfo* contactInfo, const Transform& body1Transform,
const Transform& body2Transform);
/// Destructor
~ContactPoint() = default;
@ -134,11 +88,9 @@ class ContactPoint {
/// Deleted assignment operator
ContactPoint& operator=(const ContactPoint& contact) = delete;
/// Update the world contact points
void updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform);
/// Update the penetration depth
void updatePenetrationDepth();
/// Update the contact point with a new one that is similar (very close)
void update(const ContactPointInfo* contactInfo, const Transform& body1Transform,
const Transform& body2Transform);
/// Return the normal vector of the contact
Vector3 getNormal() const;
@ -158,6 +110,9 @@ class ContactPoint {
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
/// Return true if the contact point is similar (close enougth) to another given contact point
bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
@ -174,17 +129,6 @@ class ContactPoint {
size_t getSizeInBytes() const;
};
// Update the world contact points
inline void ContactPoint::updateWorldContactPoints(const Transform& body1Transform, const Transform& body2Transform) {
mWorldPointOnBody1 = body1Transform * mLocalPointOnBody1;
mWorldPointOnBody2 = body2Transform * mLocalPointOnBody2;
}
// Update the penetration depth
inline void ContactPoint::updatePenetrationDepth() {
mPenetrationDepth = (mWorldPointOnBody1 - mWorldPointOnBody2).dot(mNormal);
}
// Return the normal vector of the contact
inline Vector3 ContactPoint::getNormal() const {
return mNormal;
@ -215,6 +159,12 @@ inline decimal ContactPoint::getPenetrationImpulse() const {
return mPenetrationImpulse;
}
// Return true if the contact point is similar (close enougth) to another given contact point
inline bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* localContactPointBody1) const {
return (localContactPointBody1->localPoint1 - mLocalPointOnBody1).lengthSquare() <= (PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD);
}
// Set the cached penetration impulse
inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
mPenetrationImpulse = impulse;

6
src/engine/CollisionWorld.h
View File

@ -247,16 +247,16 @@ class CollisionCallback {
struct CollisionCallbackInfo {
public:
const ContactPointInfo& contactPoint;
const ContactManifoldInfo& contactManifold;
CollisionBody* body1;
CollisionBody* body2;
const ProxyShape* proxyShape1;
const ProxyShape* proxyShape2;
// Constructor
CollisionCallbackInfo(const ContactPointInfo& point, CollisionBody* b1, CollisionBody* b2,
CollisionCallbackInfo(const ContactManifoldInfo& manifold, CollisionBody* b1, CollisionBody* b2,
const ProxyShape* proxShape1, const ProxyShape* proxShape2) :
contactPoint(point), body1(b1), body2(b2),
contactManifold(manifold), body1(b1), body2(b2),
proxyShape1(proxShape1), proxyShape2(proxShape2) {
}

6
src/engine/EventListener.h
View File

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_EVENT_LISTENER_H
// Libraries
#include "constraint/ContactPoint.h"
#include "collision/ContactManifoldInfo.h"
namespace reactphysics3d {
@ -53,13 +53,13 @@ class EventListener {
/**
* @param contact Information about the contact
*/
virtual void beginContact(const ContactPointInfo& contact) {}
virtual void beginContact(const ContactManifoldInfo& contactManifold) {}
/// Called when a new contact point is found between two bodies
/**
* @param contact Information about the contact
*/
virtual void newContact(const ContactPointInfo& contact) {}
virtual void newContact(const ContactManifoldInfo& contactManifold) {}
/// Called at the beginning of an internal tick of the simulation step.
/// Each time the DynamicsWorld::update() method is called, the physics

16
src/engine/OverlappingPair.h
View File

@ -80,11 +80,8 @@ class OverlappingPair {
/// Return the pointer to second body
ProxyShape* getShape2() const;
/// Add a contact to the contact cache
void addContact(ContactPoint* contact);
/// Update the contact cache
void update();
/// Add a contact manifold
void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
/// Return the cached separating axis
Vector3 getCachedSeparatingAxis() const;
@ -123,13 +120,8 @@ inline ProxyShape* OverlappingPair::getShape2() const {
}
// Add a contact to the contact manifold
inline void OverlappingPair::addContact(ContactPoint* contact) {
mContactManifoldSet.addContactPoint(contact);
}
// Update the contact manifold
inline void OverlappingPair::update() {
mContactManifoldSet.update();
inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& contactManifoldInfo) {
mContactManifoldSet.addContactManifold(contactManifoldInfo);
}
// Return the cached separating axis