diff --git a/include/reactphysics3d/collision/ContactManifoldInfo.h b/include/reactphysics3d/collision/ContactManifoldInfo.h index b6dd9219..0e45eed0 100644 --- a/include/reactphysics3d/collision/ContactManifoldInfo.h +++ b/include/reactphysics3d/collision/ContactManifoldInfo.h @@ -49,7 +49,7 @@ struct ContactManifoldInfo { uint8 nbPotentialContactPoints; /// Indices of the contact points in the mPotentialContactPoints array - uint potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD]; + uint32 potentialContactPointsIndices[NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD]; /// Overlapping pair id diff --git a/include/reactphysics3d/configuration.h b/include/reactphysics3d/configuration.h index be47bc7b..469b7cdb 100644 --- a/include/reactphysics3d/configuration.h +++ b/include/reactphysics3d/configuration.h @@ -124,7 +124,10 @@ constexpr uint8 NB_MAX_CONTACT_MANIFOLDS = 3; constexpr uint8 NB_MAX_POTENTIAL_CONTACT_MANIFOLDS = 4 * NB_MAX_CONTACT_MANIFOLDS; /// Maximum number of contact points in potential contact manifold -constexpr uint8 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 16; +constexpr uint16 NB_MAX_CONTACT_POINTS_IN_POTENTIAL_MANIFOLD = 256; + +/// Distance threshold to consider that two contact points in a manifold are the same +constexpr decimal SAME_CONTACT_POINT_DISTANCE_THRESHOLD = decimal(0.01); /// Current version of ReactPhysics3D const std::string RP3D_VERSION = std::string("0.8.0"); diff --git a/include/reactphysics3d/systems/CollisionDetectionSystem.h b/include/reactphysics3d/systems/CollisionDetectionSystem.h index a318d7dd..c8c4983c 100644 --- a/include/reactphysics3d/systems/CollisionDetectionSystem.h +++ b/include/reactphysics3d/systems/CollisionDetectionSystem.h @@ -291,6 +291,9 @@ class CollisionDetectionSystem { /// Remove an element in an array (and replace it by the last one in the array) void removeItemAtInArray(uint array[], uint8 index, uint8& arraySize) const; + /// Remove the duplicated contact points in a given contact manifold + void removeDuplicatedContactPointsInManifold(ContactManifoldInfo& manifold, const Array& potentialContactPoints) const; + public : // -------------------- Methods -------------------- // diff --git a/src/systems/CollisionDetectionSystem.cpp b/src/systems/CollisionDetectionSystem.cpp index cdbe20b4..13b9d1c3 100644 --- a/src/systems/CollisionDetectionSystem.cpp +++ b/src/systems/CollisionDetectionSystem.cpp @@ -1359,6 +1359,9 @@ void CollisionDetectionSystem::reducePotentialContactManifolds(Array& potentialContactPoints) const { + + RP3D_PROFILE("CollisionDetectionSystem::removeDuplicatedContactPointsInManifold()", mProfiler); + + const decimal distThresholdSqr = SAME_CONTACT_POINT_DISTANCE_THRESHOLD * SAME_CONTACT_POINT_DISTANCE_THRESHOLD; + + // For each contact point of the manifold + for (uint32 i=0; i < manifold.nbPotentialContactPoints; i++) { + for (uint32 j=i+1; j < manifold.nbPotentialContactPoints; j++) { + + const ContactPointInfo& point1 = potentialContactPoints[manifold.potentialContactPointsIndices[i]]; + const ContactPointInfo& point2 = potentialContactPoints[manifold.potentialContactPointsIndices[j]]; + + // Compute the distance between the two contact points + const decimal distSqr = (point2.localPoint1 - point1.localPoint1).lengthSquare(); + + // We have a found a duplicated contact point + if (distSqr < distThresholdSqr) { + + // Remove the duplicated contact point + manifold.potentialContactPointsIndices[j] = manifold.potentialContactPointsIndices[manifold.nbPotentialContactPoints-1]; + manifold.nbPotentialContactPoints--; + + j--; + } + } + } +} + // Report contacts and triggers void CollisionDetectionSystem::reportContactsAndTriggers() {