From 475ec5be5fca73c4ce78e3250375631da96fa03b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 31 Aug 2013 19:03:21 +0200 Subject: [PATCH] Iterate over the islands to solve the contacts and joints --- src/body/RigidBody.cpp | 1 + src/engine/ConstraintSolver.cpp | 70 ++++++----- src/engine/ConstraintSolver.h | 62 +++++----- src/engine/ContactSolver.cpp | 90 ++++---------- src/engine/ContactSolver.h | 85 ++++++-------- src/engine/DynamicsWorld.cpp | 200 +++++++++++++++++++------------- src/engine/DynamicsWorld.h | 16 ++- 7 files changed, 250 insertions(+), 274 deletions(-) diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 8b61ed6f..82e5c928 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -70,6 +70,7 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons memoryAllocator.release(elementToRemove, sizeof(JointListElement)); break; } + currentElement = currentElement->next; } } } diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 2154ceb0..25ef6641 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -30,18 +30,14 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver(std::set& joints, - std::vector& linearVelocities, - std::vector& angularVelocities, - std::vector& positions, +ConstraintSolver::ConstraintSolver(std::vector& positions, std::vector& orientations, const std::map& 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::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; igetNbJoints(); 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::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; igetNbJoints(); 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::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); } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 79cac7f7..f52c6412 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -30,6 +30,7 @@ #include "../configuration.h" #include "mathematics/mathematics.h" #include "../constraint/Constraint.h" +#include "Island.h" #include #include @@ -47,11 +48,11 @@ struct ConstraintSolverData { /// Current time step of the simulation decimal timeStep; - /// Reference to the bodies linear velocities - std::vector& linearVelocities; + /// Array with the bodies linear velocities + Vector3* linearVelocities; - /// Reference to the bodies angular velocities - std::vector& angularVelocities; + /// Array with the bodies angular velocities + Vector3* angularVelocities; /// Reference to the bodies positions std::vector& positions; @@ -67,13 +68,11 @@ struct ConstraintSolverData { bool isWarmStartingActive; /// Constructor - ConstraintSolverData(std::vector& refLinearVelocities, - std::vector& refAngularVelocities, - std::vector& refPositions, + ConstraintSolverData(std::vector& refPositions, std::vector& refOrientations, const std::map& 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& mJoints; - - /// Constrained bodies - std::set 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& 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& mAngularVelocities; + Vector3* mAngularVelocities; /// Reference to the array of bodies positions (for position error correction) std::vector& mPositions; @@ -194,24 +187,20 @@ class ConstraintSolver { // -------------------- Methods -------------------- // /// Constructor - ConstraintSolver(std::set& joints, - std::vector& linearVelocities, - std::vector& angularVelocities, - std::vector& positions, - std::vector& orientations, + ConstraintSolver(std::vector& positions, std::vector& orientations, const std::map& 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; } } diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 8467a710..f70abbec 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -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& contactManifolds, - std::vector& constrainedLinearVelocities, - std::vector& constrainedAngularVelocities, - const std::map& mapBodyToVelocityIndex) - :mContactManifolds(contactManifolds), - mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), - mContactConstraints(NULL), - mLinearVelocities(constrainedLinearVelocities), - mAngularVelocities(constrainedAngularVelocities), +ContactSolver::ContactSolver(const std::map& 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::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; igetNbContactPoints() > 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::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; - } } diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 2c635fb0..979a0abf 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -31,6 +31,7 @@ #include "../configuration.h" #include "../constraint/Constraint.h" #include "ContactManifold.h" +#include "Island.h" #include "Impulse.h" #include #include @@ -311,9 +312,6 @@ class ContactSolver { // -------------------- Attributes -------------------- // - /// Reference to all the contact manifold of the world - std::vector& 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 mConstraintBodies; + /// Array of linear velocities + Vector3* mLinearVelocities; - /// Reference to the array of linear velocities - std::vector& mLinearVelocities; - - /// Reference to the array of angular velocities - std::vector& 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& 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& contactManifolds, - std::vector& constrainedLinearVelocities, - std::vector& constrainedAngularVelocities, - const std::map& mapBodyToVelocityIndex); + ContactSolver(const std::map& 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 diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 4ca989a9..511f4630 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -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(mRigidBodies.size(), Vector3(0, 0, 0)); - mConstrainedAngularVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); + // Initialize the bodies velocity arrays + initVelocityArrays(); decimal dt = static_cast(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::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { RigidBody* rigidBody = *it; mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(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(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(mRigidBodies.size()); mConstrainedOrientations = std::vector(mRigidBodies.size()); - for (std::set::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::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 diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 89ebec5d..b1bc41ca 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -87,11 +87,17 @@ class DynamicsWorld : public CollisionWorld { /// Array of constrained linear velocities (state of the linear velocities /// after solving the constraints) - std::vector mConstrainedLinearVelocities; + Vector3* mConstrainedLinearVelocities; /// Array of constrained angular velocities (state of the angular velocities /// after solving the constraints) - std::vector 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 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();