internal PrismaticJoint(PrismaticJointDef def)
: base(def)
{
LocalAnchorA = def.LocalAnchorA;
LocalAnchorB = def.LocalAnchorB;
LocalXAxisA = def.LocalAxisA;
LocalXAxisA.Normalize();
LocalYAxisA = MathUtils.Cross(1.0f, LocalXAxisA);
ReferenceAngle = def.ReferenceAngle;
_impulse.SetZero();
_axialMass = 0.0f;
_motorImpulse = 0.0f;
_lowerImpulse = 0.0f;
_upperImpulse = 0.0f;
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
Debug.Assert(_lowerTranslation <= _upperTranslation);
_maxMotorForce = def.MaxMotorForce;
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_translation = 0.0f;
_axis.SetZero();
_perp.SetZero();
}
/// 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);
}
internal WeldJoint(WeldJointDef def)
: base(def)
{
_localAnchorA = def.LocalAnchorA;
_localAnchorB = def.LocalAnchorB;
_referenceAngle = def.ReferenceAngle;
Damping = def.Damping;
Stiffness = def.Stiffness;
_impulse.SetZero();
}
/// 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);
}
/// <inheritdoc />
public override Vector2 GetReactionForce(float inv_dt)
{
var P = new Vector2(_impulse.x, _impulse.y);
return(inv_dt * P);
}
