private void SetupConstraint(int index, float targetVelocity, Vector3 axis, 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, Vector3.Zero, -axis, Vector3.Zero, 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, Vector3 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, Vector3.Zero, -axis, Vector3.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, Vector3 axis,
Vector3 rA, Vector3 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, -Vector3.Cross(rA, axis), axis, Vector3.Cross(rB, axis));
}
private void SetupConstraint(int index, float position, Vector3 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, Vector3.Zero, -axis, Vector3.Zero, axis);
}
