From b21a6bb59b104e54364ddf6a004484a1c5824f25 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 26 Feb 2017 13:48:50 +0200 Subject: [PATCH] Refactor contact manifold and contact point creation --- CMakeLists.txt | 2 + src/collision/CollisionDetection.cpp | 216 ++------------- src/collision/ContactManifold.cpp | 254 ++++-------------- src/collision/ContactManifold.h | 27 +- src/collision/ContactManifoldInfo.h | 130 +++++++++ src/collision/ContactManifoldSet.cpp | 217 +++++++++------ src/collision/ContactManifoldSet.h | 17 +- src/collision/ContactPointInfo.h | 88 ++++++ .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 25 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +- .../narrowphase/DefaultCollisionDispatch.cpp | 2 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 6 +- src/collision/narrowphase/GJK/GJKAlgorithm.h | 5 +- .../narrowphase/NarrowPhaseAlgorithm.h | 4 +- .../narrowphase/SAT/SATAlgorithm.cpp | 2 +- src/collision/narrowphase/SAT/SATAlgorithm.h | 5 +- .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 11 +- .../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +- .../SphereVsConvexMeshAlgorithm.cpp | 6 +- .../narrowphase/SphereVsConvexMeshAlgorithm.h | 2 +- .../narrowphase/SphereVsSphereAlgorithm.cpp | 11 +- .../narrowphase/SphereVsSphereAlgorithm.h | 2 +- src/collision/shapes/CollisionShape.h | 2 +- src/constraint/ContactPoint.cpp | 32 ++- src/constraint/ContactPoint.h | 86 ++---- src/engine/CollisionWorld.h | 6 +- src/engine/EventListener.h | 6 +- src/engine/OverlappingPair.h | 16 +- 28 files changed, 543 insertions(+), 641 deletions(-) create mode 100644 src/collision/ContactManifoldInfo.h create mode 100644 src/collision/ContactPointInfo.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 37b8febe..58ea37d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 96f683d7..4a0ef718 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -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& shapes1, - const std::set& shapes2) { - - // For each possible collision pair of bodies - map::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; jgetNbContactPoints(); 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& shapes1, - const std::set& shapes2) { - - mContactOverlappingPairs.clear(); - - // For each possible collision pair of bodies - map::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::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(shape1Type); - const int shape2Index = static_cast(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 diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index dc40774b..36103642 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -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; igetWorldPointOnBody1() - - 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; iupdateWorldContactPoints(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(mNbContactPoints)-1; i>=0; i--) { - assert(i < static_cast(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; igetPenetrationDepth() > 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; igetLocalToWorldTransform(), 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--; +} diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index c36c1476..da323c0f 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -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; diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h new file mode 100644 index 00000000..d4afc590 --- /dev/null +++ b/src/collision/ContactManifoldInfo.h @@ -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 + diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index a7dc9d2e..c9b0d5bb 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -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; igetNbContactPoints() > 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; igetNbContactPoints(); ) { + + 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; igetNbContactPoints() > 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; igetLargestContactDepth(); 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; igetNbContactPoints() > 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++; } diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index 21944e36..1a2bac97 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -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(); diff --git a/src/collision/ContactPointInfo.h b/src/collision/ContactPointInfo.h new file mode 100644 index 00000000..1ff8bfdd --- /dev/null +++ b/src/collision/ContactPointInfo.h @@ -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 diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 2f9f8703..e83d694c 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -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(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; } diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 7a46f73a..898d6337 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -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; }; } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 88d4b9b2..5fdd485b 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -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 diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 4ea55ba4..6be90769 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -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; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index 8ca25e33..f40592c4 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -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); diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index f7f68907..f665181b 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -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; }; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index d74faf09..b46ca386 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -37,7 +37,7 @@ using namespace reactphysics3d; bool SATAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, - ContactPointInfo& contactPointInfo) { + ContactManifoldInfo& contactManifoldInfo) { } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index 30156cc2..80feece1 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -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); }; } diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index 24cb4e96..aace17fd 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -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(narrowPhaseInfo->collisionShape1); const CapsuleShape* capsuleShape = static_cast(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; } diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 98eaf894..784df6f9 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -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; }; } diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp index 65371e4f..06dbc615 100644 --- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.cpp @@ -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; diff --git a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h index 448b8a5d..f0046545 100644 --- a/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexMeshAlgorithm.h @@ -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; }; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp index e80688d5..54e51e52 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.cpp @@ -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(narrowPhaseInfo->collisionShape1); const SphereShape* sphereShape2 = static_cast(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; } diff --git a/src/collision/narrowphase/SphereVsSphereAlgorithm.h b/src/collision/narrowphase/SphereVsSphereAlgorithm.h index be10d5a1..b70befd2 100644 --- a/src/collision/narrowphase/SphereVsSphereAlgorithm.h +++ b/src/collision/narrowphase/SphereVsSphereAlgorithm.h @@ -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; }; } diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 464c1bd5..c2db3466 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -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 diff --git a/src/constraint/ContactPoint.cpp b/src/constraint/ContactPoint.cpp index 00ed60d7..f420bbff 100644 --- a/src/constraint/ContactPoint.cpp +++ b/src/constraint/ContactPoint.cpp @@ -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; } diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 4028ed8f..ec4c6556 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -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; diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index dc162716..0e24f0c8 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -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) { } diff --git a/src/engine/EventListener.h b/src/engine/EventListener.h index 562e8ac7..e8594255 100644 --- a/src/engine/EventListener.h +++ b/src/engine/EventListener.h @@ -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 diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 70ee1c3f..c34b91b7 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -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