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

Use vectors in the Vconstraint array

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

44
src/engine/ConstraintSolver.cpp
View File

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

19
src/engine/ConstraintSolver.h
View File

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