333 lines
13 KiB
C++
333 lines
13 KiB
C++
/********************************************************************************
|
|
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
|
|
* Copyright (c) 2010-2012 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"
|
|
|
|
// Namespaces
|
|
using namespace reactphysics3d;
|
|
using namespace std;
|
|
|
|
// Constructor
|
|
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
|
|
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
|
|
mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
|
|
mMapBodyToConstrainedVelocityIndex),
|
|
mIsDeactivationActive(DEACTIVATION_ENABLED) {
|
|
|
|
}
|
|
|
|
// Destructor
|
|
DynamicsWorld::~DynamicsWorld() {
|
|
|
|
// Delete the remaining overlapping pairs
|
|
for (map<std::pair<bodyindex, bodyindex>, OverlappingPair*>::iterator it=mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) {
|
|
// Delete the overlapping pair
|
|
(*it).second->OverlappingPair::~OverlappingPair();
|
|
mMemoryPoolOverlappingPairs.freeObject((*it).second);
|
|
}
|
|
|
|
// Free the allocated memory for the constrained velocities
|
|
cleanupConstrainedVelocitiesArray();
|
|
}
|
|
|
|
// Update the physics simulation
|
|
void DynamicsWorld::update() {
|
|
|
|
assert(mTimer.getIsRunning());
|
|
|
|
// Compute the time since the last update() call and update the timer
|
|
mTimer.update();
|
|
|
|
// Apply the gravity force to all bodies
|
|
applyGravity();
|
|
|
|
// While the time accumulator is not empty
|
|
while(mTimer.isPossibleToTakeStep()) {
|
|
|
|
// Remove all contact manifolds
|
|
mContactManifolds.clear();
|
|
|
|
// Compute the collision detection
|
|
mCollisionDetection.computeCollisionDetection();
|
|
|
|
// Initialize the constrained velocities
|
|
initConstrainedVelocitiesArray();
|
|
|
|
// If there are contacts
|
|
if (!mContactManifolds.empty()) {
|
|
|
|
// Solve the contacts
|
|
mContactSolver.solve(mTimer.getTimeStep());
|
|
}
|
|
|
|
// Update the timer
|
|
mTimer.nextStep();
|
|
|
|
// Reset the movement boolean variable of each body to false
|
|
resetBodiesMovementVariable();
|
|
|
|
// Update the position and orientation of each body
|
|
updateRigidBodiesPositionAndOrientation();
|
|
|
|
// Cleanup of the contact solver
|
|
mContactSolver.cleanup();
|
|
|
|
// Cleanup the constrained velocities
|
|
cleanupConstrainedVelocitiesArray();
|
|
}
|
|
|
|
// Compute and set the interpolation factor to all the bodies
|
|
setInterpolationFactorToAllBodies();
|
|
}
|
|
|
|
// Update the position and orientation of the rigid bodies
|
|
void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
|
|
decimal dt = mTimer.getTimeStep();
|
|
|
|
// For each rigid body of the world
|
|
for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
|
|
|
RigidBody* rigidBody = *it;
|
|
assert(rigidBody);
|
|
|
|
// If the body is allowed to move
|
|
if (rigidBody->getIsMotionEnabled()) {
|
|
|
|
// Update the old Transform of the body
|
|
rigidBody->updateOldTransform();
|
|
|
|
// Get the constrained velocity
|
|
uint indexArray = mMapBodyToConstrainedVelocityIndex[rigidBody];
|
|
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
|
|
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
|
|
|
|
// Update the linear and angular velocity of the body
|
|
rigidBody->setLinearVelocity(newLinVelocity);
|
|
rigidBody->setAngularVelocity(newAngVelocity);
|
|
|
|
// Add the split impulse velocity from Contact Solver (only used to update the position)
|
|
if (mContactSolver.isConstrainedBody(rigidBody)) {
|
|
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
|
|
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
|
|
}
|
|
|
|
// 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(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
|
|
|
|
// Update the Transform of the body
|
|
Transform newTransform(newPosition, newOrientation.getUnit());
|
|
rigidBody->setTransform(newTransform);
|
|
|
|
// Update the AABB of the rigid body
|
|
rigidBody->updateAABB();
|
|
}
|
|
}
|
|
}
|
|
|
|
// Compute and set the interpolation factor to all bodies
|
|
void DynamicsWorld::setInterpolationFactorToAllBodies() {
|
|
|
|
// Compute the interpolation factor
|
|
decimal factor = mTimer.computeInterpolationFactor();
|
|
assert(factor >= 0.0 && factor <= 1.0);
|
|
|
|
// Set the factor to all bodies
|
|
for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
|
|
|
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
|
|
assert(rigidBody);
|
|
|
|
rigidBody->setInterpolationFactor(factor);
|
|
}
|
|
}
|
|
|
|
// Initialize the constrained velocities array at each step
|
|
void DynamicsWorld::initConstrainedVelocitiesArray() {
|
|
|
|
// TODO : Use better memory allocation here
|
|
mConstrainedLinearVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
|
mConstrainedAngularVelocities = std::vector<Vector3>(mRigidBodies.size(), Vector3(0, 0, 0));
|
|
|
|
double dt = mTimer.getTimeStep();
|
|
|
|
// Fill in the mapping of rigid body to their index in the constrained
|
|
// velocities arrays
|
|
uint i = 0;
|
|
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
|
RigidBody* rigidBody = *it;
|
|
mMapBodyToConstrainedVelocityIndex.insert(std::make_pair<RigidBody*, uint>(rigidBody, i));
|
|
|
|
// TODO : Move it somewhere else
|
|
mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + dt * rigidBody->getMassInverse() *rigidBody->getExternalForce();
|
|
mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
|
|
|
i++;
|
|
}
|
|
}
|
|
|
|
// 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();
|
|
}
|
|
|
|
// Apply the gravity force to all bodies of the physics world
|
|
void DynamicsWorld::applyGravity() {
|
|
|
|
// For each body of the physics world
|
|
for (set<RigidBody*>::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) {
|
|
|
|
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
|
|
assert(rigidBody);
|
|
|
|
// If the gravity force is on
|
|
if(mIsGravityOn) {
|
|
// Apply the current gravity force to the body
|
|
rigidBody->setExternalForce(rigidBody->getMass() * mGravity);
|
|
}
|
|
}
|
|
}
|
|
|
|
// Create a rigid body into the physics world
|
|
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass,
|
|
const Matrix3x3& inertiaTensorLocal,
|
|
CollisionShape* collisionShape) {
|
|
|
|
// 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 (mMemoryPoolRigidBodies.allocateObject()) RigidBody(transform, mass, inertiaTensorLocal, collisionShape, bodyID);
|
|
|
|
// Add the rigid body to the physics world
|
|
mBodies.insert(rigidBody);
|
|
mRigidBodies.insert(rigidBody);
|
|
|
|
// Add the rigid body to the collision detection
|
|
mCollisionDetection.addBody(rigidBody);
|
|
|
|
// Return the pointer to the rigid body
|
|
return rigidBody;
|
|
}
|
|
|
|
// Destroy a rigid body
|
|
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
|
|
|
|
// Remove the body from the collision detection
|
|
mCollisionDetection.removeBody(rigidBody);
|
|
|
|
// Add the body ID to the list of free IDs
|
|
mFreeBodiesIDs.push_back(rigidBody->getID());
|
|
|
|
// Call the constructor of the rigid body
|
|
rigidBody->RigidBody::~RigidBody();
|
|
|
|
// Remove the rigid body from the list of rigid bodies
|
|
mBodies.erase(rigidBody); // TOOD : Maybe use a set to make this faster
|
|
mRigidBodies.erase(rigidBody); // TOOD : Maybe use a set to make this faster
|
|
|
|
// Free the object from the memory pool
|
|
mMemoryPoolRigidBodies.freeObject(rigidBody);
|
|
}
|
|
|
|
// Remove all constraints in the physics world
|
|
void DynamicsWorld::removeAllConstraints() {
|
|
mConstraints.clear();
|
|
}
|
|
|
|
// Notify the world about a new broad-phase overlapping pair
|
|
void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
|
|
|
|
// Get the pair of body index
|
|
std::pair<bodyindex, bodyindex> indexPair = addedPair->getBodiesIndexPair();
|
|
|
|
// Add the pair into the set of overlapping pairs (if not there yet)
|
|
OverlappingPair* newPair = new (mMemoryPoolOverlappingPairs.allocateObject()) OverlappingPair(addedPair->body1, addedPair->body2, mMemoryPoolContacts);
|
|
std::pair<map<std::pair<bodyindex, bodyindex>, OverlappingPair*>::iterator, bool> check = mOverlappingPairs.insert(make_pair(indexPair, newPair));
|
|
assert(check.second);
|
|
}
|
|
|
|
// Notify the world about a removed broad-phase overlapping pair
|
|
void DynamicsWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPair) {
|
|
|
|
// Get the pair of body index
|
|
std::pair<bodyindex, bodyindex> indexPair = removedPair->getBodiesIndexPair();
|
|
|
|
// Remove the overlapping pair from the memory pool
|
|
mOverlappingPairs[indexPair]->OverlappingPair::~OverlappingPair();
|
|
mMemoryPoolOverlappingPairs.freeObject(mOverlappingPairs[indexPair]);
|
|
mOverlappingPairs.erase(indexPair);
|
|
}
|
|
|
|
// Notify the world about a new narrow-phase contact
|
|
void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const ContactInfo* contactInfo) {
|
|
|
|
RigidBody* const rigidBody1 = dynamic_cast<RigidBody* const>(broadPhasePair->body1);
|
|
RigidBody* const rigidBody2 = dynamic_cast<RigidBody* const>(broadPhasePair->body2);
|
|
|
|
assert(rigidBody1);
|
|
assert(rigidBody2);
|
|
|
|
// Create a new contact
|
|
Contact* contact = new (mMemoryPoolContacts.allocateObject()) Contact(rigidBody1, rigidBody2, contactInfo);
|
|
assert(contact);
|
|
|
|
// Get the corresponding overlapping pair
|
|
pair<bodyindex, bodyindex> indexPair = broadPhasePair->getBodiesIndexPair();
|
|
OverlappingPair* overlappingPair = mOverlappingPairs[indexPair];
|
|
assert(overlappingPair);
|
|
|
|
// Add the contact to the contact cache of the corresponding overlapping pair
|
|
overlappingPair->addContact(contact);
|
|
|
|
// Create a contact manifold with the contact points of the two bodies
|
|
ContactManifold contactManifold;
|
|
contactManifold.nbContacts = 0;
|
|
|
|
// Add all the contacts in the contact cache of the two bodies
|
|
// to the set of constraints in the physics world
|
|
for (uint i=0; i<overlappingPair->getNbContacts(); i++) {
|
|
contactManifold.contacts[i] = overlappingPair->getContact(i);
|
|
contactManifold.nbContacts++;
|
|
}
|
|
|
|
// Add the contact manifold to the world
|
|
mContactManifolds.push_back(contactManifold);
|
|
}
|