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

Use Vector3 inside the V1 array

Browse Source
This commit is contained in:
Daniel Chappuis 2012-12-21 11:33:11 +01:00
parent d6b3b18aee
commit c8d216aafe
2 changed files with 33 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/engine/ConstraintSolver.cpp
View File

@ -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 ;

12
src/engine/ConstraintSolver.h
View File

@ -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