diff --git a/src/body/Body.cpp b/src/body/Body.cpp index a9c639f3..243b0c3f 100644 --- a/src/body/Body.cpp +++ b/src/body/Body.cpp @@ -23,7 +23,7 @@ * * ********************************************************************************/ - // Libraries +// Libraries #include "Body.h" #include "../collision/shapes/CollisionShape.h" @@ -31,7 +31,8 @@ using namespace reactphysics3d; // Constructor -Body::Body(bodyindex id) : mID(id) { +Body::Body(bodyindex id) + : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsSleeping(false) { } diff --git a/src/body/Body.h b/src/body/Body.h index d131eec8..f0358570 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -47,6 +47,15 @@ class Body { /// ID of the body bodyindex mID; + /// True if the body has already been added in an island (for sleeping technique) + bool mIsAlreadyInIsland; + + /// True if the body is allowed to go to sleep for better efficiency + bool mIsAllowedToSleep; + + /// True if the body is sleeping (for sleeping technique) + bool mIsSleeping; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -68,6 +77,24 @@ class Body { /// Return the id of the body bodyindex getID() const; + /// Return true if the body has already been added in an island (for the sleeping technique) + bool isAlreadyInIsland() const; + + /// Set the value of to know if the body has already been added into an island + void setIsAlreadyInIsland(bool isAlreadyInIsland); + + /// Return whether or not the body is allowed to sleep + bool isAllowedToSleep() const; + + /// Set whether or not the body is allowed to go to sleep + void setIsAllowedToSleep(bool isAllowedToSleep); + + /// Return whether or not the body is sleeping + bool isSleeping() const; + + /// Set the variable to know whether or not the body is sleeping + void setIsSleeping(bool isSleeping); + /// Smaller than operator bool operator<(const Body& body2) const; @@ -79,6 +106,10 @@ class Body { /// Not equal operator bool operator!=(const Body& body2) const; + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; }; // Return the id of the body @@ -86,6 +117,36 @@ inline bodyindex Body::getID() const { return mID; } +// Return true if the body has already been added in an island (for the sleeping technique) +inline bool Body::isAlreadyInIsland() const { + return mIsAlreadyInIsland; +} + +// Set the value of to know if the body has already been added into an island +inline void Body::setIsAlreadyInIsland(bool isAlreadyInIsland) { + mIsAlreadyInIsland = isAlreadyInIsland; +} + +// Return whether or not the body is allowed to sleep +inline bool Body::isAllowedToSleep() const { + return mIsAllowedToSleep; +} + +// Set whether or not the body is allowed to go to sleep +inline void Body::setIsAllowedToSleep(bool isAllowedToSleep) { + mIsAllowedToSleep = isAllowedToSleep; +} + +// Return whether or not the body is sleeping +inline bool Body::isSleeping() const { + return mIsSleeping; +} + +// Set the variable to know whether or not the body is sleeping +inline void Body::setIsSleeping(bool isSleeping) { + mIsSleeping = isSleeping; +} + // Smaller than operator inline bool Body::operator<(const Body& body2) const { return (mID < body2.mID); diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp index 98e6c600..0ba418b4 100644 --- a/src/body/CollisionBody.cpp +++ b/src/body/CollisionBody.cpp @@ -25,6 +25,7 @@ // Libraries #include "CollisionBody.h" +#include "../engine/ContactManifold.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -33,7 +34,7 @@ using namespace reactphysics3d; CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisionShape, bodyindex id) : Body(id), mCollisionShape(collisionShape), mTransform(transform), - mIsActive(true), mHasMoved(false) { + mIsActive(true), mHasMoved(false), mContactManifoldsList(NULL) { assert(collisionShape); @@ -50,5 +51,24 @@ CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisi // Destructor CollisionBody::~CollisionBody() { - + assert(mContactManifoldsList == NULL); +} + +// Reset the contact manifold lists +void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator) { + + // Delete the linked list of contact manifolds of that body + ContactManifoldListElement* currentElement = mContactManifoldsList; + while (currentElement != NULL) { + ContactManifoldListElement* nextElement = currentElement->next; + + // Delete the current element + currentElement->ContactManifoldListElement::~ContactManifoldListElement(); + memoryAllocator.release(currentElement, sizeof(ContactManifoldListElement)); + + currentElement = nextElement; + } + mContactManifoldsList = NULL; + + assert(mContactManifoldsList == NULL); } diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index df484e33..e050f8e3 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -33,11 +33,15 @@ #include "../mathematics/Transform.h" #include "../collision/shapes/AABB.h" #include "../collision/shapes/CollisionShape.h" +#include "../memory/MemoryAllocator.h" #include "../configuration.h" /// Namespace reactphysics3d namespace reactphysics3d { +// Class declarations +struct ContactManifoldListElement; + // Class CollisionBody /** * This class represents a body that is able to collide with others @@ -76,6 +80,9 @@ class CollisionBody : public Body { /// True if the body has moved during the last frame bool mHasMoved; + /// First element of the linked list of contact manifolds involving this body + ContactManifoldListElement* mContactManifoldsList; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -84,6 +91,9 @@ class CollisionBody : public Body { /// Private assignment operator CollisionBody& operator=(const CollisionBody& body); + /// Reset the contact manifold lists + void resetContactManifoldsList(MemoryAllocator& memoryAllocator); + public : // -------------------- Methods -------------------- // @@ -144,6 +154,13 @@ class CollisionBody : public Body { /// Update the Axis-Aligned Bounding Box coordinates void updateAABB(); + + /// Return the first element of the linked list of contact manifolds involving this body + const ContactManifoldListElement* getContactManifoldsLists() const; + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; }; // Return true if the body has moved during the last frame @@ -243,6 +260,11 @@ inline void CollisionBody::updateAABB() { mCollisionShape->updateAABB(mAabb, mTransform); } +// Return the first element of the linked list of contact manifolds involving this body +inline const ContactManifoldListElement* CollisionBody::getContactManifoldsLists() const { + return mContactManifoldsList; +} + } #endif diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 757af86d..8b61ed6f 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -25,6 +25,7 @@ // Libraries #include "RigidBody.h" +#include "constraint/Constraint.h" #include "../collision/shapes/CollisionShape.h" // We want to use the ReactPhysics3D namespace @@ -36,13 +37,41 @@ RigidBody::RigidBody(const Transform& transform, decimal mass, const Matrix3x3& : CollisionBody(transform, collisionShape, id), mInertiaTensorLocal(inertiaTensorLocal), mMass(mass), mInertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), mMassInverse(decimal(1.0) / mass), mIsGravityEnabled(true), - mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)) { + mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mJointsList(NULL) { assert(collisionShape); } // Destructor RigidBody::~RigidBody() { - + assert(mJointsList == NULL); } +// Remove a joint from the joints list +void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Constraint* joint) { + + assert(joint != NULL); + assert(mJointsList != NULL); + + // Remove the joint from the linked list of the joints of the first body + if (mJointsList->joint == joint) { // If the first element is the one to remove + JointListElement* elementToRemove = mJointsList; + mJointsList = elementToRemove->next; + elementToRemove->JointListElement::~JointListElement(); + memoryAllocator.release(elementToRemove, sizeof(JointListElement)); + } + else { // If the element to remove is not the first one in the list + JointListElement* currentElement = mJointsList; + while (currentElement->next != NULL) { + if (currentElement->next->joint == joint) { + JointListElement* elementToRemove = currentElement->next; + currentElement->next = elementToRemove->next; + elementToRemove->JointListElement::~JointListElement(); + memoryAllocator.release(elementToRemove, sizeof(JointListElement)); + break; + } + } + } +} + + diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 6136f1dd..2856bc64 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -31,10 +31,15 @@ #include "CollisionBody.h" #include "../engine/Material.h" #include "../mathematics/mathematics.h" +#include "../memory/MemoryAllocator.h" /// Namespace reactphysics3d namespace reactphysics3d { +// Class declarations +struct JointListElement; +class Constraint; + // Class RigidBody /** * This class represents a rigid body of the physics @@ -86,6 +91,9 @@ class RigidBody : public CollisionBody { /// Angular velocity damping factor decimal mAngularDamping; + /// First element of the linked list of joints involving this body + JointListElement* mJointsList; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -94,6 +102,9 @@ class RigidBody : public CollisionBody { /// Private assignment operator RigidBody& operator=(const RigidBody& body); + /// Remove a joint from the joints list + void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Constraint* joint); + public : // -------------------- Methods -------------------- // @@ -179,6 +190,13 @@ class RigidBody : public CollisionBody { /// Set the angular damping factor void setAngularDamping(decimal angularDamping); + + /// Return the first element of the linked list of joints involving this body + const JointListElement* getJointsList() const; + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; }; // Method that return the mass of the body @@ -328,6 +346,11 @@ inline void RigidBody::setAngularDamping(decimal angularDamping) { mAngularDamping = angularDamping; } +// Return the first element of the linked list of joints involving this body +inline const JointListElement* RigidBody::getJointsList() const { + return mJointsList; +} + } #endif diff --git a/src/configuration.h b/src/configuration.h index c762f14e..3cb8c3f0 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -92,8 +92,8 @@ const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3); /// Default bounciness factor for a rigid body const decimal DEFAULT_BOUNCINESS = decimal(0.5); -/// True if the deactivation (sleeping) of inactive bodies is enabled -const bool DEACTIVATION_ENABLED = true; +/// True if the spleeping technique for inactive bodies is enabled +const bool SPLEEPING_ENABLED = true; /// Object margin for collision detection in meters (for the GJK-EPA Algorithm) const decimal OBJECT_MARGIN = decimal(0.04); diff --git a/src/constraint/Constraint.cpp b/src/constraint/Constraint.cpp index 5da1c4eb..ab9c7d99 100644 --- a/src/constraint/Constraint.cpp +++ b/src/constraint/Constraint.cpp @@ -33,7 +33,7 @@ Constraint::Constraint(const ConstraintInfo& constraintInfo) :mBody1(constraintInfo.body1), mBody2(constraintInfo.body2), mActive(true), mType(constraintInfo.type), mPositionCorrectionTechnique(constraintInfo.positionCorrectionTechnique), - mIsCollisionEnabled(constraintInfo.isCollisionEnabled){ + mIsCollisionEnabled(constraintInfo.isCollisionEnabled), mIsAlreadyInIsland(false) { assert(mBody1 != NULL); assert(mBody2 != NULL); diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index ae7bfa77..adc4092a 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -39,6 +39,32 @@ enum ConstraintType {CONTACT, BALLSOCKETJOINT, SLIDERJOINT, HINGEJOINT, FIXEDJOI // Class declarations struct ConstraintSolverData; +class Constraint; + +// Structure JointListElement +/** + * This structure represents a single element of a linked list of joints + */ +struct JointListElement { + + public: + + // -------------------- Attributes -------------------- // + + /// Pointer to the actual joint + Constraint* joint; + + /// Next element of the list + JointListElement* next; + + // -------------------- Methods -------------------- // + + /// Constructor + JointListElement(Constraint* initJoint, JointListElement* initNext) + :joint(initJoint), next(initNext){ + + } +}; // Structure ConstraintInfo /** @@ -87,9 +113,8 @@ struct ConstraintInfo { // Class Constraint /** * This abstract class represents a constraint in the physics engine. - * A constraint can be a collision contact or a joint for - * instance. Each constraint can be made of several "mathematical - * constraints" needed to represent the main constraint. + * A constraint can be a collision contact point or a joint for + * instance. */ class Constraint { @@ -121,6 +146,9 @@ class Constraint { /// True if the two bodies of the constraint are allowed to collide with each other bool mIsCollisionEnabled; + /// True if the joint has already been added into an island + bool mIsAlreadyInIsland; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -154,6 +182,9 @@ class Constraint { /// Return true if the collision between the two bodies of the constraint is enabled bool isCollisionEnabled() const; + /// Return true if the joint has already been added into an island + bool isAlreadyInIsland() const; + /// Return the number of bytes used by the constraint virtual size_t getSizeInBytes() const = 0; @@ -168,6 +199,11 @@ class Constraint { /// Solve the position constraint virtual void solvePositionConstraint(const ConstraintSolverData& constraintSolverData) = 0; + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; + friend class Island; }; // Return the reference to the body 1 @@ -195,6 +231,11 @@ inline bool Constraint::isCollisionEnabled() const { return mIsCollisionEnabled; } +// Return true if the joint has already been added into an island +inline bool Constraint::isAlreadyInIsland() const { + return mIsAlreadyInIsland; +} + } #endif diff --git a/src/constraint/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 57c8764f..30b929b7 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -59,7 +59,7 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo) // Compute the inverse of the initial orientation difference between the two bodies mInitOrientationDifferenceInv = transform2.getOrientation() * - transform1.getOrientation().getInverse(); + transform1.getOrientation().getInverse(); mInitOrientationDifferenceInv.normalize(); mInitOrientationDifferenceInv.inverse(); } diff --git a/src/engine/ContactManifold.cpp b/src/engine/ContactManifold.cpp index 69a67e97..b2b9987e 100644 --- a/src/engine/ContactManifold.cpp +++ b/src/engine/ContactManifold.cpp @@ -30,10 +30,10 @@ using namespace reactphysics3d; // Constructor -ContactManifold::ContactManifold(Body* const body1, Body* const body2, +ContactManifold::ContactManifold(CollisionBody* body1, CollisionBody* body2, MemoryAllocator& memoryAllocator) : mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0), - mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), + mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false), mMemoryAllocator(memoryAllocator) { } @@ -107,9 +107,12 @@ void ContactManifold::update(const Transform& transform1, const Transform& trans // Update the world coordinates and penetration depth of the contact points in the manifold for (uint i=0; isetWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1()); - mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2()); - mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal())); + mContactPoints[i]->setWorldPointOnBody1(transform1 * + mContactPoints[i]->getLocalPointOnBody1()); + mContactPoints[i]->setWorldPointOnBody2(transform2 * + mContactPoints[i]->getLocalPointOnBody2()); + mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - + mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal())); } const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * @@ -172,7 +175,8 @@ int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) cons /// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that /// when we compute this area, we do not calculate it exactly but we /// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, -/// this is only a guess that is faster to compute. +/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library +/// by Erwin Coumans (http://wwww.bulletphysics.org). int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); @@ -185,28 +189,32 @@ int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& ne if (indexMaxPenetration != 0) { // Compute the area Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - + mContactPoints[2]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area0 = crossProduct.lengthSquare(); } if (indexMaxPenetration != 1) { // Compute the area Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - + mContactPoints[2]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area1 = crossProduct.lengthSquare(); } if (indexMaxPenetration != 2) { // Compute the area Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - + mContactPoints[1]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area2 = crossProduct.lengthSquare(); } if (indexMaxPenetration != 3) { // Compute the area Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - + mContactPoints[1]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area3 = crossProduct.lengthSquare(); } diff --git a/src/engine/ContactManifold.h b/src/engine/ContactManifold.h index 25fd18c1..70ebaae9 100644 --- a/src/engine/ContactManifold.h +++ b/src/engine/ContactManifold.h @@ -28,7 +28,7 @@ // Libraries #include -#include "../body/Body.h" +#include "../body/CollisionBody.h" #include "../constraint/ContactPoint.h" #include "../memory/MemoryAllocator.h" @@ -38,6 +38,35 @@ namespace reactphysics3d { // Constants const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold +// Class declarations +class ContactManifold; + +// Structure ContactManifoldListElement +/** + * This structure represents a single element of a linked list of contact manifolds + */ +struct ContactManifoldListElement { + + public: + + // -------------------- Attributes -------------------- // + + /// Pointer to the actual contact manifold + ContactManifold* contactManifold; + + /// Next element of the list + ContactManifoldListElement* next; + + // -------------------- Methods -------------------- // + + /// Constructor + ContactManifoldListElement(ContactManifold* initContactManifold, + ContactManifoldListElement* initNext) + :contactManifold(initContactManifold), next(initNext) { + + } +}; + // Class ContactManifold /** * This class represents the set of contact points between two bodies. @@ -59,11 +88,11 @@ class ContactManifold { // -------------------- Attributes -------------------- // - /// Pointer to the first body - Body* const mBody1; + /// Pointer to the first body of the contact + CollisionBody* mBody1; - /// Pointer to the second body - Body* const mBody2; + /// Pointer to the second body of the contact + CollisionBody* mBody2; /// Contact points in the manifold ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; @@ -86,6 +115,9 @@ class ContactManifold { /// Twist friction constraint accumulated impulse decimal mFrictionTwistImpulse; + /// True if the contact manifold has already been added into an island + bool mIsAlreadyInIsland; + /// Reference to the memory allocator MemoryAllocator& mMemoryAllocator; @@ -97,6 +129,12 @@ class ContactManifold { /// Private assignment operator ContactManifold& operator=(const ContactManifold& contactManifold); + /// Return a pointer to the first body of the contact manifold + CollisionBody* getBody1() const; + + /// Return a pointer to the second body of the contact manifold + CollisionBody* getBody2() const; + /// Return the index of maximum area int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; @@ -109,15 +147,16 @@ class ContactManifold { /// Remove a contact point from the manifold void removeContactPoint(uint index); - /// Return true if two vectors are approximatively equal - bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; + /// Return true if the contact manifold has already been added into an island + bool isAlreadyInIsland() const; public: // -------------------- Methods -------------------- // /// Constructor - ContactManifold(Body* const mBody1, Body* const mBody2, MemoryAllocator& memoryAllocator); + ContactManifold(CollisionBody* body1, CollisionBody* body2, + MemoryAllocator& memoryAllocator); /// Destructor ~ContactManifold(); @@ -166,8 +205,23 @@ class ContactManifold { /// Return a contact point of the manifold ContactPoint* getContactPoint(uint index) const; + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; + friend class Island; }; +// Return a pointer to the first body of the contact manifold +inline CollisionBody* ContactManifold::getBody1() const { + return mBody1; +} + +// Return a pointer to the second body of the contact manifold +inline CollisionBody* ContactManifold::getBody2() const { + return mBody2; +} + // Return the number of contact points in the manifold inline uint ContactManifold::getNbContactPoints() const { return mNbContactPoints; @@ -229,6 +283,11 @@ inline ContactPoint* ContactManifold::getContactPoint(uint index) const { return mContactPoints[index]; } +// Return true if the contact manifold has already been added into an island +inline bool ContactManifold::isAlreadyInIsland() const { + return mIsAlreadyInIsland; +} + } #endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 605aec54..4ca989a9 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -44,7 +44,8 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_ mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), - mIsDeactivationActive(DEACTIVATION_ENABLED) { + mIsSleepingEnabled(SPLEEPING_ENABLED), mNbIslands(0), mNbIslandsCapacity(0), + mIslands(NULL) { } @@ -59,6 +60,19 @@ DynamicsWorld::~DynamicsWorld() { mMemoryAllocator.release((*it).second, sizeof(OverlappingPair)); } + // Release the memory allocated for the islands + for (uint i=0; iIsland::~Island(); + + // Release the allocated memory for the island + mMemoryAllocator.release(mIslands[i], sizeof(Island)); + } + if (mNbIslandsCapacity > 0) { + mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity); + } + // Free the allocated memory for the constrained velocities cleanupConstrainedVelocitiesArray(); @@ -93,6 +107,9 @@ void DynamicsWorld::update() { // Remove all contact manifolds mContactManifolds.clear(); + + // Reset all the contact manifolds lists of each body + resetContactManifoldListsOfBodies(); // Compute the collision detection mCollisionDetection.computeCollisionDetection(); @@ -106,6 +123,9 @@ void DynamicsWorld::update() { // Update the timer mTimer.nextStep(); + // Compute the islands (separate groups of bodies with constraints between each others) + computeIslands(); + // Solve the contacts and constraints solveContactsAndConstraints(); @@ -448,7 +468,8 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { // Remove the collision shape from the world removeCollisionShape(rigidBody->getCollisionShape()); - // Destroy all the joints that contains the rigid body to be destroyed + // Destroy all the joints in which the rigid body to be destroyed is involved + // TODO : Iterate on the mJointList of the rigid body instead over all the joints of the world bodyindex idToRemove = rigidBody->getID(); for (std::set::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { if ((*it)->getBody1()->getID() == idToRemove || (*it)->getBody2()->getID() == idToRemove) { @@ -456,6 +477,9 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { } } + // Reset the contact manifold list of the body + rigidBody->resetContactManifoldsList(mMemoryAllocator); + // Call the destructor of the rigid body rigidBody->RigidBody::~RigidBody(); @@ -529,6 +553,9 @@ Constraint* DynamicsWorld::createJoint(const ConstraintInfo& jointInfo) { // Add the joint into the world mJoints.insert(newJoint); + // Add the joint into the joint list of the bodies involved in the joint + addJointToBody(newJoint); + // Return the pointer to the created joint return newJoint; } @@ -548,14 +575,235 @@ void DynamicsWorld::destroyJoint(Constraint* joint) { // Remove the joint from the world mJoints.erase(joint); - // Get the size in bytes of the joint - size_t nbBytes = joint->getSizeInBytes(); + // Remove the joint from the joint list of the bodies involved in the joint + joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint); + joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint); // Call the destructor of the joint joint->Constraint::~Constraint(); // Release the allocated memory - mMemoryAllocator.release(joint, nbBytes); + mMemoryAllocator.release(joint, joint->getSizeInBytes()); +} + +// Add the joint to the list of joints of the two bodies involved in the joint +void DynamicsWorld::addJointToBody(Constraint* joint) { + + assert(joint != NULL); + + // Add the joint at the beginning of the linked list of joints of the first body + void* allocatedMemory1 = mMemoryAllocator.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)); + JointListElement* jointListElement2 = new (allocatedMemory2) JointListElement(joint, + joint->mBody2->mJointsList); + joint->mBody2->mJointsList = jointListElement2; +} + +// Add a contact manifold to the linked list of contact manifolds of the two bodies involed +// in the corresponding contact +void DynamicsWorld::addContactManifoldToBody(ContactManifold* contactManifold, + CollisionBody* body1, CollisionBody* body2) { + + assert(contactManifold != NULL); + + // Add the contact manifold at the beginning of the linked + // list of contact manifolds of the first body + void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + ContactManifoldListElement* listElement1 = new (allocatedMemory1) + ContactManifoldListElement(contactManifold, + body1->mContactManifoldsList); + body1->mContactManifoldsList = listElement1; + + // Add the joint at the beginning of the linked list of joints of the second body + void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement)); + ContactManifoldListElement* listElement2 = new (allocatedMemory2) + ContactManifoldListElement(contactManifold, + body2->mContactManifoldsList); + body2->mContactManifoldsList = listElement2; +} + +// Reset all the contact manifolds linked list of each body +void DynamicsWorld::resetContactManifoldListsOfBodies() { + + // For each rigid body of the world + for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + + // Reset the contact manifold list of the body + (*it)->resetContactManifoldsList(mMemoryAllocator); + } +} + +// Compute the islands of awake bodies. +/// An island is an isolated group of rigid bodies that have constraints (joints or contacts) +/// between each other. This method computes the islands at each time step as follows: For each +/// awake rigid body, we run a Depth First Search (DFS) through the constraint graph of that body +/// (graph where nodes are the bodies and where the edges are the constraints between the bodies) to +/// find all the bodies that are connected with it (the bodies that share joints or contacts with +/// it). Then, we create an island with this group of connected bodies. +void DynamicsWorld::computeIslands() { + + PROFILE("DynamicsWorld::computeIslands()"); + + uint nbBodies = mRigidBodies.size(); + + // Clear all the islands + for (uint i=0; iIsland::~Island(); + + // Release the allocated memory for the island + mMemoryAllocator.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); + } + mNbIslandsCapacity = nbBodies; + mIslands = (Island**)mMemoryAllocator.allocate(sizeof(Island*) * mNbIslandsCapacity); + } + mNbIslands = 0; + + // Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds + for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + (*it)->mIsAlreadyInIsland = false; + } + for (std::vector::iterator it = mContactManifolds.begin(); + it != mContactManifolds.end(); ++it) { + (*it)->mIsAlreadyInIsland = false; + } + for (std::set::iterator it = mJoints.begin(); it != mJoints.end(); ++it) { + (*it)->mIsAlreadyInIsland = false; + } + + // 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); + + uint idIsland = 0; // TODO : REMOVE THIS + + // For each rigid body of the world + for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + + RigidBody* body = *it; + + // If the body has already been added to an island, we go to the next body + if (body->isAlreadyInIsland()) continue; + + // If the body is not moving, we go to the next body + // TODO : When we will use STATIC bodies, we will need to take care of this case here + if (!body->getIsMotionEnabled()) continue; + + // If the body is sleeping, we go to the next body + if (body->isSleeping()) continue; + + // Reset the stack of bodies to visit + uint stackIndex = 0; + stackBodiesToVisit[stackIndex] = body; + stackIndex++; + body->mIsAlreadyInIsland = true; + + // Create the new island + void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island)); + mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(idIsland, nbBodies,mContactManifolds.size(), + mJoints.size(), mMemoryAllocator); + idIsland++; + + // While there are still some bodies to visit in the stack + while (stackIndex > 0) { + + // Get the next body to visit from the stack + stackIndex--; + RigidBody* bodyToVisit = stackBodiesToVisit[stackIndex]; + + // Awake the body if it is slepping + bodyToVisit->setIsSleeping(false); + + // Add the body into the island + mIslands[mNbIslands]->addBody(bodyToVisit); + + // If the current body is not moving, we do not want to perform the DFS + // search across that body + if (!bodyToVisit->getIsMotionEnabled()) continue; + + // For each contact manifold in which the current body is involded + ContactManifoldListElement* contactElement; + for (contactElement = bodyToVisit->mContactManifoldsList; contactElement != NULL; + contactElement = contactElement->next) { + + ContactManifold* contactManifold = contactElement->contactManifold; + + // Check if the current contact manifold has already been added into an island + if (contactManifold->isAlreadyInIsland()) continue; + + // Add the contact manifold into the island + mIslands[mNbIslands]->addContactManifold(contactManifold); + contactManifold->mIsAlreadyInIsland = true; + + // Get the other body of the contact manifold + RigidBody* body1 = dynamic_cast(contactManifold->getBody1()); + RigidBody* body2 = dynamic_cast(contactManifold->getBody2()); + RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; + + // Check if the other body has already been added to the island + if (otherBody->isAlreadyInIsland()) continue; + + // Insert the other body into the stack of bodies to visit + stackBodiesToVisit[stackIndex] = otherBody; + stackIndex++; + otherBody->mIsAlreadyInIsland = true; + } + + // For each joint in which the current body is involved + JointListElement* jointElement; + for (jointElement = bodyToVisit->mJointsList; jointElement != NULL; + jointElement = jointElement->next) { + + Constraint* joint = jointElement->joint; + + // Check if the current joint has already been added into an island + if (joint->isAlreadyInIsland()) continue; + + // Add the joint into the island + mIslands[mNbIslands]->addJoint(joint); + joint->mIsAlreadyInIsland = true; + + // Get the other body of the contact manifold + RigidBody* body1 = dynamic_cast(joint->getBody1()); + RigidBody* body2 = dynamic_cast(joint->getBody2()); + RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1; + + // Check if the other body has already been added to the island + if (otherBody->isAlreadyInIsland()) continue; + + // Insert the other body into the stack of bodies to visit + stackBodiesToVisit[stackIndex] = otherBody; + stackIndex++; + otherBody->mIsAlreadyInIsland = true; + } + } + + // Reset the isAlreadyIsland variable of the static bodies so that they + // can also be included in the other islands + for (uint i=0; i < mIslands[mNbIslands]->mNbBodies; i++) { + + if (!mIslands[mNbIslands]->mBodies[i]->getIsMotionEnabled()) { + mIslands[mNbIslands]->mBodies[i]->mIsAlreadyInIsland = false; + } + } + + mNbIslands++; + } + + // Release the allocated memory for the stack of bodies to visit + mMemoryAllocator.release(stackBodiesToVisit, nbBytesStack); } // Notify the world about a new broad-phase overlapping pair @@ -565,8 +813,8 @@ void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) bodyindexpair indexPair = addedPair->getBodiesIndexPair(); // Add the pair into the set of overlapping pairs (if not there yet) - OverlappingPair* newPair = new (mMemoryAllocator.allocate(sizeof(OverlappingPair))) OverlappingPair( - addedPair->body1, addedPair->body2, mMemoryAllocator); + OverlappingPair* newPair = new (mMemoryAllocator.allocate(sizeof(OverlappingPair))) + OverlappingPair(addedPair->body1, addedPair->body2, mMemoryAllocator); assert(newPair != NULL); std::pair::iterator, bool> check = mOverlappingPairs.insert(make_pair(indexPair, newPair)); @@ -604,4 +852,9 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, // Add the contact manifold to the world mContactManifolds.push_back(overlappingPair->getContactManifold()); + + // Add the contact manifold into the list of contact manifolds + // of the two bodies involved in the contact + addContactManifoldToBody(overlappingPair->getContactManifold(), overlappingPair->mBody1, + overlappingPair->mBody2); } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index a1a1f821..89ebec5d 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -33,6 +33,7 @@ #include "ConstraintSolver.h" #include "../body/RigidBody.h" #include "Timer.h" +#include "Island.h" #include "../configuration.h" /// Namespace ReactPhysics3D @@ -65,13 +66,14 @@ class DynamicsWorld : public CollisionWorld { /// Number of iterations for the position solver of the Sequential Impulses technique uint mNbPositionSolverIterations; - /// True if the deactivation (sleeping) of inactive bodies is enabled - bool mIsDeactivationActive; + /// True if the spleeping technique for inactive bodies is enabled + bool mIsSleepingEnabled; /// All the rigid bodies of the physics world std::set mRigidBodies; /// All the contact constraints + // TODO : Remove this variable (we will use the ones in the island now) std::vector mContactManifolds; /// All the joints of the world @@ -100,6 +102,15 @@ class DynamicsWorld : public CollisionWorld { /// Map body to their index in the constrained velocities array std::map mMapBodyToConstrainedVelocityIndex; + /// Number of islands in the world + uint mNbIslands; + + /// Current allocated capacity for the islands + uint mNbIslandsCapacity; + + /// Array with all the islands of awaken bodies + Island** mIslands; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -136,6 +147,9 @@ class DynamicsWorld : public CollisionWorld { /// Reset the boolean movement variable of each body void resetBodiesMovementVariable(); + /// Compute the islands of awake bodies. + void computeIslands(); + /// Update the overlapping pair virtual void updateOverlappingPair(const BroadPhasePair* pair); @@ -198,6 +212,17 @@ public : /// Destroy a joint void destroyJoint(Constraint* joint); + /// Add the joint to the list of joints of the two bodies involved in the joint + void addJointToBody(Constraint* joint); + + //// Add a contact manifold to the linked list of contact manifolds of the two bodies involed + //// in the corresponding contact. + void addContactManifoldToBody(ContactManifold* contactManifold, + CollisionBody *body1, CollisionBody *body2); + + /// Reset all the contact manifolds linked list of each body + void resetContactManifoldListsOfBodies(); + /// Return the gravity vector of the world Vector3 getGravity() const; @@ -227,6 +252,12 @@ public : /// Return a reference to the contact manifolds of the world const std::vector& getContactManifolds() const; + + // TODO : REMOVE THIS + Island** getIslands() { return mIslands;} + + // TODO : REMOVE THIS + uint getNbIslands() const {return mNbIslands;} }; // Start the physics simulation diff --git a/src/engine/Island.cpp b/src/engine/Island.cpp new file mode 100644 index 00000000..06b3734b --- /dev/null +++ b/src/engine/Island.cpp @@ -0,0 +1,54 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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 "Island.h" + +using namespace reactphysics3d; + +// Constructor +Island::Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, + MemoryAllocator& memoryAllocator) + : mID(id), mBodies(NULL), mContactManifolds(NULL), mJoints(NULL), mNbBodies(0), + mNbContactManifolds(0), mNbJoints(0), mMemoryAllocator(memoryAllocator) { + + // Allocate memory for the arrays + mNbAllocatedBytesBodies = sizeof(RigidBody*) * nbMaxBodies; + mBodies = (RigidBody**) mMemoryAllocator.allocate(mNbAllocatedBytesBodies); + mNbAllocatedBytesContactManifolds = sizeof(ContactManifold*) * nbMaxContactManifolds; + mContactManifolds = (ContactManifold**) mMemoryAllocator.allocate( + mNbAllocatedBytesContactManifolds); + mNbAllocatedBytesJoints = sizeof(Constraint*) * nbMaxJoints; + mJoints = (Constraint**) mMemoryAllocator.allocate(mNbAllocatedBytesJoints); +} + +// Destructor +Island::~Island() { + + // Release the memory of the arrays + mMemoryAllocator.release(mBodies, mNbAllocatedBytesBodies); + mMemoryAllocator.release(mContactManifolds, mNbAllocatedBytesContactManifolds); + mMemoryAllocator.release(mJoints, mNbAllocatedBytesJoints); +} diff --git a/src/engine/Island.h b/src/engine/Island.h new file mode 100644 index 00000000..bcd9d990 --- /dev/null +++ b/src/engine/Island.h @@ -0,0 +1,186 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2013 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_ISLAND_H +#define REACTPHYSICS3D_ISLAND_H + +// Libraries +#include "../memory/MemoryAllocator.h" +#include "../body/RigidBody.h" +#include "../constraint/Constraint.h" +#include "ContactManifold.h" + +namespace reactphysics3d { + +// Class Island +/** + * An island represent an isolated group of awake bodies that are connected with each other by + * some contraints (contacts or joints). + */ +class Island { + + private: + + // -------------------- Attributes -------------------- // + + // TODO : REMOVE THIS + uint mID; + + /// Array with all the bodies of the island + RigidBody** mBodies; + + /// Array with all the contact manifolds between bodies of the island + ContactManifold** mContactManifolds; + + /// Array with all the joints between bodies of the island + Constraint** mJoints; + + /// Current number of bodies in the island + uint mNbBodies; + + /// Current number of contact manifold in the island + uint mNbContactManifolds; + + /// Current number of joints in the island + uint mNbJoints; + + /// Reference to the memory allocator + MemoryAllocator& mMemoryAllocator; + + /// Number of bytes allocated for the bodies array + size_t mNbAllocatedBytesBodies; + + /// Number of bytes allocated for the contact manifolds array + size_t mNbAllocatedBytesContactManifolds; + + /// Number of bytes allocated for the joints array + size_t mNbAllocatedBytesJoints; + + // -------------------- Methods -------------------- // + + /// Private assignment operator + Island& operator=(const Island& island); + + /// Private copy-constructor + Island(const Island& island); + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + Island(uint id, uint nbMaxBodies, uint nbMaxContactManifolds, uint nbMaxJoints, + MemoryAllocator& memoryAllocator); + + /// Destructor + ~Island(); + + /// Add a body into the island + void addBody(RigidBody* body); + + /// Add a contact manifold into the island + void addContactManifold(ContactManifold* contactManifold); + + /// Add a joint into the island + void addJoint(Constraint* joint); + + /// Return the number of bodies in the island + uint getNbBodies() const; + + /// Return the number of contact manifolds in the island + uint getNbContactManifolds() const; + + /// Return the number of joints in the island + uint getNbJoints() const; + + /// Return a pointer to the array of bodies + RigidBody** getBodies(); + + /// Return a pointer to the array of contact manifolds + ContactManifold** getContactManifold(); + + /// Return a pointer to the array of joints + Constraint** getJoints(); + + // TODO : REMOVE THIS + uint getID() const {return mID;} + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; +}; + +// Add a body into the island +inline void Island::addBody(RigidBody* body) { + assert(!body->isSleeping()); + mBodies[mNbBodies] = body; + mNbBodies++; +} + +// Add a contact manifold into the island +inline void Island::addContactManifold(ContactManifold* contactManifold) { + mContactManifolds[mNbContactManifolds] = contactManifold; + mNbContactManifolds++; +} + +// Add a joint into the island +inline void Island::addJoint(Constraint* joint) { + mJoints[mNbJoints] = joint; + mNbJoints++; +} + +// Return the number of bodies in the island +inline uint Island::getNbBodies() const { + return mNbBodies; +} + +// Return the number of contact manifolds in the island +inline uint Island::getNbContactManifolds() const { + return mNbContactManifolds; +} + +// Return the number of joints in the island +inline uint Island::getNbJoints() const { + return mNbJoints; +} + +// Return a pointer to the array of bodies +inline RigidBody** Island::getBodies() { + return mBodies; +} + +// Return a pointer to the array of contact manifolds +inline ContactManifold** Island::getContactManifold() { + return mContactManifolds; +} + +// Return a pointer to the array of joints +inline Constraint** Island::getJoints() { + return mJoints; +} + +} + +#endif diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index 40f1958b..1ecfe0c6 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -47,10 +47,10 @@ class OverlappingPair { // -------------------- Attributes -------------------- // /// Pointer to the first body of the contact - CollisionBody* const mBody1; + CollisionBody* mBody1; /// Pointer to the second body of the contact - CollisionBody* const mBody2; + CollisionBody* mBody2; /// Persistent contact manifold ContactManifold mContactManifold; @@ -100,6 +100,10 @@ class OverlappingPair { /// Return the contact manifold ContactManifold* getContactManifold(); + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; }; // Return the pointer to first body diff --git a/src/memory/MemoryAllocator.cpp b/src/memory/MemoryAllocator.cpp index e715a157..ec074042 100644 --- a/src/memory/MemoryAllocator.cpp +++ b/src/memory/MemoryAllocator.cpp @@ -148,10 +148,10 @@ void* MemoryAllocator::allocate(size_t size) { assert(nbUnits * unitSize <= BLOCK_SIZE); for (size_t i=0; i < nbUnits - 1; i++) { MemoryUnit* unit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * i); - MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i + 1)); + MemoryUnit* nextUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (i+1)); unit->nextUnit = nextUnit; } - MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize * (nbUnits - 1)); + MemoryUnit* lastUnit = (MemoryUnit*) ((size_t)newBlock->memoryUnits + unitSize*(nbUnits-1)); lastUnit->nextUnit = NULL; // Add the new allocated block into the list of free memory units in the heap