From 9e499be150154c83bc8e0844f05f4883744abab0 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 26 Feb 2013 08:15:58 +0100 Subject: [PATCH] Move the constrained velocities outside the contact solver --- src/engine/ContactSolver.cpp | 91 ++++++++++----------- src/engine/ContactSolver.h | 44 ++++------ src/engine/DynamicsWorld.cpp | 150 +++++++++++++++++++---------------- src/engine/DynamicsWorld.h | 27 ++++++- 4 files changed, 167 insertions(+), 145 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 56329af2..e10bc29b 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -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& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, + const std::map& 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 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; } } diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 6b1d9c61..bae7a9bb 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -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 mConstraintBodies; - // Map body to index - std::map mMapBodyToIndex; + // Pointer to the array of constrained linear velocities (state of the linear velocities + // after solving the constraints) + std::vector& mConstrainedLinearVelocities; + + // Pointer to the array of constrained angular velocities (state of the angular velocities + // after solving the constraints) + std::vector& mConstrainedAngularVelocities; + + // Reference to the map of rigid body to their index in the constrained velocities array + const std::map& 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& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, const std::map& + 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 diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index f638bd8d..d1ad2f96 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -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::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(mRigidBodies.size(), Vector3(0, 0, 0)); + mConstrainedAngularVelocities = std::vector(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::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + RigidBody* rigidBody = *it; + mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(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() { diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 51f3b6b1..b2254fca 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -84,6 +84,17 @@ class DynamicsWorld : public CollisionWorld { // Memory pool for the contacts MemoryPool mMemoryPoolContacts; + // Array of constrained linear velocities (state of the linear velocities + // after solving the constraints) + std::vector mConstrainedLinearVelocities; + + // Array of constrained angular velocities (state of the angular velocities + // after solving the constraints) + std::vector mConstrainedAngularVelocities; + + // Map body to their index in the constrained velocities array + std::map 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::iterator DynamicsWorld::getRigidBodiesBeginIterator() { return mRigidBodies.begin();