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