Lemke and PGS Solvers in Bullet
Posted: Thu Apr 27, 2017 4:38 pm
Hi all,
I first learned about LCP solvers from Eberly's Game Physics book for basic impulsive collision response (frictionless) and I've played around with his WildMagic5's LCP solver based on Lemke's algorithm. Now, I tried testing Bullet's Lemke and PGS as an alternative but I think the formulations for the matrix A and/or vector b are different between the two implementations - plugging in my matrix that worked with WM5 produces incorrect torques with Lemke (in Bullet) and the PGS solver (btSolveProjectedGaussSeidel) produces ludicrous linear impulses. What exactly should the matrix A and vector b be for Bullet's solvers (for basic contact impulsive collision response)? The b in Eberly's book is the magnitude of the relative velocity in the normal direction (n.dot(v_A - v_B)) but according to this b is taken as the change in relative velocity but I'm unsure how that would be computed? I've changed my computation of A to follow the linked presentation (A=JM_invJ_transpose), so I'm guessing my formulation for vector b is wrong. Here's how I'm constructing my inputs at the moment:
Any help would be greatly appreciated!
Thanks in advance!
I first learned about LCP solvers from Eberly's Game Physics book for basic impulsive collision response (frictionless) and I've played around with his WildMagic5's LCP solver based on Lemke's algorithm. Now, I tried testing Bullet's Lemke and PGS as an alternative but I think the formulations for the matrix A and/or vector b are different between the two implementations - plugging in my matrix that worked with WM5 produces incorrect torques with Lemke (in Bullet) and the PGS solver (btSolveProjectedGaussSeidel) produces ludicrous linear impulses. What exactly should the matrix A and vector b be for Bullet's solvers (for basic contact impulsive collision response)? The b in Eberly's book is the magnitude of the relative velocity in the normal direction (n.dot(v_A - v_B)) but according to this b is taken as the change in relative velocity but I'm unsure how that would be computed? I've changed my computation of A to follow the linked presentation (A=JM_invJ_transpose), so I'm guessing my formulation for vector b is wrong. Here's how I'm constructing my inputs at the moment:
Code: Select all
// construct the inverse mass matrix
btMatrixXu M_inv(6 * numBodies, 6 * numBodies);
M_inv.setZero();
auto Insert3x3 = [&M_inv](UINT entryIndex, btMatrix3x3 mat) {
for (UINT row = 0; row < 3; ++row) {
for (UINT col = 0; col < 3; ++col) {
M_inv.setElem(entryIndex * 3 + row, entryIndex * 3 + col, mat[row][col]);
}
}
};
// two 3x3 matrices per body
UINT bodyID = 0;
for (UINT i = 0; i < (2 * numBodies); i += 2) {
btMatrix3x3 invMass;
invMass.setIdentity();
invMass = invMass.scaled(btVector3(bodies[bodyID].body->getInvMass(), bodies[bodyID].body->getInvMass(), bodies[bodyID].body->getInvMass()));
Insert3x3(i, invMass);
btMatrix3x3 invI = bodies[bodyID].body->getInvInertiaTensorWorld();
Insert3x3(i + 1, invI);
++bodyID;
}
// construct the Jacobian matrix
btMatrixXu J(numContacts, 6 * numBodies);
J.setZero();
auto Insert1x6 = [&J](UINT row, UINT col, btVector3 n, btVector3 rxn) {
for (UINT i = 0; i < 3; ++i) {
J.setElem(row, col + 0 + i, n[i]);
J.setElem(row, col + 3 + i, rxn[i]);
}
};
for (UINT i = 0; i < numContacts; ++i) {
btVector3 n1 = contacts[i].N;
btVector3 rxn1 = contacts[i].r1xN;
btVector3 n2 = -contacts[i].N; // normal from B
btVector3 rxn2 = contacts[i].r2xN;
Insert1x6(i, bodies[i].index * 6, n1, rxn1); // add the first body involved
Insert1x6(i, bodies[numBodies - 1].index * 6, n2, rxn2); // simplified setup - all only on contact with fixed ground which is guaranteed to be last in the body list
}
// construct the b vector
btVectorXu b(numContacts);
b.setZero();
for (UINT i = 0; i < numContacts; ++i) {
btVector3 v_A = contacts[i].A->getLinearVelocity() + contacts[i].A->getAngularVelocity().cross(contacts[i].P - contacts[i].A->getWorldTransform().getOrigin());
btVector3 v_B = contacts[i].B->getLinearVelocity() + contacts[i].B->getAngularVelocity().cross(contacts[i].P - contacts[i].B->getWorldTransform().getOrigin());
b[i] = (v_A - v_B).dot(contacts[i].N);
}
// construct the lo and hi vectors
btVectorXu lo(numContacts);
btVectorXu hi(numContacts);
lo.setZero();
hi.setZero();
for (UINT i = 0; i < numContacts; ++i) {
lo[i] = 0;
hi[i] = BT_INFINITY;
}
btMatrixXu J_T = J.transpose();
btMatrixXu JM_inv = J * M_inv;
btMatrixXu A = JM_inv * J_T;
// modify the diagonal of A to avoid divisions by zero - tried modifying this constant without luck
for (UINT i = 0; i < numContacts; ++i) {
A.setElem(i, i, A(i, i) + 1e-6f);
}
// construct the solution vector
btVectorXu x(numContacts);
x.setZero();
btAlignedObjectArray<int> limitDependency;
limitDependency.resize(numContacts, 1);
btSolveProjectedGaussSeidel solver;
solver.solveMLCP(A, b, x, lo, hi, limitDependency, 10);
Thanks in advance!