//-------------------------------------------------------------- #region Creation & Cleanup //-------------------------------------------------------------- public WheelConstraint(ConstraintWheel wheel) { _wheel = wheel; _suspensionSpringConstraint = new Constraint1D(); _suspensionLimitConstraint = new Constraint1D(); _sideConstraint = new Constraint1D(); _forwardConstraint = new Constraint1D(); }
private void SetupConstraint(int index, float targetVelocity, Vector3F axis, Vector3F rA, Vector3F rB, float deltaTime) { Constraint1D constraint = _constraints[index]; constraint.TargetRelativeVelocity = targetVelocity; // Impulse limits float impulseLimit = MaxForce * deltaTime; _minImpulseLimits[index] = -impulseLimit; _maxImpulseLimits[index] = impulseLimit; // Note: Softness must be set before! constraint.Softness = Softness / deltaTime; constraint.Prepare(BodyA, BodyB, -axis, -Vector3F.Cross(rA, axis), axis, Vector3F.Cross(rB, axis)); }
private void Warmstart(int index, LimitState oldState) { // If the limit state has not changed and is active, we warmstart. // Otherwise, we reset the cached constraint impulse. Constraint1D constraint = _constraints[index]; if (oldState != LimitState.Inactive && oldState == _limitStates[index]) { constraint.Warmstart(BodyA, BodyB); } else if (constraint != null) { constraint.ConstraintImpulse = 0; } }
private void SetupConstraint(int index, float angle, float targetAngle, Vector3F axis, float deltaTime, float errorReduction, float softness) { // Note: Cached constraint impulses are reset in Warmstart() if necessary. Constraint1D constraint = _constraints[index]; Simulation simulation = Simulation; // ----- Error correction float deviation = targetAngle - angle; if (Numeric.IsZero(deviation)) { // deviation is 0 or targetAngle is NaN. Motor is off. _minImpulseLimits[index] = 0; _maxImpulseLimits[index] = 0; constraint.ConstraintImpulse = 0; return; } float fullCorrectionSpeed = deviation / deltaTime; float targetVelocity = fullCorrectionSpeed * errorReduction; //float minSpeed = MathHelper.Clamp(MinVelocity, -Math.Abs(fullCorrectionSpeed), Math.Abs(fullCorrectionSpeed)); //if (targetVelocity >= 0 && targetVelocity < minSpeed) // targetVelocity = minSpeed; //else if (targetVelocity <= 0 && targetVelocity > -minSpeed) // targetVelocity = -minSpeed; float maxErrorCorrectionVelocity = simulation.Settings.Constraints.MaxErrorCorrectionVelocity; targetVelocity = MathHelper.Clamp(targetVelocity, -maxErrorCorrectionVelocity, maxErrorCorrectionVelocity); targetVelocity = MathHelper.Clamp(targetVelocity, -MaxVelocity, MaxVelocity); constraint.TargetRelativeVelocity = targetVelocity; // ----- Impulse limits float impulseLimit = MaxForce * deltaTime; _minImpulseLimits[index] = -impulseLimit; _maxImpulseLimits[index] = impulseLimit; // Note: Softness must be set before! // TODO: Add softness to parameters. constraint.Softness = softness / deltaTime; constraint.Prepare(BodyA, BodyB, Vector3F.Zero, -axis, Vector3F.Zero, axis); }
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); }
private void SetupConstraint(int index, float position, float targetPosition, Vector3F axis, Vector3F rA, Vector3F rB, float deltaTime, float errorReduction, float softness) { // Note: Cached constraint impulses are reset in Warmstart() if necessary. Constraint1D constraint = _constraints[index]; Simulation simulation = Simulation; // ----- Error correction float deviation = targetPosition - position; if (Numeric.IsZero(deviation)) { _minImpulseLimits[index] = 0; _maxImpulseLimits[index] = 0; constraint.ConstraintImpulse = 0; return; } float fullCorrectionSpeed = deviation / deltaTime; float targetVelocity = fullCorrectionSpeed * errorReduction; //float minSpeed = MathHelper.Clamp(MinVelocity, -Math.Abs(fullCorrectionSpeed), Math.Abs(fullCorrectionSpeed)); //if (targetVelocity >= 0 && targetVelocity < minSpeed) // targetVelocity = minSpeed; //else if (targetVelocity <= 0 && targetVelocity > -minSpeed) // targetVelocity = -minSpeed; float maxErrorCorrectionVelocity = simulation.Settings.Constraints.MaxErrorCorrectionVelocity; targetVelocity = MathHelper.Clamp(targetVelocity, -maxErrorCorrectionVelocity, maxErrorCorrectionVelocity); targetVelocity = MathHelper.Clamp(targetVelocity, -MaxVelocity, MaxVelocity); constraint.TargetRelativeVelocity = targetVelocity; // ----- Impulse limits float impulseLimit = MaxForce * deltaTime; _minImpulseLimits[index] = -impulseLimit; _maxImpulseLimits[index] = impulseLimit; constraint.Softness = softness / deltaTime; constraint.Prepare(BodyA, BodyB, -axis, -Vector3F.Cross(rA, axis), axis, Vector3F.Cross(rB, axis)); }
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); }