public bool SolvePositionConstraints(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < _constraintCount; ++i) { ContactConstraint c = _constraints[i]; Body b1 = c.Body1; Body b2 = c.Body2; float invMass1 = b1._mass * b1._invMass; float invI1 = b1._mass * b1._invI; float invMass2 = b2._mass * b2._invMass; float invI2 = b2._mass * b2._invI; Vec2 normal = c.Normal; // Solver normal constraints for (int j = 0; j < c.PointCount; ++j) { ContactConstraintPoint ccp = c.Points[j]; Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, ccp.LocalAnchor1 - b1.GetLocalCenter()); Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, ccp.LocalAnchor2 - b2.GetLocalCenter()); Vec2 p1 = b1._sweep.C + r1; Vec2 p2 = b2._sweep.C + r2; Vec2 dp = p2 - p1; // Approximate the current separation. float separation = Vec2.Dot(dp, normal) + ccp.Separation; // Track max constraint error. minSeparation = Common.Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = baumgarte * Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); // Compute normal impulse float impulse = -ccp.EqualizedMass * C; Vec2 P = impulse * normal; b1._sweep.C -= invMass1 * P; b1._sweep.A -= invI1 * Vec2.Cross(r1, P); b1.SynchronizeTransform(); b2._sweep.C += invMass2 * P; b2._sweep.A += invI2 * Vec2.Cross(r2, P); b2.SynchronizeTransform(); } } // We can't expect minSpeparation >= -Settings.LinearSlop because we don't // push the separation above -Settings.LinearSlop. return(minSeparation >= -1.5f * Settings.LinearSlop); }
// Sequential solver. public bool SolvePositionConstraints(float baumgarte) { float minSeparation = 0.0f; for (int i = 0; i < ConstraintCount; ++i) { ContactConstraint c = Constraints[i]; Body bodyA = c.BodyA; Body bodyB = c.BodyB; float invMassA = bodyA._mass * bodyA._invMass; float invIA = bodyA._mass * bodyA._invI; float invMassB = bodyB._mass * bodyB._invMass; float invIB = bodyB._mass * bodyB._invI; PositionSolverManifold psm = new PositionSolverManifold(); psm.Initialize(c); Vec2 normal = psm.Normal; // Solve normal constraints for (int j = 0; j < c.PointCount; ++j) { ContactConstraintPoint ccp = c.Points[j]; Vec2 point = psm.Points[j]; float separation = psm.Separations[j]; Vec2 rA = point - bodyA._sweep.C; Vec2 rB = point - bodyB._sweep.C; // Track max constraint error. minSeparation = Common.Math.Min(minSeparation, separation); // Prevent large corrections and allow slop. float C = Common.Math.Clamp(baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f); // Compute normal impulse float impulse = -ccp.EqualizedMass * C; Vec2 P = impulse * normal; bodyA._sweep.C -= invMassA * P; bodyA._sweep.A -= invIA * Vec2.Cross(rA, P); bodyA.SynchronizeTransform(); bodyB._sweep.C += invMassB * P; bodyB._sweep.A += invIB * Vec2.Cross(rB, P); bodyB.SynchronizeTransform(); } } // We can't expect minSpeparation >= -b2_linearSlop because we don't // push the separation above -b2_linearSlop. return(minSeparation >= -1.5f * Settings.LinearSlop); }
internal override bool SolvePositionConstraints(float baumgarte) { //B2_NOT_USED(baumgarte); float linearError = 0.0f; Body b1 = _bodyA; Body b2 = _bodyB; float coordinate1, coordinate2; if (_revolute1 != null) { coordinate1 = _revolute1.JointAngle; } else { coordinate1 = _prismatic1.JointTranslation; } if (_revolute2 != null) { coordinate2 = _revolute2.JointAngle; } else { coordinate2 = _prismatic2.JointTranslation; } float C = _constant - (coordinate1 + _ratio * coordinate2); float impulse = _mass * (-C); b1._sweep.C += b1._invMass * impulse * _J.Linear1; b1._sweep.A += b1._invI * impulse * _J.Angular1; b2._sweep.C += b2._invMass * impulse * _J.Linear2; b2._sweep.A += b2._invI * impulse * _J.Angular2; b1.SynchronizeTransform(); b2.SynchronizeTransform(); //TODO_ERIN not implemented return(linearError < Settings.LinearSlop); }
internal override bool SolvePositionConstraints(float baumgarte) { float num = 0f; Body body = this._body1; Body body2 = this._body2; float num2; if (this._revolute1 != null) { num2 = this._revolute1.JointAngle; } else { num2 = this._prismatic1.JointTranslation; } float num3; if (this._revolute2 != null) { num3 = this._revolute2.JointAngle; } else { num3 = this._prismatic2.JointTranslation; } float num4 = this._constant - (num2 + this._ratio * num3); float num5 = this._mass * -num4; Body expr_97_cp_0 = body; expr_97_cp_0._sweep.C = expr_97_cp_0._sweep.C + body._invMass * num5 * this._J.Linear1; Body expr_C6_cp_0 = body; expr_C6_cp_0._sweep.A = expr_C6_cp_0._sweep.A + body._invI * num5 * this._J.Angular1; Body expr_ED_cp_0 = body2; expr_ED_cp_0._sweep.C = expr_ED_cp_0._sweep.C + body2._invMass * num5 * this._J.Linear2; Body expr_11C_cp_0 = body2; expr_11C_cp_0._sweep.A = expr_11C_cp_0._sweep.A + body2._invI * num5 * this._J.Angular2; body.SynchronizeTransform(); body2.SynchronizeTransform(); return(num < Settings.LinearSlop); }
public bool SolvePositionConstraints(float baumgarte) { float num = 0f; for (int i = 0; i < this._constraintCount; i++) { ContactConstraint contactConstraint = this._constraints[i]; Body body = contactConstraint.Body1; Body body2 = contactConstraint.Body2; float a = body._mass * body._invMass; float num2 = body._mass * body._invI; float a2 = body2._mass * body2._invMass; float num3 = body2._mass * body2._invI; Vec2 normal = contactConstraint.Normal; for (int j = 0; j < contactConstraint.PointCount; j++) { ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j]; Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, contactConstraintPoint.LocalAnchor1 - body.GetLocalCenter()); Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, contactConstraintPoint.LocalAnchor2 - body2.GetLocalCenter()); Vec2 v = body._sweep.C + vec; Vec2 v2 = body2._sweep.C + vec2; Vec2 a3 = v2 - v; float num4 = Vec2.Dot(a3, normal) + contactConstraintPoint.Separation; num = Box2DX.Common.Math.Min(num, num4); float num5 = baumgarte * Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f); float a4 = -contactConstraintPoint.EqualizedMass * num5; Vec2 vec3 = a4 * normal; Body expr_157_cp_0 = body; expr_157_cp_0._sweep.C = expr_157_cp_0._sweep.C - a * vec3; Body expr_176_cp_0 = body; expr_176_cp_0._sweep.A = expr_176_cp_0._sweep.A - num2 * Vec2.Cross(vec, vec3); body.SynchronizeTransform(); Body expr_19C_cp_0 = body2; expr_19C_cp_0._sweep.C = expr_19C_cp_0._sweep.C + a2 * vec3; Body expr_1BC_cp_0 = body2; expr_1BC_cp_0._sweep.A = expr_1BC_cp_0._sweep.A + num3 * Vec2.Cross(vec2, vec3); body2.SynchronizeTransform(); } } return(num >= -1.5f * Settings.LinearSlop); }
internal override bool SolvePositionConstraints() { float linearError = 0.0f; Body b1 = _body1; Body b2 = _body2; float coordinate1, coordinate2; if (_revolute1 != null) { coordinate1 = _revolute1.JointAngle; } else { coordinate1 = _prismatic1.JointTranslation; } if (_revolute2 != null) { coordinate2 = _revolute2.JointAngle; } else { coordinate2 = _prismatic2.JointTranslation; } float C = _constant - (coordinate1 + _ratio * coordinate2); float impulse = -_mass * C; b1._sweep.C += b1._invMass * impulse * _J.Linear1; b1._sweep.A += b1._invI * impulse * _J.Angular1; b2._sweep.C += b2._invMass * impulse * _J.Linear2; b2._sweep.A += b2._invI * impulse * _J.Angular2; b1.SynchronizeTransform(); b2.SynchronizeTransform(); return(linearError < Settings.LinearSlop); }
internal override bool SolvePositionConstraints(float baumgarte) { if (_frequencyHz > 0.0f) { //There is no possition correction for soft distace constraint. return(true); } Body b1 = _body1; Body b2 = _body2; Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1; float length = d.magnitude; d.Normalize(); float C = length - _length; C = Mathf.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); float impulse = -_mass * C; _u = d; Vector2 P = impulse * _u; b1._sweep.C -= b1._invMass * P; b1._sweep.A -= b1._invI * r1.Cross(P); b2._sweep.C += b2._invMass * P; b2._sweep.A += b2._invI * r2.Cross(P); b1.SynchronizeTransform(); b2.SynchronizeTransform(); return(System.Math.Abs(C) < Settings.LinearSlop); }
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep) { // Integrate velocities and apply damping. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.IsStatic()) { continue; } // Integrate velocities. b._linearVelocity += step.Dt * (gravity + b._invMass * b._force); b._angularVelocity += step.Dt * b._invI * b._torque; // Reset forces. b._force.Set(0.0f, 0.0f); b._torque = 0.0f; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f); b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f); // Check for large velocities. #if TARGET_FLOAT32_IS_FIXED // Fixed point code written this way to prevent // overflows, float code is optimized for speed float vMagnitude = b._linearVelocity.Length(); if (vMagnitude > Settings.MaxLinearVelocity) { b._linearVelocity *= Settings.MaxLinearVelocity / vMagnitude; } b._angularVelocity = Vector2.Clamp(b._angularVelocity, -Settings.MaxAngularVelocity, Settings.MaxAngularVelocity); #else if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared) { b._linearVelocity.Normalize(); b._linearVelocity *= Settings.MaxLinearVelocity; } if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared) { if (b._angularVelocity < 0.0f) { b._angularVelocity = -Settings.MaxAngularVelocity; } else { b._angularVelocity = Settings.MaxAngularVelocity; } } #endif } ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount); // Initialize velocity constraints. contactSolver.InitVelocityConstraints(step); for (int i = 0; i < _jointCount; ++i) { _joints[i].InitVelocityConstraints(step); } // Solve velocity constraints. for (int i = 0; i < step.VelocityIterations; ++i) { for (int j = 0; j < _jointCount; ++j) { _joints[j].SolveVelocityConstraints(step); } contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). contactSolver.FinalizeVelocityConstraints(); // Integrate positions. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.IsStatic()) { continue; } // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; // Integrate b._sweep.C += step.Dt * b._linearVelocity; b._sweep.A += step.Dt * b._angularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int ii = 0; ii < step.PositionIterations; ++ii) { bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); bool jointsOkay = true; for (int i = 0; i < _jointCount; ++i) { bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } Report(contactSolver._constraints); if (allowSleep) { float minSleepTime = Common.Settings.FLT_MAX; #if !TARGET_FLOAT32_IS_FIXED float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; #endif for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b._invMass == 0.0f) { continue; } if ((b._flags & Body.BodyFlags.AllowSleep) == 0) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } if ((b._flags & Body.BodyFlags.AllowSleep) == 0 || #if TARGET_FLOAT32_IS_FIXED Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance || Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance || Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance) #else b._angularVelocity *b._angularVelocity > angTolSqr || Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr) #endif { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += step.Dt; minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime); } }
internal override bool SolvePositionConstraints(float baumgarte) { Body b1 = _body1; Body b2 = _body2; Vector2 c1 = b1._sweep.C; float a1 = b1._sweep.A; Vector2 c2 = b2._sweep.C; float a2 = b2._sweep.A; // Solve linear limit constraint. float linearError = 0.0f, angularError = 0.0f; bool active = false; float C2 = 0.0f; Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2); Vector2 r1 = R1.Multiply(_localAnchor1 - _localCenter1); Vector2 r2 = R2.Multiply(_localAnchor2 - _localCenter2); Vector2 d = c2 + r2 - c1 - r1; if (_enableLimit) { _axis = R1.Multiply(_localXAxis1); _a1 = (d + r1).Cross(_axis); _a2 = r2.Cross(_axis); float translation = Vector2.Dot(_axis, d); if (System.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Box2DX.Common.Math.Abs(translation); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = _lowerTranslation - translation; active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = translation - _upperTranslation; active = true; } } _perp = R1.Multiply(_localYAxis1); _s1 = (d + r1).Cross(_perp); _s2 = r2.Cross(_perp); Vector2 impulse; float C1; C1 = Vector2.Dot(_perp, d); linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1)); angularError = 0.0f; 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 * _a1 + i2 * _s2 * _a2; float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2; _K.Col1 = new Vector2(k11, k12); _K.Col2 = new Vector2(k12, k22); Vector2 C = new Vector2(); C.X = C1; C.Y = C2; impulse = _K.Solve(-C); } else { float m1 = _invMass1, m2 = _invMass2; float i1 = _invI1, i2 = _invI2; float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2; float impulse1 = (-C1) / k11; impulse.X = impulse1; impulse.Y = 0.0f; } Vector2 P = impulse.X * _perp + impulse.Y * _axis; float L1 = impulse.X * _s1 + impulse.Y * _a1; float L2 = impulse.X * _s2 + impulse.Y * _a2; c1 -= _invMass1 * P; a1 -= _invI1 * L1; c2 += _invMass2 * P; a2 += _invI2 * L2; // TODO_ERIN remove need for this. b1._sweep.C = c1; b1._sweep.A = a1; b2._sweep.C = c2; b2._sweep.A = a2; b1.SynchronizeTransform(); b2.SynchronizeTransform(); return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
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 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); }
// Find TOI contacts and solve them. private void SolveTOI(TimeStep step) { // Reserve an island and a queue for TOI island solution. Island island = new Island(_bodyCount, Settings.MaxTOIContactsPerIsland, Settings.MaxTOIJointsPerIsland, _contactManager._contactListener); //Simple one pass queue //Relies on the fact that we're only making one pass //through and each body can only be pushed/popped once. //To push: // queue[queueStart+queueSize++] = newElement; //To pop: // poppedElement = queue[queueStart++]; // --queueSize; int queueCapacity = _bodyCount; Body[] queue = new Body[queueCapacity]; for (Body b = _bodyList; b != null; b = b._next) { b._flags &= ~Body.BodyFlags.Island; b._sweep.T0 = 0.0f; } for (Contact c = _contactManager._contactList; c != null; c = c.Next) { // Invalidate TOI c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag); } for (Joint j = _jointList; j != null; j = j._next) { j._islandFlag = false; } // Find TOI events and solve them. for (; ;) { // Find the first TOI. Contact minContact = null; float minTOI = 1.0f; for (Contact c = _contactManager._contactList; c != null; c = c.Next) { // Can this contact generate a solid TOI contact? if (c.IsSolid() == false || c.IsContinuous() == false) { continue; } // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact. float toi = 1.0f; if ((c.Flags & ContactFlag.ToiFlag) != 0) { // This contact has a valid cached TOI. toi = c.Toi; } else { // Compute the TOI for this contact. Fixture s1 = c.GetFixtureA(); Fixture s2 = c.GetFixtureB(); Body b1 = s1.GetBody(); Body b2 = s2.GetBody(); if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping())) { continue; } // Put the sweeps onto the same time interval. float t0 = b1._sweep.T0; if (b1._sweep.T0 < b2._sweep.T0) { t0 = b2._sweep.T0; b1._sweep.Advance(t0); } else if (b2._sweep.T0 < b1._sweep.T0) { t0 = b1._sweep.T0; b2._sweep.Advance(t0); } Box2DXDebug.Assert(t0 < 1.0f); // Compute the time of impact. toi = c.ComputeTOI(b1._sweep, b2._sweep); Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f); // If the TOI is in range ... if (0.0f < toi && toi < 1.0f) { // Interpolate on the actual range. toi = Math.Min((1.0f - toi) * t0 + toi, 1.0f); } c.Toi = toi; c.Flags |= ContactFlag.ToiFlag; } if (Settings.FLT_EPSILON < toi && toi < minTOI) { // This is the minimum TOI found so far. minContact = c; minTOI = toi; } } if (minContact == null || 1.0f - 100.0f * Settings.FLT_EPSILON < minTOI) { // No more TOI events. Done! break; } // Advance the bodies to the TOI. Fixture f1 = minContact.GetFixtureA(); Fixture f2 = minContact.GetFixtureB(); Body b3 = f1.GetBody(); Body b4 = f2.GetBody(); Sweep backup1 = b3._sweep; Sweep backup2 = b4._sweep; b3.Advance(minTOI); b4.Advance(minTOI); // The TOI contact likely has some new contact points. minContact.Update(_contactManager._contactListener); minContact.Flags &= ~ContactFlag.ToiFlag; // Is the contact solid? if (minContact.IsSolid() == false) { // Restore the sweeps. b3._sweep = backup1; b4._sweep = backup2; b3.SynchronizeTransform(); b4.SynchronizeTransform(); continue; } // Did numerical issues prevent a contact point from being generated? if (minContact.IsTouching() == false) { // Give up on this TOI. continue; } // Build the TOI island. We need a dynamic seed. Body seed = b3; if (seed.IsStatic()) { seed = b4; } // Reset island and queue. island.Clear(); int queueStart = 0; // starting index for queue int queueSize = 0; // elements in queue queue[queueStart + queueSize++] = seed; seed._flags |= Body.BodyFlags.Island; // Perform a breadth first search (BFS) on the contact/joint graph. while (queueSize > 0) { // Grab the next body off the stack and add it to the island. Body b = queue[queueStart++]; --queueSize; island.Add(ref b); // Make sure the body is awake. b._flags &= ~Body.BodyFlags.Sleep; // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.IsStatic()) { continue; } // Search all contacts connected to this body. for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next) { // Does the TOI island still have space for contacts? if (island.ContactCount == island.ContactCapacity) { break; } // Has this contact already been added to an island? Skip slow or non-solid contacts. if ((cEdge.Contact.Flags & ContactFlag.IslandFlag) != 0) { continue; } // Is this contact touching? For performance we are not updating this contact. if (cEdge.Contact.IsSolid() == false || cEdge.Contact.IsTouching() == false) { continue; } island.Add(ref cEdge.Contact); cEdge.Contact.Flags |= ContactFlag.IslandFlag; // Update other body. Body other = cEdge.Other; // Was the other body already added to this island? if ((other._flags & Body.BodyFlags.Island) != 0) { continue; } // March forward, this can do no harm since this is the min TOI. if (other.IsStatic() == false) { other.Advance(minTOI); other.WakeUp(); } Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); queue[queueStart + queueSize] = other; ++queueSize; other._flags |= Body.BodyFlags.Island; } for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next) { if (island.JointCount == island.JointCapacity) { continue; } if (jEdge.Joint._islandFlag == true) { continue; } island.Add(jEdge.Joint); jEdge.Joint._islandFlag = true; Body other = jEdge.Other; if ((other._flags & Body.BodyFlags.Island) != 0) { continue; } if (!other.IsStatic()) { other.Advance(minTOI); other.WakeUp(); } Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); queue[queueStart + queueSize] = other; ++queueSize; other._flags |= Body.BodyFlags.Island; } } TimeStep subStep; subStep.WarmStarting = false; subStep.Dt = (1.0f - minTOI) * step.Dt; subStep.Inv_Dt = 1.0f / subStep.Dt; subStep.DtRatio = 0.0f; subStep.VelocityIterations = step.VelocityIterations; subStep.PositionIterations = step.PositionIterations; island.SolveTOI(ref subStep); // Post solve cleanup. for (int i = 0; i < island.BodyCount; ++i) { // Allow bodies to participate in future TOI islands. Body b = island.Bodies[i]; b._flags &= ~Body.BodyFlags.Island; if ((b._flags & Body.BodyFlags.Sleep) != 0) { continue; } if (b.IsStatic()) { continue; } b.SynchronizeFixtures(); // Invalidate all contact TOIs associated with this body. Some of these // may not be in the island because they were not touching. for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next) { ce.Contact.Flags &= ~ContactFlag.ToiFlag; } } for (int i = 0; i < island.ContactCount; ++i) { // Allow contacts to participate in future TOI islands. Contact c = island.Contacts[i]; c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag); } for (int i = 0; i < island.JointCount; ++i) { // Allow joints to participate in future TOI islands. Joint j = island.Joints[i]; j._islandFlag = false; } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. _contactManager.FindNewContacts(); } queue = null; }
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep) { // Integrate velocities and apply damping. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.IsStatic()) { continue; } // Integrate velocities. b._linearVelocity += step.Dt * (gravity + b._invMass * b._force); b._angularVelocity += step.Dt * b._invI * b._torque; // Reset forces. b._force.Set(0.0f, 0.0f); b._torque = 0.0f; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f); b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f); } ContactSolver contactSolver = new ContactSolver(step, Contacts, ContactCount); // Initialize velocity constraints. contactSolver.InitVelocityConstraints(step); for (int i = 0; i < JointCount; ++i) { Joints[i].InitVelocityConstraints(step); } // Solve velocity constraints. for (int i = 0; i < step.VelocityIterations; ++i) { for (int j = 0; j < JointCount; ++j) { Joints[j].SolveVelocityConstraints(step); } contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). contactSolver.FinalizeVelocityConstraints(); // Integrate positions. for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b.IsStatic()) { continue; } // Check for large velocities. Vec2 translation = step.Dt * b._linearVelocity; if (Vec2.Dot(translation, translation) > Settings.MaxTranslationSquared) { translation.Normalize(); b._linearVelocity = (Settings.MaxTranslation * step.Inv_Dt) * translation; } float rotation = step.Dt * Bodies[i]._angularVelocity; if (rotation * rotation > Settings.MaxRotationSquared) { if (rotation < 0.0) { b._angularVelocity = -step.Inv_Dt * Settings.MaxRotation; } else { b._angularVelocity = step.Inv_Dt * Settings.MaxRotation; } } // Store positions for continuous collision. b._sweep.C0 = b._sweep.C; b._sweep.A0 = b._sweep.A; // Integrate b._sweep.C += step.Dt * b._linearVelocity; b._sweep.A += step.Dt * b._angularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < step.PositionIterations; ++i) { bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); bool jointsOkay = true; for (int j = 0; j < JointCount; ++j) { bool jointOkay = Joints[j].SolvePositionConstraints(Settings.ContactBaumgarte); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } Report(contactSolver.Constraints); if (allowSleep) { float minSleepTime = Settings.FLT_MAX; #if !TARGET_FLOAT32_IS_FIXED float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; #endif for (int i = 0; i < BodyCount; ++i) { Body b = Bodies[i]; if (b._invMass == 0.0f) { continue; } if ((b._flags & Body.BodyFlags.AllowSleep) == 0) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } if ((b._flags & Body.BodyFlags.AllowSleep) == 0 || #if TARGET_FLOAT32_IS_FIXED Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance || Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance || Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance) #else b._angularVelocity *b._angularVelocity > angTolSqr || Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr) #endif { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += step.Dt; minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime); } }
internal override bool SolvePositionConstraints(float baumgarte) { Body body = this._body1; Body body2 = this._body2; float num = 0f; if (this._enableLimit && this._limitState != LimitState.InactiveLimit) { float num2 = body2._sweep.A - body._sweep.A - this._referenceAngle; float num3 = 0f; if (this._limitState == LimitState.EqualLimits) { float num4 = Box2DX.Common.Math.Clamp(num2, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); num3 = -this._motorMass * num4; num = Box2DX.Common.Math.Abs(num4); } else { if (this._limitState == LimitState.AtLowerLimit) { float num4 = num2 - this._lowerAngle; num = -num4; num4 = Box2DX.Common.Math.Clamp(num4 + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0f); num3 = -this._motorMass * num4; } else { if (this._limitState == LimitState.AtUpperLimit) { float num4 = num2 - this._upperAngle; num = num4; num4 = Box2DX.Common.Math.Clamp(num4 - Settings.AngularSlop, 0f, Settings.MaxAngularCorrection); num3 = -this._motorMass * num4; } } } Body expr_139_cp_0 = body; expr_139_cp_0._sweep.A = expr_139_cp_0._sweep.A - body._invI * num3; Body expr_154_cp_0 = body2; expr_154_cp_0._sweep.A = expr_154_cp_0._sweep.A + body2._invI * num3; body.SynchronizeTransform(); body2.SynchronizeTransform(); } Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter()); Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter()); Vec2 vec3 = body2._sweep.C + vec2 - body._sweep.C - vec; float num5 = vec3.Length(); float invMass = body._invMass; float invMass2 = body2._invMass; float invI = body._invI; float invI2 = body2._invI; float num6 = 10f * Settings.LinearSlop; if (vec3.LengthSquared() > num6 * num6) { Vec2 vec4 = vec3; vec4.Normalize(); float num7 = invMass + invMass2; Box2DXDebug.Assert(num7 > Settings.FLT_EPSILON); float a = 1f / num7; Vec2 v = a * -vec3; float num8 = 0.5f; Body expr_283_cp_0 = body; expr_283_cp_0._sweep.C = expr_283_cp_0._sweep.C - num8 * invMass * v; Body expr_2A5_cp_0 = body2; expr_2A5_cp_0._sweep.C = expr_2A5_cp_0._sweep.C + num8 * invMass2 * v; vec3 = body2._sweep.C + vec2 - body._sweep.C - vec; } Mat22 a2 = default(Mat22); a2.Col1.X = invMass + invMass2; a2.Col2.X = 0f; a2.Col1.Y = 0f; a2.Col2.Y = invMass + invMass2; Mat22 b = default(Mat22); b.Col1.X = invI * vec.Y * vec.Y; b.Col2.X = -invI * vec.X * vec.Y; b.Col1.Y = -invI * vec.X * vec.Y; b.Col2.Y = invI * vec.X * vec.X; Mat22 b2 = default(Mat22); b2.Col1.X = invI2 * vec2.Y * vec2.Y; b2.Col2.X = -invI2 * vec2.X * vec2.Y; b2.Col1.Y = -invI2 * vec2.X * vec2.Y; b2.Col2.Y = invI2 * vec2.X * vec2.X; Vec2 vec5 = (a2 + b + b2).Solve(-vec3); Body expr_465_cp_0 = body; expr_465_cp_0._sweep.C = expr_465_cp_0._sweep.C - body._invMass * vec5; Body expr_488_cp_0 = body; expr_488_cp_0._sweep.A = expr_488_cp_0._sweep.A - body._invI * Vec2.Cross(vec, vec5); Body expr_4AA_cp_0 = body2; expr_4AA_cp_0._sweep.C = expr_4AA_cp_0._sweep.C + body2._invMass * vec5; Body expr_4CD_cp_0 = body2; expr_4CD_cp_0._sweep.A = expr_4CD_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec5); body.SynchronizeTransform(); body2.SynchronizeTransform(); return(num5 <= Settings.LinearSlop && num <= Settings.AngularSlop); }
internal override bool SolvePositionConstraints(float baumgarte) { Body body = this._body1; Body body2 = this._body2; Vec2 v = this._ground.GetXForm().Position + this._groundAnchor1; Vec2 v2 = this._ground.GetXForm().Position + this._groundAnchor2; float num = 0f; if (this._state == LimitState.AtUpperLimit) { Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter()); Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter()); Vec2 v3 = body._sweep.C + vec; Vec2 v4 = body2._sweep.C + vec2; this._u1 = v3 - v; this._u2 = v4 - v2; float num2 = this._u1.Length(); float num3 = this._u2.Length(); if (num2 > Settings.LinearSlop) { this._u1 *= 1f / num2; } else { this._u1.SetZero(); } if (num3 > Settings.LinearSlop) { this._u2 *= 1f / num3; } else { this._u2.SetZero(); } float num4 = this._constant - num2 - this._ratio * num3; num = Box2DX.Common.Math.Max(num, -num4); num4 = Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f); float num5 = -this._pulleyMass * num4; Vec2 vec3 = -num5 * this._u1; Vec2 vec4 = -this._ratio * num5 * this._u2; Body expr_1F6_cp_0 = body; expr_1F6_cp_0._sweep.C = expr_1F6_cp_0._sweep.C + body._invMass * vec3; Body expr_219_cp_0 = body; expr_219_cp_0._sweep.A = expr_219_cp_0._sweep.A + body._invI * Vec2.Cross(vec, vec3); Body expr_23B_cp_0 = body2; expr_23B_cp_0._sweep.C = expr_23B_cp_0._sweep.C + body2._invMass * vec4; Body expr_25E_cp_0 = body2; expr_25E_cp_0._sweep.A = expr_25E_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec4); body.SynchronizeTransform(); body2.SynchronizeTransform(); } if (this._limitState1 == LimitState.AtUpperLimit) { Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter()); Vec2 v3 = body._sweep.C + vec; this._u1 = v3 - v; float num2 = this._u1.Length(); if (num2 > Settings.LinearSlop) { this._u1 *= 1f / num2; } else { this._u1.SetZero(); } float num4 = this._maxLength1 - num2; num = Box2DX.Common.Math.Max(num, -num4); num4 = Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f); float num5 = -this._limitMass1 * num4; Vec2 vec3 = -num5 * this._u1; Body expr_381_cp_0 = body; expr_381_cp_0._sweep.C = expr_381_cp_0._sweep.C + body._invMass * vec3; Body expr_3A4_cp_0 = body; expr_3A4_cp_0._sweep.A = expr_3A4_cp_0._sweep.A + body._invI * Vec2.Cross(vec, vec3); body.SynchronizeTransform(); } if (this._limitState2 == LimitState.AtUpperLimit) { Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter()); Vec2 v4 = body2._sweep.C + vec2; this._u2 = v4 - v2; float num3 = this._u2.Length(); if (num3 > Settings.LinearSlop) { this._u2 *= 1f / num3; } else { this._u2.SetZero(); } float num4 = this._maxLength2 - num3; num = Box2DX.Common.Math.Max(num, -num4); num4 = Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f); float num5 = -this._limitMass2 * num4; Vec2 vec4 = -num5 * this._u2; Body expr_4C0_cp_0 = body2; expr_4C0_cp_0._sweep.C = expr_4C0_cp_0._sweep.C + body2._invMass * vec4; Body expr_4E3_cp_0 = body2; expr_4E3_cp_0._sweep.A = expr_4E3_cp_0._sweep.A + body2._invI * Vec2.Cross(vec2, vec4); body2.SynchronizeTransform(); } return(num < 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); }
public void Solve(TimeStep step, Vec2 gravity, bool allowSleep) { for (int i = 0; i < this._bodyCount; i++) { Body body = this._bodies[i]; if (!body.IsStatic()) { Body expr_27 = body; //expr_27._linearVelocity += step.Dt * (gravity + body._invMass * body._force); expr_27._linearVelocity += step.Dt * (body._useGravity ? body._gravity : gravity + body._invMass * body._force); //Steve body gravity body._angularVelocity += step.Dt * body._invI * body._torque; body._force.Set(0f, 0f); body._torque = 0f; Body expr_9E = body; expr_9E._linearVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._linearDamping, 0f, 1f); body._angularVelocity *= Box2DX.Common.Math.Clamp(1f - step.Dt * body._angularDamping, 0f, 1f); if (Vec2.Dot(body._linearVelocity, body._linearVelocity) > Settings.MaxLinearVelocitySquared) { body._linearVelocity.Normalize(); Body expr_130 = body; expr_130._linearVelocity *= Settings.MaxLinearVelocity; } if (body._angularVelocity * body._angularVelocity > Settings.MaxAngularVelocitySquared) { if (body._angularVelocity < 0f) { body._angularVelocity = -Settings.MaxAngularVelocity; } else { body._angularVelocity = Settings.MaxAngularVelocity; } } } } ContactSolver contactSolver = new ContactSolver(step, this._contacts, this._contactCount); contactSolver.InitVelocityConstraints(step); for (int i = 0; i < this._jointCount; i++) { this._joints[i].InitVelocityConstraints(step); } for (int i = 0; i < step.VelocityIterations; i++) { for (int j = 0; j < this._jointCount; j++) { this._joints[j].SolveVelocityConstraints(step); } contactSolver.SolveVelocityConstraints(); } contactSolver.FinalizeVelocityConstraints(); for (int i = 0; i < this._bodyCount; i++) { Body body = this._bodies[i]; if (!body.IsStatic()) { body._sweep.C0 = body._sweep.C; body._sweep.A0 = body._sweep.A; Body expr_296_cp_0 = body; expr_296_cp_0._sweep.C = expr_296_cp_0._sweep.C + step.Dt * body._linearVelocity; Body expr_2BE_cp_0 = body; expr_2BE_cp_0._sweep.A = expr_2BE_cp_0._sweep.A + step.Dt * body._angularVelocity; body.SynchronizeTransform(); } } for (int k = 0; k < step.PositionIterations; k++) { bool flag = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte); bool flag2 = true; for (int i = 0; i < this._jointCount; i++) { bool flag3 = this._joints[i].SolvePositionConstraints(Settings.ContactBaumgarte); flag2 = (flag2 && flag3); } if (flag && flag2) { break; } } this.Report(contactSolver._constraints); if (allowSleep) { float num = Settings.FLT_MAX; float num2 = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance; float num3 = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance; for (int i = 0; i < this._bodyCount; i++) { Body body = this._bodies[i]; if (body._invMass != 0f) { if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags) 0) { body._sleepTime = 0f; num = 0f; } if ((body._flags & Body.BodyFlags.AllowSleep) == (Body.BodyFlags) 0 || body._angularVelocity * body._angularVelocity > num3 || Vec2.Dot(body._linearVelocity, body._linearVelocity) > num2) { body._sleepTime = 0f; num = 0f; } else { body._sleepTime += step.Dt; num = Box2DX.Common.Math.Min(num, body._sleepTime); } } } if (num >= Settings.TimeToSleep) { for (int i = 0; i < this._bodyCount; i++) { Body body = this._bodies[i]; body._flags |= Body.BodyFlags.Sleep; body._linearVelocity = Vec2.Zero; body._angularVelocity = 0f; } } } }
internal override bool SolvePositionConstraints(float baumgarte) { Body b1 = _bodyA; Body b2 = _bodyB; Vec2 c1 = b1._sweep.C; float a1 = b1._sweep.A; Vec2 c2 = b2._sweep.C; float a2 = b2._sweep.A; // Solve linear limit constraint. float linearError = 0.0f, angularError = 0.0f; bool active = false; float C2 = 0.0f; Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2); Vec2 r1 = Box2DX.Common.Math.Mul(R1, _localAnchor1 - _localCenter1); Vec2 r2 = Box2DX.Common.Math.Mul(R2, _localAnchor2 - _localCenter2); Vec2 d = c2 + r2 - c1 - r1; if (_enableLimit) { _axis = Box2DX.Common.Math.Mul(R1, _localXAxis1); _a1 = Vec2.Cross(d + r1, _axis); _a2 = Vec2.Cross(r2, _axis); float translation = Vec2.Dot(_axis, d); if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop) { // Prevent large angular corrections C2 = Box2DX.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); linearError = Box2DX.Common.Math.Abs(translation); active = true; } else if (translation <= _lowerTranslation) { // Prevent large linear corrections and allow some slop. C2 = Box2DX.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f); linearError = _lowerTranslation - translation; active = true; } else if (translation >= _upperTranslation) { // Prevent large linear corrections and allow some slop. C2 = Box2DX.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection); linearError = translation - _upperTranslation; active = true; } } _perp = Box2DX.Common.Math.Mul(R1, _localYAxis1); _s1 = Vec2.Cross(d + r1, _perp); _s2 = Vec2.Cross(r2, _perp); Vec3 impulse; Vec2 C1 = new Vec2(); C1.X = Vec2.Dot(_perp, d); C1.Y = a2 - a1 - _refAngle; linearError = Box2DX.Common.Math.Max(linearError, Box2DX.Common.Math.Abs(C1.X)); angularError = Box2DX.Common.Math.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; c1 -= _invMass1 * P; a1 -= _invI1 * L1; c2 += _invMass2 * P; a2 += _invI2 * L2; // TODO_ERIN remove need for this. b1._sweep.C = c1; b1._sweep.A = a1; b2._sweep.C = c2; b2._sweep.A = a2; b1.SynchronizeTransform(); b2.SynchronizeTransform(); return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override bool SolvePositionConstraints(float baumgarte) { // TODO_ERIN block solve with limit. Body b1 = _body1; Body b2 = _body2; float angularError = 0.0f; float positionError = 0.0f; // Solve angular limit constraint. 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 C = Box2DX.Common.Math.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = Box2DXMath.Abs(C); } else if (_limitState == LimitState.AtLowerLimit) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = Box2DX.Common.Math.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpperLimit) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = Box2DX.Common.Math.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } b1._sweep.A -= b1._invI * limitImpulse; b2._sweep.A += b2._invI * limitImpulse; b1.SynchronizeTransform(); b2.SynchronizeTransform(); } // Solve point-to-point constraint. { Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter()); Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter()); Vector2 C = b2._sweep.C + r2 - b1._sweep.C - r1; positionError = C.Length; float invMass1 = b1._invMass, invMass2 = b2._invMass; float invI1 = b1._invI, invI2 = b2._invI; // Handle large detachment. float k_allowedStretch = 10.0f * Settings.LinearSlop; if (C.LengthSquared > k_allowedStretch * k_allowedStretch) { // Use a particle solution (no rotation). Vector2 u = C; u.Normalize(); float k = invMass1 + invMass2; Box2DXDebug.Assert(k > Settings.FLT_EPSILON); float m = 1.0f / k; Vector2 impulse = m * (-C); float k_beta = 0.5f; b1._sweep.C -= k_beta * invMass1 * impulse; b2._sweep.C += k_beta * invMass2 * impulse; C = b2._sweep.C + r2 - b1._sweep.C - r1; } 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; Vector2 impulse_ = K.Solve(-C); b1._sweep.C -= b1._invMass * impulse_; b1._sweep.A -= b1._invI * r1.Cross(impulse_); b2._sweep.C += b2._invMass * impulse_; b2._sweep.A += b2._invI * r2.Cross(impulse_); b1.SynchronizeTransform(); b2.SynchronizeTransform(); } return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
internal override bool SolvePositionConstraints(float baumgarte) { Body body = this._body1; Body body2 = this._body2; Vec2 vec = body._sweep.C; float num = body._sweep.A; Vec2 vec2 = body2._sweep.C; float num2 = body2._sweep.A; float num3 = 0f; bool flag = false; float z = 0f; Mat22 a = new Mat22(num); Mat22 a2 = new Mat22(num2); Vec2 v = Box2DX.Common.Math.Mul(a, this._localAnchor1 - this._localCenter1); Vec2 vec3 = Box2DX.Common.Math.Mul(a2, this._localAnchor2 - this._localCenter2); Vec2 vec4 = vec2 + vec3 - vec - v; if (this._enableLimit) { this._axis = Box2DX.Common.Math.Mul(a, this._localXAxis1); this._a1 = Vec2.Cross(vec4 + v, this._axis); this._a2 = Vec2.Cross(vec3, this._axis); float num4 = Vec2.Dot(this._axis, vec4); if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop) { z = Box2DX.Common.Math.Clamp(num4, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection); num3 = Box2DX.Common.Math.Abs(num4); flag = true; } else { if (num4 <= this._lowerTranslation) { z = Box2DX.Common.Math.Clamp(num4 - this._lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f); num3 = this._lowerTranslation - num4; flag = true; } else { if (num4 >= this._upperTranslation) { z = Box2DX.Common.Math.Clamp(num4 - this._upperTranslation - Settings.LinearSlop, 0f, Settings.MaxLinearCorrection); num3 = num4 - this._upperTranslation; flag = true; } } } } this._perp = Box2DX.Common.Math.Mul(a, this._localYAxis1); this._s1 = Vec2.Cross(vec4 + v, this._perp); this._s2 = Vec2.Cross(vec3, this._perp); Vec2 v2 = default(Vec2); v2.X = Vec2.Dot(this._perp, vec4); v2.Y = num2 - num - this._refAngle; num3 = Box2DX.Common.Math.Max(num3, Box2DX.Common.Math.Abs(v2.X)); float num5 = Box2DX.Common.Math.Abs(v2.Y); Vec3 vec5; if (flag) { float invMass = this._invMass1; float invMass2 = this._invMass2; float invI = this._invI1; float invI2 = this._invI2; float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2; float num6 = invI * this._s1 + invI2 * this._s2; float num7 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2; float y = invI + invI2; float num8 = invI * this._a1 + invI2 * this._a2; float z2 = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2; this._K.Col1.Set(x, num6, num7); this._K.Col2.Set(num6, y, num8); this._K.Col3.Set(num7, num8, z2); vec5 = this._K.Solve33(-new Vec3 { X = v2.X, Y = v2.Y, Z = z }); } else { float invMass = this._invMass1; float invMass2 = this._invMass2; float invI = this._invI1; float invI2 = this._invI2; float x = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2; float num6 = invI * this._s1 + invI2 * this._s2; float y = invI + invI2; this._K.Col1.Set(x, num6, 0f); this._K.Col2.Set(num6, y, 0f); Vec2 vec6 = this._K.Solve22(-v2); vec5.X = vec6.X; vec5.Y = vec6.Y; vec5.Z = 0f; } Vec2 v3 = vec5.X * this._perp + vec5.Z * this._axis; float num9 = vec5.X * this._s1 + vec5.Y + vec5.Z * this._a1; float num10 = vec5.X * this._s2 + vec5.Y + vec5.Z * this._a2; vec -= this._invMass1 * v3; num -= this._invI1 * num9; vec2 += this._invMass2 * v3; num2 += this._invI2 * num10; body._sweep.C = vec; body._sweep.A = num; body2._sweep.C = vec2; body2._sweep.A = num2; body.SynchronizeTransform(); body2.SynchronizeTransform(); return(num3 <= Settings.LinearSlop && num5 <= Settings.AngularSlop); }