From fc91540a79644daa231f7f1a22fce6a29b0c9911 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 2 Oct 2015 07:04:05 +0200 Subject: [PATCH] Add ContactManifoldSet class --- CMakeLists.txt | 6 +- src/body/CollisionBody.cpp | 2 +- src/collision/CollisionDetection.cpp | 6 +- src/{engine => collision}/ContactManifold.cpp | 0 src/{engine => collision}/ContactManifold.h | 17 +++ src/collision/ContactManifoldSet.cpp | 108 +++++++++++++++ src/collision/ContactManifoldSet.h | 129 ++++++++++++++++++ src/configuration.h | 5 + src/constraint/ContactPoint.h | 4 +- src/engine/ContactSolver.h | 2 +- src/engine/Island.h | 2 +- src/engine/OverlappingPair.cpp | 2 +- src/engine/OverlappingPair.h | 20 +-- 13 files changed, 284 insertions(+), 19 deletions(-) rename src/{engine => collision}/ContactManifold.cpp (100%) rename src/{engine => collision}/ContactManifold.h (95%) create mode 100644 src/collision/ContactManifoldSet.cpp create mode 100644 src/collision/ContactManifoldSet.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 12618719..b71e60b6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -109,6 +109,10 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/CollisionDetection.h" "src/collision/CollisionDetection.cpp" "src/collision/CollisionShapeInfo.h" + "src/collision/ContactManifold.h" + "src/collision/ContactManifold.cpp" + "src/collision/ContactManifoldSet.h" + "src/collision/ContactManifoldSet.cpp" "src/constraint/BallAndSocketJoint.h" "src/constraint/BallAndSocketJoint.cpp" "src/constraint/ContactPoint.h" @@ -125,8 +129,6 @@ SET (REACTPHYSICS3D_SOURCES "src/engine/CollisionWorld.cpp" "src/engine/ConstraintSolver.h" "src/engine/ConstraintSolver.cpp" - "src/engine/ContactManifold.h" - "src/engine/ContactManifold.cpp" "src/engine/ContactSolver.h" "src/engine/ContactSolver.cpp" "src/engine/DynamicsWorld.h" diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 1de04876..2297157a 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -26,7 +26,7 @@ // Libraries #include "CollisionBody.h" #include "engine/CollisionWorld.h" -#include "engine/ContactManifold.h" +#include "collision/ContactManifold.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 0ff3ea55..904b03c9 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -431,11 +431,15 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair, ContactPoint(*contactInfo); assert(contact != NULL); - // Add the contact to the contact cache of the corresponding overlapping pair + // Add the contact to the contact manifold set of the corresponding overlapping pair overlappingPair->addContact(contact); // Add the contact manifold into the list of contact manifolds // of the two bodies involved in the contact + + // TODO : It seems that we add the same manifold multiple times to the same body + // in case we call createContact() multiple time for different contact points from the + // same manifold. Check that it is not the case. addContactManifoldToBody(overlappingPair->getContactManifold(), overlappingPair->getShape1()->getBody(), overlappingPair->getShape2()->getBody()); diff --git a/src/engine/ContactManifold.cpp b/src/collision/ContactManifold.cpp similarity index 100% rename from src/engine/ContactManifold.cpp rename to src/collision/ContactManifold.cpp diff --git a/src/engine/ContactManifold.h b/src/collision/ContactManifold.h similarity index 95% rename from src/engine/ContactManifold.h rename to src/collision/ContactManifold.h index ac661525..4f7f0115 100644 --- a/src/engine/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -119,6 +119,9 @@ class ContactManifold { /// True if the contact manifold has already been added into an island bool mIsAlreadyInIsland; + /// Average contact normal vector of all points in the manifold + Vector3 mAverageContactNormal; + /// Reference to the memory allocator MemoryAllocator& mMemoryAllocator; @@ -144,6 +147,9 @@ class ContactManifold { /// Return true if the contact manifold has already been added into an island bool isAlreadyInIsland() const; + + /// Return the normalized averaged normal vector + Vector3 getAverageContactNormal() const; public: @@ -306,6 +312,17 @@ inline bool ContactManifold::isAlreadyInIsland() const { return mIsAlreadyInIsland; } +// Return the normalized averaged normal vector +inline Vector3 ContactManifold::getAverageContactNormal() const { + Vector3 averageNormal; + + for (int i=0; igetNormal(); + } + + return averageNormal.getUnit(); +} + } #endif diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp new file mode 100644 index 00000000..5fd7f57f --- /dev/null +++ b/src/collision/ContactManifoldSet.cpp @@ -0,0 +1,108 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2015 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "ContactManifoldSet.h" + +using namespace reactphysics3d; + +// Constructor +ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, + MemoryAllocator& memoryAllocator, int nbMaxManifolds) + : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), + mShape2(shape2), mMemoryAllocator(memoryAllocator) { + +} + +// Destructor +ContactManifoldSet::~ContactManifoldSet() { + +} + +// Add a contact point to the manifold set +void ContactManifoldSet::addContactPoint(ContactPoint* contact) { + + // If there is no contact manifold yet + if (mNbManifolds == 0) { + + createManifold(); + mManifolds[0]->addContactPoint(contact); + + return; + } + + +} + +// Return the index of the contact manifold with a similar average normal. +// If no manifold has close enough average normal, it returns -1 +int ContactManifoldSet::selectManifoldWithSimilarNormal(const Vector3& normal) { + + decimal maxDotProduct; + int indexManifold = -1; + for (int i=0; igetAverageContactNormal()); + if (dotProduct > maxDotProduct) { + maxDotProduct = dotProduct; + indexManifold = i; + } + } + + return indexManifold; +} + +// Update the contact manifolds +void ContactManifoldSet::update(const Transform& transform1, const Transform& transform2) { + + for (int i=0; iupdate(transform1, transform2); + } +} + +// Clear the contact manifold set +void ContactManifoldSet::clear() { + +} + +// Create a new contact manifold and add it to the set +void ContactManifoldSet::createManifold() { + assert(mNbManifolds < MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET); + + mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold))) + ContactManifold(mShape1, mShape2, mMemoryAllocator); + mNbManifolds++; +} + +// Remove a contact manifold from the set +void ContactManifoldSet::removeManifold(int index) { + + assert(index >= 0 && index < mNbManifolds); + + // Delete the new contact + mManifolds[index]->~ContactManifold(); + mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold)); + + mNbManifolds--; +} diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h new file mode 100644 index 00000000..948f4421 --- /dev/null +++ b/src/collision/ContactManifoldSet.h @@ -0,0 +1,129 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2015 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_SET_H +#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H + +// Libraries +#include "ContactManifold.h" + +namespace reactphysics3d { + +// Constants +const uint MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set + +// Class ContactManifoldSet +/** + * This class represents a set of one or several contact manifolds. Typically a + * convex/convex collision will have a set with a single manifold and a convex-concave + * collision can have more than one manifolds. Note that a contact manifold can + * contains several contact points. + */ +class ContactManifoldSet { + + private: + + // -------------------- Attributes -------------------- // + + /// Maximum number of contact manifolds in the set + int mNbMaxManifolds; + + /// Current number of contact manifolds in the set + int mNbManifolds; + + /// Pointer to the first proxy shape of the contact + ProxyShape* mShape1; + + /// Pointer to the second proxy shape of the contact + ProxyShape* mShape2; + + /// Reference to the memory allocator + MemoryAllocator& mMemoryAllocator; + + /// Contact manifolds of the set + ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; + + // -------------------- Methods -------------------- // + + /// Create a new contact manifold and add it to the set + void createManifold(); + + /// Remove a contact manifold from the set + void removeManifold(int index); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, + MemoryAllocator& memoryAllocator, int nbMaxManifolds); + + /// Destructor + ~ContactManifoldSet(); + + /// Add a contact point to the manifold set + void addContactPoint(ContactPoint* contact); + + /// Update the contact manifolds + void update(const Transform& transform1, const Transform& transform2); + + /// Clear the contact manifold set + void clear(); + + /// Return the number of manifolds in the set + int getNbContactManifolds() const; + + /// Return a given contact manifold + ContactManifold* getContactManifold(uint index); + + /// Return the total number of contact points in the set of manifolds + int getTotalNbContactPoints() const; +}; + +// Return the number of manifolds in the set +inline int ContactManifoldSet::getNbContactManifolds() const { + return mNbManifolds; +} + +// Return a given contact manifold +inline ContactManifold* ContactManifoldSet::getContactManifold(uint index) { + assert(index < mNbManifolds); + return mManifolds[index]; +} + +// Return the total number of contact points in the set of manifolds +inline int ContactManifoldSet::getTotalNbContactPoints() const { + int nbPoints = 0; + for (int i=0; igetNbContactPoints(); + } + return nbPoints; +} + +} + +#endif + diff --git a/src/configuration.h b/src/configuration.h index 50d1872c..3a8d792b 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -128,6 +128,11 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1); /// followin constant with the linear velocity and the elapsed time between two frames. const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7); +/// This value is used to compare the normal of a contact point and the average +/// normal of a whole contact manifold to decide if the point should be part +/// of the manifold or not +const decimal MANIFOLD_SIMILAR_NORMAL_DOTPRODUCT_THRESHOLD = 0.99f; + } #endif diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index 67e92705..c4240143 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -65,7 +65,7 @@ struct ContactPointInfo { /// Second proxy shape of the contact ProxyShape* shape2; - /// Normal vector the the collision contact in world space + /// Normalized normal vector the the collision contact in world space const Vector3 normal; /// Penetration depth of the contact @@ -107,7 +107,7 @@ class ContactPoint { /// Second rigid body of the contact CollisionBody* mBody2; - /// Normal vector of the contact (From body1 toward body2) in world space + /// Normalized normal vector of the contact (From body1 toward body2) in world space const Vector3 mNormal; /// Penetration depth diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index dc2e52ee..9c4255f1 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -30,7 +30,7 @@ #include "constraint/ContactPoint.h" #include "configuration.h" #include "constraint/Joint.h" -#include "ContactManifold.h" +#include "collision/ContactManifold.h" #include "Island.h" #include "Impulse.h" #include diff --git a/src/engine/Island.h b/src/engine/Island.h index d4aa01a6..b29088f7 100644 --- a/src/engine/Island.h +++ b/src/engine/Island.h @@ -30,7 +30,7 @@ #include "memory/MemoryAllocator.h" #include "body/RigidBody.h" #include "constraint/Joint.h" -#include "ContactManifold.h" +#include "collision/ContactManifold.h" namespace reactphysics3d { diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 979189b6..3eb4c9f1 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -33,7 +33,7 @@ using namespace reactphysics3d; OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator) : mShape1(shape1), mShape2(shape2), - mContactManifold(shape1, shape2, memoryAllocator), + mContactManifoldSet(shape1, shape2, memoryAllocator), mCachedSeparatingAxis(1.0, 1.0, 1.0) { } diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index da16d6e4..946e72ea 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_OVERLAPPING_PAIR_H // Libraries -#include "ContactManifold.h" +#include "collision/ContactManifoldSet.h" #include "collision/ProxyShape.h" #include "collision/shapes/CollisionShape.h" @@ -57,8 +57,8 @@ class OverlappingPair { /// Pointer to the second proxy collision shape ProxyShape* mShape2; - /// Persistent contact manifold - ContactManifold mContactManifold; + /// Set of persistent contact manifolds + ContactManifoldSet mContactManifoldSet; /// Cached previous separating axis Vector3 mCachedSeparatingAxis; @@ -131,13 +131,13 @@ inline ProxyShape* OverlappingPair::getShape2() const { // Add a contact to the contact manifold inline void OverlappingPair::addContact(ContactPoint* contact) { - mContactManifold.addContactPoint(contact); + mContactManifoldSet.addContactPoint(contact); } // Update the contact manifold inline void OverlappingPair::update() { - mContactManifold.update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(), - mShape2->getBody()->getTransform() *mShape2->getLocalToBodyTransform()); + mContactManifoldSet.update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(), + mShape2->getBody()->getTransform() *mShape2->getLocalToBodyTransform()); } // Return the cached separating axis @@ -153,12 +153,12 @@ inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) { // Return the number of contact points in the contact manifold inline uint OverlappingPair::getNbContactPoints() const { - return mContactManifold.getNbContactPoints(); + return mContactManifoldSet.getTotalNbContactPoints(); } // Return the contact manifold -inline ContactManifold* OverlappingPair::getContactManifold() { - return &mContactManifold; +inline ContactManifold* OverlappingPair::getContactManifoldSet() { + return &mContactManifoldSet; } // Return the pair of bodies index @@ -187,7 +187,7 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body // Clear the contact points of the contact manifold inline void OverlappingPair::clearContactPoints() { - mContactManifold.clear(); + mContactManifoldSet.clear(); } }