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); }