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); }