diff --git a/CMakeLists.txt b/CMakeLists.txt index e85693d7..69f20522 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -174,8 +174,10 @@ SET (REACTPHYSICS3D_SOURCES "src/mathematics/Vector3.h" "src/mathematics/Ray.h" "src/mathematics/Vector3.cpp" - "src/memory/MemoryAllocator.h" - "src/memory/MemoryAllocator.cpp" + "src/memory/PoolAllocator.h" + "src/memory/PoolAllocator.cpp" + "src/memory/SingleFrameAllocator.h" + "src/memory/SingleFrameAllocator.cpp" "src/memory/Stack.h" ) diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index daa4a71e..db22f606 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -70,7 +70,7 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape, const Transform& transform) { // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, transform, decimal(1)); @@ -116,7 +116,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { } current->~ProxyShape(); - mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); + mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -136,7 +136,7 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) { } elementToRemove->~ProxyShape(); - mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape)); + mWorld.mPoolAllocator.release(elementToRemove, sizeof(ProxyShape)); mNbCollisionShapes--; return; } @@ -162,7 +162,7 @@ void CollisionBody::removeAllCollisionShapes() { } current->~ProxyShape(); - mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape)); + mWorld.mPoolAllocator.release(current, sizeof(ProxyShape)); // Get the next element in the list current = nextElement; @@ -181,7 +181,7 @@ void CollisionBody::resetContactManifoldsList() { // Delete the current element currentElement->~ContactManifoldListElement(); - mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement)); + mWorld.mPoolAllocator.release(currentElement, sizeof(ContactManifoldListElement)); currentElement = nextElement; } diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 5fda6958..bd5c7818 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -34,7 +34,7 @@ #include "collision/shapes/AABB.h" #include "collision/shapes/CollisionShape.h" #include "collision/RaycastInfo.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "configuration.h" /// Namespace reactphysics3d diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index ff77b228..74ee8a05 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -166,7 +166,7 @@ void RigidBody::setMass(decimal mass) { } // Remove a joint from the joints list -void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) { +void RigidBody::removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint) { assert(joint != nullptr); assert(mJointsList != nullptr); @@ -214,7 +214,7 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape, decimal mass) { // Create a new proxy collision shape to attach the collision shape to the body - ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate( + ProxyShape* proxyShape = new (mWorld.mPoolAllocator.allocate( sizeof(ProxyShape))) ProxyShape(this, collisionShape, transform, mass); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 8fb36d0e..0ffc6257 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -31,7 +31,7 @@ #include "CollisionBody.h" #include "engine/Material.h" #include "mathematics/mathematics.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" /// Namespace reactphysics3d namespace reactphysics3d { @@ -104,7 +104,7 @@ class RigidBody : public CollisionBody { // -------------------- Methods -------------------- // /// Remove a joint from the joints list - void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint); + void removeJointFromJointsList(PoolAllocator& memoryAllocator, const Joint* joint); /// Update the transform of the body after a change of the center of mass void updateTransformWithCenterOfMass(); diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 7d9f6fb2..c3caa97a 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -41,7 +41,7 @@ using namespace reactphysics3d; using namespace std; // Constructor -CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator) +CollisionDetection::CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator), mWorld(world), mBroadPhaseAlgorithm(*this), mIsCollisionShapesAdded(false) { @@ -189,7 +189,7 @@ void CollisionDetection::computeNarrowPhase() { // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; } @@ -294,7 +294,7 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); continue; } @@ -370,8 +370,8 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro shape2->getCollisionShape()->getType()); // Create the overlapping pair and add it into the set of overlapping pairs - OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair))) - OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator); + OverlappingPair* newPair = new (mWorld->mPoolAllocator.allocate(sizeof(OverlappingPair))) + OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mPoolAllocator); assert(newPair != nullptr); #ifndef NDEBUG @@ -400,7 +400,7 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) { // Destroy the overlapping pair itToRemove->second->~OverlappingPair(); - mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair)); + mWorld->mPoolAllocator.release(itToRemove->second, sizeof(OverlappingPair)); mOverlappingPairs.erase(itToRemove); } else { @@ -434,7 +434,7 @@ void CollisionDetection::createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) { // Create a new contact - ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint))) + ContactPoint* contact = new (mWorld->mPoolAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactInfo); // Add the contact to the contact manifold set of the corresponding overlapping pair @@ -477,7 +477,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { // Add the contact manifold at the beginning of the linked // list of contact manifolds of the first body - void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + void* allocatedMemory1 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); ContactManifoldListElement* listElement1 = new (allocatedMemory1) ContactManifoldListElement(contactManifold, body1->mContactManifoldsList); @@ -485,7 +485,7 @@ void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) { // Add the contact manifold at the beginning of the linked // list of the contact manifolds of the second body - void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + void* allocatedMemory2 = mWorld->mPoolAllocator.allocate(sizeof(ContactManifoldListElement)); ContactManifoldListElement* listElement2 = new (allocatedMemory2) ContactManifoldListElement(contactManifold, body2->mContactManifoldsList); @@ -520,8 +520,8 @@ EventListener* CollisionDetection::getWorldEventListener() { } /// Return a reference to the world memory allocator -MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() { - return mWorld->mMemoryAllocator; +PoolAllocator& CollisionDetection::getWorldMemoryAllocator() { + return mWorld->mPoolAllocator; } // Called by a narrow-phase collision algorithm when a new contact has been found diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 75f278ba..0fe486c4 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -32,7 +32,7 @@ #include "engine/OverlappingPair.h" #include "engine/EventListener.h" #include "narrowphase/DefaultCollisionDispatch.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "constraint/ContactPoint.h" #include #include @@ -93,7 +93,7 @@ class CollisionDetection : public NarrowPhaseCallback { NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES]; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; /// Pointer to the physics world CollisionWorld* mWorld; @@ -143,7 +143,7 @@ class CollisionDetection : public NarrowPhaseCallback { // -------------------- Methods -------------------- // /// Constructor - CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator); + CollisionDetection(CollisionWorld* world, PoolAllocator& memoryAllocator); /// Destructor ~CollisionDetection() = default; @@ -220,7 +220,7 @@ class CollisionDetection : public NarrowPhaseCallback { EventListener* getWorldEventListener(); /// Return a reference to the world memory allocator - MemoryAllocator& getWorldMemoryAllocator(); + PoolAllocator& getWorldMemoryAllocator(); /// Called by a narrow-phase collision algorithm when a new contact has been found virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) override; diff --git a/src/collision/ContactManifold.cpp b/src/collision/ContactManifold.cpp index cfa905ff..ff15bce6 100644 --- a/src/collision/ContactManifold.cpp +++ b/src/collision/ContactManifold.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; // Constructor ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, short normalDirectionId) + PoolAllocator& memoryAllocator, short normalDirectionId) : mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId), mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index eb7716df..2f77e9b2 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -31,7 +31,7 @@ #include "body/CollisionBody.h" #include "collision/ProxyShape.h" #include "constraint/ContactPoint.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -126,7 +126,7 @@ class ContactManifold { bool mIsAlreadyInIsland; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; // -------------------- Methods -------------------- // @@ -151,7 +151,7 @@ class ContactManifold { /// Constructor ContactManifold(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, short int normalDirectionId); + PoolAllocator& memoryAllocator, short int normalDirectionId); /// Destructor ~ContactManifold(); diff --git a/src/collision/ContactManifoldSet.cpp b/src/collision/ContactManifoldSet.cpp index 54e3614e..26f3a66f 100644 --- a/src/collision/ContactManifoldSet.cpp +++ b/src/collision/ContactManifoldSet.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, int nbMaxManifolds) + PoolAllocator& memoryAllocator, int nbMaxManifolds) : mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1), mShape2(shape2), mMemoryAllocator(memoryAllocator) { assert(nbMaxManifolds >= 1); diff --git a/src/collision/ContactManifoldSet.h b/src/collision/ContactManifoldSet.h index b167f506..21944e36 100644 --- a/src/collision/ContactManifoldSet.h +++ b/src/collision/ContactManifoldSet.h @@ -61,7 +61,7 @@ class ContactManifoldSet { ProxyShape* mShape2; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; /// Contact manifolds of the set ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET]; @@ -88,7 +88,7 @@ class ContactManifoldSet { /// Constructor ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2, - MemoryAllocator& memoryAllocator, int nbMaxManifolds); + PoolAllocator& memoryAllocator, int nbMaxManifolds); /// Destructor ~ContactManifoldSet(); diff --git a/src/collision/narrowphase/CollisionDispatch.h b/src/collision/narrowphase/CollisionDispatch.h index 8fa24f87..6e027721 100644 --- a/src/collision/narrowphase/CollisionDispatch.h +++ b/src/collision/narrowphase/CollisionDispatch.h @@ -51,7 +51,7 @@ class CollisionDispatch { /// Initialize the collision dispatch configuration virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + PoolAllocator* memoryAllocator) { } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 109b5de3..f6812d74 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; /// Initialize the collision dispatch configuration void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + PoolAllocator* memoryAllocator) { // Initialize the collision algorithms mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator); diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 9bf15b11..5dd07bf2 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -63,7 +63,7 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Initialize the collision dispatch configuration virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) override; + PoolAllocator* memoryAllocator) override; /// Select and return the narrow-phase collision detection algorithm to /// use between two types of collision shapes. diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h index f585ac15..05a78eea 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h @@ -34,7 +34,7 @@ #include "collision/narrowphase/NarrowPhaseAlgorithm.h" #include "mathematics/mathematics.h" #include "TriangleEPA.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include /// ReactPhysics3D namespace @@ -88,7 +88,7 @@ class EPAAlgorithm { // -------------------- Attributes -------------------- // /// Reference to the memory allocator - MemoryAllocator* mMemoryAllocator; + PoolAllocator* mMemoryAllocator; /// Triangle comparison operator TriangleComparison mTriangleComparison; @@ -120,7 +120,7 @@ class EPAAlgorithm { EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete; /// Initalize the algorithm - void init(MemoryAllocator* memoryAllocator); + void init(PoolAllocator* memoryAllocator); /// Compute the penetration depth with EPA algorithm. bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, @@ -151,7 +151,7 @@ inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** } // Initalize the algorithm -inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) { +inline void EPAAlgorithm::init(PoolAllocator* memoryAllocator) { mMemoryAllocator = memoryAllocator; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index e81ee9b3..b6cce5c2 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -94,7 +94,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { /// Initalize the algorithm virtual void init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) override; + PoolAllocator* memoryAllocator) override; /// Compute a contact info if the two bounding volumes collide. virtual void testCollision(const CollisionShapeInfo& shape1Info, @@ -110,7 +110,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { // Initalize the algorithm inline void GJKAlgorithm::init(CollisionDetection* collisionDetection, - MemoryAllocator* memoryAllocator) { + PoolAllocator* memoryAllocator) { NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator); mAlgoEPA.init(memoryAllocator); } diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp index 382ccc9b..590182ac 100644 --- a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp @@ -563,7 +563,6 @@ bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Ve if (squareDist < closestSquareDistance) { // Use it as the current closest point - closestSquareDistance = squareDist; baryCoordsAB.setAllValues(0.0, triangleBaryCoords[0]); baryCoordsCD.setAllValues(triangleBaryCoords[2], triangleBaryCoords[1]); bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 1, 3, 2); diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp b/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp index 0fbbe5fe..782f8d6f 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.cpp @@ -36,7 +36,7 @@ NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() } // Initalize the algorithm -void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) { +void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator) { mCollisionDetection = collisionDetection; mMemoryAllocator = memoryAllocator; } diff --git a/src/collision/narrowphase/NarrowPhaseAlgorithm.h b/src/collision/narrowphase/NarrowPhaseAlgorithm.h index 30b216f0..0759d138 100644 --- a/src/collision/narrowphase/NarrowPhaseAlgorithm.h +++ b/src/collision/narrowphase/NarrowPhaseAlgorithm.h @@ -29,7 +29,7 @@ // Libraries #include "body/Body.h" #include "constraint/ContactPoint.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "engine/OverlappingPair.h" #include "collision/CollisionShapeInfo.h" @@ -71,7 +71,7 @@ class NarrowPhaseAlgorithm { CollisionDetection* mCollisionDetection; /// Pointer to the memory allocator - MemoryAllocator* mMemoryAllocator; + PoolAllocator* mMemoryAllocator; /// Overlapping pair of the bodies currently tested for collision OverlappingPair* mCurrentOverlappingPair; @@ -93,7 +93,7 @@ class NarrowPhaseAlgorithm { NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete; /// Initalize the algorithm - virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator); + virtual void init(CollisionDetection* collisionDetection, PoolAllocator* memoryAllocator); /// Set the current overlapping pair of bodies void setCurrentOverlappingPair(OverlappingPair* overlappingPair); diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 1e2e3f14..b33ed4f1 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -34,7 +34,7 @@ #include "mathematics/Ray.h" #include "AABB.h" #include "collision/RaycastInfo.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" /// ReactPhysics3D namespace namespace reactphysics3d { diff --git a/src/configuration.h b/src/configuration.h index 4364c548..87747756 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -144,6 +144,9 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1; /// least one concave collision shape. constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3; +/// Size (in bytes) of the single frame allocator +constexpr size_t SIZE_SINGLE_FRAME_ALLOCATOR_BYTES = 1048576; + } #endif diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 68e37625..b1b004ec 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -33,7 +33,7 @@ using namespace std; // Constructor CollisionWorld::CollisionWorld() - : mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0), + : mCollisionDetection(this, mPoolAllocator), mCurrentBodyID(0), mEventListener(nullptr) { } @@ -66,7 +66,7 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the collision body - CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody))) + CollisionBody* collisionBody = new (mPoolAllocator.allocate(sizeof(CollisionBody))) CollisionBody(transform, *this, bodyID); assert(collisionBody != nullptr); @@ -97,7 +97,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { mBodies.erase(collisionBody); // Free the object from the memory allocator - mMemoryAllocator.release(collisionBody, sizeof(CollisionBody)); + mPoolAllocator.release(collisionBody, sizeof(CollisionBody)); } // Return the next available body ID diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 77189e89..d04acb4e 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -39,7 +39,7 @@ #include "collision/CollisionDetection.h" #include "constraint/Joint.h" #include "constraint/ContactPoint.h" -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "EventListener.h" /// Namespace reactphysics3d @@ -72,8 +72,8 @@ class CollisionWorld { /// List of free ID for rigid bodies std::vector mFreeBodiesIDs; - /// Memory allocator - MemoryAllocator mMemoryAllocator; + /// Pool Memory allocator + PoolAllocator mPoolAllocator; /// Pointer to an event listener object EventListener* mEventListener; diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index a0db1119..b271aa6e 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -39,55 +39,75 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP= decimal(0.01); // Constructor -ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex) +ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex, + SingleFrameAllocator& singleFrameAllocator) :mSplitLinearVelocities(nullptr), mSplitAngularVelocities(nullptr), - mContactConstraints(nullptr), mPenetrationConstraints(nullptr), - mFrictionConstraints(nullptr), mLinearVelocities(nullptr), mAngularVelocities(nullptr), + mSingleFrameAllocator(singleFrameAllocator), + mPenetrationConstraints(nullptr), mFrictionConstraints(nullptr), + mLinearVelocities(nullptr), mAngularVelocities(nullptr), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsWarmStartingActive(true), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { } +// Initialize the contact constraints +void ContactSolver::init(Island** islands, uint nbIslands, decimal timeStep) { + + mTimeStep = timeStep; + + // TODO : Try not to count manifolds here + // Count the contact manifolds + uint nbContactManifolds = 0; + for (uint i = 0; i < nbIslands; i++) { + nbContactManifolds += islands[i]->getNbContactManifolds(); + } + + mNbFrictionConstraints = 0; + mNbPenetrationConstraints = 0; + + mPenetrationConstraints = nullptr; + mFrictionConstraints = nullptr; + + if (nbContactManifolds == 0) return; + + // TODO : Count exactly the number of constraints to allocate here + uint nbPenetrationConstraints = nbContactManifolds * MAX_CONTACT_POINTS_IN_MANIFOLD; + mPenetrationConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(PenetrationConstraint) * nbPenetrationConstraints)); + //mPenetrationConstraints = new PenetrationConstraint[nbPenetrationConstraints]; + assert(mPenetrationConstraints != nullptr); + //mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4]; + + mFrictionConstraints = static_cast(mSingleFrameAllocator.allocate(sizeof(FrictionConstraint) * nbContactManifolds)); + //mFrictionConstraints = new FrictionConstraint[nbContactManifolds]; + assert(mFrictionConstraints != nullptr); + //mFrictionConstraints = new FrictionConstraint[mNbContactManifolds]; + + // For each island of the world + for (uint islandIndex = 0; islandIndex < nbIslands; islandIndex++) { + initializeForIsland(islands[islandIndex]); + } + + // Warmstarting + warmStart(); +} + // Initialize the constraint solver for a given island -void ContactSolver::initializeForIsland(decimal dt, Island* island) { +void ContactSolver::initializeForIsland(Island* island) { PROFILE("ContactSolver::initializeForIsland()"); assert(island != nullptr); assert(island->getNbBodies() > 0); - assert(island->getNbContactManifolds() > 0); assert(mSplitLinearVelocities != nullptr); assert(mSplitAngularVelocities != nullptr); - // Set the current time step - mTimeStep = dt; - - mNbContactManifolds = island->getNbContactManifolds(); - - mNbFrictionConstraints = 0; - mNbPenetrationConstraints = 0; - - // TODO : Try to do faster allocation here - mContactConstraints = new ContactManifoldSolver[mNbContactManifolds]; - assert(mContactConstraints != nullptr); - - // TODO : Count exactly the number of constraints to allocate here (do not reallocate each frame) - mPenetrationConstraints = new PenetrationConstraint[mNbContactManifolds * 4]; - assert(mPenetrationConstraints != nullptr); - - // TODO : Do not reallocate each frame) - mFrictionConstraints = new FrictionConstraint[mNbContactManifolds]; - assert(mFrictionConstraints != nullptr); - // For each contact manifold of the island ContactManifold** contactManifolds = island->getContactManifolds(); - for (uint i=0; igetNbContactManifolds(); i++) { ContactManifold* externalManifold = contactManifolds[i]; - ContactManifoldSolver& internalManifold = mContactConstraints[i]; - assert(externalManifold->getNbContactPoints() > 0); // Get the two bodies of the contact @@ -100,6 +120,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { uint indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; uint indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + new (mFrictionConstraints + mNbFrictionConstraints) FrictionConstraint(); mFrictionConstraints[mNbFrictionConstraints].indexBody1 = indexBody1; mFrictionConstraints[mNbFrictionConstraints].indexBody2 = indexBody2; mFrictionConstraints[mNbFrictionConstraints].contactManifold = externalManifold; @@ -129,7 +150,6 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { decimal restitutionFactor = computeMixedRestitutionFactor(body1, body2); mFrictionConstraints[mNbFrictionConstraints].frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); mFrictionConstraints[mNbFrictionConstraints].rollingResistanceFactor = computeMixedRollingResistance(body1, body2); - internalManifold.externalContactManifold = externalManifold; mFrictionConstraints[mNbFrictionConstraints].hasAtLeastOneRestingContactPoint = false; //internalManifold.isBody1DynamicType = body1->getType() == BodyType::DYNAMIC; //internalManifold.isBody2DynamicType = body2->getType() == BodyType::DYNAMIC; @@ -159,6 +179,7 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { // Get a contact point ContactPoint* externalContact = externalManifold->getContactPoint(c); + new (mPenetrationConstraints + mNbPenetrationConstraints) PenetrationConstraint(); mPenetrationConstraints[mNbPenetrationConstraints].indexBody1 = indexBody1; mPenetrationConstraints[mNbPenetrationConstraints].indexBody2 = indexBody2; mPenetrationConstraints[mNbPenetrationConstraints].inverseInertiaTensorBody1 = I1; @@ -301,168 +322,19 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) { frictionTwistMass > decimal(0.0) ? mFrictionConstraints[mNbFrictionConstraints].inverseTwistFrictionMass = decimal(1.0) / frictionTwistMass : decimal(0.0); - //} mNbFrictionConstraints++; } - - // Fill-in all the matrices needed to solve the LCP problem - //initializeContactConstraints(); } -// TODO : Delete this method -// Initialize the contact constraints before solving the system -void ContactSolver::initializeContactConstraints() { +// Solve the contact constraints of one iteration of the solve +void ContactSolver::solve() { - PROFILE("ContactSolver::initializeContactConstraints()"); - - // For each contact constraint - //for (uint c=0; c decimal(0.0)); - // ContactManifoldSolver& manifold = mContactConstraints[c]; - -// // Get the inertia tensors of both bodies -// Matrix3x3& I1 = manifold.inverseInertiaTensorBody1; -// Matrix3x3& I2 = manifold.inverseInertiaTensorBody2; - - // If we solve the friction constraints at the center of the contact manifold -// if (mIsSolveFrictionAtContactManifoldCenterActive) { -// manifold.normal = Vector3(0.0, 0.0, 0.0); -// } - - // Get the velocities of the bodies -// const Vector3& v1 = mLinearVelocities[manifold.indexBody1]; -// const Vector3& w1 = mAngularVelocities[manifold.indexBody1]; -// const Vector3& v2 = mLinearVelocities[manifold.indexBody2]; -// const Vector3& w2 = mAngularVelocities[manifold.indexBody2]; - - // For each contact point constraint - //for (uint i=0; i 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / -// massPenetration : -// decimal(0.0); - - // If we do not solve the friction constraints at the center of the contact manifold -// if (!mIsSolveFrictionAtContactManifoldCenterActive) { - -// // Compute the friction vectors -// computeFrictionVectors(deltaV, contactPoint); - -// contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); -// contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2); -// contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); -// contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); - -// // Compute the inverse mass matrix K for the friction -// // constraints at each contact point -// decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + -// ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( -// contactPoint.frictionVector1) + -// ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( -// contactPoint.frictionVector1); -// decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + -// ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( -// contactPoint.frictionVector2) + -// ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( -// contactPoint.frictionVector2); -// friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / -// friction1Mass : -// decimal(0.0); -// friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / -// friction2Mass : -// decimal(0.0); -// } - - // Compute the restitution velocity bias "b". We compute this here instead - // of inside the solve() method because we need to use the velocity difference - // at the beginning of the contact. Note that if it is a resting contact (normal - // velocity bellow a given threshold), we do not add a restitution velocity bias -// contactPoint.restitutionBias = 0.0; -// decimal deltaVDotN = deltaV.dot(contactPoint.normal); -// if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { -// contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; -// } - -// // If the warm starting of the contact solver is active -// if (mIsWarmStartingActive) { - -// // Get the cached accumulated impulses from the previous step -// contactPoint.penetrationImpulse = externalContact->getPenetrationImpulse(); -// contactPoint.friction1Impulse = externalContact->getFrictionImpulse1(); -// contactPoint.friction2Impulse = externalContact->getFrictionImpulse2(); -// contactPoint.rollingResistanceImpulse = externalContact->getRollingResistanceImpulse(); -// } - -// // Initialize the split impulses to zero -// contactPoint.penetrationSplitImpulse = 0.0; - -// // If we solve the friction constraints at the center of the contact manifold -// if (mIsSolveFrictionAtContactManifoldCenterActive) { -// manifold.normal += contactPoint.normal; -// } - //} - -// // Compute the inverse K matrix for the rolling resistance constraint -// manifold.inverseRollingResistance.setToZero(); -// if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) { -// manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1 + manifold.inverseInertiaTensorBody2; -// manifold.inverseRollingResistance = manifold.inverseRollingResistance.getInverse(); -// } - - // If we solve the friction constraints at the center of the contact manifold - //if (mIsSolveFrictionAtContactManifoldCenterActive) { - -// manifold.normal.normalize(); - -// Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - -// v1 - w1.cross(manifold.r1Friction); - -// // Compute the friction vectors -// computeFrictionVectors(deltaVFrictionPoint, manifold); - -// // Compute the inverse mass matrix K for the friction constraints at the center of -// // the contact manifold -// manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); -// manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); -// manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); -// manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); -// decimal friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + -// ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( -// manifold.frictionVector1) + -// ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( -// manifold.frictionVector1); -// decimal friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + -// ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( -// manifold.frictionVector2) + -// ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( -// manifold.frictionVector2); -// decimal frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1 * -// manifold.normal) + -// manifold.normal.dot(manifold.inverseInertiaTensorBody2 * -// manifold.normal); -// friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass -// : decimal(0.0); -// friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass -// : decimal(0.0); -// frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) / -// frictionTwistMass : -// decimal(0.0); - //} - //} + resetTotalPenetrationImpulse(); + solvePenetrationConstraints(); + solveFrictionConstraints(); } // Warm start the solver. @@ -576,177 +448,6 @@ void ContactSolver::warmStart() { mFrictionConstraints[i].rollingResistanceImpulse.setToZero(); } } - - - /* - - - // Check that warm starting is active - if (!mIsWarmStartingActive) return; - - // For each constraint - for (uint c=0; c 0) { - - // Compute the impulse P = J^T * lambda - const Impulse impulseRollingResistance(Vector3::zero(), -contactPoint.rollingResistanceImpulse, - Vector3::zero(), contactPoint.rollingResistanceImpulse); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRollingResistance, contactManifold); - } - } - } - else { // If it is a new contact point - - // Initialize the accumulated impulses to zero - contactPoint.penetrationImpulse = 0.0; - contactPoint.friction1Impulse = 0.0; - contactPoint.friction2Impulse = 0.0; - contactPoint.rollingResistanceImpulse = Vector3::zero(); - } - } - - // If we solve the friction constraints at the center of the contact manifold and there is - // at least one resting contact point in the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) { - - // Project the old friction impulses (with old friction vectors) into the new friction - // vectors to get the new friction impulses - Vector3 oldFrictionImpulse = contactManifold.friction1Impulse * - contactManifold.oldFrictionVector1 + - contactManifold.friction2Impulse * - contactManifold.oldFrictionVector2; - contactManifold.friction1Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector1); - contactManifold.friction2Impulse = oldFrictionImpulse.dot( - contactManifold.frictionVector2); - - // ------ First friction constraint at the center of the contact manifold ------ // - - // Compute the impulse P = J^T * lambda - Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * - contactManifold.friction1Impulse; - Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * - contactManifold.friction1Impulse; - Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * - contactManifold.friction1Impulse; - const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, contactManifold); - - // ------ Second friction constraint at the center of the contact manifold ----- // - - // Compute the impulse P = J^T * lambda - linearImpulseBody1 = -contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody1 = -contactManifold.r1CrossT2 * - contactManifold.friction2Impulse; - linearImpulseBody2 = contactManifold.frictionVector2 * - contactManifold.friction2Impulse; - angularImpulseBody2 = contactManifold.r2CrossT2 * - contactManifold.friction2Impulse; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, contactManifold); - - // ------ Twist friction constraint at the center of the contact manifold ------ // - - // Compute the impulse P = J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody1 = -contactManifold.normal * contactManifold.frictionTwistImpulse; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody2 = contactManifold.normal * contactManifold.frictionTwistImpulse; - const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseTwistFriction, contactManifold); - - // ------ Rolling resistance at the center of the contact manifold ------ // - - // Compute the impulse P = J^T * lambda - angularImpulseBody1 = -contactManifold.rollingResistanceImpulse; - angularImpulseBody2 = contactManifold.rollingResistanceImpulse; - const Impulse impulseRollingResistance(Vector3::zero(), angularImpulseBody1, - Vector3::zero(), angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseRollingResistance, contactManifold); - } - else { // If it is a new contact manifold - - // Initialize the accumulated impulses to zero - contactManifold.friction1Impulse = 0.0; - contactManifold.friction2Impulse = 0.0; - contactManifold.frictionTwistImpulse = 0.0; - contactManifold.rollingResistanceImpulse = Vector3::zero(); - } - } - */ } // Reset the total penetration impulse of friction constraints @@ -978,288 +679,13 @@ void ContactSolver::solveFrictionConstraints() { } } -// Solve the contacts -//void ContactSolver::solve() { - -// PROFILE("ContactSolver::solve()"); - -// decimal deltaLambda; -// decimal lambdaTemp; - -// // For each contact manifold -// for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * -// max(0.0f, float(contactPoint.penetrationDepth - SLOP)); -// decimal b = biasPenetrationDepth + contactPoint.restitutionBias; - -// // Compute the Lagrange multiplier lambda -// if (mIsSplitImpulseActive) { -// deltaLambda = - (Jv + contactPoint.restitutionBias) * -// contactPoint.inversePenetrationMass; -// } -// else { -// deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; -// } -// lambdaTemp = contactPoint.penetrationImpulse; -// contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + -// deltaLambda, decimal(0.0)); -// deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; - -// // Compute the impulse P=J^T * lambda -// const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, -// contactPoint); - -// // Apply the impulse to the bodies of the constraint -// applyImpulse(impulsePenetration, contactManifold); - -// sumPenetrationImpulse += contactPoint.penetrationImpulse; - -// // If the split impulse position correction is active -// if (mIsSplitImpulseActive) { - -// // Split impulse (position correction) -// const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; -// const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; -// const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; -// const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; -// Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - -// v1Split - w1Split.cross(contactPoint.r1); -// decimal JvSplit = deltaVSplit.dot(contactPoint.normal); -// decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * -// contactPoint.inversePenetrationMass; -// decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; -// contactPoint.penetrationSplitImpulse = std::max( -// contactPoint.penetrationSplitImpulse + -// deltaLambdaSplit, decimal(0.0)); -// deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; - -// // Compute the impulse P=J^T * lambda -// const Impulse splitImpulsePenetration = computePenetrationImpulse( -// deltaLambdaSplit, contactPoint); - -// applySplitImpulse(splitImpulsePenetration, contactManifold); -// } - -// // If we do not solve the friction constraints at the center of the contact manifold -// if (!mIsSolveFrictionAtContactManifoldCenterActive) { - -// // --------- Friction 1 --------- // - -// // Compute J*v -// deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); -// Jv = deltaV.dot(contactPoint.frictionVector1); - -// // Compute the Lagrange multiplier lambda -// deltaLambda = -Jv; -// deltaLambda *= contactPoint.inverseFriction1Mass; -// decimal frictionLimit = contactManifold.frictionCoefficient * -// contactPoint.penetrationImpulse; -// lambdaTemp = contactPoint.friction1Impulse; -// contactPoint.friction1Impulse = std::max(-frictionLimit, -// std::min(contactPoint.friction1Impulse -// + deltaLambda, frictionLimit)); -// deltaLambda = contactPoint.friction1Impulse - lambdaTemp; - -// // Compute the impulse P=J^T * lambda -// const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, -// contactPoint); - -// // Apply the impulses to the bodies of the constraint -// applyImpulse(impulseFriction1, contactManifold); - -// // --------- Friction 2 --------- // - -// // Compute J*v -// deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); -// Jv = deltaV.dot(contactPoint.frictionVector2); - -// // Compute the Lagrange multiplier lambda -// deltaLambda = -Jv; -// deltaLambda *= contactPoint.inverseFriction2Mass; -// frictionLimit = contactManifold.frictionCoefficient * -// contactPoint.penetrationImpulse; -// lambdaTemp = contactPoint.friction2Impulse; -// contactPoint.friction2Impulse = std::max(-frictionLimit, -// std::min(contactPoint.friction2Impulse -// + deltaLambda, frictionLimit)); -// deltaLambda = contactPoint.friction2Impulse - lambdaTemp; - -// // Compute the impulse P=J^T * lambda -// const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, -// contactPoint); - -// // Apply the impulses to the bodies of the constraint -// applyImpulse(impulseFriction2, contactManifold); - -// // --------- Rolling resistance constraint --------- // - -// if (contactManifold.rollingResistanceFactor > 0) { - -// // Compute J*v -// const Vector3 JvRolling = w2 - w1; - -// // Compute the Lagrange multiplier lambda -// Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); -// decimal rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse; -// Vector3 lambdaTempRolling = contactPoint.rollingResistanceImpulse; -// contactPoint.rollingResistanceImpulse = clamp(contactPoint.rollingResistanceImpulse + -// deltaLambdaRolling, rollingLimit); -// deltaLambdaRolling = contactPoint.rollingResistanceImpulse - lambdaTempRolling; - -// // Compute the impulse P=J^T * lambda -// const Impulse impulseRolling(Vector3::zero(), -deltaLambdaRolling, -// Vector3::zero(), deltaLambdaRolling); - -// // Apply the impulses to the bodies of the constraint -// applyImpulse(impulseRolling, contactManifold); -// } -// } - //} - - // If we solve the friction constraints at the center of the contact manifold -// if (mIsSolveFrictionAtContactManifoldCenterActive) { - -// // ------ First friction constraint at the center of the contact manifol ------ // - -// // Compute J*v -// Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) -// - v1 - w1.cross(contactManifold.r1Friction); -// decimal Jv = deltaV.dot(contactManifold.frictionVector1); - -// // Compute the Lagrange multiplier lambda -// decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; -// decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; -// lambdaTemp = contactManifold.friction1Impulse; -// contactManifold.friction1Impulse = std::max(-frictionLimit, -// std::min(contactManifold.friction1Impulse + -// deltaLambda, frictionLimit)); -// deltaLambda = contactManifold.friction1Impulse - lambdaTemp; - -// // Compute the impulse P=J^T * lambda -// Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; -// Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; -// Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; -// Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; -// const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, -// linearImpulseBody2, angularImpulseBody2); - -// // Apply the impulses to the bodies of the constraint -// applyImpulse(impulseFriction1, contactManifold); - -// // ------ Second friction constraint at the center of the contact manifol ----- // - -// // Compute J*v -// deltaV = v2 + w2.cross(contactManifold.r2Friction) -// - v1 - w1.cross(contactManifold.r1Friction); -// Jv = deltaV.dot(contactManifold.frictionVector2); - -// // Compute the Lagrange multiplier lambda -// deltaLambda = -Jv * contactManifold.inverseFriction2Mass; -// frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; -// lambdaTemp = contactManifold.friction2Impulse; -// contactManifold.friction2Impulse = std::max(-frictionLimit, -// std::min(contactManifold.friction2Impulse + -// deltaLambda, frictionLimit)); -// deltaLambda = contactManifold.friction2Impulse - lambdaTemp; - -// // Compute the impulse P=J^T * lambda -// linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; -// angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; -// linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; -// angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; -// const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, -// linearImpulseBody2, angularImpulseBody2); - -// // Apply the impulses to the bodies of the constraint -// applyImpulse(impulseFriction2, contactManifold); - -// // ------ Twist friction constraint at the center of the contact manifol ------ // - -// // Compute J*v -// deltaV = w2 - w1; -// Jv = deltaV.dot(contactManifold.normal); - -// deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass); -// frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; -// lambdaTemp = contactManifold.frictionTwistImpulse; -// contactManifold.frictionTwistImpulse = std::max(-frictionLimit, -// std::min(contactManifold.frictionTwistImpulse -// + deltaLambda, frictionLimit)); -// deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; - -// // Compute the impulse P=J^T * lambda -// linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); -// angularImpulseBody1 = -contactManifold.normal * deltaLambda; -// linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; -// angularImpulseBody2 = contactManifold.normal * deltaLambda; -// const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, -// linearImpulseBody2, angularImpulseBody2); - -// // Apply the impulses to the bodies of the constraint -// applyImpulse(impulseTwistFriction, contactManifold); - -// // --------- Rolling resistance constraint at the center of the contact manifold --------- // - -// if (contactManifold.rollingResistanceFactor > 0) { - -// // Compute J*v -// const Vector3 JvRolling = w2 - w1; - -// // Compute the Lagrange multiplier lambda -// Vector3 deltaLambdaRolling = contactManifold.inverseRollingResistance * (-JvRolling); -// decimal rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse; -// Vector3 lambdaTempRolling = contactManifold.rollingResistanceImpulse; -// contactManifold.rollingResistanceImpulse = clamp(contactManifold.rollingResistanceImpulse + -// deltaLambdaRolling, rollingLimit); -// deltaLambdaRolling = contactManifold.rollingResistanceImpulse - lambdaTempRolling; - -// // Compute the impulse P=J^T * lambda -// angularImpulseBody1 = -deltaLambdaRolling; -// angularImpulseBody2 = deltaLambdaRolling; -// const Impulse impulseRolling(Vector3::zero(), angularImpulseBody1, -// Vector3::zero(), angularImpulseBody2); - -// // Apply the impulses to the bodies of the constraint -// applyImpulse(impulseRolling, contactManifold); -// } -// } -// } -//} - // Store the computed impulses to use them to // warm start the solver at the next iteration void ContactSolver::storeImpulses() { // Penetration constraints for (uint i=0; isetPenetrationImpulse(mPenetrationConstraints[i].penetrationImpulse); - } // Friction constraints @@ -1274,105 +700,17 @@ void ContactSolver::storeImpulses() { } /* - // For each contact manifold - for (uint c=0; csetPenetrationImpulse(contactPoint.penetrationImpulse); - contactPoint.externalContact->setFrictionImpulse1(contactPoint.friction1Impulse); - contactPoint.externalContact->setFrictionImpulse2(contactPoint.friction2Impulse); - contactPoint.externalContact->setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse); - - contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); - contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); - } - - manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); - manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); - manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); - manifold.externalContactManifold->setRollingResistanceImpulse(manifold.rollingResistanceImpulse); - manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); - manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); + if (mFrictionConstraints != nullptr) { + delete[] mFrictionConstraints; } */ } -/* -// Apply an impulse to the two bodies of a constraint -void ContactSolver::applyImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold) { - - PROFILE("ContactSolver::applyImpulse()"); - - // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * - impulse.linearImpulseBody1; - mAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - - // Update the velocities of the body 1 by applying the impulse P - mLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * - impulse.linearImpulseBody2; - mAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; -} -*/ - -/* -// Apply an impulse to the two bodies of a constraint -void ContactSolver::applySplitImpulse(const Impulse& impulse, - const ContactManifoldSolver& manifold) { - - // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * - impulse.linearImpulseBody1; - mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - - // Update the velocities of the body 1 by applying the impulse P - mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * - impulse.linearImpulseBody2; - mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; -} -*/ - -// TODO : Delete this -// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane -// for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal. -//void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, -// ContactPointSolver& contactPoint) const { - -// assert(contactPoint.normal.length() > 0.0); - -// // Compute the velocity difference vector in the tangential plane -// Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; -// Vector3 tangentVelocity = deltaVelocity - normalVelocity; - -// // If the velocty difference in the tangential plane is not zero -// decimal lengthTangenVelocity = tangentVelocity.length(); -// if (lengthTangenVelocity > MACHINE_EPSILON) { - -// // Compute the first friction vector in the direction of the tangent -// // velocity difference -// contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; -// } -// else { - -// // Get any orthogonal vector to the normal as the first friction vector -// contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); -// } - -// // The second friction vector is computed by the cross product of the firs -// // friction vector and the contact normal -// contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); -//} - // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, @@ -1402,22 +740,3 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // friction vector and the contact normal frictionConstraint.frictionVector2 = frictionConstraint.normal.cross(frictionConstraint.frictionVector1).getUnit(); } - -// Clean up the constraint solver -void ContactSolver::cleanup() { - - if (mContactConstraints != nullptr) { - delete[] mContactConstraints; - mContactConstraints = nullptr; - } - - if (mPenetrationConstraints != nullptr) { - delete[] mPenetrationConstraints; - mPenetrationConstraints = nullptr; - } - - if (mFrictionConstraints != nullptr) { - delete[] mFrictionConstraints; - mFrictionConstraints = nullptr; - } -} diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index f3f5dfa7..e5d5840d 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -31,6 +31,7 @@ #include "configuration.h" #include "constraint/Joint.h" #include "collision/ContactManifold.h" +#include "memory/SingleFrameAllocator.h" #include "Island.h" #include "Impulse.h" #include @@ -278,228 +279,6 @@ class ContactSolver { bool hasAtLeastOneRestingContactPoint; }; - // Structure ContactPointSolver - /** - * Contact solver internal data structure that to store all the - * information relative to a contact point - */ - struct ContactPointSolver { - - /// Index of body 1 in the constraint solver - uint indexBody1; - - /// Index of body 2 in the constraint solver - uint indexBody2; - - /// Inverse of the mass of body 1 - decimal massInverseBody1; - - /// Inverse of the mass of body 2 - decimal massInverseBody2; - - /// Inverse inertia tensor of body 1 - Matrix3x3 inverseInertiaTensorBody1; - - /// Inverse inertia tensor of body 2 - Matrix3x3 inverseInertiaTensorBody2; - - /// Point on body 1 where to apply the friction constraints - Vector3 frictionPointBody1; - - /// Point on body 2 where to apply the friction constraints - Vector3 frictionPointBody2; - - /// Accumulated normal impulse - decimal penetrationImpulse; - - /// Accumulated impulse in the 1st friction direction - decimal friction1Impulse; - - /// Accumulated impulse in the 2nd friction direction - decimal friction2Impulse; - - /// Accumulated split impulse for penetration correction - decimal penetrationSplitImpulse; - - /// Accumulated rolling resistance impulse - Vector3 rollingResistanceImpulse; - - /// Normal vector of the contact - Vector3 normal; - - /// First friction vector in the tangent plane - //Vector3 frictionVector1; - - /// Second friction vector in the tangent plane - //Vector3 frictionVector2; - - /// Old first friction vector in the tangent plane - Vector3 oldFrictionVector1; - - /// Old second friction vector in the tangent plane - Vector3 oldFrictionVector2; - - /// Vector from the body 1 center to the contact point - Vector3 r1; - - /// Vector from the body 2 center to the contact point - Vector3 r2; - - /// Cross product of r1 with 1st friction vector - //Vector3 r1CrossT1; - - /// Cross product of r1 with 2nd friction vector - //Vector3 r1CrossT2; - - /// Cross product of r2 with 1st friction vector - //Vector3 r2CrossT1; - - /// Cross product of r2 with 2nd friction vector - //Vector3 r2CrossT2; - - /// Cross product of r1 with the contact normal - //Vector3 r1CrossN; - - /// Cross product of r2 with the contact normal - //Vector3 r2CrossN; - - /// Penetration depth - decimal penetrationDepth; - - /// Velocity restitution bias - decimal restitutionBias; - - /// Inverse of the matrix K for the penenetration - //decimal inversePenetrationMass; - - /// Inverse of the matrix K for the 1st friction - decimal inverseFriction1Mass; - - /// Inverse of the matrix K for the 2nd friction - decimal inverseFriction2Mass; - - /// True if the contact was existing last time step - bool isRestingContact; - - /// Pointer to the external contact - ContactPoint* externalContact; - }; - - // Structure ContactManifoldSolver - /** - * Contact solver internal data structure to store all the - * information relative to a contact manifold. - */ - struct ContactManifoldSolver { - - /// Index of body 1 in the constraint solver - //uint indexBody1; - - /// Index of body 2 in the constraint solver - //uint indexBody2; - - /// Inverse of the mass of body 1 - //decimal massInverseBody1; - - // Inverse of the mass of body 2 - //decimal massInverseBody2; - - /// Inverse inertia tensor of body 1 - //Matrix3x3 inverseInertiaTensorBody1; - - /// Inverse inertia tensor of body 2 - //Matrix3x3 inverseInertiaTensorBody2; - - /// Contact point constraints - //ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD]; - - /// Number of contact points - //uint nbContacts; - - /// True if the body 1 is of type dynamic - //bool isBody1DynamicType; - - /// True if the body 2 is of type dynamic - //bool isBody2DynamicType; - - /// Mix of the restitution factor for two bodies - //decimal restitutionFactor; - - /// Mix friction coefficient for the two bodies - //decimal frictionCoefficient; - - /// Rolling resistance factor between the two bodies - decimal rollingResistanceFactor; - - /// Pointer to the external contact manifold - ContactManifold* externalContactManifold; - - // - Variables used when friction constraints are apply at the center of the manifold-// - - /// Average normal vector of the contact manifold - //Vector3 normal; - - /// Point on body 1 where to apply the friction constraints - //Vector3 frictionPointBody1; - - /// Point on body 2 where to apply the friction constraints - //Vector3 frictionPointBody2; - - /// R1 vector for the friction constraints - //Vector3 r1Friction; - - /// R2 vector for the friction constraints - //Vector3 r2Friction; - - /// Cross product of r1 with 1st friction vector - //Vector3 r1CrossT1; - - /// Cross product of r1 with 2nd friction vector - //Vector3 r1CrossT2; - - /// Cross product of r2 with 1st friction vector - //Vector3 r2CrossT1; - - /// Cross product of r2 with 2nd friction vector - //Vector3 r2CrossT2; - - /// Matrix K for the first friction constraint - //decimal inverseFriction1Mass; - - /// Matrix K for the second friction constraint - //decimal inverseFriction2Mass; - - /// Matrix K for the twist friction constraint - //decimal inverseTwistFrictionMass; - - /// Matrix K for the rolling resistance constraint - //Matrix3x3 inverseRollingResistance; - - /// First friction direction at contact manifold center - //Vector3 frictionVector1; - - /// Second friction direction at contact manifold center - //Vector3 frictionVector2; - - /// Old 1st friction direction at contact manifold center - //Vector3 oldFrictionVector1; - - /// Old 2nd friction direction at contact manifold center - //Vector3 oldFrictionVector2; - - /// First friction direction impulse at manifold center - //decimal friction1Impulse; - - /// Second friction direction impulse at manifold center - //decimal friction2Impulse; - - /// Twist friction impulse at contact manifold center - //decimal frictionTwistImpulse; - - /// Rolling resistance impulse - //Vector3 rollingResistanceImpulse; - }; - // -------------------- Constants --------------------- // /// Beta value for the penetration depth position correction without split impulses @@ -519,12 +298,12 @@ class ContactSolver { /// Split angular velocities for the position contact solver (split impulse) Vector3* mSplitAngularVelocities; + /// Reference to the single frame memory allocator + SingleFrameAllocator& mSingleFrameAllocator; + /// Current time step decimal mTimeStep; - /// Contact constraints - ContactManifoldSolver* mContactConstraints; - PenetrationConstraint* mPenetrationConstraints; FrictionConstraint* mFrictionConstraints; @@ -533,9 +312,6 @@ class ContactSolver { uint mNbFrictionConstraints; - /// Number of contact constraints - uint mNbContactManifolds; - /// Array of linear velocities Vector3* mLinearVelocities; @@ -557,8 +333,8 @@ class ContactSolver { // -------------------- Methods -------------------- // - /// Initialize the contact constraints before solving the system - void initializeContactConstraints(); + /// Initialize the constraint solver for a given island + void initializeForIsland(Island* island); /// Apply an impulse to the two bodies of a constraint //void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); @@ -608,13 +384,17 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(const std::map& mapBodyToVelocityIndex); + ContactSolver(const std::map& mapBodyToVelocityIndex, + SingleFrameAllocator& singleFrameAllocator); /// Destructor ~ContactSolver() = default; - /// Initialize the constraint solver for a given island - void initializeForIsland(decimal dt, Island* island); + /// Initialize the contact constraints + void init(Island** islands, uint nbIslands, decimal timeStep); + + /// Solve the contact constraints of one iteration of the solve + void solve(); /// Set the split velocities arrays void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, @@ -653,9 +433,6 @@ class ContactSolver { /// the contact manifold instead of solving them at each contact point void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); - /// Clean up the constraint solver - void cleanup(); - /// Return true if warmstarting is active bool IsWarmStartingActive() const; }; diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 189ef0e0..0b1110af 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -40,7 +40,8 @@ using namespace std; */ DynamicsWorld::DynamicsWorld(const Vector3 &gravity) : CollisionWorld(), - mContactSolver(mMapBodyToConstrainedVelocityIndex), + mSingleFrameAllocator(SIZE_SINGLE_FRAME_ALLOCATOR_BYTES), + mContactSolver(mMapBodyToConstrainedVelocityIndex, mSingleFrameAllocator), mConstraintSolver(mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), @@ -82,10 +83,10 @@ DynamicsWorld::~DynamicsWorld() { mIslands[i]->~Island(); // Release the allocated memory for the island - mMemoryAllocator.release(mIslands[i], sizeof(Island)); + mPoolAllocator.release(mIslands[i], sizeof(Island)); } if (mNbIslandsCapacity > 0) { - mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); + mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); } // Release the memory allocated for the bodies velocity arrays @@ -161,6 +162,9 @@ void DynamicsWorld::update(decimal timeStep) { // Reset the external force and torque applied to the bodies resetBodiesForceAndTorque(); + + // Reset the single frame memory allocator + mSingleFrameAllocator.reset(); } // Integrate position and orientation of the rigid bodies. @@ -364,27 +368,28 @@ void DynamicsWorld::solveContactsAndConstraints() { mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions, mConstrainedOrientations); - // ---------- Solve velocity constraints for joints and contacts ---------- // + // Initialize the contact solver + mContactSolver.init(mIslands, mNbIslands, mTimeStep); // For each island of the world for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { // Check if there are contacts and constraints to solve bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0; - bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; - if (!isConstraintsToSolve && !isContactsToSolve) continue; + //bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0; + //if (!isConstraintsToSolve && !isContactsToSolve) continue; // If there are contacts in the current island - if (isContactsToSolve) { +// if (isContactsToSolve) { - // Initialize the solver - mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); +// // Initialize the solver +// mContactSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); - // Warm start the contact solver - if (mContactSolver.IsWarmStartingActive()) { - mContactSolver.warmStart(); - } - } +// // Warm start the contact solver +// if (mContactSolver.IsWarmStartingActive()) { +// mContactSolver.warmStart(); +// } +// } // If there are constraints if (isConstraintsToSolve) { @@ -392,32 +397,40 @@ void DynamicsWorld::solveContactsAndConstraints() { // Initialize the constraint solver mConstraintSolver.initializeForIsland(mTimeStep, mIslands[islandIndex]); } + } // For each iteration of the velocity solver for (uint i=0; igetNbJoints() > 0; + if (isConstraintsToSolve) { + mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]); + } } + mContactSolver.solve(); + // Solve the contacts - if (isContactsToSolve) { +// if (isContactsToSolve) { - mContactSolver.resetTotalPenetrationImpulse(); +// mContactSolver.resetTotalPenetrationImpulse(); - mContactSolver.solvePenetrationConstraints(); - mContactSolver.solveFrictionConstraints(); - } +// mContactSolver.solvePenetrationConstraints(); +// mContactSolver.solveFrictionConstraints(); +// } } // Cache the lambda values in order to use them in the next // step and cleanup the contact solver - if (isContactsToSolve) { - mContactSolver.storeImpulses(); - mContactSolver.cleanup(); - } - } +// if (isContactsToSolve) { +// mContactSolver.storeImpulses(); +// mContactSolver.cleanup(); +// } + //} + + mContactSolver.storeImpulses(); } // Solve the position error correction of the constraints @@ -456,7 +469,7 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) { assert(bodyID < std::numeric_limits::max()); // Create the rigid body - RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, + RigidBody* rigidBody = new (mPoolAllocator.allocate(sizeof(RigidBody))) RigidBody(transform, *this, bodyID); assert(rigidBody != nullptr); @@ -497,7 +510,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { mRigidBodies.erase(rigidBody); // Free the object from the memory allocator - mMemoryAllocator.release(rigidBody, sizeof(RigidBody)); + mPoolAllocator.release(rigidBody, sizeof(RigidBody)); } // Create a joint between two bodies in the world and return a pointer to the new joint @@ -515,7 +528,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Ball-and-Socket joint case JointType::BALLSOCKETJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(BallAndSocketJoint)); const BallAndSocketJointInfo& info = static_cast( jointInfo); newJoint = new (allocatedMemory) BallAndSocketJoint(info); @@ -525,7 +538,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Slider joint case JointType::SLIDERJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(SliderJoint)); const SliderJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) SliderJoint(info); break; @@ -534,7 +547,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Hinge joint case JointType::HINGEJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(HingeJoint)); const HingeJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) HingeJoint(info); break; @@ -543,7 +556,7 @@ Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) { // Fixed joint case JointType::FIXEDJOINT: { - void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint)); + void* allocatedMemory = mPoolAllocator.allocate(sizeof(FixedJoint)); const FixedJointInfo& info = static_cast(jointInfo); newJoint = new (allocatedMemory) FixedJoint(info); break; @@ -596,8 +609,8 @@ void DynamicsWorld::destroyJoint(Joint* joint) { mJoints.erase(joint); // Remove the joint from the joint list of the bodies involved in the joint - joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint); - joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint); + joint->mBody1->removeJointFromJointsList(mPoolAllocator, joint); + joint->mBody2->removeJointFromJointsList(mPoolAllocator, joint); size_t nbBytes = joint->getSizeInBytes(); @@ -605,7 +618,7 @@ void DynamicsWorld::destroyJoint(Joint* joint) { joint->~Joint(); // Release the allocated memory - mMemoryAllocator.release(joint, nbBytes); + mPoolAllocator.release(joint, nbBytes); } // Add the joint to the list of joints of the two bodies involved in the joint @@ -614,13 +627,13 @@ void DynamicsWorld::addJointToBody(Joint* joint) { assert(joint != nullptr); // Add the joint at the beginning of the linked list of joints of the first body - void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory1 = mPoolAllocator.allocate(sizeof(JointListElement)); JointListElement* jointListElement1 = new (allocatedMemory1) JointListElement(joint, joint->mBody1->mJointsList); joint->mBody1->mJointsList = jointListElement1; // Add the joint at the beginning of the linked list of joints of the second body - void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(JointListElement)); + void* allocatedMemory2 = mPoolAllocator.allocate(sizeof(JointListElement)); JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, joint->mBody2->mJointsList); joint->mBody2->mJointsList = jointListElement2; @@ -646,16 +659,16 @@ void DynamicsWorld::computeIslands() { mIslands[i]->~Island(); // Release the allocated memory for the island - mMemoryAllocator.release(mIslands[i], sizeof(Island)); + mPoolAllocator.release(mIslands[i], sizeof(Island)); } // Allocate and create the array of islands if (mNbIslandsCapacity != nbBodies && nbBodies > 0) { if (mNbIslandsCapacity > 0) { - mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); + mPoolAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); } mNbIslandsCapacity = nbBodies; - mIslands = (Island**)mMemoryAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity); + mIslands = (Island**)mPoolAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity); } mNbIslands = 0; @@ -672,7 +685,7 @@ void DynamicsWorld::computeIslands() { // Create a stack (using an array) for the rigid bodies to visit during the Depth First Search size_t nbBytesStack = sizeof(RigidBody*) * nbBodies; - RigidBody** stackBodiesToVisit = (RigidBody**)mMemoryAllocator.allocate(nbBytesStack); + RigidBody** stackBodiesToVisit = (RigidBody**)mPoolAllocator.allocate(nbBytesStack); // For each rigid body of the world for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { @@ -695,10 +708,10 @@ void DynamicsWorld::computeIslands() { body->mIsAlreadyInIsland = true; // Create the new island - void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island)); + void* allocatedMemoryIsland = mPoolAllocator.allocate(sizeof(Island)); mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies, nbContactManifolds, - mJoints.size(), mMemoryAllocator); + mJoints.size(), mPoolAllocator); // While there are still some bodies to visit in the stack while (stackIndex > 0) { @@ -790,7 +803,7 @@ void DynamicsWorld::computeIslands() { } // Release the allocated memory for the stack of bodies to visit - mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack); + mPoolAllocator.release(stackBodiesToVisit, nbBytesStack); } // Put bodies to sleep if needed. diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 7dc715c9..542e0f4b 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -50,6 +50,9 @@ class DynamicsWorld : public CollisionWorld { // -------------------- Attributes -------------------- // + /// Single frame Memory allocator + SingleFrameAllocator mSingleFrameAllocator; + /// Contact solver ContactSolver mContactSolver; diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp index 41560bde..8cf743a3 100644 --- a/src/engine/Island.cpp +++ b/src/engine/Island.cpp @@ -30,7 +30,7 @@ using namespace reactphysics3d; // Constructor Island::Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - MemoryAllocator& memoryAllocator) + PoolAllocator& memoryAllocator) : mBodies(nullptr), mContactManifolds(nullptr), mJoints(nullptr), mNbBodies(0), mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { diff --git a/src/engine/Island.h b/src/engine/Island.h index 512a0d1b..227a78e5 100644 --- a/src/engine/Island.h +++ b/src/engine/Island.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_ISLAND_H // Libraries -#include "memory/MemoryAllocator.h" +#include "memory/PoolAllocator.h" #include "body/RigidBody.h" #include "constraint/Joint.h" #include "collision/ContactManifold.h" @@ -64,7 +64,7 @@ class Island { uint mNbJoints; /// Reference to the memory allocator - MemoryAllocator& mMemoryAllocator; + PoolAllocator& mMemoryAllocator; /// Number of bytes allocated for the bodies array size_t mNbAllocatedBytesBodies; @@ -81,7 +81,7 @@ class Island { /// Constructor Island(uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, - MemoryAllocator& memoryAllocator); + PoolAllocator& memoryAllocator); /// Destructor ~Island(); diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 7efca599..48658f5c 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -31,7 +31,7 @@ using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, MemoryAllocator& memoryAllocator) + int nbMaxContactManifolds, PoolAllocator& memoryAllocator) : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), mCachedSeparatingAxis(0.0, 1.0, 0.0) { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 7995b0d2..70ee1c3f 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -63,7 +63,7 @@ class OverlappingPair { /// Constructor OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - int nbMaxContactManifolds, MemoryAllocator& memoryAllocator); + int nbMaxContactManifolds, PoolAllocator& memoryAllocator); /// Destructor ~OverlappingPair() = default;