diff --git a/src/body/Body.cpp b/src/body/Body.cpp index a9c639f3..5cdff7d2 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,9 @@ using namespace reactphysics3d; // Constructor -Body::Body(bodyindex id) : mID(id) { +Body::Body(bodyindex id) + : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsSleeping(false), + mSleepTime(0) { } diff --git a/src/body/Body.h b/src/body/Body.h index d131eec8..299de91f 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -47,6 +47,18 @@ 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; + + /// Elapsed time since the body velocity was bellow the sleep velocity + decimal mSleepTime; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -68,6 +80,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 + virtual void setIsSleeping(bool isSleeping); + /// Smaller than operator bool operator<(const Body& body2) const; @@ -79,6 +109,10 @@ class Body { /// Not equal operator bool operator!=(const Body& body2) const; + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; }; // Return the id of the body @@ -86,6 +120,48 @@ 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; + + if (!mIsAllowedToSleep) setIsSleeping(false); +} + +// 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) { + + if (isSleeping) { + mSleepTime = decimal(0.0); + } + else { + if (mIsSleeping) { + mSleepTime = decimal(0.0); + } + } + + 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..82e5c928 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,42 @@ 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; + } + currentElement = currentElement->next; + } + } +} + + diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 6136f1dd..1d38d610 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,16 @@ 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; + + /// Set the variable to know whether or not the body is sleeping + virtual void setIsSleeping(bool isSleeping); + + // -------------------- Friendship -------------------- // + + friend class DynamicsWorld; }; // Method that return the mass of the body @@ -328,6 +349,24 @@ 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; +} + +// Set the variable to know whether or not the body is sleeping +inline void RigidBody::setIsSleeping(bool isSleeping) { + + if (isSleeping) { + mLinearVelocity.setToZero(); + mAngularVelocity.setToZero(); + mExternalForce.setToZero(); + mExternalTorque.setToZero(); + } + + Body::setIsSleeping(isSleeping); +} + } #endif diff --git a/src/configuration.h b/src/configuration.h index c762f14e..7141ab18 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 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); @@ -110,6 +110,17 @@ const uint DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 15; /// Number of iterations when solving the position constraints of the Sequential Impulse technique const uint DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5; +/// Time (in seconds) that a body must stay still to be considered sleeping +const float DEFAULT_TIME_BEFORE_SLEEP = 1.0f; + +/// A body with a linear velocity smaller than the sleep linear velocity (in m/s) +/// might enter sleeping mode. +const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02); + +/// A body with angular velocity smaller than the sleep angular velocity (in rad/s) +/// might enter sleeping mode +const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0)); + } #endif 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..b3eb9f9b 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(); } @@ -732,7 +732,9 @@ void HingeJoint::enableMotor(bool isMotorEnabled) { mIsMotorEnabled = isMotorEnabled; mImpulseMotor = 0.0; - // TODO : Wake up the bodies of the joint here when sleeping is implemented + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } // Set the minimum angle limit @@ -770,7 +772,9 @@ void HingeJoint::resetLimits() { mImpulseLowerLimit = 0.0; mImpulseUpperLimit = 0.0; - // TODO : Wake up the bodies of the joint here when sleeping is implemented + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } // Set the motor speed @@ -780,7 +784,9 @@ void HingeJoint::setMotorSpeed(decimal motorSpeed) { mMotorSpeed = motorSpeed; - // TODO : Wake up the bodies of the joint here when sleeping is implemented + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } } @@ -792,7 +798,9 @@ void HingeJoint::setMaxMotorTorque(decimal maxMotorTorque) { assert(mMaxMotorTorque >= 0.0); mMaxMotorTorque = maxMotorTorque; - // TODO : Wake up the bodies of the joint here when sleeping is implemented + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } } diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp index e1f2daed..9a0f1321 100644 --- a/src/constraint/SliderJoint.cpp +++ b/src/constraint/SliderJoint.cpp @@ -768,7 +768,9 @@ void SliderJoint::enableMotor(bool isMotorEnabled) { mIsMotorEnabled = isMotorEnabled; mImpulseMotor = 0.0; - // TODO : Wake up the bodies of the joint here when sleeping is implemented + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } // Return the current translation value of the joint @@ -830,7 +832,9 @@ void SliderJoint::resetLimits() { mImpulseLowerLimit = 0.0; mImpulseUpperLimit = 0.0; - // TODO : Wake up the bodies of the joint here when sleeping is implemented + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } // Set the motor speed @@ -840,7 +844,9 @@ void SliderJoint::setMotorSpeed(decimal motorSpeed) { mMotorSpeed = motorSpeed; - // TODO : Wake up the bodies of the joint here when sleeping is implemented + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } } @@ -852,6 +858,8 @@ void SliderJoint::setMaxMotorForce(decimal maxMotorForce) { assert(mMaxMotorForce >= 0.0); mMaxMotorForce = maxMotorForce; - // TODO : Wake up the bodies of the joint here when sleeping is implemented + // Wake up the two bodies of the joint + mBody1->setIsSleeping(false); + mBody2->setIsSleeping(false); } } diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 2154ceb0..25ef6641 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -30,18 +30,14 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver(std::set& joints, - std::vector& linearVelocities, - std::vector& angularVelocities, - std::vector& positions, +ConstraintSolver::ConstraintSolver(std::vector& positions, std::vector& orientations, const std::map& mapBodyToVelocityIndex) - : mJoints(joints), mLinearVelocities(linearVelocities), - mAngularVelocities(angularVelocities), mPositions(positions), + : mLinearVelocities(NULL), mAngularVelocities(NULL), mPositions(positions), mOrientations(orientations), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), mConstraintSolverData(linearVelocities, - angularVelocities, positions, orientations, mapBodyToVelocityIndex){ + mIsWarmStartingActive(true), mConstraintSolverData(positions, orientations, + mapBodyToVelocityIndex){ } @@ -50,10 +46,16 @@ ConstraintSolver::~ConstraintSolver() { } -// Initialize the constraint solver -void ConstraintSolver::initialize(decimal dt) { +// Initialize the constraint solver for a given island +void ConstraintSolver::initializeForIsland(decimal dt, Island* island) { - PROFILE("ConstraintSolver::initialize()"); + PROFILE("ConstraintSolver::initializeForIsland()"); + + assert(mLinearVelocities != NULL); + assert(mAngularVelocities != NULL); + assert(island != NULL); + assert(island->getNbBodies() > 0); + assert(island->getNbJoints() > 0); // Set the current time step mTimeStep = dt; @@ -62,54 +64,50 @@ void ConstraintSolver::initialize(decimal dt) { mConstraintSolverData.timeStep = mTimeStep; mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive; - // For each joint - std::set::iterator it; - for (it = mJoints.begin(); it != mJoints.end(); ++it) { - - Constraint* joint = (*it); - - // Get the rigid bodies of the joint - RigidBody* body1 = joint->getBody1(); - RigidBody* body2 = joint->getBody2(); - - // Add the bodies to the set of constrained bodies - mConstraintBodies.insert(body1); - mConstraintBodies.insert(body2); + // For each joint of the island + Constraint** joints = island->getJoints(); + for (uint i=0; igetNbJoints(); i++) { // Initialize the constraint before solving it - joint->initBeforeSolve(mConstraintSolverData); + joints[i]->initBeforeSolve(mConstraintSolverData); // Warm-start the constraint if warm-starting is enabled if (mIsWarmStartingActive) { - joint->warmstart(mConstraintSolverData); + joints[i]->warmstart(mConstraintSolverData); } } } // Solve the velocity constraints -void ConstraintSolver::solveVelocityConstraints() { +void ConstraintSolver::solveVelocityConstraints(Island* island) { PROFILE("ConstraintSolver::solveVelocityConstraints()"); - // For each joint - std::set::iterator it; - for (it = mJoints.begin(); it != mJoints.end(); ++it) { + assert(island != NULL); + assert(island->getNbJoints() > 0); + + // For each joint of the island + Constraint** joints = island->getJoints(); + for (uint i=0; igetNbJoints(); i++) { // Solve the constraint - (*it)->solveVelocityConstraint(mConstraintSolverData); + joints[i]->solveVelocityConstraint(mConstraintSolverData); } } // Solve the position constraints -void ConstraintSolver::solvePositionConstraints() { +void ConstraintSolver::solvePositionConstraints(Island* island) { PROFILE("ConstraintSolver::solvePositionConstraints()"); - // For each joint - std::set::iterator it; - for (it = mJoints.begin(); it != mJoints.end(); ++it) { + assert(island != NULL); + assert(island->getNbJoints() > 0); + + // For each joint of the island + Constraint** joints = island->getJoints(); + for (uint i=0; i < island->getNbJoints(); i++) { // Solve the constraint - (*it)->solvePositionConstraint(mConstraintSolverData); + joints[i]->solvePositionConstraint(mConstraintSolverData); } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 79cac7f7..f52c6412 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -30,6 +30,7 @@ #include "../configuration.h" #include "mathematics/mathematics.h" #include "../constraint/Constraint.h" +#include "Island.h" #include #include @@ -47,11 +48,11 @@ struct ConstraintSolverData { /// Current time step of the simulation decimal timeStep; - /// Reference to the bodies linear velocities - std::vector& linearVelocities; + /// Array with the bodies linear velocities + Vector3* linearVelocities; - /// Reference to the bodies angular velocities - std::vector& angularVelocities; + /// Array with the bodies angular velocities + Vector3* angularVelocities; /// Reference to the bodies positions std::vector& positions; @@ -67,13 +68,11 @@ struct ConstraintSolverData { bool isWarmStartingActive; /// Constructor - ConstraintSolverData(std::vector& refLinearVelocities, - std::vector& refAngularVelocities, - std::vector& refPositions, + ConstraintSolverData(std::vector& refPositions, std::vector& refOrientations, const std::map& refMapBodyToConstrainedVelocityIndex) - :linearVelocities(refLinearVelocities), - angularVelocities(refAngularVelocities), + :linearVelocities(NULL), + angularVelocities(NULL), positions(refPositions), orientations(refOrientations), mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){ @@ -156,19 +155,13 @@ class ConstraintSolver { // -------------------- Attributes -------------------- // - /// Reference to all the joints of the world - std::set& mJoints; - - /// Constrained bodies - std::set mConstraintBodies; - - /// Reference to the array of constrained linear velocities (state of the linear velocities + /// Array of constrained linear velocities (state of the linear velocities /// after solving the constraints) - std::vector& mLinearVelocities; + Vector3* mLinearVelocities; - /// Reference to the array of constrained angular velocities (state of the angular velocities + /// Array of constrained angular velocities (state of the angular velocities /// after solving the constraints) - std::vector& mAngularVelocities; + Vector3* mAngularVelocities; /// Reference to the array of bodies positions (for position error correction) std::vector& mPositions; @@ -194,24 +187,20 @@ class ConstraintSolver { // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(std::set& joints, - std::vector& linearVelocities, - std::vector& angularVelocities, - std::vector& positions, - std::vector& orientations, + ConstraintSolver(std::vector& positions, std::vector& orientations, const std::map& mapBodyToVelocityIndex); /// Destructor ~ConstraintSolver(); - /// Initialize the constraint solver - void initialize(decimal dt); + /// Initialize the constraint solver for a given island + void initializeForIsland(decimal dt, Island* island); /// Solve the constraints - void solveVelocityConstraints(); + void solveVelocityConstraints(Island* island); /// Solve the position constraints - void solvePositionConstraints(); + void solvePositionConstraints(Island* island); /// Return true if the Non-Linear-Gauss-Seidel position correction technique is active bool getIsNonLinearGaussSeidelPositionCorrectionActive() const; @@ -219,13 +208,20 @@ class ConstraintSolver { /// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique. void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive); - /// Return true if the body is in at least one constraint - bool isConstrainedBody(RigidBody* body) const; + /// Set the constrained velocities arrays + void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, + Vector3* constrainedAngularVelocities); }; -// Return true if the body is in at least one constraint -inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { - return mConstraintBodies.count(body) == 1; +// Set the constrained velocities arrays +inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, + Vector3* constrainedAngularVelocities) { + assert(constrainedLinearVelocities != NULL); + assert(constrainedAngularVelocities != NULL); + mLinearVelocities = constrainedLinearVelocities; + mAngularVelocities = constrainedAngularVelocities; + mConstraintSolverData.linearVelocities = mLinearVelocities; + mConstraintSolverData.angularVelocities = mAngularVelocities; } } 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/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 8467a710..b40eb8f2 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -36,18 +36,12 @@ using namespace std; // Constants initialization const decimal ContactSolver::BETA = decimal(0.2); const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); -const decimal ContactSolver::SLOP = decimal(0.01); +const decimal ContactSolver::SLOP= decimal(0.01); // Constructor -ContactSolver::ContactSolver(std::vector& contactManifolds, - std::vector& constrainedLinearVelocities, - std::vector& constrainedAngularVelocities, - const std::map& mapBodyToVelocityIndex) - :mContactManifolds(contactManifolds), - mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), - mContactConstraints(NULL), - mLinearVelocities(constrainedLinearVelocities), - mAngularVelocities(constrainedAngularVelocities), +ContactSolver::ContactSolver(const std::map& mapBodyToVelocityIndex) + :mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), + mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), mIsWarmStartingActive(true), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { @@ -59,26 +53,32 @@ ContactSolver::~ContactSolver() { } -// Initialize the constraint solver -void ContactSolver::initialize(decimal dt) { +// Initialize the constraint solver for a given island +void ContactSolver::initializeForIsland(decimal dt, Island* island) { - PROFILE("ContactSolver::initialize()"); + PROFILE("ContactSolver::initializeForIsland()"); + + assert(island != NULL); + assert(island->getNbBodies() > 0); + assert(island->getNbContactManifolds() > 0); + assert(mSplitLinearVelocities != NULL); + assert(mSplitAngularVelocities != NULL); // Set the current time step mTimeStep = dt; - // TODO : Use better memory allocation here - mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()]; + mNbContactManifolds = island->getNbContactManifolds(); - mNbContactManifolds = 0; + mContactConstraints = new ContactManifoldSolver[mNbContactManifolds]; + assert(mContactConstraints != NULL); - // For each contact manifold of the world - vector::iterator it; - for (it = mContactManifolds.begin(); it != mContactManifolds.end(); ++it) { + // For each contact manifold of the island + ContactManifold** contactManifolds = island->getContactManifold(); + for (uint i=0; igetNbContactPoints() > 0); @@ -86,10 +86,6 @@ void ContactSolver::initialize(decimal dt) { RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1(); RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2(); - // Add the two bodies of the constraint in the constraintBodies list - mConstraintBodies.insert(body1); - mConstraintBodies.insert(body2); - // Get the position of the two bodies Vector3 x1 = body1->getTransform().getPosition(); Vector3 x2 = body2->getTransform().getPosition(); @@ -173,46 +169,12 @@ void ContactSolver::initialize(decimal dt) { internalManifold.frictionTwistImpulse = 0.0; } } - - mNbContactManifolds++; } - // Allocated memory for split impulse velocities - // TODO : Use better memory allocation here - mSplitLinearVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()]; - mSplitAngularVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()]; - assert(mSplitLinearVelocities != NULL); - assert(mSplitAngularVelocities != NULL); - - assert(mConstraintBodies.size() > 0); - assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size()); - assert(mLinearVelocities.size() >= mConstraintBodies.size()); - assert(mAngularVelocities.size() >= mConstraintBodies.size()); - - // Initialize the split impulse velocities - initializeSplitImpulseVelocities(); - // Fill-in all the matrices needed to solve the LCP problem initializeContactConstraints(); } -// Initialize the split impulse velocities -void ContactSolver::initializeSplitImpulseVelocities() { - - // For each current body that is implied in some constraint - set::iterator it; - for (it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) { - RigidBody* rigidBody = *it; - assert(rigidBody); - - uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second; - - // Initialize the split impulse velocities to zero - mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); - mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); - } -} - // Initialize the contact constraints before solving the system void ContactSolver::initializeContactConstraints() { @@ -304,10 +266,10 @@ void ContactSolver::initializeContactConstraints() { // 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 under a given threshold), we don't add a restitution velocity bias + // 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) { + if (deltaVDotN < -RESTITUTION_VELOCITY_THRESHOLD) { contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; } @@ -884,18 +846,8 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // Clean up the constraint solver void ContactSolver::cleanup() { - mConstraintBodies.clear(); - if (mContactConstraints != NULL) { delete[] mContactConstraints; mContactConstraints = NULL; } - if (mSplitLinearVelocities != NULL) { - delete[] mSplitLinearVelocities; - mSplitLinearVelocities = NULL; - } - if (mSplitAngularVelocities != NULL) { - delete[] mSplitAngularVelocities; - mSplitAngularVelocities = NULL; - } } diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 2c635fb0..979a0abf 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -31,6 +31,7 @@ #include "../configuration.h" #include "../constraint/Constraint.h" #include "ContactManifold.h" +#include "Island.h" #include "Impulse.h" #include #include @@ -311,9 +312,6 @@ class ContactSolver { // -------------------- Attributes -------------------- // - /// Reference to all the contact manifold of the world - std::vector& mContactManifolds; - /// Split linear velocities for the position contact solver (split impulse) Vector3* mSplitLinearVelocities; @@ -329,14 +327,11 @@ class ContactSolver { /// Number of contact constraints uint mNbContactManifolds; - /// Constrained bodies - std::set mConstraintBodies; + /// Array of linear velocities + Vector3* mLinearVelocities; - /// Reference to the array of linear velocities - std::vector& mLinearVelocities; - - /// Reference to the array of angular velocities - std::vector& mAngularVelocities; + /// Array of angular velocities + Vector3* mAngularVelocities; /// Reference to the map of rigid body to their index in the constrained velocities array const std::map& mMapBodyToConstrainedVelocityIndex; @@ -353,9 +348,6 @@ class ContactSolver { // -------------------- Methods -------------------- // - /// Initialize the split impulse velocities - void initializeSplitImpulseVelocities(); - /// Initialize the contact constraints before solving the system void initializeContactConstraints(); @@ -403,16 +395,21 @@ class ContactSolver { // -------------------- Methods -------------------- // /// Constructor - ContactSolver(std::vector& contactManifolds, - std::vector& constrainedLinearVelocities, - std::vector& constrainedAngularVelocities, - const std::map& mapBodyToVelocityIndex); + ContactSolver(const std::map& mapBodyToVelocityIndex); /// Destructor virtual ~ContactSolver(); - /// Initialize the constraint solver - void initialize(decimal dt); + /// Initialize the constraint solver for a given island + void initializeForIsland(decimal dt, Island* island); + + /// Set the split velocities arrays + void setSplitVelocitiesArrays(Vector3* splitLinearVelocities, + Vector3* splitAngularVelocities); + + /// Set the constrained velocities arrays + void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, + Vector3* constrainedAngularVelocities); /// Warm start the solver. void warmStart(); @@ -424,24 +421,6 @@ class ContactSolver { /// Solve the contacts void solve(); - /// Return true if the body is in at least one constraint - bool isConstrainedBody(RigidBody* body) const; - - /// Return the constrained linear velocity of a body after solving the constraints - Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); - - /// Return the split linear velocity - Vector3 getSplitLinearVelocityOfBody(RigidBody* body); - - /// Return the constrained angular velocity of a body after solving the constraints - Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); - - /// Return the split angular velocity - Vector3 getSplitAngularVelocityOfBody(RigidBody* body); - - /// Clean up the constraint solver - void cleanup(); - /// Return true if the split impulses position correction technique is used for contacts bool isSplitImpulseActive() const; @@ -451,25 +430,27 @@ class ContactSolver { /// Activate or deactivate the solving of friction constraints at the center of /// 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 the body is in at least one constraint -inline bool ContactSolver::isConstrainedBody(RigidBody* body) const { - return mConstraintBodies.count(body) == 1; +// Set the split velocities arrays +inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities, + Vector3* splitAngularVelocities) { + assert(splitLinearVelocities != NULL); + assert(splitAngularVelocities != NULL); + mSplitLinearVelocities = splitLinearVelocities; + mSplitAngularVelocities = splitAngularVelocities; } -// Return the split linear velocity -inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) { - assert(isConstrainedBody(body)); - const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second; - return mSplitLinearVelocities[indexBody]; -} - -// Return the split angular velocity -inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) { - assert(isConstrainedBody(body)); - const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second; - return mSplitAngularVelocities[indexBody]; +// Set the constrained velocities arrays +inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities, + Vector3* constrainedAngularVelocities) { + assert(constrainedLinearVelocities != NULL); + assert(constrainedAngularVelocities != NULL); + mLinearVelocities = constrainedLinearVelocities; + mAngularVelocities = constrainedAngularVelocities; } // Return true if the split impulses position correction technique is used for contacts diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 605aec54..cf585e43 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -36,15 +36,19 @@ using namespace std; // Constructor DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP) - : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), - mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities, - mMapBodyToConstrainedVelocityIndex), - mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities, - mConstrainedPositions, mConstrainedOrientations, + : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true), + mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL), + mContactSolver(mMapBodyToConstrainedVelocityIndex), + mConstraintSolver(mConstrainedPositions, mConstrainedOrientations, mMapBodyToConstrainedVelocityIndex), mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS), mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), - mIsDeactivationActive(DEACTIVATION_ENABLED) { + mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL), + mSplitAngularVelocities(NULL), mNbIslands(0), mNbIslandsCapacity(0), + mIslands(NULL), mNbBodiesCapacity(0), + mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY), + mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), + mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) { } @@ -59,8 +63,26 @@ DynamicsWorld::~DynamicsWorld() { mMemoryAllocator.release((*it).second, sizeof(OverlappingPair)); } - // Free the allocated memory for the constrained velocities - cleanupConstrainedVelocitiesArray(); + // 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); + } + + // Release the memory allocated for the bodies velocity arrays + if (mNbBodiesCapacity > 0) { + delete[] mSplitLinearVelocities; + delete[] mSplitAngularVelocities; + delete[] mConstrainedLinearVelocities; + delete[] mConstrainedAngularVelocities; + } #ifdef IS_PROFILING_ACTIVE @@ -93,10 +115,16 @@ 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(); + // Compute the islands (separate groups of bodies with constraints between each others) + computeIslands(); + // Integrate the velocities integrateRigidBodiesVelocities(); @@ -115,14 +143,10 @@ void DynamicsWorld::update() { // Solve the position correction for constraints solvePositionCorrection(); + if (mIsSleepingEnabled) updateSleepingBodies(); + // Update the AABBs of the bodies updateRigidBodiesAABB(); - - // Cleanup of the contact solver - mContactSolver.cleanup(); - - // Cleanup the constrained velocities - cleanupConstrainedVelocitiesArray(); } // Compute and set the interpolation factor to all the bodies @@ -138,45 +162,47 @@ void DynamicsWorld::integrateRigidBodiesPositions() { decimal dt = static_cast(mTimer.getTimeStep()); - // For each rigid body of the world - set::iterator it; - for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { + // For each island of the world + for (uint i=0; i < mNbIslands; i++) { - RigidBody* rigidBody = *it; - assert(rigidBody != NULL); + RigidBody** bodies = mIslands[i]->getBodies(); - // If the body is allowed to move - if (rigidBody->getIsMotionEnabled()) { + // For each body of the island + for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { - // Get the constrained velocity - uint indexArray = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second; - Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; - Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; + // If the body is allowed to move + if (bodies[b]->getIsMotionEnabled()) { - // Update the linear and angular velocity of the body - rigidBody->setLinearVelocity(newLinVelocity); - rigidBody->setAngularVelocity(newAngVelocity); + // Get the constrained velocity + uint indexArray = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; + Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; + Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; - // Add the split impulse velocity from Contact Solver (only used to update the position) - if (mContactSolver.isConstrainedBody(rigidBody) && - mContactSolver.isSplitImpulseActive()) { + // Update the linear and angular velocity of the body + bodies[b]->setLinearVelocity(newLinVelocity); + bodies[b]->setAngularVelocity(newAngVelocity); - newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody); - newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody); + // Add the split impulse velocity from Contact Solver (only used + // to update the position) + if (mContactSolver.isSplitImpulseActive()) { + + newLinVelocity += mSplitLinearVelocities[indexArray]; + newAngVelocity += mSplitAngularVelocities[indexArray]; + } + + // Get current position and orientation of the body + const Vector3& currentPosition = bodies[b]->getTransform().getPosition(); + const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation(); + + // Compute the new position of the body + Vector3 newPosition = currentPosition + newLinVelocity * dt; + Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) * + currentOrientation * decimal(0.5) * dt; + + // Update the Transform of the body + Transform newTransform(newPosition, newOrientation.getUnit()); + bodies[b]->setTransform(newTransform); } - - // Get current position and orientation of the body - const Vector3& currentPosition = rigidBody->getTransform().getPosition(); - const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); - - // Compute the new position of the body - Vector3 newPosition = currentPosition + newLinVelocity * dt; - Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) * - currentOrientation * decimal(0.5) * dt; - - // Update the Transform of the body - Transform newTransform(newPosition, newOrientation.getUnit()); - rigidBody->setTransform(newTransform); } } } @@ -188,7 +214,7 @@ void DynamicsWorld::updateRigidBodiesAABB() { // For each rigid body of the world set::iterator it; - for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { + for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { // If the body has moved if ((*it)->getHasMoved()) { @@ -210,12 +236,49 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() { // Set the factor to all bodies set::iterator it; - for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { + for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - RigidBody* rigidBody = dynamic_cast(*it); - assert(rigidBody); + (*it)->setInterpolationFactor(factor); + } +} - rigidBody->setInterpolationFactor(factor); +// Initialize the bodies velocities arrays for the next simulation step. +void DynamicsWorld::initVelocityArrays() { + + // Allocate memory for the bodies velocity arrays + uint nbBodies = mRigidBodies.size(); + if (mNbBodiesCapacity != nbBodies && nbBodies > 0) { + if (mNbBodiesCapacity > 0) { + delete[] mSplitLinearVelocities; + delete[] mSplitAngularVelocities; + } + mNbBodiesCapacity = nbBodies; + mSplitLinearVelocities = new Vector3[mNbBodiesCapacity]; + mSplitAngularVelocities = new Vector3[mNbBodiesCapacity]; + mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity]; + mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity]; + assert(mSplitLinearVelocities != NULL); + assert(mSplitAngularVelocities != NULL); + assert(mConstrainedLinearVelocities != NULL); + assert(mConstrainedAngularVelocities != NULL); + } + + // Reset the velocities arrays + for (uint i=0; i::const_iterator it; + uint indexBody = 0; + for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + + // Add the body into the map + mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(*it, indexBody)); + indexBody++; } } @@ -228,66 +291,74 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()"); - // TODO : Use better memory allocation here - mConstrainedLinearVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); - mConstrainedAngularVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); + // Initialize the bodies velocity arrays + initVelocityArrays(); decimal dt = static_cast(mTimer.getTimeStep()); - // Fill in the mapping of rigid body to their index in the constrained - // velocities arrays - uint i = 0; - for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - RigidBody* rigidBody = *it; - mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(rigidBody, i)); + // For each island of the world + for (uint i=0; i < mNbIslands; i++) { - // If the body is allowed to move - if (rigidBody->getIsMotionEnabled()) { + RigidBody** bodies = mIslands[i]->getBodies(); - // Integrate the external force to get the new velocity of the body - mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + - dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); - mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + - dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + // For each body of the island + for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { - // If the gravity has to be applied to this rigid body - if (rigidBody->isGravityEnabled() && mIsGravityOn) { + // Insert the body into the map of constrained velocities + uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - // Integrate the gravity force - mConstrainedLinearVelocities[i] += dt * rigidBody->getMassInverse() * - rigidBody->getMass() * mGravity; + assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0)); + assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); + + // If the body is allowed to move + if (bodies[b]->getIsMotionEnabled()) { + + // Integrate the external force to get the new velocity of the body + mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() + + dt * bodies[b]->getMassInverse() * bodies[b]->getExternalForce(); + mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() + + dt * bodies[b]->getInertiaTensorInverseWorld() * + bodies[b]->getExternalTorque(); + + // If the gravity has to be applied to this rigid body + if (bodies[b]->isGravityEnabled() && mIsGravityEnabled) { + + // Integrate the gravity force + mConstrainedLinearVelocities[indexBody] += dt * bodies[b]->getMassInverse() * + bodies[b]->getMass() * mGravity; + } + + // Apply the velocity damping + // Damping force : F_c = -c' * v (c=damping factor) + // Equation : m * dv/dt = -c' * v + // => dv/dt = -c * v (with c=c'/m) + // => dv/dt + c * v = 0 + // Solution : v(t) = v0 * e^(-c * t) + // => v(t + dt) = v0 * e^(-c(t + dt)) + // = v0 * e^(-ct) * e^(-c * dt) + // = v(t) * e^(-c * dt) + // => v2 = v1 * e^(-c * dt) + // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... + // => e^(-x) ~ 1 - x + // => v2 = v1 * (1 - c * dt) + decimal linDampingFactor = bodies[b]->getLinearDamping(); + decimal angDampingFactor = bodies[b]->getAngularDamping(); + decimal linearDamping = clamp(decimal(1.0) - dt * linDampingFactor, + decimal(0.0), decimal(1.0)); + decimal angularDamping = clamp(decimal(1.0) - dt * angDampingFactor, + decimal(0.0), decimal(1.0)); + mConstrainedLinearVelocities[indexBody] *= clamp(linearDamping, decimal(0.0), + decimal(1.0)); + mConstrainedAngularVelocities[indexBody] *= clamp(angularDamping, decimal(0.0), + decimal(1.0)); + + // Update the old Transform of the body + bodies[b]->updateOldTransform(); } - // Apply the velocity damping - // Damping force : F_c = -c' * v (c=damping factor) - // Equation : m * dv/dt = -c' * v - // => dv/dt = -c * v (with c=c'/m) - // => dv/dt + c * v = 0 - // Solution : v(t) = v0 * e^(-c * t) - // => v(t + dt) = v0 * e^(-c(t + dt)) - // = v0 * e^(-ct) * e^(-c * dt) - // = v(t) * e^(-c * dt) - // => v2 = v1 * e^(-c * dt) - // Using Taylor Serie for e^(-x) : e^x ~ 1 + x + x^2/2! + ... - // => e^(-x) ~ 1 - x - // => v2 = v1 * (1 - c * dt) - decimal linDampingFactor = rigidBody->getLinearDamping(); - decimal angDampingFactor = rigidBody->getAngularDamping(); - decimal linearDamping = clamp(decimal(1.0) - dt * linDampingFactor, - decimal(0.0), decimal(1.0)); - decimal angularDamping = clamp(decimal(1.0) - dt * angDampingFactor, - decimal(0.0), decimal(1.0)); - mConstrainedLinearVelocities[i] *= clamp(linearDamping, decimal(0.0), decimal(1.0)); - mConstrainedAngularVelocities[i] *= clamp(angularDamping, decimal(0.0), decimal(1.0)); - - // Update the old Transform of the body - rigidBody->updateOldTransform(); + indexBody++; } - - i++; } - - assert(mMapBodyToConstrainedVelocityIndex.size() == mRigidBodies.size()); } // Solve the contacts and constraints @@ -298,42 +369,59 @@ void DynamicsWorld::solveContactsAndConstraints() { // Get the current time step decimal dt = static_cast(mTimer.getTimeStep()); - // Check if there are contacts and constraints to solve - bool isConstraintsToSolve = !mJoints.empty(); - bool isContactsToSolve = !mContactManifolds.empty(); - if (!isConstraintsToSolve && !isContactsToSolve) return; + // Set the velocities arrays + mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities); + mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, + mConstrainedAngularVelocities); + mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities, + mConstrainedAngularVelocities); // ---------- Solve velocity constraints for joints and contacts ---------- // - // If there are contacts - if (isContactsToSolve) { + // For each island of the world + for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { - // Initialize the solver - mContactSolver.initialize(dt); + // 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; - // Warm start the contact solver - mContactSolver.warmStart(); + // If there are contacts in the current island + if (isContactsToSolve) { + + // Initialize the solver + mContactSolver.initializeForIsland(dt, mIslands[islandIndex]); + + // Warm start the contact solver + mContactSolver.warmStart(); + } + + // If there are constraints + if (isConstraintsToSolve) { + + // Initialize the constraint solver + mConstraintSolver.initializeForIsland(dt, mIslands[islandIndex]); + } + + // For each iteration of the velocity solver + for (uint i=0; i(mRigidBodies.size()); mConstrainedOrientations = std::vector(mRigidBodies.size()); - for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { - // If it is a constrained bodies (by a joint) - if (mConstraintSolver.isConstrainedBody(*it)) { + // For each island of the world + for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) { - uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second; + // For each body of the island + RigidBody** bodies = mIslands[islandIndex]->getBodies(); + for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) { + + uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; // Get the position/orientation of the rigid body - const Transform& transform = (*it)->getTransform(); + const Transform& transform = bodies[b]->getTransform(); mConstrainedPositions[index] = transform.getPosition(); mConstrainedOrientations[index]= transform.getOrientation(); } - } - // ---------- Solve the position error correction for the constraints ---------- // + // ---------- Solve the position error correction for the constraints ---------- // - // For each iteration of the position (error correction) solver - for (uint i=0; i::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) { - // If it is a constrained bodies (by a joint) - if (mConstraintSolver.isConstrainedBody(*it)) { - - uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second; + uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; // Get the new position/orientation of the body const Vector3& newPosition = mConstrainedPositions[index]; @@ -387,22 +474,11 @@ void DynamicsWorld::solvePositionCorrection() { // Update the Transform of the body Transform newTransform(newPosition, newOrientation.getUnit()); - (*it)->setTransform(newTransform); + bodies[b]->setTransform(newTransform); } } } -// Cleanup the constrained velocities array at each step -void DynamicsWorld::cleanupConstrainedVelocitiesArray() { - - // Clear the constrained velocites - mConstrainedLinearVelocities.clear(); - mConstrainedAngularVelocities.clear(); - - // Clear the rigid body to velocities array index mapping - mMapBodyToConstrainedVelocityIndex.clear(); -} - // Create a rigid body into the physics world RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass, const Matrix3x3& inertiaTensorLocal, @@ -448,7 +524,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 +533,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 +609,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; } @@ -545,10 +628,17 @@ void DynamicsWorld::destroyJoint(Constraint* joint) { mCollisionDetection.removeNoCollisionPair(joint->getBody1(), joint->getBody2()); } + // Wake up the two bodies of the joint + joint->getBody1()->setIsSleeping(false); + joint->getBody2()->setIsSleeping(false); + // Remove the joint from the world mJoints.erase(joint); - // Get the size in bytes of the 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); + size_t nbBytes = joint->getSizeInBytes(); // Call the destructor of the joint @@ -558,6 +648,281 @@ void DynamicsWorld::destroyJoint(Constraint* joint) { mMemoryAllocator.release(joint, nbBytes); } +// 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); +} + +// Put bodies to sleep if needed. +/// For each island, if all the bodies have been almost still for a long enough period of +/// time, we put all the bodies of the island to sleep. +void DynamicsWorld::updateSleepingBodies() { + + PROFILE("DynamicsWorld::updateSleepingBodies()"); + + const decimal dt = static_cast(mTimer.getTimeStep()); + const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity; + const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity; + + // For each island of the world + for (uint i=0; igetBodies(); + for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { + + // Skip static bodies + if (!bodies[b]->getIsMotionEnabled()) continue; + + // If the body is velocity is large enough to stay awake + if (bodies[b]->getLinearVelocity().lengthSquare() > sleepLinearVelocitySquare || + bodies[b]->getAngularVelocity().lengthSquare() > sleepAngularVelocitySquare || + !bodies[b]->isAllowedToSleep()) { + + // Reset the sleep time of the body + bodies[b]->mSleepTime = decimal(0.0); + minSleepTime = decimal(0.0); + } + else { // If the body velocity is bellow the sleeping velocity threshold + + // Increase the sleep time + bodies[b]->mSleepTime += dt; + if (bodies[b]->mSleepTime < minSleepTime) { + minSleepTime = bodies[b]->mSleepTime; + } + } + } + + // If the velocity of all the bodies of the island is under the + // sleeping velocity threshold for a period of time larger than + // the time required to become a sleeping body + if (minSleepTime >= mTimeBeforeSleep) { + + // Put all the bodies of the island to sleep + for (uint b=0; b < mIslands[i]->getNbBodies(); b++) { + bodies[b]->setIsSleeping(true); + } + } + } +} + // Notify the world about a new broad-phase overlapping pair void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) { @@ -565,8 +930,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 +969,25 @@ 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); +} + +// Enable/Disable the sleeping technique +void DynamicsWorld::enableSleeping(bool isSleepingEnabled) { + mIsSleepingEnabled = isSleepingEnabled; + + if (!mIsSleepingEnabled) { + + // For each body of the world + std::set::iterator it; + for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + + // Wake up the rigid body + (*it)->setIsSleeping(false); + } + } } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index a1a1f821..7f783ffd 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 @@ -81,15 +83,21 @@ class DynamicsWorld : public CollisionWorld { Vector3 mGravity; /// True if the gravity force is on - bool mIsGravityOn; + bool mIsGravityEnabled; /// Array of constrained linear velocities (state of the linear velocities /// after solving the constraints) - std::vector mConstrainedLinearVelocities; + Vector3* mConstrainedLinearVelocities; /// Array of constrained angular velocities (state of the angular velocities /// after solving the constraints) - std::vector mConstrainedAngularVelocities; + Vector3* mConstrainedAngularVelocities; + + /// Split linear velocities for the position contact solver (split impulse) + Vector3* mSplitLinearVelocities; + + /// Split angular velocities for the position contact solver (split impulse) + Vector3* mSplitAngularVelocities; /// Array of constrained rigid bodies position (for position error correction) std::vector mConstrainedPositions; @@ -100,6 +108,28 @@ 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; + + /// Current allocated capacity for the bodies + uint mNbBodiesCapacity; + + /// Sleep linear velocity threshold + decimal mSleepLinearVelocity; + + /// Sleep angular velocity threshold + decimal mSleepAngularVelocity; + + /// Time (in seconds) before a body is put to sleep if its velocity + /// becomes smaller than the sleep velocity. + decimal mTimeBeforeSleep; + // -------------------- Methods -------------------- // /// Private copy-constructor @@ -121,6 +151,9 @@ class DynamicsWorld : public CollisionWorld { /// Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); + /// Initialize the bodies velocities arrays for the next simulation step. + void initVelocityArrays(); + /// Integrate the velocities of rigid bodies. void integrateRigidBodiesVelocities(); @@ -136,6 +169,12 @@ class DynamicsWorld : public CollisionWorld { /// Reset the boolean movement variable of each body void resetBodiesMovementVariable(); + /// Compute the islands of awake bodies. + void computeIslands(); + + /// Put bodies to sleep if needed. + void updateSleepingBodies(); + /// Update the overlapping pair virtual void updateOverlappingPair(const BroadPhasePair* pair); @@ -198,14 +237,25 @@ 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; /// Return if the gravity is on - bool getIsGravityOn() const; + bool isGravityEnabled() const; - /// Set the isGravityOn attribute - void setIsGratityOn(bool isGravityOn); + /// Enable/Disable the gravity + void setIsGratityEnabled(bool isGravityEnabled); /// Return the number of rigid bodies in the world uint getNbRigidBodies() const; @@ -227,6 +277,36 @@ public : /// Return a reference to the contact manifolds of the world const std::vector& getContactManifolds() const; + + /// Return true if the sleeping technique is enabled + bool isSleepingEnabled() const; + + /// Enable/Disable the sleeping technique + void enableSleeping(bool isSleepingEnabled); + + /// Return the current sleep linear velocity + decimal getSleepLinearVelocity() const; + + /// Set the sleep linear velocity. + void setSleepLinearVelocity(decimal sleepLinearVelocity); + + /// Return the current sleep angular velocity + decimal getSleepAngularVelocity() const; + + /// Set the sleep angular velocity. + void setSleepAngularVelocity(decimal sleepAngularVelocity); + + /// Return the time a body is required to stay still before sleeping + decimal getTimeBeforeSleep() const; + + /// Set the time a body is required to stay still before sleeping + void setTimeBeforeSleep(decimal timeBeforeSleep); + + // TODO : REMOVE THIS + Island** getIslands() { return mIslands;} + + // TODO : REMOVE THIS + uint getNbIslands() const {return mNbIslands;} }; // Start the physics simulation @@ -306,14 +386,14 @@ inline Vector3 DynamicsWorld::getGravity() const { return mGravity; } -// Return if the gravity is on -inline bool DynamicsWorld::getIsGravityOn() const { - return mIsGravityOn; +// Return if the gravity is enaled +inline bool DynamicsWorld::isGravityEnabled() const { + return mIsGravityEnabled; } -// Set the isGravityOn attribute -inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) { - mIsGravityOn = isGravityOn; +// Enable/Disable the gravity +inline void DynamicsWorld::setIsGratityEnabled(bool isGravityEnabled) { + mIsGravityEnabled = isGravityEnabled; } // Return the number of rigid bodies in the world @@ -351,6 +431,51 @@ inline long double DynamicsWorld::getPhysicsTime() const { return mTimer.getPhysicsTime(); } +// Return true if the sleeping technique is enabled +inline bool DynamicsWorld::isSleepingEnabled() const { + return mIsSleepingEnabled; +} + +// Return the current sleep linear velocity +inline decimal DynamicsWorld::getSleepLinearVelocity() const { + return mSleepLinearVelocity; +} + +// Set the sleep linear velocity. +/// When the velocity of a body becomes smaller than the sleep linear/angular +/// velocity for a given amount of time, the body starts sleeping and does not need +/// to be simulated anymore. +inline void DynamicsWorld::setSleepLinearVelocity(decimal sleepLinearVelocity) { + assert(sleepLinearVelocity >= decimal(0.0)); + mSleepLinearVelocity = sleepLinearVelocity; +} + +// Return the current sleep angular velocity +inline decimal DynamicsWorld::getSleepAngularVelocity() const { + return mSleepAngularVelocity; +} + +// Set the sleep angular velocity. +/// When the velocity of a body becomes smaller than the sleep linear/angular +/// velocity for a given amount of time, the body starts sleeping and does not need +/// to be simulated anymore. +inline void DynamicsWorld::setSleepAngularVelocity(decimal sleepAngularVelocity) { + assert(sleepAngularVelocity >= decimal(0.0)); + mSleepAngularVelocity = sleepAngularVelocity; +} + +// Return the time a body is required to stay still before sleeping +inline decimal DynamicsWorld::getTimeBeforeSleep() const { + return mTimeBeforeSleep; +} + + +// Set the time a body is required to stay still before sleeping +inline void DynamicsWorld::setTimeBeforeSleep(decimal timeBeforeSleep) { + assert(timeBeforeSleep >= decimal(0.0)); + mTimeBeforeSleep = timeBeforeSleep; +} + } #endif 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