*
* @param invDT The amount of time the simulation is being stepped by
*/
public void preStep(float invDT) {
// Pre-compute anchors, mass matrix, and bias.
Matrix2f rot1 = new Matrix2f(body1.getRotation());
Matrix2f rot2 = new Matrix2f(body2.getRotation());
r1 = MathUtil.mul(rot1,localAnchor1);
r2 = MathUtil.mul(rot2,localAnchor2);
// deltaV = deltaV0 + K * impulse
// invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
Matrix2f K1 = new Matrix2f();
K1.col1.x = body1.getInvMass() + body2.getInvMass(); K1.col2.x = 0.0f;
K1.col1.y = 0.0f; K1.col2.y = body1.getInvMass() + body2.getInvMass();
Matrix2f K2 = new Matrix2f();
K2.col1.x = body1.getInvI() * r1.y * r1.y; K2.col2.x = -body1.getInvI() * r1.x * r1.y;
K2.col1.y = -body1.getInvI() * r1.x * r1.y; K2.col2.y = body1.getInvI() * r1.x * r1.x;
Matrix2f K3 = new Matrix2f();
K3.col1.x = body2.getInvI() * r2.y * r2.y; K3.col2.x = -body2.getInvI() * r2.x * r2.y;
K3.col1.y = -body2.getInvI() * r2.x * r2.y; K3.col2.y = body2.getInvI() * r2.x * r2.x;
Matrix2f K = MathUtil.add(MathUtil.add(K1,K2),K3);
M = K.invert();
Vector2f p1 = new Vector2f(body1.getPosition());
p1.add(r1);
Vector2f p2 = new Vector2f(body2.getPosition());
p2.add(r2);