/// <summary> /// Solves the velocity constraints using the specified step /// </summary> /// <param name="step">The step</param> internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = Body1; Body b2 = Body2; Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, LocalAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, LocalAnchor2 - b2.GetLocalCenter()); if (State == LimitState.AtUpperLimit) { Vec2 v1 = b1.LinearVelocity + Vec2.Cross(b1.AngularVelocity, r1); Vec2 v2 = b2.LinearVelocity + Vec2.Cross(b2.AngularVelocity, r2); float cdot = -Vec2.Dot(U1, v1) - Ratio * Vec2.Dot(U2, v2); float impulse = PulleyMass * -cdot; float oldImpulse = Impulse; Impulse = Box2DXMath.Max(0.0f, Impulse + impulse); impulse = Impulse - oldImpulse; Vec2 p1 = -impulse * U1; Vec2 p2 = -Ratio * impulse * U2; b1.LinearVelocity += b1.InvMass * p1; b1.AngularVelocity += b1.InvI * Vec2.Cross(r1, p1); b2.LinearVelocity += b2.InvMass * p2; b2.AngularVelocity += b2.InvI * Vec2.Cross(r2, p2); } if (LimitState1 == LimitState.AtUpperLimit) { Vec2 v1 = b1.LinearVelocity + Vec2.Cross(b1.AngularVelocity, r1); float cdot = -Vec2.Dot(U1, v1); float impulse = -LimitMass1 * cdot; float oldImpulse = LimitImpulse1; LimitImpulse1 = Box2DXMath.Max(0.0f, LimitImpulse1 + impulse); impulse = LimitImpulse1 - oldImpulse; Vec2 p1 = -impulse * U1; b1.LinearVelocity += b1.InvMass * p1; b1.AngularVelocity += b1.InvI * Vec2.Cross(r1, p1); } if (LimitState2 == LimitState.AtUpperLimit) { Vec2 v2 = b2.LinearVelocity + Vec2.Cross(b2.AngularVelocity, r2); float cdot = -Vec2.Dot(U2, v2); float impulse = -LimitMass2 * cdot; float oldImpulse = LimitImpulse2; LimitImpulse2 = Box2DXMath.Max(0.0f, LimitImpulse2 + impulse); impulse = LimitImpulse2 - oldImpulse; Vec2 p2 = -impulse * U2; b2.LinearVelocity += b2.InvMass * p2; b2.AngularVelocity += b2.InvI * Vec2.Cross(r2, p2); } }
internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); if (_state == LimitState.AtUpperLimit) { Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1); Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2); float Cdot = -Vec2.Dot(_u1, v1) - _ratio * Vec2.Dot(_u2, v2); float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _pulleyMass * Cdot; float oldForce = _force; _force = Box2DXMath.Max(0.0f, _force + force); force = _force - oldForce; Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1; Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * _ratio * force * _u2; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1); b2._linearVelocity += b2._invMass * P2; b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2); } if (_limitState1 == LimitState.AtUpperLimit) { Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1); float Cdot = -Vec2.Dot(_u1, v1); float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass1 * Cdot; float oldForce = _limitForce1; _limitForce1 = Box2DXMath.Max(0.0f, _limitForce1 + force); force = _limitForce1 - oldForce; Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1; b1._linearVelocity += b1._invMass * P1; b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1); } if (_limitState2 == LimitState.AtUpperLimit) { Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2); float Cdot = -Vec2.Dot(_u2, v2); float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass2 * Cdot; float oldForce = _limitForce2; _limitForce2 = Box2DXMath.Max(0.0f, _limitForce2 + force); force = _limitForce2 - oldForce; Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * force * _u2; b2._linearVelocity += b2._invMass * P2; b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2); } }
public override void Step(Settings settings) { //B2_NOT_USED(settings); if (_automated == true) { int actionCount = Box2DXMath.Max(1, k_actorCount >> 2); for (int i = 0; i < actionCount; ++i) { Action(); } } _broadPhase.Commit(); for (int i = 0; i < k_actorCount; ++i) { Actor actor = _actors[i]; if (actor.proxyId == PairManager.NullProxy) { continue; } Color c = new Color(); switch (actor.overlapCount) { case 0: c.R = 0.9f; c.G = 0.9f; c.B = 0.9f; break; case 1: c.R = 0.6f; c.G = 0.9f; c.B = 0.6f; break; default: c.R = 0.9f; c.G = 0.6f; c.B = 0.6f; break; } OpenGLDebugDraw.DrawAABB(actor.aabb, c); } StringBuilder strBld = new StringBuilder(); strBld.AppendFormat("overlaps = {0}, exact = {1}, diff = {2}", new object[] { _overlapCount, _overlapCountExact, _overlapCount - _overlapCountExact }); OpenGLDebugDraw.DrawString(5, 30, strBld.ToString()); Validate(); ++_stepCount; }
/// <summary> /// Describes whether this instance solve position constraints /// </summary> /// <param name="baumgarte">The baumgarte</param> /// <returns>The bool</returns> internal override bool SolvePositionConstraints(float baumgarte) { Body body1 = Body1; Body body2 = Body2; Vec2 groundAnchor1 = Ground.GetXForm().Position + GroundAnchor1; Vec2 groundAnchor2 = Ground.GetXForm().Position + GroundAnchor2; float linearError = 0.0f; if (State == LimitState.AtUpperLimit) { Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter()); Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter()); Vec2 body1SweepC = body1.Sweep.C + mulR1; Vec2 body2SweepC = body2.Sweep.C + mulR2; // Get the pulley axes. U1 = body1SweepC - groundAnchor1; U2 = body2SweepC - groundAnchor2; float length1 = U1.Length(); float length2 = U2.Length(); if (length1 > Settings.LinearSlop) { U1 *= 1.0f / length1; } else { U1.SetZero(); } if (length2 > Settings.LinearSlop) { U2 *= 1.0f / length2; } else { U2.SetZero(); } float c = Constant - length1 - Ratio * length2; linearError = Box2DXMath.Max(linearError, -c); c = Box2DXMath.Clamp(c + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -PulleyMass * c; Vec2 p1 = -impulse * U1; Vec2 p2 = -Ratio * impulse * U2; body1.Sweep.C += body1.InvMass * p1; body1.Sweep.A += body1.InvI * Vec2.Cross(mulR1, p1); body2.Sweep.C += body2.InvMass * p2; body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, p2); body1.SynchronizeTransform(); body2.SynchronizeTransform(); } if (LimitState1 == LimitState.AtUpperLimit) { Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter()); Vec2 body1SweepC = body1.Sweep.C + mulR1; U1 = body1SweepC - groundAnchor1; float length1 = U1.Length(); if (length1 > Settings.LinearSlop) { U1 *= 1.0f / length1; } else { U1.SetZero(); } float c = MaxLength1 - length1; linearError = Box2DXMath.Max(linearError, -c); c = Box2DXMath.Clamp(c + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -LimitMass1 * c; Vec2 p1 = -impulse * U1; body1.Sweep.C += body1.InvMass * p1; body1.Sweep.A += body1.InvI * Vec2.Cross(mulR1, p1); body1.SynchronizeTransform(); } if (LimitState2 == LimitState.AtUpperLimit) { Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter()); Vec2 body2SweepC = body2.Sweep.C + mulR2; U2 = body2SweepC - groundAnchor2; float length2 = U2.Length(); if (length2 > Settings.LinearSlop) { U2 *= 1.0f / length2; } else { U2.SetZero(); } float c = MaxLength2 - length2; linearError = Box2DXMath.Max(linearError, -c); c = Box2DXMath.Clamp(c + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -LimitMass2 * c; Vec2 p2 = -impulse * U2; body2.Sweep.C += body2.InvMass * p2; body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, p2); body2.SynchronizeTransform(); } return(linearError < Settings.LinearSlop); }
/// <summary> /// Describes whether this instance solve position constraints /// </summary> /// <param name="baumgarte">The baumgarte</param> /// <returns>The bool</returns> internal override bool SolvePositionConstraints(float baumgarte) { Body body1 = Body1; Body body2 = Body2; Vec2 body1SweepC = body1.Sweep.C; float body1SweepA = body1.Sweep.A; Vec2 body2SweepC = body2.Sweep.C; float body2SweepA = body2.Sweep.A; // Solve linear limit constraint. var linearError = 0.0f; float angularError; bool active = false; float c2 = 0.0f; var mat22R1 = new Mat22(body1SweepA); var mat22R2 = new Mat22(body2SweepA); Vec2 r1 = Box2DXMath.Mul(mat22R1, LocalAnchor1 - LocalCenter1); Vec2 r2 = Box2DXMath.Mul(mat22R2, LocalAnchor2 - LocalCenter2); Vec2 distance = body2SweepC + r2 - body1SweepC - r1; if (IsLimitEnabled) { Axis = Box2DXMath.Mul(mat22R1, LocalXAxis1); a1 = Vec2.Cross(distance + r1, Axis); A2 = Vec2.Cross(r2, Axis); float translation = Vec2.Dot(Axis, distance); if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections c2 = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Box2DXMath.Abs(translation); active = true; } else if (translation <= LowerLimit) { // Prevent large linear corrections and allow some slop. c2 = Box2DXMath.Clamp(translation - LowerLimit + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = LowerLimit - translation; active = true; } else if (translation >= UpperLimit) { // Prevent large linear corrections and allow some slop. c2 = Box2DXMath.Clamp(translation - UpperLimit - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = translation - UpperLimit; active = true; } } Perp = Box2DXMath.Mul(mat22R1, LocalYAxis1); s1 = Vec2.Cross(distance + r1, Perp); s2 = Vec2.Cross(r2, Perp); Vec3 impulse; Vec2 c1 = new Vec2(); c1.X = Vec2.Dot(Perp, distance); c1.Y = body2SweepA - body1SweepA - refAngle; linearError = Box2DXMath.Max(linearError, Box2DXMath.Abs(c1.X)); angularError = Box2DXMath.Abs(c1.Y); if (active) { float m1 = InvMass1, m2 = InvMass2; float i1 = InvI1, i2 = InvI2; float k11 = m1 + m2 + i1 * s1 * s1 + i2 * s2 * s2; float k12 = i1 * s1 + i2 * s2; float k13 = i1 * s1 * a1 + i2 * s2 * A2; float k22 = i1 + i2; float k23 = i1 * a1 + i2 * A2; float k33 = m1 + m2 + i1 * a1 * a1 + i2 * A2 * A2; K.Col1.Set(k11, k12, k13); K.Col2.Set(k12, k22, k23); K.Col3.Set(k13, k23, k33); Vec3 c = new Vec3(); c.X = c1.X; c.Y = c1.Y; c.Z = c2; impulse = K.Solve33(-c); } else { float m1 = InvMass1, m2 = InvMass2; float i1 = InvI1, i2 = InvI2; float k11 = m1 + m2 + i1 * s1 * s1 + i2 * s2 * s2; float k12 = i1 * s1 + i2 * s2; float k22 = i1 + i2; K.Col1.Set(k11, k12, 0.0f); K.Col2.Set(k12, k22, 0.0f); Vec2 impulse1 = K.Solve22(-c1); impulse.X = impulse1.X; impulse.Y = impulse1.Y; impulse.Z = 0.0f; } Vec2 p = impulse.X * Perp + impulse.Z * Axis; float l1 = impulse.X * s1 + impulse.Y + impulse.Z * a1; float l2 = impulse.X * s2 + impulse.Y + impulse.Z * A2; body1SweepC -= InvMass1 * p; body1SweepA -= InvI1 * l1; body2SweepC += InvMass2 * p; body2SweepA += InvI2 * l2; // TODO_ERIN remove need for this. body1.Sweep.C = body1SweepC; body1.Sweep.A = body1SweepA; body2.Sweep.C = body2SweepC; body2.Sweep.A = body2SweepA; body1.SynchronizeTransform(); body2.SynchronizeTransform(); return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
/// <summary> /// Solves the velocity constraints using the specified step /// </summary> /// <param name="step">The step</param> internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = Body1; Body b2 = Body2; Vec2 v1 = b1.LinearVelocity; float w1 = b1.AngularVelocity; Vec2 v2 = b2.LinearVelocity; float w2 = b2.AngularVelocity; // Solve linear motor constraint. if (IsMotorEnabled && LimitState != LimitState.EqualLimits) { float cdot = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1; float impulse = MotorMass * (motorSpeedx - cdot); float oldImpulse = MotorForce; float maxImpulse = step.Dt * MaxMotorForce; MotorForce = Box2DXMath.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse); impulse = MotorForce - oldImpulse; Vec2 p = impulse * Axis; float l1 = impulse * a1; float l2 = impulse * A2; v1 -= InvMass1 * p; w1 -= InvI1 * l1; v2 += InvMass2 * p; w2 += InvI2 * l2; } Vec2 cdot1; cdot1.X = Vec2.Dot(Perp, v2 - v1) + s2 * w2 - s1 * w1; cdot1.Y = w2 - w1; if (IsLimitEnabled && LimitState != LimitState.InactiveLimit) { // Solve prismatic and limit constraint in block form. float cdot2; cdot2 = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1; Vec3 cdot = new Vec3(cdot1.X, cdot1.Y, cdot2); Vec3 f1 = Impulse; Vec3 df = K.Solve33(-cdot); Impulse += df; if (LimitState == LimitState.AtLowerLimit) { Impulse.Z = Box2DXMath.Max(Impulse.Z, 0.0f); } else if (LimitState == LimitState.AtUpperLimit) { Impulse.Z = Box2DXMath.Min(Impulse.Z, 0.0f); } // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2) Vec2 b = -cdot1 - (Impulse.Z - f1.Z) * new Vec2(K.Col3.X, K.Col3.Y); Vec2 f2R = K.Solve22(b) + new Vec2(f1.X, f1.Y); Impulse.X = f2R.X; Impulse.Y = f2R.Y; df = Impulse - f1; Vec2 p = df.X * Perp + df.Z * Axis; float l1 = df.X * s1 + df.Y + df.Z * a1; float l2 = df.X * s2 + df.Y + df.Z * A2; v1 -= InvMass1 * p; w1 -= InvI1 * l1; v2 += InvMass2 * p; w2 += InvI2 * l2; } else { // Limit is inactive, just solve the prismatic constraint in block form. Vec2 df = K.Solve22(-cdot1); Impulse.X += df.X; Impulse.Y += df.Y; Vec2 p = df.X * Perp; float l1 = df.X * s1 + df.Y; float l2 = df.X * s2 + df.Y; v1 -= InvMass1 * p; w1 -= InvI1 * l1; v2 += InvMass2 * p; w2 += InvI2 * l2; } b1.LinearVelocity = v1; b1.AngularVelocity = w1; b2.LinearVelocity = v2; b2.AngularVelocity = w2; }
internal override bool SolvePositionConstraints(float baumgarte) { Body b1 = _body1; Body b2 = _body2; Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1; Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2; float linearError = 0.0f; if (_state == LimitState.AtUpperLimit) { Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vec2 p1 = b1._sweep.C + r1; Vec2 p2 = b2._sweep.C + r2; // Get the pulley axes. _u1 = p1 - s1; _u2 = p2 - s2; float length1 = _u1.Length(); float length2 = _u2.Length(); if (length1 > Settings.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1.SetZero(); } if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2.SetZero(); } float C = _constant - length1 - _ratio * length2; linearError = Box2DXMath.Max(linearError, -C); C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_pulleyMass * C; Vec2 P1 = -impulse * _u1; Vec2 P2 = -_ratio * impulse * _u2; b1._sweep.C += b1._invMass * P1; b1._sweep.A += b1._invI * Vec2.Cross(r1, P1); b2._sweep.C += b2._invMass * P2; b2._sweep.A += b2._invI * Vec2.Cross(r2, P2); b1.SynchronizeTransform(); b2.SynchronizeTransform(); } if (_limitState1 == LimitState.AtUpperLimit) { Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 p1 = b1._sweep.C + r1; _u1 = p1 - s1; float length1 = _u1.Length(); if (length1 > Settings.LinearSlop) { _u1 *= 1.0f / length1; } else { _u1.SetZero(); } float C = _maxLength1 - length1; linearError = Box2DXMath.Max(linearError, -C); C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass1 * C; Vec2 P1 = -impulse * _u1; b1._sweep.C += b1._invMass * P1; b1._sweep.A += b1._invI * Vec2.Cross(r1, P1); b1.SynchronizeTransform(); } if (_limitState2 == LimitState.AtUpperLimit) { Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vec2 p2 = b2._sweep.C + r2; _u2 = p2 - s2; float length2 = _u2.Length(); if (length2 > Settings.LinearSlop) { _u2 *= 1.0f / length2; } else { _u2.SetZero(); } float C = _maxLength2 - length2; linearError = Box2DXMath.Max(linearError, -C); C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); float impulse = -_limitMass2 * C; Vec2 P2 = -impulse * _u2; b2._sweep.C += b2._invMass * P2; b2._sweep.A += b2._invI * Vec2.Cross(r2, P2); b2.SynchronizeTransform(); } return(linearError < Settings.LinearSlop); }
internal override bool SolvePositionConstraints() { Body b1 = _body1; Body b2 = _body2; float invMass1 = b1._invMass, invMass2 = b2._invMass; float invI1 = b1._invI, invI2 = b2._invI; Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vec2 p1 = b1._sweep.C + r1; Vec2 p2 = b2._sweep.C + r2; Vec2 d = p2 - p1; Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1); // Solve linear (point-to-line) constraint. float linearC = Vec2.Dot(ay1, d); // Prevent overly large corrections. linearC = Box2DXMath.Clamp(linearC, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); float linearImpulse = -_linearMass * linearC; b1._sweep.C += (invMass1 * linearImpulse) * _linearJacobian.Linear1; b1._sweep.A += invI1 * linearImpulse * _linearJacobian.Angular1; //b1->SynchronizeTransform(); // updated by angular constraint b2._sweep.C += (invMass2 * linearImpulse) * _linearJacobian.Linear2; b2._sweep.A += invI2 * linearImpulse * _linearJacobian.Angular2; //b2->SynchronizeTransform(); // updated by angular constraint float positionError = Box2DXMath.Abs(linearC); // Solve angular constraint. float angularC = b2._sweep.A - b1._sweep.A - _refAngle; // Prevent overly large corrections. angularC = Box2DXMath.Clamp(angularC, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); float angularImpulse = -_angularMass * angularC; b1._sweep.A -= b1._invI * angularImpulse; b2._sweep.A += b2._invI * angularImpulse; b1.SynchronizeTransform(); b2.SynchronizeTransform(); float angularError = Box2DXMath.Abs(angularC); // Solve linear limit constraint. if (_enableLimit && _limitState != LimitState.InactiveLimit) { Vec2 r1_ = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2_ = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vec2 p1_ = b1._sweep.C + r1_; Vec2 p2_ = b2._sweep.C + r2_; Vec2 d_ = p2_ - p1_; Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1); float translation = Vec2.Dot(ax1, d_); float limitImpulse = 0.0f; if (_limitState == LimitState.EqualLimits) { // Prevent large angular corrections float limitC = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); limitImpulse = -_motorMass * limitC; positionError = Box2DXMath.Max(positionError, Box2DXMath.Abs(angularC)); } else if (_limitState == LimitState.AtLowerLimit) { float limitC = translation - _lowerTranslation; positionError = Box2DXMath.Max(positionError, -limitC); // Prevent large linear corrections and allow some slop. limitC = Box2DXMath.Clamp(limitC + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); limitImpulse = -_motorMass * limitC; float oldLimitImpulse = _limitPositionImpulse; _limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f); limitImpulse = _limitPositionImpulse - oldLimitImpulse; } else if (_limitState == LimitState.AtUpperLimit) { float limitC = translation - _upperTranslation; positionError = Box2DXMath.Max(positionError, limitC); // Prevent large linear corrections and allow some slop. limitC = Box2DXMath.Clamp(limitC - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); limitImpulse = -_motorMass * limitC; float oldLimitImpulse = _limitPositionImpulse; _limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f); limitImpulse = _limitPositionImpulse - oldLimitImpulse; } b1._sweep.C += (invMass1 * limitImpulse) * _motorJacobian.Linear1; b1._sweep.A += invI1 * limitImpulse * _motorJacobian.Angular1; b2._sweep.C += (invMass2 * limitImpulse) * _motorJacobian.Linear2; b2._sweep.A += invI2 * limitImpulse * _motorJacobian.Angular2; b1.SynchronizeTransform(); b2.SynchronizeTransform(); } return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; float invMass1 = b1._invMass, invMass2 = b2._invMass; float invI1 = b1._invI, invI2 = b2._invI; // Solve linear constraint. float linearCdot = _linearJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity); float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _linearMass * linearCdot; _force += force; float P = Settings.FORCE_SCALE(step.Dt) * force; b1._linearVelocity += (invMass1 * P) * _linearJacobian.Linear1; b1._angularVelocity += invI1 * P * _linearJacobian.Angular1; b2._linearVelocity += (invMass2 * P) * _linearJacobian.Linear2; b2._angularVelocity += invI2 * P * _linearJacobian.Angular2; // Solve angular constraint. float angularCdot = b2._angularVelocity - b1._angularVelocity; float torque = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _angularMass * angularCdot; _torque += torque; float L = Settings.FORCE_SCALE(step.Dt) * torque; b1._angularVelocity -= invI1 * L; b2._angularVelocity += invI2 * L; // Solve linear motor constraint. if (_enableMotor && _limitState != LimitState.EqualLimits) { float motorCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity) - _motorSpeed; float motorForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * motorCdot; float oldMotorForce = _motorForce; _motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorForce, _maxMotorForce); motorForce = _motorForce - oldMotorForce; float P_ = Settings.FORCE_SCALE(step.Dt) * motorForce; b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1; b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1; b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2; b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2; } // Solve linear limit constraint. if (_enableLimit && _limitState != LimitState.InactiveLimit) { float limitCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity); float limitForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * limitCdot; if (_limitState == LimitState.EqualLimits) { _limitForce += limitForce; } else if (_limitState == LimitState.AtLowerLimit) { float oldLimitForce = _limitForce; _limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f); limitForce = _limitForce - oldLimitForce; } else if (_limitState == LimitState.AtUpperLimit) { float oldLimitForce = _limitForce; _limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f); limitForce = _limitForce - oldLimitForce; } float P_ = Settings.FORCE_SCALE(step.Dt) * limitForce; b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1; b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1; b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2; b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2; } }
internal override bool SolvePositionConstraints() { Body b1 = _body1; Body b2 = _body2; float positionError = 0.0f; // Solve point-to-point position error. Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); Vec2 p1 = b1._sweep.C + r1; Vec2 p2 = b2._sweep.C + r2; Vec2 ptpC = p2 - p1; positionError = ptpC.Length(); // Prevent overly large corrections. //b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection); //ptpC = b2Clamp(ptpC, -dpMax, dpMax); float invMass1 = b1._invMass, invMass2 = b2._invMass; float invI1 = b1._invI, invI2 = b2._invI; Mat22 K1 = new Mat22(); K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f; K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2; Mat22 K2 = new Mat22(); K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y; K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X; Mat22 K3 = new Mat22(); K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y; K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X; Mat22 K = K1 + K2 + K3; Vec2 impulse = K.Solve(-ptpC); b1._sweep.C -= b1._invMass * impulse; b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse); b2._sweep.C += b2._invMass * impulse; b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse); b1.SynchronizeTransform(); b2.SynchronizeTransform(); // Handle limits. float angularError = 0.0f; if (_enableLimit && _limitState != LimitState.InactiveLimit) { float angle = b2._sweep.A - b1._sweep.A - _referenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.EqualLimits) { // Prevent large angular corrections float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * limitC; angularError = Box2DXMath.Abs(limitC); } else if (_limitState == LimitState.AtLowerLimit) { float limitC = angle - _lowerAngle; angularError = Box2DXMath.Max(0.0f, -limitC); // Prevent large angular corrections and allow some slop. limitC = Box2DXMath.Clamp(limitC + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * limitC; float oldLimitImpulse = _limitPositionImpulse; _limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f); limitImpulse = _limitPositionImpulse - oldLimitImpulse; } else if (_limitState == LimitState.AtUpperLimit) { float limitC = angle - _upperAngle; angularError = Box2DXMath.Max(0.0f, limitC); // Prevent large angular corrections and allow some slop. limitC = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * limitC; float oldLimitImpulse = _limitPositionImpulse; _limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f); limitImpulse = _limitPositionImpulse - oldLimitImpulse; } b1._sweep.A -= b1._invI * limitImpulse; b2._sweep.A += b2._invI * limitImpulse; b1.SynchronizeTransform(); b2.SynchronizeTransform(); } return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override void SolveVelocityConstraints(TimeStep step) { Body b1 = _body1; Body b2 = _body2; Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter()); // Solve point-to-point constraint Vec2 pivotCdot = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2) - b1._linearVelocity - Vec2.Cross(b1._angularVelocity, r1); Vec2 pivotForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Box2DXMath.Mul(_pivotMass, pivotCdot); #if B2_TOI_JOINTS if (step.WarmStarting) { _pivotForce += pivotForce; _lastWarmStartingPivotForce = _pivotForce; } else { _pivotForce = _lastWarmStartingPivotForce; //Do not update warm starting value! } #else _pivotForce += pivotForce; #endif Vec2 P = Settings.FORCE_SCALE(step.Dt) * pivotForce; b1._linearVelocity -= b1._invMass * P; b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P); b2._linearVelocity += b2._invMass * P; b2._angularVelocity += b2._invI * Vec2.Cross(r2, P); if (_enableMotor && _limitState != LimitState.EqualLimits) { float motorCdot = b2._angularVelocity - b1._angularVelocity - _motorSpeed; float motorForce = -step.Inv_Dt * _motorMass * motorCdot; float oldMotorForce = _motorForce; _motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorTorque, _maxMotorTorque); motorForce = _motorForce - oldMotorForce; float P_ = step.Dt * motorForce; b1._angularVelocity -= b1._invI * P_; b2._angularVelocity += b2._invI * P_; } if (_enableLimit && _limitState != LimitState.InactiveLimit) { float limitCdot = b2._angularVelocity - b1._angularVelocity; float limitForce = -step.Inv_Dt * _motorMass * limitCdot; if (_limitState == LimitState.EqualLimits) { _limitForce += limitForce; } else if (_limitState == LimitState.AtLowerLimit) { float oldLimitForce = _limitForce; _limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f); limitForce = _limitForce - oldLimitForce; } else if (_limitState == LimitState.AtUpperLimit) { float oldLimitForce = _limitForce; _limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f); limitForce = _limitForce - oldLimitForce; } float P_ = step.Dt * limitForce; b1._angularVelocity -= b1._invI * P_; b2._angularVelocity += b2._invI * P_; } }