public static Vector2 Multiply(Matrix2 A, Vector2 V)
{
float x = A.M11 * V.X + A.M21 * V.Y;
float y = A.M12 * V.X + A.M22 * V.Y;
return new Vector2(x, y);
}
// Multiply Matrices
public static Matrix2 Multiply(Matrix2 A, Matrix2 B)
{
float m11 = A.M11 * B.M11 + A.M21 * B.M12;
float m12 = A.M12 * B.M11 + A.M22 * B.M12;
float m21 = A.M11 * B.M21 + A.M21 * B.M22;
float m22 = A.M12 * B.M21 + A.M22 * B.M22;
return new Matrix2(m11, m21, m12, m22);
}
public static Matrix2 Add(Matrix2 A, float C)
{
float m11 = A.M11 + C;
float m12 = A.M12 + C;
float m21 = A.M21 + C;
float m22 = A.M22 + C;
return new Matrix2(m11, m21, m12, m22);
}
public static Matrix2 Add(Matrix2 A, Matrix2 B)
{
float m11 = A.M11 + B.M11;
float m12 = A.M12 + B.M12;
float m21 = A.M21 + B.M21;
float m22 = A.M22 + B.M22;
return new Matrix2(m11, m21, m12, m22);
}
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;
}
// Invert matrix values
public Matrix2 Invert()
{
Matrix2 returnMatrix = new Matrix2();
float det = M11 * M22 - M21 * M12;
det = 1.0f / det;
returnMatrix.M11 = det * M22;
returnMatrix.M21 = -det * M21;
returnMatrix.M12 = -det * M12;
returnMatrix.M22 = det * M11;
return returnMatrix;
}
public void Prestep(float inverseDt)
{
Matrix2 rotation1 = new Matrix2(Body1.Rotation);
Matrix2 rotation2 = new Matrix2(Body2.Rotation);
R1 = Matrix2.Multiply(rotation1, LocalAnchor1);
R2 = Matrix2.Multiply(rotation2, LocalAnchor2);
// Incoming loads of math
Matrix2 k1 = new Matrix2
(
Body1.InverseMass + Body2.InverseMass, 0f,
0f, Body1.InverseMass + Body2.InverseMass
);
Matrix2 k2 = new Matrix2
(
Body1.InverseInertia * R1.Y * R1.Y, -Body1.InverseInertia * R1.X * R1.Y,
-Body1.InverseInertia * R1.X * R1.Y, Body1.InverseInertia * R1.X * R1.X
);
Matrix2 k3 = new Matrix2
(
Body1.InverseInertia * R2.Y * R2.Y, -Body1.InverseInertia * R2.X * R2.Y,
-Body1.InverseInertia * R2.X * R2.Y, Body1.InverseInertia * R2.X * R2.X
);
// Add together the matrices
Matrix2 K = Matrix2.Add(k1, Matrix2.Add(k2, k3));
K = Matrix2.Add(K, Softness);
// Set it back to the main Matrix
M = K.Invert();
// Position correction?
Vector2 p1 = Vector2.Add(Body1.Position, R1);
Vector2 p2 = Vector2.Add(Body2.Position, R2);
Vector2 dp = p2 - p1;
if (World.PositionCorrection)
{
Bias = -BiasFactor * inverseDt * dp;
}
else
{
Bias = Vector2.Zero;
}
// Impulse accumulation
if (World.WarmStarting)
{
Body1.Velocity = Vector2.Subtract(Body1.Velocity, Vector2.Multiply(AccumulatedImpulses, Body1.InverseMass));
Body1.AngularVelocity = Body1.AngularVelocity - Body1.InverseInertia * Cross(R1, AccumulatedImpulses);
Body2.Velocity = Vector2.Add(Body2.Velocity, Vector2.Multiply(AccumulatedImpulses, Body2.InverseMass));
Body2.AngularVelocity = Body2.AngularVelocity + Body2.InverseInertia * Cross(R2, AccumulatedImpulses);
}
else
{
AccumulatedImpulses = Vector2.Zero;
}
}