I figured it out partially, but it doesn't work for masses other than 1.0
Can you please tell me how to fix this?
Mx J = new Mx(2, 4);
Mx Jdot = new Mx(2, 4);
Mx M = new Mx(4);
vector Fext = new vector(4);
vector qdot = new vector(4);
vector lambda;
M.Zero();
M.data[0, 0] = 1.0 / tp.mass;
M.data[1, 1] = 1.0 / tp.mass;
M.data[2, 2] = 1.0 / tp3.mass;
M.data[3, 3] = 1.0 / tp3.mass;
Fext.zero();
Fext.data[0] = 0;
Fext.data[1] = body.g;
Fext.data[2] = 0;
Fext.data[3] = body.g;
qdot.zero();
qdot.data[0] = tp.velocity.x;
qdot.data[1] = tp.velocity.y;
qdot.data[2] = tp3.velocity.x;
qdot.data[3] = tp3.velocity.y;
Jdot.Zero();
Jdot.data[0, 0] = tp.velocity.x;
Jdot.data[0, 1] = tp.velocity.y;
Jdot.data[1, 0] = tp.velocity.x - tp3.velocity.x;
Jdot.data[1, 1] = tp.velocity.y - tp3.velocity.y;
Jdot.data[1, 2] = tp3.velocity.x - tp.velocity.x;
Jdot.data[1, 3] = tp3.velocity.y - tp.velocity.y;
J.Zero();
J.data[0, 0] = tp.position.x;
J.data[0, 1] = tp.position.y;
J.data[1, 0] = tp.position.x - tp3.position.x;
J.data[1, 1] = tp.position.y - tp3.position.y;
J.data[1, 2] = tp3.position.x - tp.position.x;
J.data[1, 3] = tp3.position.y - tp.position.y;
lambda = Mx.Invert(J * M * !J) * (-Jdot * qdot - J * M * Fext);
var fvec = !J * lambda;