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

Move the constrained velocities outside the contact solver

Browse Source
This commit is contained in:
Daniel Chappuis 2013-02-26 08:15:58 +01:00
parent aa236286de
commit 9e499be150
4 changed files with 167 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
src/engine/ContactSolver.cpp
View File

@ -37,10 +37,17 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2);
const decimal ContactSolver::SLOP = decimal(0.01);
// Constructor
ContactSolver::ContactSolver(DynamicsWorld& world)
:mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), mContactConstraints(0),
mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true),
mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) {
ContactSolver::ContactSolver(DynamicsWorld& world, std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
:mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS),
mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mContactConstraints(NULL),
mConstrainedLinearVelocities(constrainedLinearVelocities),
mConstrainedAngularVelocities(constrainedAngularVelocities),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true),
mIsSplitImpulseActive(true),
mIsSolveFrictionAtContactManifoldCenterActive(true) {
}
@ -72,10 +79,6 @@ void ContactSolver::initialize() {
RigidBody* body1 = externalContactManifold.contacts[0]->getBody1();
RigidBody* body2 = externalContactManifold.contacts[0]->getBody2();
// Fill in the body number maping
mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size()));
mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size()));
// Add the two bodies of the constraint in the constraintBodies list
mConstraintBodies.insert(body1);
mConstraintBodies.insert(body2);
@ -84,8 +87,8 @@ void ContactSolver::initialize() {
Vector3 x1 = body1->getTransform().getPosition();
Vector3 x2 = body2->getTransform().getPosition();
internalContactManifold.indexBody1 = mMapBodyToIndex[body1];
internalContactManifold.indexBody2 = mMapBodyToIndex[body2];
internalContactManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second;
internalContactManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second;
internalContactManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld();
internalContactManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld();
internalContactManifold.isBody1Moving = body1->getIsMotionEnabled();
@ -165,17 +168,17 @@ void ContactSolver::initialize() {
mNbContactConstraints++;
}
// Compute the number of bodies that are part of some active constraint
uint nbBodies = mConstraintBodies.size();
// Allocated memory for velocities
// Allocated memory for split impulse velocities
// TODO : Use better memory allocation here
mLinearVelocities = new Vector3[nbBodies];
mAngularVelocities = new Vector3[nbBodies];
mSplitLinearVelocities = new Vector3[nbBodies];
mSplitAngularVelocities = new Vector3[nbBodies];
mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()];
mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()];
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);
assert(mMapBodyToIndex.size() == nbBodies);
assert(mConstraintBodies.size() > 0);
assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size());
assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size());
assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size());
}
// Initialize the constrained bodies
@ -187,10 +190,8 @@ void ContactSolver::initializeBodies() {
RigidBody* rigidBody = *it;
assert(rigidBody);
uint bodyNumber = mMapBodyToIndex[rigidBody];
uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second;
mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce();
mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0);
mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0);
}
@ -214,10 +215,10 @@ void ContactSolver::initializeContactConstraints() {
}
// Get the velocities of the bodies
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
// For each contact point constraint
for (uint i=0; i<contactManifold.nbContacts; i++) {
@ -474,10 +475,10 @@ void ContactSolver::solveContactConstraints() {
decimal sumPenetrationImpulse = 0.0;
const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2];
const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1];
const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1];
const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2];
const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2];
for (uint i=0; i<contactManifold.nbContacts; i++) {
@ -721,15 +722,15 @@ void ContactSolver::applyImpulse(const Impulse& impulse, const ContactManifoldSo
// Update the velocities of the bodies by applying the impulse P
if (contactManifold.isBody1Moving) {
mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 *
mConstrainedLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 *
impulse.linearImpulseBody1;
mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
mConstrainedAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 *
impulse.angularImpulseBody1;
}
if (contactManifold.isBody2Moving) {
mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 *
mConstrainedLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 *
impulse.linearImpulseBody2;
mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
mConstrainedAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 *
impulse.angularImpulseBody2;
}
}
@ -758,10 +759,6 @@ void ContactSolver::applySplitImpulse(const Impulse& impulse,
void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
ContactPointSolver& contactPoint) const {
// Update the old friction vectors
//contact.oldFrictionVector1 = contact.frictionVector1;
//contact.oldFrictionVector2 = contact.frictionVector2;
assert(contactPoint.normal.length() > 0.0);
// Compute the velocity difference vector in the tangential plane
@ -819,19 +816,19 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity,
// Clean up the constraint solver
void ContactSolver::cleanup() {
mMapBodyToIndex.clear();
//mMapBodyToIndex.clear();
mConstraintBodies.clear();
if (mContactConstraints != 0) {
if (mContactConstraints != NULL) {
delete[] mContactConstraints;
mContactConstraints = 0;
mContactConstraints = NULL;
}
if (mLinearVelocities != 0) {
delete[] mLinearVelocities;
mLinearVelocities = 0;
if (mSplitLinearVelocities != NULL) {
delete[] mSplitLinearVelocities;
mSplitLinearVelocities = NULL;
}
if (mAngularVelocities != 0) {
delete[] mAngularVelocities;
mAngularVelocities = 0;
if (mSplitAngularVelocities != NULL) {
delete[] mSplitAngularVelocities;
mSplitAngularVelocities = NULL;
}
}

44
src/engine/ContactSolver.h
View File

@ -62,7 +62,7 @@ struct Impulse {
/* -------------------------------------------------------------------
Class ContactSolver :
This class represents the contacts solver that is used is solve rigid bodies contacts.
This class represents the contact solver that is used is solve rigid bodies contacts.
The constraint solver is based on the "Sequential Impulse" technique described by
Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list).
@ -228,12 +228,6 @@ class ContactSolver {
// Number of iterations of the constraints solver
uint mNbIterations;
// Array of constrained linear velocities
Vector3* mLinearVelocities;
// Array of constrained angular velocities
Vector3* mAngularVelocities;
// Split linear velocities for the position contact solver (split impulse)
Vector3* mSplitLinearVelocities;
@ -252,8 +246,16 @@ class ContactSolver {
// Constrained bodies
std::set<RigidBody*> mConstraintBodies;
// Map body to index
std::map<RigidBody*, uint> mMapBodyToIndex;
// Pointer to the array of constrained linear velocities (state of the linear velocities
// after solving the constraints)
std::vector<Vector3>& mConstrainedLinearVelocities;
// Pointer to the array of constrained angular velocities (state of the angular velocities
// after solving the constraints)
std::vector<Vector3>& mConstrainedAngularVelocities;
// Reference to the map of rigid body to their index in the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
// True if the warm starting of the solver is active
bool mIsWarmStartingActive;
@ -327,7 +329,9 @@ class ContactSolver {
// -------------------- Methods -------------------- //
// Constructor
ContactSolver(DynamicsWorld& mWorld);
ContactSolver(DynamicsWorld& mWorld, std::vector<Vector3>& constrainedLinearVelocities,
std::vector<Vector3>& constrainedAngularVelocities, const std::map<RigidBody*, uint>&
mapBodyToVelocityIndex);
// Destructor
virtual ~ContactSolver();
@ -369,31 +373,17 @@ inline bool ContactSolver::isConstrainedBody(RigidBody* body) const {
return mConstraintBodies.count(body) == 1;
}
// Return the constrained linear velocity of a body after solving the constraints
inline Vector3 ContactSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
uint indexBody = mMapBodyToIndex[body];
return mLinearVelocities[indexBody];
}
// Return the split linear velocity
inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
uint indexBody = mMapBodyToIndex[body];
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
return mSplitLinearVelocities[indexBody];
}
// Return the constrained angular velocity of a body after solving the constraints
inline Vector3 ContactSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
assert(isConstrainedBody(body));
uint indexBody = mMapBodyToIndex[body];
return mAngularVelocities[indexBody];
}
// Return the split angular velocity
inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) {
assert(isConstrainedBody(body));
uint indexBody = mMapBodyToIndex[body];
const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second;
return mSplitAngularVelocities[indexBody];
}
@ -454,6 +444,6 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda,
contactPoint.frictionVector2 * deltaLambda, contactPoint.r2CrossT2 * deltaLambda);
}
} // End of ReactPhysics3D namespace
}
#endif

150
src/engine/DynamicsWorld.cpp
View File

@ -26,13 +26,15 @@
// Libraries
#include "DynamicsWorld.h"
// We want to use the ReactPhysics3D namespace
// 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),
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true),
mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities,
mMapBodyToConstrainedVelocityIndex),
mIsDeactivationActive(DEACTIVATION_ENABLED) {
}
@ -46,6 +48,9 @@ DynamicsWorld::~DynamicsWorld() {
(*it).second->OverlappingPair::~OverlappingPair();
mMemoryPoolOverlappingPairs.freeObject((*it).second);
}
// Free the allocated memory for the constrained velocities
cleanupConstrainedVelocitiesArray();
}
// Update the physics simulation
@ -68,6 +73,9 @@ void DynamicsWorld::update() {
// Compute the collision detection
mCollisionDetection.computeCollisionDetection();
// Initialize the constrained velocities
initConstrainedVelocitiesArray();
// If there are contacts
if (!mContactManifolds.empty()) {
@ -82,60 +90,61 @@ void DynamicsWorld::update() {
resetBodiesMovementVariable();
// Update the position and orientation of each body
updateAllBodiesMotion();
updateRigidBodiesPositionAndOrientation();
// Cleanup of the contact solver
mContactSolver.cleanup();
// Cleanup the constrained velocities
cleanupConstrainedVelocitiesArray();
}
// Compute and set the interpolation factor to all the bodies
setInterpolationFactorToAllBodies();
}
// Compute the motion of all bodies and update their positions and orientations
// First this method compute the vector V2 = V_constraint + V_forces + V1 where
// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext)
// V2 is the final velocity after the timestep and V1 is the velocity before the
// timestep.
// After having computed the velocity V2, this method will update the position
// and orientation of each body.
// This method uses the semi-implicit Euler method to update the position and
// orientation of the body
void DynamicsWorld::updateAllBodiesMotion() {
// Update the position and orientation of the rigid bodies
void DynamicsWorld::updateRigidBodiesPositionAndOrientation() {
decimal dt = mTimer.getTimeStep();
Vector3 newLinearVelocity;
Vector3 newAngularVelocity;
// For each body of thephysics world
// 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 able to move
// If the body is allowed to move
if (rigidBody->getIsMotionEnabled()) {
newLinearVelocity.setAllValues(0.0, 0.0, 0.0);
newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
// If it's a constrained body
if (mContactSolver.isConstrainedBody(*it)) {
// Get the constrained linear and angular velocities from the constraint solver
newLinearVelocity = mContactSolver.getConstrainedLinearVelocityOfBody(rigidBody);
newAngularVelocity = mContactSolver.getConstrainedAngularVelocityOfBody(rigidBody);
}
else {
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
// external forces and torques.
newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
// Update the old Transform of the body
rigidBody->updateOldTransform();
// Add the velocity V1 to the new velocity
newLinearVelocity += rigidBody->getLinearVelocity();
newAngularVelocity += rigidBody->getAngularVelocity();
// 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);
}
// Update the position and the orientation of the body according to the new velocity
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
// 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();
@ -143,40 +152,6 @@ void DynamicsWorld::updateAllBodiesMotion() {
}
}
// Update the position and orientation of a body
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
// orientation of the body
void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody,
Vector3 newLinVelocity,
Vector3 newAngVelocity) {
decimal dt = mTimer.getTimeStep();
assert(rigidBody);
// Update the old position and orientation of the body
rigidBody->updateOldTransform();
// Update the linear and angular velocity of the body
rigidBody->setLinearVelocity(newLinVelocity);
rigidBody->setAngularVelocity(newAngVelocity);
// Split velocity (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();
Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt;
Transform newTransform(newPosition, newOrientation.getUnit());
rigidBody->setTransform(newTransform);
}
// Compute and set the interpolation factor to all bodies
void DynamicsWorld::setInterpolationFactorToAllBodies() {
@ -194,6 +169,41 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() {
}
}
// 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() {

27
src/engine/DynamicsWorld.h
View File

@ -84,6 +84,17 @@ class DynamicsWorld : public CollisionWorld {
// Memory pool for the contacts
MemoryPool<Contact> mMemoryPoolContacts;
// Array of constrained linear velocities (state of the linear velocities
// after solving the constraints)
std::vector<Vector3> mConstrainedLinearVelocities;
// Array of constrained angular velocities (state of the angular velocities
// after solving the constraints)
std::vector<Vector3> mConstrainedAngularVelocities;
// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
// -------------------- Methods -------------------- //
// Private copy-constructor
@ -93,7 +104,7 @@ class DynamicsWorld : public CollisionWorld {
DynamicsWorld& operator=(const DynamicsWorld& world);
// Compute the motion of all bodies and update their positions and orientations
void updateAllBodiesMotion();
void updateRigidBodiesPositionAndOrientation();
// Update the position and orientation of a body
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
@ -102,6 +113,12 @@ class DynamicsWorld : public CollisionWorld {
// Compute and set the interpolation factor to all bodies
void setInterpolationFactorToAllBodies();
// Initialize the constrained velocities array at each step
void initConstrainedVelocitiesArray();
// Cleanup the constrained velocities array at each step
void cleanupConstrainedVelocitiesArray();
// Apply the gravity force to all bodies
void applyGravity();
@ -169,6 +186,9 @@ public :
// Set the isGravityOn attribute
void setIsGratityOn(bool isGravityOn);
// Return the number of rigid bodies in the world
uint getNbRigidBodies() const;
// Add a constraint
void addConstraint(Constraint* constraint);
@ -286,6 +306,11 @@ inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) {
mIsGravityOn = isGravityOn;
}
// Return the number of rigid bodies in the world
inline uint DynamicsWorld::getNbRigidBodies() const {
return mRigidBodies.size();
}
// Return an iterator to the beginning of the bodies of the physics world
inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesBeginIterator() {
return mRigidBodies.begin();