mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Iterate over the islands to solve the contacts and joints

Browse Source
This commit is contained in:
Daniel Chappuis 2013-08-31 19:03:21 +02:00
parent f1d29b5123
commit 475ec5be5f
7 changed files with 250 additions and 274 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/body/RigidBody.cpp
View File

@ -70,6 +70,7 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
memoryAllocator.release(elementToRemove, sizeof(JointListElement));
break;
}
currentElement = currentElement->next;
}
}
}

70
src/engine/ConstraintSolver.cpp
View File

@ -30,18 +30,14 @@
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& linearVelocities,
std::vector<Vector3>& angularVelocities,
std::vector<Vector3>& positions,
ConstraintSolver::ConstraintSolver(std::vector<Vector3>& positions,
std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
: mJoints(joints), mLinearVelocities(linearVelocities),
mAngularVelocities(angularVelocities), mPositions(positions),
: mLinearVelocities(NULL), mAngularVelocities(NULL), mPositions(positions),
mOrientations(orientations),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(linearVelocities,
angularVelocities, positions, orientations, mapBodyToVelocityIndex){
mIsWarmStartingActive(true), mConstraintSolverData(positions, orientations,
mapBodyToVelocityIndex){
}
@ -50,10 +46,16 @@ ConstraintSolver::~ConstraintSolver() {
}
// Initialize the constraint solver
void ConstraintSolver::initialize(decimal dt) {
// Initialize the constraint solver for a given island
void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
PROFILE("ConstraintSolver::initialize()");
PROFILE("ConstraintSolver::initializeForIsland()");
assert(mLinearVelocities != NULL);
assert(mAngularVelocities != NULL);
assert(island != NULL);
assert(island->getNbBodies() > 0);
assert(island->getNbJoints() > 0);
// Set the current time step
mTimeStep = dt;
@ -62,54 +64,50 @@ void ConstraintSolver::initialize(decimal dt) {
mConstraintSolverData.timeStep = mTimeStep;
mConstraintSolverData.isWarmStartingActive = mIsWarmStartingActive;
// For each joint
std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
Constraint* joint = (*it);
// Get the rigid bodies of the joint
RigidBody* body1 = joint->getBody1();
RigidBody* body2 = joint->getBody2();
// Add the bodies to the set of constrained bodies
mConstraintBodies.insert(body1);
mConstraintBodies.insert(body2);
// For each joint of the island
Constraint** joints = island->getJoints();
for (uint i=0; i<island->getNbJoints(); i++) {
// Initialize the constraint before solving it
joint->initBeforeSolve(mConstraintSolverData);
joints[i]->initBeforeSolve(mConstraintSolverData);
// Warm-start the constraint if warm-starting is enabled
if (mIsWarmStartingActive) {
joint->warmstart(mConstraintSolverData);
joints[i]->warmstart(mConstraintSolverData);
}
}
}
// Solve the velocity constraints
void ConstraintSolver::solveVelocityConstraints() {
void ConstraintSolver::solveVelocityConstraints(Island* island) {
PROFILE("ConstraintSolver::solveVelocityConstraints()");
// For each joint
std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
assert(island != NULL);
assert(island->getNbJoints() > 0);
// For each joint of the island
Constraint** joints = island->getJoints();
for (uint i=0; i<island->getNbJoints(); i++) {
// Solve the constraint
(*it)->solveVelocityConstraint(mConstraintSolverData);
joints[i]->solveVelocityConstraint(mConstraintSolverData);
}
}
// Solve the position constraints
void ConstraintSolver::solvePositionConstraints() {
void ConstraintSolver::solvePositionConstraints(Island* island) {
PROFILE("ConstraintSolver::solvePositionConstraints()");
// For each joint
std::set<Constraint*>::iterator it;
for (it = mJoints.begin(); it != mJoints.end(); ++it) {
assert(island != NULL);
assert(island->getNbJoints() > 0);
// For each joint of the island
Constraint** joints = island->getJoints();
for (uint i=0; i < island->getNbJoints(); i++) {
// Solve the constraint
(*it)->solvePositionConstraint(mConstraintSolverData);
joints[i]->solvePositionConstraint(mConstraintSolverData);
}
}

62
src/engine/ConstraintSolver.h
View File

@ -30,6 +30,7 @@
#include "../configuration.h"
#include "mathematics/mathematics.h"
#include "../constraint/Constraint.h"
#include "Island.h"
#include <map>
#include <set>
@ -47,11 +48,11 @@ struct ConstraintSolverData {
/// Current time step of the simulation
decimal timeStep;
/// Reference to the bodies linear velocities
std::vector<Vector3>& linearVelocities;
/// Array with the bodies linear velocities
Vector3* linearVelocities;
/// Reference to the bodies angular velocities
std::vector<Vector3>& angularVelocities;
/// Array with the bodies angular velocities
Vector3* angularVelocities;
/// Reference to the bodies positions
std::vector<Vector3>& positions;
@ -67,13 +68,11 @@ struct ConstraintSolverData {
bool isWarmStartingActive;
/// Constructor
ConstraintSolverData(std::vector<Vector3>& refLinearVelocities,
std::vector<Vector3>& refAngularVelocities,
std::vector<Vector3>& refPositions,
ConstraintSolverData(std::vector<Vector3>& refPositions,
std::vector<Quaternion>& refOrientations,
const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(refLinearVelocities),
angularVelocities(refAngularVelocities),
:linearVelocities(NULL),
angularVelocities(NULL),
positions(refPositions), orientations(refOrientations),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
@ -156,19 +155,13 @@ class ConstraintSolver {
// -------------------- Attributes -------------------- //
/// Reference to all the joints of the world
std::set<Constraint*>& mJoints;
/// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
/// Reference to the array of constrained linear velocities (state of the linear velocities
/// Array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
std::vector<Vector3>& mLinearVelocities;
Vector3* mLinearVelocities;
/// Reference to the array of constrained angular velocities (state of the angular velocities
/// Array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
std::vector<Vector3>& mAngularVelocities;
Vector3* mAngularVelocities;
/// Reference to the array of bodies positions (for position error correction)
std::vector<Vector3>& mPositions;
@ -194,24 +187,20 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver(std::set<Constraint*>& joints,
std::vector<Vector3>& linearVelocities,
std::vector<Vector3>& angularVelocities,
std::vector<Vector3>& positions,
std::vector<Quaternion>& orientations,
ConstraintSolver(std::vector<Vector3>& positions, std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
/// Destructor
~ConstraintSolver();
/// Initialize the constraint solver
void initialize(decimal dt);
/// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island);
/// Solve the constraints
void solveVelocityConstraints();
void solveVelocityConstraints(Island* island);
/// Solve the position constraints
void solvePositionConstraints();
void solvePositionConstraints(Island* island);
/// Return true if the Non-Linear-Gauss-Seidel position correction technique is active
bool getIsNonLinearGaussSeidelPositionCorrectionActive() const;
@ -219,13 +208,20 @@ class ConstraintSolver {
/// Enable/Disable the Non-Linear-Gauss-Seidel position correction technique.
void setIsNonLinearGaussSeidelPositionCorrectionActive(bool isActive);
/// Return true if the body is in at least one constraint
bool isConstrainedBody(RigidBody* body) const;
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities);
};
// Return true if the body is in at least one constraint
inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
return mConstraintBodies.count(body) == 1;
// Set the constrained velocities arrays
inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL);
mLinearVelocities = constrainedLinearVelocities;
mAngularVelocities = constrainedAngularVelocities;
mConstraintSolverData.linearVelocities = mLinearVelocities;
mConstraintSolverData.angularVelocities = mAngularVelocities;
}
}

90
src/engine/ContactSolver.cpp
View File

@ -36,18 +36,12 @@ using namespace std;
// Constants initialization
const decimal ContactSolver::BETA = decimal(0.2);
const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
const decimal ContactSolver::SLOP= decimal(0.01);
// Constructor
ContactSolver::ContactSolver(std::vector<ContactManifold*>& contactManifolds,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
:mContactManifolds(contactManifolds),
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
mContactConstraints(NULL),
mLinearVelocities(constrainedLinearVelocities),
mAngularVelocities(constrainedAngularVelocities),
ContactSolver::ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
:mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL),
mContactConstraints(NULL), mLinearVelocities(NULL), mAngularVelocities(NULL),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
@ -59,26 +53,32 @@ ContactSolver::~ContactSolver() {
}
// Initialize the constraint solver
void ContactSolver::initialize(decimal dt) {
// Initialize the constraint solver for a given island
void ContactSolver::initializeForIsland(decimal dt, Island* island) {
PROFILE("ContactSolver::initialize()");
PROFILE("ContactSolver::initializeForIsland()");
assert(island != NULL);
assert(island->getNbBodies() > 0);
assert(island->getNbContactManifolds() > 0);
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);
// Set the current time step
mTimeStep = dt;
// TODO : Use better memory allocation here
mContactConstraints = new ContactManifoldSolver[mContactManifolds.size()];
mNbContactManifolds = island->getNbContactManifolds();
mNbContactManifolds = 0;
mContactConstraints = new ContactManifoldSolver[mNbContactManifolds];
assert(mContactConstraints != NULL);
// For each contact manifold of the world
vector<ContactManifold*>::iterator it;
for (it = mContactManifolds.begin(); it != mContactManifolds.end(); ++it) {
// For each contact manifold of the island
ContactManifold** contactManifolds = island->getContactManifold();
for (uint i=0; i<mNbContactManifolds; i++) {
ContactManifold* externalManifold = *it;
ContactManifold* externalManifold = contactManifolds[i];
ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds];
ContactManifoldSolver& internalManifold = mContactConstraints[i];
assert(externalManifold->getNbContactPoints() > 0);
@ -86,10 +86,6 @@ void ContactSolver::initialize(decimal dt) {
RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1();
RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2();
// Add the two bodies of the constraint in the constraintBodies list
mConstraintBodies.insert(body1);
mConstraintBodies.insert(body2);
// Get the position of the two bodies
Vector3 x1 = body1->getTransform().getPosition();
Vector3 x2 = body2->getTransform().getPosition();
@ -173,46 +169,12 @@ void ContactSolver::initialize(decimal dt) {
internalManifold.frictionTwistImpulse = 0.0;
}
}
mNbContactManifolds++;
}
// Allocated memory for split impulse velocities
// TODO : Use better memory allocation here
mSplitLinearVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
mSplitAngularVelocities = new Vector3[mMapBodyToConstrainedVelocityIndex.size()];
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);
assert(mConstraintBodies.size() > 0);
assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
assert(mLinearVelocities.size() >= mConstraintBodies.size());
assert(mAngularVelocities.size() >= mConstraintBodies.size());
// Initialize the split impulse velocities
initializeSplitImpulseVelocities();
// Fill-in all the matrices needed to solve the LCP problem
initializeContactConstraints();
}
// Initialize the split impulse velocities
void ContactSolver::initializeSplitImpulseVelocities() {
// For each current body that is implied in some constraint
set<RigidBody*>::iterator it;
for (it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
RigidBody* rigidBody = *it;
assert(rigidBody);
uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
// Initialize the split impulse velocities to zero
mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0);
mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0);
}
}
// Initialize the contact constraints before solving the system
void ContactSolver::initializeContactConstraints() {
@ -884,18 +846,8 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// Clean up the constraint solver
void ContactSolver::cleanup() {
mConstraintBodies.clear();
if (mContactConstraints != NULL) {
delete[] mContactConstraints;
mContactConstraints = NULL;
}
if (mSplitLinearVelocities != NULL) {
delete[] mSplitLinearVelocities;
mSplitLinearVelocities = NULL;
}
if (mSplitAngularVelocities != NULL) {
delete[] mSplitAngularVelocities;
mSplitAngularVelocities = NULL;
}
}

85
src/engine/ContactSolver.h
View File

@ -31,6 +31,7 @@
#include "../configuration.h"
#include "../constraint/Constraint.h"
#include "ContactManifold.h"
#include "Island.h"
#include "Impulse.h"
#include <map>
#include <set>
@ -311,9 +312,6 @@ class ContactSolver {
// -------------------- Attributes -------------------- //
/// Reference to all the contact manifold of the world
std::vector<ContactManifold*>& mContactManifolds;
/// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities;
@ -329,14 +327,11 @@ class ContactSolver {
/// Number of contact constraints
uint mNbContactManifolds;
/// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
/// Array of linear velocities
Vector3* mLinearVelocities;
/// Reference to the array of linear velocities
std::vector<Vector3>& mLinearVelocities;
/// Reference to the array of angular velocities
std::vector<Vector3>& mAngularVelocities;
/// Array of angular velocities
Vector3* mAngularVelocities;
/// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
@ -353,9 +348,6 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Initialize the split impulse velocities
void initializeSplitImpulseVelocities();
/// Initialize the contact constraints before solving the system
void initializeContactConstraints();
@ -403,16 +395,21 @@ class ContactSolver {
// -------------------- Methods -------------------- //
/// Constructor
ContactSolver(std::vector<ContactManifold*>& contactManifolds,
std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
ContactSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
/// Destructor
virtual ~ContactSolver();
/// Initialize the constraint solver
void initialize(decimal dt);
/// Initialize the constraint solver for a given island
void initializeForIsland(decimal dt, Island* island);
/// Set the split velocities arrays
void setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
Vector3* splitAngularVelocities);
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities);
/// Warm start the solver.
void warmStart();
@ -424,24 +421,6 @@ class ContactSolver {
/// Solve the contacts
void solve();
/// Return true if the body is in at least one constraint
bool isConstrainedBody(RigidBody* body) const;
/// Return the constrained linear velocity of a body after solving the constraints
Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body);
/// Return the split linear velocity
Vector3 getSplitLinearVelocityOfBody(RigidBody* body);
/// Return the constrained angular velocity of a body after solving the constraints
Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body);
/// Return the split angular velocity
Vector3 getSplitAngularVelocityOfBody(RigidBody* body);
/// Clean up the constraint solver
void cleanup();
/// Return true if the split impulses position correction technique is used for contacts
bool isSplitImpulseActive() const;
@ -451,25 +430,27 @@ class ContactSolver {
/// Activate or deactivate the solving of friction constraints at the center of
/// the contact manifold instead of solving them at each contact point
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Clean up the constraint solver
void cleanup();
};
// Return true if the body is in at least one constraint
inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
return mConstraintBodies.count(body) == 1;
// Set the split velocities arrays
inline void ContactSolver::setSplitVelocitiesArrays(Vector3* splitLinearVelocities,
Vector3* splitAngularVelocities) {
assert(splitLinearVelocities != NULL);
assert(splitAngularVelocities != NULL);
mSplitLinearVelocities = splitLinearVelocities;
mSplitAngularVelocities = splitAngularVelocities;
}
// Return the split linear velocity
inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
return mSplitLinearVelocities[indexBody];
}
// Return the split angular velocity
inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
return mSplitAngularVelocities[indexBody];
// Set the constrained velocities arrays
inline void ContactSolver::setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL);
mLinearVelocities = constrainedLinearVelocities;
mAngularVelocities = constrainedAngularVelocities;
}
// Return true if the split impulses position correction technique is used for contacts

200
src/engine/DynamicsWorld.cpp
View File

@ -37,15 +37,15 @@ using namespace std;
// Constructor
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
mContactSolver(mContactManifolds, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mJoints, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mConstrainedPositions, mConstrainedOrientations,
mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
mContactSolver(mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mConstrainedPositions, mConstrainedOrientations,
mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsSleepingEnabled(SPLEEPING_ENABLED), mNbIslands(0), mNbIslandsCapacity(0),
mIslands(NULL) {
mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
mSplitAngularVelocities(NULL), mNbIslands(0), mNbIslandsCapacity(0),
mIslands(NULL), mNbBodiesCapacity(0) {
}
@ -73,8 +73,13 @@ DynamicsWorld::~DynamicsWorld() {
mMemoryAllocator.release(mIslands, sizeof(Island*) * mNbIslandsCapacity);
}
// Free the allocated memory for the constrained velocities
cleanupConstrainedVelocitiesArray();
// Release the memory allocated for the bodies velocity arrays
if (mNbBodiesCapacity > 0) {
delete[] mSplitLinearVelocities;
delete[] mSplitAngularVelocities;
delete[] mConstrainedLinearVelocities;
delete[] mConstrainedAngularVelocities;
}
#ifdef IS_PROFILING_ACTIVE
@ -137,12 +142,6 @@ void DynamicsWorld::update() {
// Update the AABBs of the bodies
updateRigidBodiesAABB();
// Cleanup of the contact solver
mContactSolver.cleanup();
// Cleanup the constrained velocities
cleanupConstrainedVelocitiesArray();
}
// Compute and set the interpolation factor to all the bodies
@ -178,11 +177,10 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
rigidBody->setAngularVelocity(newAngVelocity);
// Add the split impulse velocity from Contact Solver (only used to update the position)
if (mContactSolver.isConstrainedBody(rigidBody) &&
mContactSolver.isSplitImpulseActive()) {
if (mContactSolver.isSplitImpulseActive()) {
newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody);
newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody);
newLinVelocity += mSplitLinearVelocities[indexArray];
newAngVelocity += mSplitAngularVelocities[indexArray];
}
// Get current position and orientation of the body
@ -239,6 +237,34 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
}
}
// Initialize the bodies velocities arrays for the next simulation step.
void DynamicsWorld::initVelocityArrays() {
// Allocate memory for the bodies velocity arrays
uint nbBodies = mRigidBodies.size();
if (mNbBodiesCapacity != nbBodies && nbBodies > 0) {
if (mNbBodiesCapacity > 0) {
delete[] mSplitLinearVelocities;
delete[] mSplitAngularVelocities;
}
mNbBodiesCapacity = nbBodies;
mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);
assert(mConstrainedLinearVelocities != NULL);
assert(mConstrainedAngularVelocities != NULL);
}
// Reset the velocities arrays
for (uint i=0; i<mNbBodiesCapacity; i++) {
mSplitLinearVelocities[i].setToZero();
mSplitAngularVelocities[i].setToZero();
}
}
// 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
@ -248,19 +274,22 @@ void DynamicsWorld::integrateRigidBodiesVelocities() {
PROFILE("DynamicsWorld::integrateRigidBodiesVelocities()");
// 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));
// Initialize the bodies velocity arrays
initVelocityArrays();
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));
assert(mSplitLinearVelocities[i] == Vector3(0, 0, 0));
assert(mSplitAngularVelocities[i] == Vector3(0, 0, 0));
// If the body is allowed to move
if (rigidBody->getIsMotionEnabled()) {
@ -318,42 +347,59 @@ void DynamicsWorld::solveContactsAndConstraints() {
// Get the current time step
decimal dt = static_cast<decimal>(mTimer.getTimeStep());
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = !mJoints.empty();
bool isContactsToSolve = !mContactManifolds.empty();
if (!isConstraintsToSolve && !isContactsToSolve) return;
// Set the velocities arrays
mContactSolver.setSplitVelocitiesArrays(mSplitLinearVelocities, mSplitAngularVelocities);
mContactSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
mConstrainedAngularVelocities);
mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
mConstrainedAngularVelocities);
// ---------- Solve velocity constraints for joints and contacts ---------- //
// If there are contacts
if (isContactsToSolve) {
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// Initialize the solver
mContactSolver.initialize(dt);
// Check if there are contacts and constraints to solve
bool isConstraintsToSolve = mIslands[islandIndex]->getNbJoints() > 0;
bool isContactsToSolve = mIslands[islandIndex]->getNbContactManifolds() > 0;
if (!isConstraintsToSolve && !isContactsToSolve) continue;
// Warm start the contact solver
mContactSolver.warmStart();
// If there are contacts in the current island
if (isContactsToSolve) {
// Initialize the solver
mContactSolver.initializeForIsland(dt, mIslands[islandIndex]);
// Warm start the contact solver
mContactSolver.warmStart();
}
// If there are constraints
if (isConstraintsToSolve) {
// Initialize the constraint solver
mConstraintSolver.initializeForIsland(dt, mIslands[islandIndex]);
}
// For each iteration of the velocity solver
for (uint i=0; i<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();
}
}
// If there are constraints
if (isConstraintsToSolve) {
// Initialize the constraint solver
mConstraintSolver.initialize(dt);
}
// For each iteration of the velocity solver
for (uint i=0; i<mNbVelocitySolverIterations; i++) {
// Solve the constraints
if (isConstraintsToSolve) mConstraintSolver.solveVelocityConstraints();
// Solve the contacts
if (isContactsToSolve) mContactSolver.solve();
}
// Cache the lambda values in order to use them in the next step
if (isContactsToSolve) mContactSolver.storeImpulses();
}
// Solve the position error correction of the constraints
@ -369,37 +415,36 @@ void DynamicsWorld::solvePositionCorrection() {
// TODO : Use better memory allocation here
mConstrainedPositions = std::vector<Vector3>(mRigidBodies.size());
mConstrainedOrientations = std::vector<Quaternion>(mRigidBodies.size());
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// If it is a constrained bodies (by a joint)
if (mConstraintSolver.isConstrainedBody(*it)) {
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
// For each body of the island
RigidBody** bodies = mIslands[islandIndex]->getBodies();
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
// Get the position/orientation of the rigid body
const Transform& transform = (*it)->getTransform();
const Transform& transform = bodies[b]->getTransform();
mConstrainedPositions[index] = transform.getPosition();
mConstrainedOrientations[index]= transform.getOrientation();
}
}
// ---------- Solve the position error correction for the constraints ---------- //
// ---------- Solve the position error correction for the constraints ---------- //
// For each iteration of the position (error correction) solver
for (uint i=0; i<mNbPositionSolverIterations; i++) {
// For each iteration of the position (error correction) solver
for (uint i=0; i<mNbPositionSolverIterations; i++) {
// Solve the position constraints
mConstraintSolver.solvePositionConstraints();
}
// Solve the position constraints
mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
}
// ---------- Update the position/orientation of the rigid bodies ---------- //
// ---------- Update the position/orientation of the rigid bodies ---------- //
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
// If it is a constrained bodies (by a joint)
if (mConstraintSolver.isConstrainedBody(*it)) {
uint index = mMapBodyToConstrainedVelocityIndex.find(*it)->second;
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
// Get the new position/orientation of the body
const Vector3& newPosition = mConstrainedPositions[index];
@ -407,22 +452,11 @@ void DynamicsWorld::solvePositionCorrection() {
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
(*it)->setTransform(newTransform);
bodies[b]->setTransform(newTransform);
}
}
}
// Cleanup the constrained velocities array at each step
void DynamicsWorld::cleanupConstrainedVelocitiesArray() {
// Clear the constrained velocites
mConstrainedLinearVelocities.clear();
mConstrainedAngularVelocities.clear();
// Clear the rigid body to velocities array index mapping
mMapBodyToConstrainedVelocityIndex.clear();
}
// Create a rigid body into the physics world
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass,
const Matrix3x3& inertiaTensorLocal,
@ -579,11 +613,13 @@ void DynamicsWorld::destroyJoint(Constraint* joint) {
joint->mBody1->removeJointFromJointsList(mMemoryAllocator, joint);
joint->mBody2->removeJointFromJointsList(mMemoryAllocator, joint);
size_t nbBytes = joint->getSizeInBytes();
// Call the destructor of the joint
joint->Constraint::~Constraint();
// Release the allocated memory
mMemoryAllocator.release(joint, joint->getSizeInBytes());
mMemoryAllocator.release(joint, nbBytes);
}
// Add the joint to the list of joints of the two bodies involved in the joint

16
src/engine/DynamicsWorld.h
View File

@ -87,11 +87,17 @@ class DynamicsWorld : public CollisionWorld {
/// Array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
std::vector<Vector3> mConstrainedLinearVelocities;
Vector3* mConstrainedLinearVelocities;
/// Array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
std::vector<Vector3> mConstrainedAngularVelocities;
Vector3* mConstrainedAngularVelocities;
/// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities;
/// Split angular velocities for the position contact solver (split impulse)
Vector3* mSplitAngularVelocities;
/// Array of constrained rigid bodies position (for position error correction)
std::vector<Vector3> mConstrainedPositions;
@ -111,6 +117,9 @@ class DynamicsWorld : public CollisionWorld {
/// Array with all the islands of awaken bodies
Island** mIslands;
/// Current allocated capacity for the bodies
uint mNbBodiesCapacity;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -132,6 +141,9 @@ class DynamicsWorld : public CollisionWorld {
/// Compute and set the interpolation factor to all bodies
void setInterpolationFactorToAllBodies();
/// Initialize the bodies velocities arrays for the next simulation step.
void initVelocityArrays();
/// Integrate the velocities of rigid bodies.
void integrateRigidBodiesVelocities();