public Joint(Body body1, Body body2, Vector2 anchor)
{
Body1 = body1;
Body2 = body2;
Matrix2 rotation1 = new Matrix2(body1.Rotation);
Matrix2 rotation2 = new Matrix2(body2.Rotation);
Matrix2 rotation1T = rotation1.Transpose();
Matrix2 rotation2T = rotation2.Transpose();
LocalAnchor1 = Matrix2.Multiply(rotation1T, Vector2.Subtract(anchor, body1.Position));
LocalAnchor2 = Matrix2.Multiply(rotation2T, Vector2.Subtract(anchor, body2.Position));
// Set specific variables
BiasFactor = 0.2f;
}
public void Add(Body body)
{
bodies.Add(body);
}
