internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); if (_state == LimitState.AtUpperLimit) { Vector2 v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity); Vector2 v2 = b2._linearVelocity + r1.CrossScalarPreMultiply(b2._angularVelocity); float Cdot = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2); float impulse = _pulleyMass * (-Cdot); float oldImpulse = _impulse; _impulse = Box2DX.Common.Math.Max(0.0f, _impulse + impulse); impulse = _impulse - oldImpulse; Vector2 P1 = -impulse * _u1; Vector2 P2 = -_ratio * impulse * _u2; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * r1.Cross(P1); b2._linearVelocity += b2._invMass * P2; b2._angularVelocity += b2._invI * r2.Cross(P2); } if (_limitState1 == LimitState.AtUpperLimit) { Vector2 v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity); float Cdot = -Vector2.Dot(_u1, v1); float impulse = -_limitMass1 * Cdot; float oldImpulse = _limitImpulse1; _limitImpulse1 = Box2DX.Common.Math.Max(0.0f, _limitImpulse1 + impulse); impulse = _limitImpulse1 - oldImpulse; Vector2 P1 = -impulse * _u1; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * r1.Cross(P1); } if (_limitState2 == LimitState.AtUpperLimit) { Vector2 v2 = b2._linearVelocity + r2.CrossScalarPreMultiply(b2._angularVelocity); float Cdot = -Vector2.Dot(_u2, v2); float impulse = -_limitMass2 * Cdot; float oldImpulse = _limitImpulse2; _limitImpulse2 = Box2DX.Common.Math.Max(0.0f, _limitImpulse2 + impulse); impulse = _limitImpulse2 - oldImpulse; Vector2 P2 = -impulse * _u2; b2._linearVelocity += b2._invMass * P2; b2._angularVelocity += b2._invI * r2.Cross(P2); } }
internal override void SolveVelocityConstraints(TimeStep step) { //B2_NOT_USED(step); Body b1 = _body1; Body b2 = _body2; Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); // Cdot = dot(u, v + cross(w, r)) Vector2 v1 = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity); Vector2 v2 = b2._linearVelocity + r2.CrossScalarPreMultiply(b2._angularVelocity); float Cdot = Vector2.Dot(_u, v2 - v1); float impulse = -_mass * (Cdot + _bias + _gamma * _impulse); _impulse += impulse; Vector2 P = impulse * _u; b1._linearVelocity -= b1._invMass * P; b1._angularVelocity -= b1._invI * r1.Cross(P); b2._linearVelocity += b2._invMass * P; b2._angularVelocity += b2._invI * r2.Cross(P); }
/// <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); }
public LineJoint(LineJointDef def) : base(def) { _localAnchor1 = def.localAnchor1; _localAnchor2 = def.localAnchor2; _localXAxis1 = def.localAxis1; _localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f); _impulse = Vector2.Zero; _motorMass = 0.0f; _motorImpulse = 0.0f; _lowerTranslation = def.lowerTranslation; _upperTranslation = def.upperTranslation; _maxMotorForce = Settings.FORCE_INV_SCALE(def.maxMotorForce); _motorSpeed = def.motorSpeed; _enableLimit = def.enableLimit; _enableMotor = def.enableMotor; _limitState = LimitState.InactiveLimit; _axis = Vector2.Zero; _perp = Vector2.Zero; }
public PrismaticJoint(PrismaticJointDef def) : base(def) { _localAnchor1 = def.LocalAnchor1; _localAnchor2 = def.LocalAnchor2; _localXAxis1 = def.LocalAxis1; _localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f); _refAngle = def.ReferenceAngle; _impulse = Vector3.Zero; _motorMass = 0.0f; _motorImpulse = 0.0f; _lowerTranslation = def.LowerTranslation; _upperTranslation = def.UpperTranslation; _maxMotorForce = Settings.FORCE_INV_SCALE(def.MaxMotorForce); _motorSpeed = def.MotorSpeed; _enableLimit = def.EnableLimit; _enableMotor = def.EnableMotor; _limitState = LimitState.InactiveLimit; _axis = Vector2.Zero; _perp = Vector2.Zero; }
internal override void SolveVelocityConstraints(TimeStep step) { Body b = _body2; Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter()); // Cdot = v + cross(w, r) Vector2 Cdot = b._linearVelocity + r.CrossScalarPreMultiply(b._angularVelocity); Vector2 impulse = _mass.Multiply(-(Cdot + _beta * _C + _gamma * _impulse)); Vector2 oldImpulse = _impulse; _impulse += impulse; float maxImpulse = step.Dt * _maxForce; if (_impulse.LengthSquared() > maxImpulse * maxImpulse) { _impulse *= maxImpulse / _impulse.Length(); } impulse = _impulse - oldImpulse; b._linearVelocity += b._invMass * impulse; b._angularVelocity += b._invI * r.Cross(impulse); }
internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vector2 v1 = b1._linearVelocity; float w1 = b1._angularVelocity; Vector2 v2 = b2._linearVelocity; float w2 = b2._angularVelocity; float m1 = b1._invMass, m2 = b2._invMass; float i1 = b1._invI, i2 = b2._invI; //Solve motor constraint. if (_enableMotor && _limitState != LimitState.EqualLimits) { float Cdot = w2 - w1 - _motorSpeed; float impulse = _motorMass * (-Cdot); float oldImpulse = _motorImpulse; float maxImpulse = step.Dt * _maxMotorTorque; _motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse); impulse = _motorImpulse - oldImpulse; w1 -= i1 * impulse; w2 += i2 * impulse; } //Solve limit constraint. if (_enableLimit && _limitState != LimitState.InactiveLimit) { Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); // Solve point-to-point constraint Vector2 Cdot1 = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1); float Cdot2 = w2 - w1; Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); Vector3 impulse = _mass.Solve33(-Cdot); if (_limitState == LimitState.EqualLimits) { _impulse += impulse; } else if (_limitState == LimitState.AtLowerLimit) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse < 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } else if (_limitState == LimitState.AtUpperLimit) { float newImpulse = _impulse.Z + impulse.Z; if (newImpulse > 0.0f) { Vector2 reduced = _mass.Solve22(-Cdot1); impulse.X = reduced.X; impulse.Y = reduced.Y; impulse.Z = -_impulse.Z; _impulse.X += reduced.X; _impulse.Y += reduced.Y; _impulse.Z = 0.0f; } } Vector2 P = impulse.ToVector2(); v1 -= m1 * P; w1 -= i1 * (r1.Cross(P) + impulse.Z); v2 += m2 * P; w2 += i2 * (r2.Cross(P) + impulse.Z); } else { Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); // Solve point-to-point constraint Vector2 Cdot = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1); Vector2 impulse = _mass.Solve22(-Cdot); _impulse.X += impulse.X; _impulse.Y += impulse.Y; v1 -= m1 * impulse; w1 -= i1 * r1.Cross(impulse); v2 += m2 * impulse; w2 += i2 * r2.Cross(impulse); } b1._linearVelocity = v1; b1._angularVelocity = w1; b2._linearVelocity = v2; b2._angularVelocity = w2; }
public PrismaticJoint(PrismaticJointDef def) : base(def) { _localAnchor1 = def.LocalAnchor1; _localAnchor2 = def.LocalAnchor2; _localXAxis1 = def.LocalAxis1; _localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f); _refAngle = def.ReferenceAngle; _impulse = Vector3.zero; _motorMass = 0.0f; _motorImpulse = 0.0f; _lowerTranslation = def.LowerTranslation; _upperTranslation = def.UpperTranslation; _maxMotorForce = Settings.FORCE_INV_SCALE(def.MaxMotorForce); _motorSpeed = def.MotorSpeed; _enableLimit = def.EnableLimit; _enableMotor = def.EnableMotor; _limitState = LimitState.InactiveLimit; _axis = Vector2.zero; _perp= Vector2.zero; }
public LineJoint(LineJointDef def) : base(def) { _localAnchor1 = def.localAnchor1; _localAnchor2 = def.localAnchor2; _localXAxis1 = def.localAxis1; _localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f); _impulse = Vector2.zero; _motorMass = 0.0f; _motorImpulse = 0.0f; _lowerTranslation = def.lowerTranslation; _upperTranslation = def.upperTranslation; _maxMotorForce = Settings.FORCE_INV_SCALE(def.maxMotorForce); _motorSpeed = def.motorSpeed; _enableLimit = def.enableLimit; _enableMotor = def.enableMotor; _limitState = LimitState.InactiveLimit; _axis = Vector2.zero; _perp = Vector2.zero; }