/// Get the current joint translation, usually in meters.
public float GetJointTranslation()
{
var bA = BodyA;
var bB = BodyB;
var pA = bA.GetWorldPoint(_localAnchorA);
var pB = bB.GetWorldPoint(_localAnchorB);
var d = pB - pA;
var axis = bA.GetWorldVector(_localXAxisA);
var translation = Vector2.Dot(d, axis);
return(translation);
}
/// Get the current joint linear speed, usually in meters per second.
public float GetJointLinearSpeed()
{
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);
}
public GearJoint(GearJointDef def)
: base(def)
{
_joint1 = def.Joint1;
_joint2 = def.Joint2;
_typeA = _joint1.JointType;
_typeB = _joint2.JointType;
Debug.Assert(_typeA == JointType.RevoluteJoint || _typeA == JointType.PrismaticJoint);
Debug.Assert(_typeB == JointType.RevoluteJoint || _typeB == JointType.PrismaticJoint);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
_bodyC = _joint1.BodyA;
BodyA = _joint1.BodyB;
// Body B on joint1 must be dynamic
Debug.Assert(BodyA.BodyType == BodyType.DynamicBody);
// Get geometry of joint1
var xfA = BodyA.Transform;
var aA = BodyA.Sweep.A;
var xfC = _bodyC.Transform;
var aC = _bodyC.Sweep.A;
if (_typeA == JointType.RevoluteJoint)
{
var revolute = (RevoluteJoint)def.Joint1;
_localAnchorC = revolute.LocalAnchorA;
_localAnchorA = revolute.LocalAnchorB;
_referenceAngleA = revolute.ReferenceAngle;
_localAxisC.SetZero();
coordinateA = aA - aC - _referenceAngleA;
}
else
{
var prismatic = (PrismaticJoint)def.Joint1;
_localAnchorC = prismatic.LocalAnchorA;
_localAnchorA = prismatic.LocalAnchorB;
_referenceAngleA = prismatic.ReferenceAngle;
_localAxisC = prismatic.LocalXAxisA;
var pC = _localAnchorC;
var pA = MathUtils.MulT(
xfC.Rotation,
MathUtils.Mul(xfA.Rotation, _localAnchorA) + (xfA.Position - xfC.Position));
coordinateA = Vector2.Dot(pA - pC, _localAxisC);
}
_bodyD = _joint2.BodyA;
BodyB = _joint2.BodyB;
// Body B on joint2 must be dynamic
Debug.Assert(BodyB.BodyType == BodyType.DynamicBody);
// Get geometry of joint2
var xfB = BodyB.Transform;
var aB = BodyB.Sweep.A;
var xfD = _bodyD.Transform;
var aD = _bodyD.Sweep.A;
if (_typeB == JointType.RevoluteJoint)
{
var revolute = (RevoluteJoint)def.Joint2;
_localAnchorD = revolute.LocalAnchorA;
_localAnchorB = revolute.LocalAnchorB;
_referenceAngleB = revolute.ReferenceAngle;
_localAxisD.SetZero();
coordinateB = aB - aD - _referenceAngleB;
}
else
{
var prismatic = (PrismaticJoint)def.Joint2;
_localAnchorD = prismatic.LocalAnchorA;
_localAnchorB = prismatic.LocalAnchorB;
_referenceAngleB = prismatic.ReferenceAngle;
_localAxisD = prismatic.LocalXAxisA;
var pD = _localAnchorD;
var pB = MathUtils.MulT(
xfD.Rotation,
MathUtils.Mul(xfB.Rotation, _localAnchorB) + (xfB.Position - xfD.Position));
coordinateB = Vector2.Dot(pB - pD, _localAxisD);
}
_ratio = def.Ratio;
_constant = coordinateA + _ratio * coordinateB;
_impulse = 0.0f;
}
