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 void OnSetup()
{
var planeALocal = PlaneALocal;
var planeNormal = BodyA.Pose.ToWorldDirection(planeALocal.Normal);
// Calculate a point on the new plane.
Vector3 pointOnPlane = BodyA.Pose.Position + planeNormal * planeALocal.DistanceFromOrigin;
// Project point on to normal vector to get the new DistanceFromOrigin.
var distanceFromOrigin = Vector3.Dot(pointOnPlane, planeNormal);
Vector3 anchorPositionB = BodyB.Pose.ToWorldPosition(AnchorPositionBLocal);
float separation = Vector3.Dot(anchorPositionB, planeNormal) - distanceFromOrigin;
float penetrationDepth = -separation;
Vector3 rA = anchorPositionB - BodyA.PoseCenterOfMass.Position;
Vector3 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, -Vector3.Cross(rA, planeNormal), planeNormal, Vector3.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;
}
}
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);
}