Continue to implement the sleeping technique
This commit is contained in:
Daniel Chappuis
parent
475ec5be5f
commit
8db7823433
|
|
@ -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) {
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<decimal>(mTimer.getTimeStep());
|
||||
|
||||
// For each rigid body of the world
|
||||
set<RigidBody*>::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<RigidBody*>::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<RigidBody*>::iterator it;
|
||||
for (it = getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
||||
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
|
||||
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*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<RigidBody*>::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<RigidBody*,
|
||||
uint>(*it, indexBody));
|
||||
indexBody++;
|
||||
}
|
||||
}
|
||||
|
||||
// Integrate the velocities of rigid bodies.
|
||||
|
|
@ -279,64 +296,69 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
|
|||
|
||||
decimal dt = static_cast<decimal>(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<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
RigidBody* rigidBody = *it;
|
||||
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(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<decimal>(mTimer.getTimeStep());
|
||||
const decimal sleepLinearVelocitySquare = mSleepLinearVelocity * mSleepLinearVelocity;
|
||||
const decimal sleepAngularVelocitySquare = mSleepAngularVelocity * mSleepAngularVelocity;
|
||||
|
||||
// For each island of the world
|
||||
for (uint i=0; i<mNbIslands; i++) {
|
||||
|
||||
decimal minSleepTime = DECIMAL_LARGEST;
|
||||
|
||||
// For each body of the island
|
||||
RigidBody** bodies = mIslands[i]->getBodies();
|
||||
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<RigidBody*>::iterator it;
|
||||
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||
|
||||
// Wake up the rigid body
|
||||
(*it)->setIsSleeping(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<ContactManifold*>& 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
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user