/// <summary>
/// Get the current joint translation, usually in meters.
/// </summary>
public float GetJointTranslation()
{
Body b1 = _body1;
Body b2 = _body2;
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);
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
public float GetJointSpeed()
{
Body b1 = _body1;
Body b2 = _body2;
Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = b2.GetTransform().TransformDirection(_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, axis.CrossScalarPreMultiply(w1)) + Vector2.Dot(axis, v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1));
return(speed);
}
/// <summary>
/// Get the current joint translation speed, usually in meters per second.
/// </summary>
public float GetJointSpeed()
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 r1 = Box2DNet.Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DNet.Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 d = p2 - p1;
Vec2 axis = b1.GetWorldVector(_localXAxis1);
Vec2 v1 = b1._linearVelocity;
Vec2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
float speed = Vec2.Dot(d, Vec2.Cross(w1, axis)) + Vec2.Dot(axis, v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1));
return(speed);
}