/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "DynamicsWorld.h"
#include "constraint/BallAndSocketJoint.h"
#include "constraint/SliderJoint.h"
#include "constraint/HingeJoint.h"
#include "constraint/FixedJoint.h"
// Namespaces
using namespace reactphysics3d;
using namespace std;
// Constructor
/**
* @param gravity Gravity vector in the world (in meters per second squared)
* @param timeStep Time step for an internal physics tick (in seconds)
*/
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
: CollisionWorld(), mTimer(timeStep),
mContactSolver(mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsSleepingEnabled(SPLEEPING_ENABLED), mGravity(gravity),
mIsGravityEnabled(true), mConstrainedLinearVelocities(NULL),
mConstrainedAngularVelocities(NULL), mSplitLinearVelocities(NULL),
mSplitAngularVelocities(NULL), mConstrainedPositions(NULL),
mConstrainedOrientations(NULL), mNbIslands(0),
mNbIslandsCapacity(0), mIslands(NULL), mNbBodiesCapacity(0),
mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY),
mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) {
}
// Destructor
DynamicsWorld::~DynamicsWorld() {
// Release the memory allocated for the islands
for (uint i=0; i<mNbIslands; i++) {
// Call the island destructor
mIslands[i]->~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;
delete[] mConstrainedPositions;
delete[] mConstrainedOrientations;
}
#ifdef IS_PROFILING_ACTIVE
// Print the profiling report
Profiler::printReport(std::cout);
// Destroy the profiler (release the allocated memory)
Profiler::destroy();
#endif
}
// Update the physics simulation
void DynamicsWorld::update() {
#ifdef IS_PROFILING_ACTIVE
// Increment the frame counter of the profiler
Profiler::incrementFrameCounter();
#endif
PROFILE("DynamicsWorld::update()");
assert(mTimer.getIsRunning());
// Compute the time since the last update() call and update the timer
mTimer.update();
// While the time accumulator is not empty
while(mTimer.isPossibleToTakeStep()) {
// Notify the event listener about the beginning of an internal tick
if (mEventListener != NULL) mEventListener->beginInternalTick();
// 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();
// Update the timer
mTimer.nextStep();
// Solve the contacts and constraints
solveContactsAndConstraints();
// Integrate the position and orientation of each body
integrateRigidBodiesPositions();
// Solve the position correction for constraints
solvePositionCorrection();
// Update the state (positions and velocities) of the bodies
updateBodiesState();
if (mIsSleepingEnabled) updateSleepingBodies();
// Notify the event listener about the end of an internal tick
if (mEventListener != NULL) mEventListener->endInternalTick();
}
// Reset the external force and torque applied to the bodies
resetBodiesForceAndTorque();
// Compute and set the interpolation factor to all the bodies
setInterpolationFactorToAllBodies();
}
// Integrate position and orientation of the rigid bodies.
/// The positions and orientations of the bodies are integrated using
/// the sympletic Euler time stepping scheme.
void DynamicsWorld::integrateRigidBodiesPositions() {
PROFILE("DynamicsWorld::integrateRigidBodiesPositions()");
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
// For each island of the world
for (uint i=0; i < mNbIslands; i++) {
RigidBody** bodies = mIslands[i]->getBodies();
// For each body of the island
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
// 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()) {
newLinVelocity += mSplitLinearVelocities[indexArray];
newAngVelocity += mSplitAngularVelocities[indexArray];
}
// Get current position and orientation of the body
const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld;
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
// Update the new constrained position and orientation of the body
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * dt;
mConstrainedOrientations[indexArray] = currentOrientation +
Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * dt;
}
}
}
// Update the postion/orientation of the bodies
void DynamicsWorld::updateBodiesState() {
PROFILE("DynamicsWorld::updateBodiesState()");
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// 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;
// Update the linear and angular velocity of the body
bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index];
// Update the position of the center of mass of the body
bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
// Update the orientation of the body
bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit());
// Update the transform of the body (using the new center of mass and new orientation)
bodies[b]->updateTransformWithCenterOfMass();
// Update the broad-phase state of the body
bodies[b]->updateBroadPhaseState();
}
}
}
// Compute and set the interpolation factor to all bodies
void DynamicsWorld::setInterpolationFactorToAllBodies() {
PROFILE("DynamicsWorld::setInterpolationFactorToAllBodies()");
// Compute the interpolation factor
decimal factor = mTimer.computeInterpolationFactor();
assert(factor >= 0.0 && factor <= 1.0);
// Set the factor to all bodies
set<RigidBody*>::iterator it;
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
(*it)->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;
// TODO : Use better memory allocation here
mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedPositions = new Vector3[mNbBodiesCapacity];
mConstrainedOrientations = new Quaternion[mNbBodiesCapacity];
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);
assert(mConstrainedLinearVelocities != NULL);
assert(mConstrainedAngularVelocities != NULL);
assert(mConstrainedPositions != NULL);
assert(mConstrainedOrientations != NULL);
}
// Reset the velocities arrays
for (uint i=0; i<mNbBodiesCapacity; i++) {
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(*it, indexBody));
indexBody++;
}
}
// Integrate the velocities of rigid bodies.
/// This method only set the temporary velocities but does not update
/// the actual velocitiy of the bodies. The velocities updated in this method
/// might violate the constraints and will be corrected in the constraint and
/// contact solver.
void DynamicsWorld::integrateRigidBodiesVelocities() {
PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
// Initialize the bodies velocity arrays
initVelocityArrays();
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
// For each island of the world
for (uint i=0; i < mNbIslands; i++) {
RigidBody** bodies = mIslands[i]->getBodies();
// For each body of the island
for (uint b=0; b < mIslands[i]->getNbBodies(); b++) {
// Insert the body into the map of constrained velocities
uint indexBody = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
assert(mSplitLinearVelocities[indexBody] == Vector3(0, 0, 0));
assert(mSplitAngularVelocities[indexBody] == Vector3(0, 0, 0));
// Integrate the external force to get the new velocity of the body
mConstrainedLinearVelocities[indexBody] = bodies[b]->getLinearVelocity() +
dt * bodies[b]->mMassInverse * bodies[b]->mExternalForce;
mConstrainedAngularVelocities[indexBody] = bodies[b]->getAngularVelocity() +
dt * bodies[b]->getInertiaTensorInverseWorld() *
bodies[b]->mExternalTorque;
// 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]->mMassInverse *
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();
indexBody++;
}
}
}
// Solve the contacts and constraints
void DynamicsWorld::solveContactsAndConstraints() {
PROFILE("DynamicsWorld::solveContactsAndConstraints()");
// Get the current time step
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
// Set the velocities arrays
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
mConstrainedAngularVelocities);
mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
mConstrainedAngularVelocities);
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
mConstrainedOrientations);
// ---------- Solve velocity constraints for joints and contacts ---------- //
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
if (!isConstraintsToSolve && !isContactsToSolve) continue;
// 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<mNbVelocitySolverIterations; i++) {
// Solve the constraints
if (isConstraintsToSolve) {
mConstraintSolver.solveVelocityConstraints(mIslands[islandIndex]);
}
// Solve the contacts
if (isContactsToSolve) mContactSolver.solve();
}
// Cache the lambda values in order to use them in the next
// step and cleanup the contact solver
if (isContactsToSolve) {
mContactSolver.storeImpulses();
mContactSolver.cleanup();
}
}
}
// Solve the position error correction of the constraints
void DynamicsWorld::solvePositionCorrection() {
PROFILE("DynamicsWorld::solvePositionCorrection()");
// Do not continue if there is no constraints
if (mJoints.empty()) return;
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// ---------- Solve the position error correction for the constraints ---------- //
// For each iteration of the position (error correction) solver
for (uint i=0; i<mNbPositionSolverIterations; i++) {
// Solve the position constraints
mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
}
}
}
// Create a rigid body into the physics world
/**
* @param transform Transformation from body local-space to world-space
* @return A pointer to the body that has been created in the world
*/
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
// Compute the body ID
bodyindex bodyID = computeNextAvailableBodyID();
// Largest index cannot be used (it is used for invalid index)
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
// Create the rigid body
RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
*this, bodyID);
assert(rigidBody != NULL);
// Add the rigid body to the physics world
mBodies.insert(rigidBody);
mRigidBodies.insert(rigidBody);
// Return the pointer to the rigid body
return rigidBody;
}
// Destroy a rigid body and all the joints which it belongs
/**
* @param rigidBody Pointer to the body you want to destroy
*/
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
// Remove all the collision shapes of the body
rigidBody->removeAllCollisionShapes();
// Add the body ID to the list of free IDs
mFreeBodiesIDs.push_back(rigidBody->getID());
// Destroy all the joints in which the rigid body to be destroyed is involved
JointListElement* element;
for (element = rigidBody->mJointsList; element != NULL; element = element->next) {
destroyJoint(element->joint);
}
// Reset the contact manifold list of the body
rigidBody->resetContactManifoldsList();
// Call the destructor of the rigid body
rigidBody->~RigidBody();
// Remove the rigid body from the list of rigid bodies
mBodies.erase(rigidBody);
mRigidBodies.erase(rigidBody);
// Free the object from the memory allocator
mMemoryAllocator.release(rigidBody, sizeof(RigidBody));
}
// Create a joint between two bodies in the world and return a pointer to the new joint
/**
* @param jointInfo The information that is necessary to create the joint
* @return A pointer to the joint that has been created in the world
*/
Joint* DynamicsWorld::createJoint(const JointInfo& jointInfo) {
Joint* newJoint = NULL;
// Allocate memory to create the new joint
switch(jointInfo.type) {
// Ball-and-Socket joint
case BALLSOCKETJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(BallAndSocketJoint));
const BallAndSocketJointInfo& info = dynamic_cast<const BallAndSocketJointInfo&>(
jointInfo);
newJoint = new (allocatedMemory) BallAndSocketJoint(info);
break;
}
// Slider joint
case SLIDERJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(SliderJoint));
const SliderJointInfo& info = dynamic_cast<const SliderJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) SliderJoint(info);
break;
}
// Hinge joint
case HINGEJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(HingeJoint));
const HingeJointInfo& info = dynamic_cast<const HingeJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) HingeJoint(info);
break;
}
// Fixed joint
case FIXEDJOINT:
{
void* allocatedMemory = mMemoryAllocator.allocate(sizeof(FixedJoint));
const FixedJointInfo& info = dynamic_cast<const FixedJointInfo&>(jointInfo);
newJoint = new (allocatedMemory) FixedJoint(info);
break;
}
default:
{
assert(false);
return NULL;
}
}
// If the collision between the two bodies of the constraint is disabled
if (!jointInfo.isCollisionEnabled) {
// Add the pair of bodies in the set of body pairs that cannot collide with each other
mCollisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2);
}
// 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;
}
// Destroy a joint
/**
* @param joint Pointer to the joint you want to destroy
*/
void DynamicsWorld::destroyJoint(Joint* joint) {
assert(joint != NULL);
// If the collision between the two bodies of the constraint was disabled
if (!joint->isCollisionEnabled()) {
// Remove the pair of bodies from the set of body pairs that cannot collide with each other
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);
// 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
joint->~Joint();
// Release the allocated memory
mMemoryAllocator.release(joint, nbBytes);
}
// Add the joint to the list of joints of the two bodies involved in the joint
void DynamicsWorld::addJointToBody(Joint* 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;
}
// 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; i<mNbIslands; i++) {
// Call the island destructor
mIslands[i]->~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;
int nbContactManifolds = 0;
// Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
nbContactManifolds += nbBodyManifolds;
}
for (std::set<Joint*>::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);
// For each rigid body of the world
for (std::set<RigidBody*>::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->mIsAlreadyInIsland) continue;
// If the body is static, we go to the next body
if (body->getType() == STATIC) continue;
// If the body is sleeping or inactive, we go to the next body
if (body->isSleeping() || !body->isActive()) 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(nbBodies,
nbContactManifolds,
mJoints.size(), mMemoryAllocator);
// 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];
assert(bodyToVisit->isActive());
// 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 static, we do not want to perform the DFS
// search across that body
if (bodyToVisit->getType() == STATIC) 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<RigidBody*>(contactManifold->getBody1());
RigidBody* body2 = dynamic_cast<RigidBody*>(contactManifold->getBody2());
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// Check if the other body has already been added to the island
if (otherBody->mIsAlreadyInIsland) 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) {
Joint* 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<RigidBody*>(joint->getBody1());
RigidBody* body2 = dynamic_cast<RigidBody*>(joint->getBody2());
RigidBody* otherBody = (body1->getID() == bodyToVisit->getID()) ? body2 : body1;
// Check if the other body has already been added to the island
if (otherBody->mIsAlreadyInIsland) 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]->getType() == STATIC) {
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<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]->getType() == STATIC) 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);
}
}
}
}
// Enable/Disable the sleeping technique.
/// The sleeping technique is used to put bodies that are not moving into sleep
/// to speed up the simulation.
/**
* @param isSleepingEnabled True if you want to enable the sleeping technique
* and false otherwise
*/
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);
}
}
}
// Test and report collisions between a given shape and all the others
// shapes of the world.
/// This method should be called after calling the
/// DynamicsWorld::update() method that will compute the collisions.
/**
* @param shape Pointer to the proxy shape to test
* @param callback Pointer to the object with the callback method
*/
void DynamicsWorld::testCollision(const ProxyShape* shape,
CollisionCallback* callback) {
// Create the sets of shapes
std::set<uint> shapes;
shapes.insert(shape->mBroadPhaseID);
std::set<uint> emptySet;
// Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
}
// Test and report collisions between two given shapes.
/// This method should be called after calling the
/// DynamicsWorld::update() method that will compute the collisions.
/**
* @param shape1 Pointer to the first proxy shape to test
* @param shape2 Pointer to the second proxy shape to test
* @param callback Pointer to the object with the callback method
*/
void DynamicsWorld::testCollision(const ProxyShape* shape1,
const ProxyShape* shape2,
CollisionCallback* callback) {
// Create the sets of shapes
std::set<uint> shapes1;
shapes1.insert(shape1->mBroadPhaseID);
std::set<uint> shapes2;
shapes2.insert(shape2->mBroadPhaseID);
// Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
}
// Test and report collisions between a body and all the others bodies of the
// world.
/// This method should be called after calling the
/// DynamicsWorld::update() method that will compute the collisions.
/**
* @param body Pointer to the first body to test
* @param callback Pointer to the object with the callback method
*/
void DynamicsWorld::testCollision(const CollisionBody* body,
CollisionCallback* callback) {
// Create the sets of shapes
std::set<uint> shapes1;
// For each shape of the body
for (const ProxyShape* shape=body->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) {
shapes1.insert(shape->mBroadPhaseID);
}
std::set<uint> emptySet;
// Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
}
// Test and report collisions between two bodies.
/// This method should be called after calling the
/// DynamicsWorld::update() method that will compute the collisions.
/**
* @param body1 Pointer to the first body to test
* @param body2 Pointer to the second body to test
* @param callback Pointer to the object with the callback method
*/
void DynamicsWorld::testCollision(const CollisionBody* body1,
const CollisionBody* body2,
CollisionCallback* callback) {
// Create the sets of shapes
std::set<uint> shapes1;
for (const ProxyShape* shape=body1->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) {
shapes1.insert(shape->mBroadPhaseID);
}
std::set<uint> shapes2;
for (const ProxyShape* shape=body2->getProxyShapesList(); shape != NULL;
shape = shape->getNext()) {
shapes2.insert(shape->mBroadPhaseID);
}
// Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
}
// Test and report collisions between all shapes of the world.
/// This method should be called after calling the
/// DynamicsWorld::update() method that will compute the collisions.
/**
* @param callback Pointer to the object with the callback method
*/
void DynamicsWorld::testCollision(CollisionCallback* callback) {
std::set<uint> emptySet;
// Perform the collision detection and report contacts
mCollisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
}