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

Start to compute the inverse mass matrix K for the friction constraints

Browse Source
This commit is contained in:
Daniel Chappuis 2012-12-26 02:15:49 +01:00
parent 3872e9615c
commit e64cac4571
1 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/engine/ConstraintSolver.cpp
View File

@ -101,6 +101,8 @@ void ConstraintSolver::initialize() {
contactPointConstraint.normal = contact->getNormal(); contactPointConstraint.normal = contact->getNormal();
contactPointConstraint.r1 = p1 - x1; contactPointConstraint.r1 = p1 - x1;
contactPointConstraint.r2 = p2 - x2; contactPointConstraint.r2 = p2 - x2;
contactPointConstraint.frictionVector1 = contact->getFrictionVector1();
contactPointConstraint.frictionVector2 = contact->getFrictionVector2();
} }
mNbContactConstraints++; mNbContactConstraints++;
@ -201,6 +203,18 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
((I2 * contact.r2CrossN).cross(contact.r2)).dot(contact.normal); ((I2 * contact.r2CrossN).cross(contact.r2)).dot(contact.normal);
} }
// Compute the inverse mass matrix K for the friction constraints
contact.inverseFriction1Mass = 0.0;
contact.inverseFriction2Mass = 0.0;
if (constraint.isBody1Moving) {
contact.inverseFriction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1);
contact.inverseFriction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2);
}
if (constraint.isBody2Moving) {
contact.inverseFriction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1);
contact.inverseFriction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2);
}
// Fill in the J_sp matrix // Fill in the J_sp matrix
realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration); realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration);
realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1); realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1);
@ -568,6 +582,9 @@ void ConstraintSolver::solveLCP() {
// --------- Friction 1 --------- // // --------- Friction 1 --------- //
//std::cout << "contact.b_Friction1 : " << contact.b_Friction1 << std::endl;
//std::cout << "contact.inverseFriction1Mass : " << contact.inverseFriction1Mass << std::endl;
deltaLambda = contact.b_Friction1; deltaLambda = contact.b_Friction1;
for (uint j=0; j<6; j++) { for (uint j=0; j<6; j++) {
deltaLambda -= (contact.J_spBody1Friction1[j] * a[indexBody1Array + j] + contact.J_spBody2Friction1[j] * a[indexBody2Array + j]); deltaLambda -= (contact.J_spBody1Friction1[j] * a[indexBody1Array + j] + contact.J_spBody2Friction1[j] * a[indexBody2Array + j]);