private float ApplyImpulse(int index) { if (_minImpulseLimits[index] != 0) { Constraint1D constraint = _constraints[index]; float relativeVelocity = constraint.GetRelativeVelocity(BodyA, BodyB); float impulse = constraint.SatisfyConstraint( BodyA, BodyB, relativeVelocity, _minImpulseLimits[index], _maxImpulseLimits[index]); return(impulse); } return(0); }
/// <inheritdoc/> protected override bool OnApplyImpulse() { // Nothing to do if no limit is active. if (!_minLimitIsActive && !_maxLimitIsActive) { return(false); } // Relative velocity is positive if bodies are separating. float relativeVelocity = _constraint.GetRelativeVelocity(BodyA, BodyB); // If the max limit is reached, we must apply a negative impulse to bring bodies closer together. // If the min limit is reached, we must apply a positive impulse to get more separation. float impulseLimit = MaxForce * _deltaTime; float minImpulseLimit = (_maxLimitIsActive) ? -impulseLimit : 0; float maxImpulseLimit = (_minLimitIsActive) ? impulseLimit : 0; // Apply constraint impulse. float impulse = _constraint.SatisfyConstraint(BodyA, BodyB, relativeVelocity, minImpulseLimit, maxImpulseLimit); return(Math.Abs(impulse) > Simulation.Settings.Constraints.MinConstraintImpulse); }
private void SetupConstraint(int index, float position, Vector3F axis, float minimum, float maximum) { // Note: Cached constraint impulses are reset in Warmstart() if necessary. Constraint1D constraint = _constraints[index]; Simulation simulation = Simulation; float deltaTime = simulation.Settings.Timing.FixedTimeStep; // ----- Determine limit state. if (minimum > maximum) { _limitStates[index] = LimitState.Inactive; // Nothing more to do. return; } if (Numeric.AreEqual(minimum, maximum)) { _limitStates[index] = LimitState.Locked; } else if (position <= minimum) { _limitStates[index] = LimitState.Min; } else if (position >= maximum) { _limitStates[index] = LimitState.Max; } else { _limitStates[index] = LimitState.Inactive; // Nothing more to do. return; } Debug.Assert(_limitStates[index] != LimitState.Inactive); // ----- Error correction float deviation = 0; var allowedAngularDeviation = simulation.Settings.Constraints.AllowedAngularDeviation; if (position > maximum + allowedAngularDeviation) { deviation = maximum - position + allowedAngularDeviation; } else if (position < minimum - allowedAngularDeviation) { deviation = minimum - position - allowedAngularDeviation; } float targetVelocity = deviation * ErrorReduction / deltaTime; float maxErrorCorrectionVelocity = simulation.Settings.Constraints.MaxErrorCorrectionVelocity; targetVelocity = MathHelper.Clamp(targetVelocity, -maxErrorCorrectionVelocity, maxErrorCorrectionVelocity); // ----- Restitution float restitution = Restitution[index]; if (restitution > simulation.Settings.Constraints.RestitutionThreshold) { float velocity = constraint.GetRelativeVelocity(BodyA, BodyB); if (_limitStates[index] == LimitState.Min) { if (velocity < -Simulation.Settings.Constraints.RestingVelocityLimit) { targetVelocity = Math.Max(targetVelocity, -velocity * restitution); } } else if (_limitStates[index] == LimitState.Max) { if (velocity > Simulation.Settings.Constraints.RestingVelocityLimit) { targetVelocity = Math.Min(targetVelocity, -velocity * restitution); } } } constraint.TargetRelativeVelocity = targetVelocity; // ----- Impulse limits float impulseLimit = MaxForce[index] * deltaTime; if (_limitStates[index] == LimitState.Min) { _minImpulseLimits[index] = 0; _maxImpulseLimits[index] = impulseLimit; } else if (_limitStates[index] == LimitState.Max) { _minImpulseLimits[index] = -impulseLimit; _maxImpulseLimits[index] = 0; } else //if (_limitStates[index] == LimitState.Locked) { _minImpulseLimits[index] = -impulseLimit; _maxImpulseLimits[index] = impulseLimit; } // Note: Softness must be set before! constraint.Softness = Softness / deltaTime; constraint.Prepare(BodyA, BodyB, Vector3F.Zero, -axis, Vector3F.Zero, axis); }
//-------------------------------------------------------------- #region Creation & Cleanup //-------------------------------------------------------------- #endregion //-------------------------------------------------------------- #region Methods //-------------------------------------------------------------- /// <inheritdoc/> protected override void OnSetup() { var planeALocal = PlaneALocal; var planeNormal = BodyA.Pose.ToWorldDirection(planeALocal.Normal); // Calculate a point on the new plane. Vector3F pointOnPlane = BodyA.Pose.Position + planeNormal * planeALocal.DistanceFromOrigin; // Project point on to normal vector to get the new DistanceFromOrigin. var distanceFromOrigin = Vector3F.Dot(pointOnPlane, planeNormal); Vector3F anchorPositionB = BodyB.Pose.ToWorldPosition(AnchorPositionBLocal); float separation = Vector3F.Dot(anchorPositionB, planeNormal) - distanceFromOrigin; float penetrationDepth = -separation; Vector3F rA = anchorPositionB - BodyA.PoseCenterOfMass.Position; Vector3F rB = anchorPositionB - BodyB.PoseCenterOfMass.Position; // Remember old state. bool wasActive = _limitIsActive; if (separation <= 0) { _limitIsActive = true; Simulation simulation = Simulation; float deltaTime = simulation.Settings.Timing.FixedTimeStep; // ----- Determine limit state. // ----- Error correction float targetVelocity = penetrationDepth * ErrorReduction / deltaTime; float maxErrorCorrectionVelocity = simulation.Settings.Constraints.MaxErrorCorrectionVelocity; targetVelocity = MathHelper.Clamp(targetVelocity, -maxErrorCorrectionVelocity, maxErrorCorrectionVelocity); // ----- Restitution if (Restitution > simulation.Settings.Constraints.RestitutionThreshold) { float velocity = _constraint.GetRelativeVelocity(BodyA, BodyB); if (velocity < -Simulation.Settings.Constraints.RestingVelocityLimit) { targetVelocity = Math.Max(targetVelocity, -velocity * Restitution); } } _constraint.TargetRelativeVelocity = targetVelocity; // ----- Impulse limits _constraint.Softness = Softness / deltaTime; _constraint.Prepare(BodyA, BodyB, -planeNormal, -Vector3F.Cross(rA, planeNormal), planeNormal, Vector3F.Cross(rB, planeNormal)); } else { _limitIsActive = false; } // If the limit state has not changed, we warmstart. // Otherwise, we reset the cached constraint impulse. if (wasActive && _limitIsActive) { _constraint.Warmstart(BodyA, BodyB); } else { _constraint.ConstraintImpulse = 0; } }