/// Get the current joint translation, usually in meters.
public float GetJointTranslation()
{
var pA = BodyA.GetWorldPoint(LocalAnchorA);
var pB = BodyB.GetWorldPoint(LocalAnchorB);
var d = pB - pA;
var axis = BodyA.GetWorldVector(LocalXAxisA);
var translation = Vector2.Dot(d, axis);
return(translation);
}
/// 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);
}