/// Get the current joint translation speed, usually in meters per second. public float GetJointSpeed() { Body b1 = _bodyA; Body b2 = _bodyB; XForm xf1, xf2; b1.GetXForm(out xf1); b2.GetXForm(out xf2); Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter()); Vector2 p1 = b1._sweep.c + r1; Vector2 p2 = b2._sweep.c + r2; Vector2 d = p2 - p1; Vector2 axis = b1.GetWorldVector(_localXAxis1); Vector2 v1 = b1._linearVelocity; Vector2 v2 = b2._linearVelocity; float w1 = b1._angularVelocity; float w2 = b2._angularVelocity; float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1)); return(speed); }
/// Get the current joint translation, usually in meters. public float GetJointTranslation() { Body b1 = _bodyA; Body b2 = _bodyB; Vector2 p1 = b1.GetWorldPoint(_localAnchor1); Vector2 p2 = b2.GetWorldPoint(_localAnchor2); Vector2 d = p2 - p1; Vector2 axis = b1.GetWorldVector(_localXAxis1); float translation = Vector2.Dot(d, axis); return(translation); }