diff --git a/src/body/Body.cpp b/src/body/Body.cpp index 243b0c3f..5cdff7d2 100644 --- a/src/body/Body.cpp +++ b/src/body/Body.cpp @@ -32,7 +32,8 @@ using namespace reactphysics3d; // Constructor Body::Body(bodyindex id) - : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsSleeping(false) { + : mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsSleeping(false), + mSleepTime(0) { } diff --git a/src/body/Body.h b/src/body/Body.h index f0358570..299de91f 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -56,6 +56,9 @@ class Body { /// 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 @@ -93,7 +96,7 @@ class Body { bool isSleeping() const; /// Set the variable to know whether or not the body is sleeping - void setIsSleeping(bool isSleeping); + virtual void setIsSleeping(bool isSleeping); /// Smaller than operator bool operator<(const Body& body2) const; @@ -135,6 +138,8 @@ inline bool Body::isAllowedToSleep() const { // 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 @@ -144,6 +149,16 @@ inline bool Body::isSleeping() const { // 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; } diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 2856bc64..1d38d610 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -194,6 +194,9 @@ class RigidBody : public CollisionBody { /// 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; @@ -351,6 +354,19 @@ 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 3cb8c3f0..7141ab18 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -92,7 +92,7 @@ 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 spleeping technique for inactive bodies is enabled +/// True if the spleeping technique is enabled const bool SPLEEPING_ENABLED = true; /// Object margin for collision detection in meters (for the GJK-EPA Algorithm) @@ -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/HingeJoint.cpp b/src/constraint/HingeJoint.cpp index 30b929b7..b3eb9f9b 100644 --- a/src/constraint/HingeJoint.cpp +++ b/src/constraint/HingeJoint.cpp @@ -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/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 511f4630..cf585e43 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -36,7 +36,7 @@ using namespace std; // Constructor DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP) - : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), + : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true), mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL), mContactSolver(mMapBodyToConstrainedVelocityIndex), mConstraintSolver(mConstrainedPositions, mConstrainedOrientations, @@ -45,7 +45,10 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_ mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS), mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mNbIslands(0), mNbIslandsCapacity(0), - mIslands(NULL), mNbBodiesCapacity(0) { + mIslands(NULL), mNbBodiesCapacity(0), + mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY), + mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY), + mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) { } @@ -119,6 +122,9 @@ void DynamicsWorld::update() { // Compute the collision detection mCollisionDetection.computeCollisionDetection(); + // Compute the islands (separate groups of bodies with constraints between each others) + computeIslands(); + // Integrate the velocities integrateRigidBodiesVelocities(); @@ -128,9 +134,6 @@ void DynamicsWorld::update() { // Update the timer mTimer.nextStep(); - // Compute the islands (separate groups of bodies with constraints between each others) - computeIslands(); - // Solve the contacts and constraints solveContactsAndConstraints(); @@ -140,6 +143,8 @@ void DynamicsWorld::update() { // Solve the position correction for constraints solvePositionCorrection(); + if (mIsSleepingEnabled) updateSleepingBodies(); + // Update the AABBs of the bodies updateRigidBodiesAABB(); } @@ -157,44 +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.isSplitImpulseActive()) { + // Update the linear and angular velocity of the body + bodies[b]->setLinearVelocity(newLinVelocity); + bodies[b]->setAngularVelocity(newAngVelocity); - newLinVelocity += mSplitLinearVelocities[indexArray]; - newAngVelocity += mSplitAngularVelocities[indexArray]; + // 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); } } } @@ -206,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()) { @@ -228,12 +236,9 @@ 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); - - rigidBody->setInterpolationFactor(factor); + (*it)->setInterpolationFactor(factor); } } @@ -263,6 +268,18 @@ void DynamicsWorld::initVelocityArrays() { mSplitLinearVelocities[i].setToZero(); mSplitAngularVelocities[i].setToZero(); } + + // Initialize the map of body indexes in the velocity arrays + mMapBodyToConstrainedVelocityIndex.clear(); + std::set::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++; + } } // Integrate the velocities of rigid bodies. @@ -279,64 +296,69 @@ void DynamicsWorld::integrateRigidBodiesVelocities() { decimal dt = static_cast(mTimer.getTimeStep()); - // Fill in the mapping of rigid body to their index in the constrained - // velocities arrays - uint i = 0; - mMapBodyToConstrainedVelocityIndex.clear(); - 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++) { - assert(mSplitLinearVelocities[i] == Vector3(0, 0, 0)); - assert(mSplitAngularVelocities[i] == Vector3(0, 0, 0)); + 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++) { - // 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(); + // Insert the body into the map of constrained velocities + uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second; - // If the gravity has to be applied to this rigid body - if (rigidBody->isGravityEnabled() && mIsGravityOn) { + assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0)); + assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0)); - // Integrate the gravity force - mConstrainedLinearVelocities[i] += dt * rigidBody->getMassInverse() * - rigidBody->getMass() * mGravity; + // 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 @@ -606,6 +628,10 @@ 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); @@ -842,6 +868,61 @@ void DynamicsWorld::computeIslands() { 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) { @@ -894,3 +975,19 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, 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 b1bc41ca..7f783ffd 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -83,7 +83,7 @@ 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) @@ -120,6 +120,16 @@ class DynamicsWorld : public CollisionWorld { /// 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 @@ -162,6 +172,9 @@ class DynamicsWorld : public CollisionWorld { /// 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); @@ -239,10 +252,10 @@ public : 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; @@ -265,6 +278,30 @@ 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;} @@ -349,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 @@ -394,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