/// Get the current joint translation speed, usually in meters per second.
public float GetJointSpeed()
{
var bA = BodyA;
var bB = BodyB;
var rA = MathUtils.Mul(bA.Transform.Rotation, LocalAnchorA - bA.Sweep.LocalCenter);
var rB = MathUtils.Mul(bB.Transform.Rotation, LocalAnchorB - bB.Sweep.LocalCenter);
var p1 = bA.Sweep.C + rA;
var p2 = bB.Sweep.C + rB;
var d = p2 - p1;
var axis = MathUtils.Mul(bA.Transform.Rotation, LocalXAxisA);
var vA = bA.LinearVelocity;
var vB = bB.LinearVelocity;
var wA = bA.AngularVelocity;
var wB = bB.AngularVelocity;
var speed = Vector2.Dot(d, MathUtils.Cross(wA, axis))
+ Vector2.Dot(axis, vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA));
return(speed);
}
