Lemke and PGS Solvers in Bullet

Post Reply
pp57
Posts: 5
Joined: Tue Oct 18, 2016 11:41 am

Lemke and PGS Solvers in Bullet

Post by pp57 »

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:

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);
Any help would be greatly appreciated!
Thanks in advance!
pp57
Posts: 5
Joined: Tue Oct 18, 2016 11:41 am

Re: Lemke and PGS Solvers in Bullet

Post by pp57 »

Just to update: I think there might be a bug in Bullet's btMatrixX routines - for whatever reason, Bullet didn't recognize non-zero entries and skipped them during multiplication. I tested my matrices with Eigen and got the correct results. Not sure if there's something conflicting in my build or not, so might be useful for other users to know that the btMatrixX routines are a bit dodgy. Also, if anyone else is as confused as I was about the LCP problem, then the change in velocity that the presentation talks about seems to be J*V where J is the Jacobian and V the stacked velocity vector with values v = {v_lin0, w_ang0, v_lin1, w_ang1, ..., v_linN, w_linN} (see this, slide 49). This produces the same matrix as the normal direction velocity of Eberly's book. Also worth mentioning is that I incorrectly thought that these are the values of A and b (or M and q) for the LCP but Eberly shows how to transform these A and b to the necessary M and q on the very last page of the last chapter of his book. I believe the linked presentation also mentions this however the approach taken seems slightly different.
Post Reply