From d6b3b18aeee4271647dc7ef0855c46951be86527 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 21 Dec 2012 11:00:13 +0100 Subject: [PATCH] Use vectors in the Vconstraint array --- src/engine/ConstraintSolver.cpp | 44 +++++++++++++++++++-------------- src/engine/ConstraintSolver.h | 19 ++++++++++---- 2 files changed, 40 insertions(+), 23 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index b4b7ad0c..84a56705 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -34,7 +34,7 @@ using namespace std; // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) - :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0) { + :world(world), nbConstraints(0), mNbIterations(100), mContactConstraints(0), Vconstraint(0), Wconstraint(0) { } @@ -98,6 +98,9 @@ void ConstraintSolver::initialize() { // Compute the number of bodies that are part of some active constraint nbBodies = mConstraintBodies.size(); + Vconstraint = new Vector3[nbBodies]; + Wconstraint = new Vector3[nbBodies]; + assert(mMapBodyToIndex.size() == nbBodies); } @@ -128,12 +131,8 @@ void ConstraintSolver::initializeBodies() { V1[bodyIndexArray + 5] = angularVelocity[2]; // Compute the vector Vconstraint with final constraint velocities - Vconstraint[bodyIndexArray] = 0.0; - Vconstraint[bodyIndexArray + 1] = 0.0; - Vconstraint[bodyIndexArray + 2] = 0.0; - Vconstraint[bodyIndexArray + 3] = 0.0; - Vconstraint[bodyIndexArray + 4] = 0.0; - Vconstraint[bodyIndexArray + 5] = 0.0; + Vconstraint[bodyNumber] = Vector3(0, 0, 0); + Wconstraint[bodyNumber] = Vector3(0, 0, 0); // Compute the vector with forces and torques values Vector3 externalForce = rigidBody->getExternalForce(); @@ -425,25 +424,34 @@ void ConstraintSolver::computeVectorVconstraint(decimal dt) { // ---------- Penetration ---------- // - indexBody1Array = 6 * constraint.indexBody1; - indexBody2Array = 6 * constraint.indexBody2; - for (j=0; j<6; j++) { - Vconstraint[indexBody1Array + j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse * dt; - Vconstraint[indexBody2Array + j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse * dt; + indexBody1Array = constraint.indexBody1; + indexBody2Array = constraint.indexBody2; + for (j=0; j<3; j++) { + Vconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse * dt; + Wconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse * dt; + + Vconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse * dt; + Wconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse * dt; } // ---------- Friction 1 ---------- // - for (j=0; j<6; j++) { - Vconstraint[indexBody1Array + j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse * dt; - Vconstraint[indexBody2Array + j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse * dt; + for (j=0; j<3; j++) { + Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse * dt; + Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse * dt; + + Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse * dt; + Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse * dt; } // ---------- Friction 2 ---------- // - for (j=0; j<6; j++) { - Vconstraint[indexBody1Array + j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse * dt; - Vconstraint[indexBody2Array + j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse * dt; + for (j=0; j<3; j++) { + Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse * dt; + Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse * dt; + + Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse * dt; + Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse * dt; } } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 0a4db5b2..fbbe0469 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -179,7 +179,8 @@ class ConstraintSolver { // This is an array of size nbBodies that contains in each cell a 6x6 matrix decimal V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities // Each cell contains a 6x1 vector with linear and angular velocities - decimal Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities + Vector3* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities + Vector3* Wconstraint; decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques // Each cell contains a 6x1 vector with external force and torque. @@ -225,15 +226,15 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { // Return the constrained linear velocity of a body after solving the LCP problem inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * mMapBodyToIndex[body]; - return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]); + uint indexBodyArray = mMapBodyToIndex[body]; + return Vconstraint[indexBodyArray]; } // Return the constrained angular velocity of a body after solving the LCP problem inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * mMapBodyToIndex[body]; - return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]); + uint indexBodyArray = mMapBodyToIndex[body]; + return Wconstraint[indexBodyArray]; } // Cleanup of the constraint solver @@ -246,6 +247,14 @@ inline void ConstraintSolver::cleanup() { delete[] mContactConstraints; mContactConstraints = 0; } + if (Vconstraint != 0) { + delete[] Vconstraint; + Vconstraint = 0; + } + if (Wconstraint != 0) { + delete[] Wconstraint; + Wconstraint = 0; + } } // Set the number of iterations of the LCP solver