From c8d216aafe8b67d8c8ce5aca0926adad4dc3c40b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 21 Dec 2012 11:33:11 +0100 Subject: [PATCH] Use Vector3 inside the V1 array --- src/engine/ConstraintSolver.cpp | 41 ++++++++++++++++++--------------- src/engine/ConstraintSolver.h | 12 ++++++++-- 2 files changed, 33 insertions(+), 20 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 84a56705..97db82c7 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(100), mContactConstraints(0), Vconstraint(0), Wconstraint(0) { + :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), Vconstraint(0), Wconstraint(0), V1(0), W1(0) { } @@ -100,6 +100,8 @@ void ConstraintSolver::initialize() { Vconstraint = new Vector3[nbBodies]; Wconstraint = new Vector3[nbBodies]; + V1 = new Vector3[nbBodies]; + W1 = new Vector3[nbBodies]; assert(mMapBodyToIndex.size() == nbBodies); } @@ -120,15 +122,9 @@ void ConstraintSolver::initializeBodies() { assert(rigidBody); // Compute the vector V1 with initial velocities values - Vector3 linearVelocity = rigidBody->getLinearVelocity(); - Vector3 angularVelocity = rigidBody->getAngularVelocity(); int bodyIndexArray = 6 * bodyNumber; - V1[bodyIndexArray] = linearVelocity[0]; - V1[bodyIndexArray + 1] = linearVelocity[1]; - V1[bodyIndexArray + 2] = linearVelocity[2]; - V1[bodyIndexArray + 3] = angularVelocity[0]; - V1[bodyIndexArray + 4] = angularVelocity[1]; - V1[bodyIndexArray + 5] = angularVelocity[2]; + V1[bodyNumber] = rigidBody->getLinearVelocity(); + W1[bodyNumber] = rigidBody->getAngularVelocity(); // Compute the vector Vconstraint with final constraint velocities Vconstraint[bodyNumber] = Vector3(0, 0, 0); @@ -226,9 +222,12 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { decimal multiplication = 0.0; int body1ArrayIndex = 6 * indexBody1; int body2ArrayIndex = 6 * indexBody2; - for (uint i=0; i<6; i++) { - multiplication += contact.J_spBody1Penetration[i] * V1[body1ArrayIndex + i]; - multiplication += contact.J_spBody2Penetration[i] * V1[body2ArrayIndex + i]; + for (uint i=0; i<3; i++) { + multiplication += contact.J_spBody1Penetration[i] * V1[indexBody1][i]; + multiplication += contact.J_spBody1Penetration[i + 3] * W1[indexBody1][i]; + + multiplication += contact.J_spBody2Penetration[i] * V1[indexBody2][i]; + multiplication += contact.J_spBody2Penetration[i + 3] * W1[indexBody2][i]; } contact.b_Penetration -= multiplication * oneOverDT ; @@ -262,9 +261,12 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { // Substract 1.0/dt*J*V to the vector b multiplication = 0.0; - for (uint i=0; i<6; i++) { - multiplication += contact.J_spBody1Friction1[i] * V1[body1ArrayIndex + i]; - multiplication += contact.J_spBody2Friction1[i] * V1[body2ArrayIndex + i]; + for (uint i=0; i<3; i++) { + multiplication += contact.J_spBody1Friction1[i] * V1[indexBody1][i]; + multiplication += contact.J_spBody1Friction1[i + 3] * W1[indexBody1][i]; + + multiplication += contact.J_spBody2Friction1[i] * V1[indexBody2][i]; + multiplication += contact.J_spBody2Friction1[i + 3] * W1[indexBody2][i]; } contact.b_Friction1 -= multiplication * oneOverDT ; @@ -298,9 +300,12 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { // Substract 1.0/dt*J*V to the vector b multiplication = 0.0; - for (uint i=0; i<6; i++) { - multiplication += contact.J_spBody1Friction2[i] * V1[body1ArrayIndex + i]; - multiplication += contact.J_spBody2Friction2[i] * V1[body2ArrayIndex + i]; + for (uint i=0; i<3; i++) { + multiplication += contact.J_spBody1Friction2[i] * V1[indexBody1][i]; + multiplication += contact.J_spBody1Friction2[i + 3] * W1[indexBody1][i]; + + multiplication += contact.J_spBody2Friction2[i] * V1[indexBody2][i]; + multiplication += contact.J_spBody2Friction2[i + 3] * W1[indexBody2][i]; } contact.b_Friction2 -= multiplication * oneOverDT ; diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index fbbe0469..01fe55d6 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -177,8 +177,8 @@ class ConstraintSolver { Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix) decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass // 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 + Vector3* V1; // Array that contains for each body the 6x1 vector that contains linear and angular velocities + Vector3* W1; 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 @@ -255,6 +255,14 @@ inline void ConstraintSolver::cleanup() { delete[] Wconstraint; Wconstraint = 0; } + if (V1 != 0) { + delete[] V1; + V1 = 0; + } + if (W1 != 0) { + delete[] W1; + W1 = 0; + } } // Set the number of iterations of the LCP solver