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));
}
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <inheritdoc/>
protected override void OnSetup()
{
_deltaTime = Simulation.Settings.Timing.FixedTimeStep;
// Anchors in world space.
var anchorA = BodyA.Pose.ToWorldPosition(AnchorPositionALocal);
var anchorB = BodyB.Pose.ToWorldPosition(AnchorPositionBLocal);
// The constraint acts on the axis between the anchors.
_axis = anchorB - anchorA;
float distance = _axis.Length;
// Check if limits are active.
_minLimitIsActive = distance < MinDistance && !Numeric.IsZero(distance);
_maxLimitIsActive = distance > MaxDistance;
// Abort if no limit is active.
if (!_minLimitIsActive && !_maxLimitIsActive)
{
_constraint.ConstraintImpulse = 0;
return;
}
_axis.TryNormalize();
_ra = anchorA - BodyA.PoseCenterOfMass.Position;
_rb = anchorB - BodyB.PoseCenterOfMass.Position;
// Too close together = positive deviation and positive target velocity.
// Too far apart = negative deviation and negative target velocity.
var deviation = (_minLimitIsActive) ? MinDistance - distance : MaxDistance - distance;
_constraint.TargetRelativeVelocity = deviation * ErrorReduction / _deltaTime;
float maxErrorCorrectionVelocity = Simulation.Settings.Constraints.MaxErrorCorrectionVelocity;
_constraint.TargetRelativeVelocity = MathHelper.Clamp(_constraint.TargetRelativeVelocity, -maxErrorCorrectionVelocity, maxErrorCorrectionVelocity);
_constraint.Softness = Softness / _deltaTime;
_constraint.Prepare(BodyA, BodyB, -_axis, -Vector3F.Cross(_ra, _axis), _axis, Vector3F.Cross(_rb, _axis));
// To keep it simple we do not warmstart. Warmstarting can only be done if the same limit
// was active the last time.
_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 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);
}
//--------------------------------------------------------------
#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;
}
}
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <inheritdoc/>
public void Setup()
{
// Cache normal. Access to Contact.Normal is expensive.
_normal = Contact.Normal;
float tolerance = StackingTolerance;
if (_rALengthSquared == -1)
{
// This is the first setup call and we must initialize the constraints.
_penetrationConstraint.Prepare(BodyA, BodyB, -_normal, -Vector3F.Cross(_rA, _normal), _normal, Vector3F.Cross(_rB, _normal));
_frictionConstraint0.Prepare(BodyA, BodyB, -_t0, -Vector3F.Cross(_rA, _t0), _t0, Vector3F.Cross(_rB, _t0));
_frictionConstraint1.Prepare(BodyA, BodyB, -_t1, -Vector3F.Cross(_rA, _t1), _t1, Vector3F.Cross(_rB, _t1));
_rALengthSquared = _rA.LengthSquared;
_rBLengthSquared = _rB.LengthSquared;
}
else if (tolerance == 0)
{
// Update _ra/_rb and related values. This creates smooth movement but less stable stacks.
_rA = Contact.PositionAWorld - BodyA.PoseCenterOfMass.Position;
_rB = Contact.PositionBWorld - BodyB.PoseCenterOfMass.Position;
_penetrationConstraint.Prepare(BodyA, BodyB, -_normal, -Vector3F.Cross(_rA, _normal), _normal, Vector3F.Cross(_rB, _normal));
_frictionConstraint0.Prepare(BodyA, BodyB, -_t0, -Vector3F.Cross(_rA, _t0), _t0, Vector3F.Cross(_rB, _t0));
_frictionConstraint1.Prepare(BodyA, BodyB, -_t1, -Vector3F.Cross(_rA, _t1), _t1, Vector3F.Cross(_rB, _t1));
}
else if (tolerance < 1)
{
// Update _ra/_rb and related values only if the angle has changed more than the tolerance.
// This creates less smooth movement for sliding/rolling objects but stable stacks.
tolerance = 1 - tolerance; // Invert, so that 1 means no tolerance and 0 is full tolerance.
var newRA = Contact.PositionAWorld - BodyA.PoseCenterOfMass.Position;
var newRB = Contact.PositionBWorld - BodyB.PoseCenterOfMass.Position;
//if (Vector3F.Dot(newRA, _rA) < tolerance * _rALengthSquared || Vector3F.Dot(newRB, _rB) < tolerance * _rBLengthSquared)
if (newRA.X * _rA.X + newRA.Y * _rA.Y + newRA.Z * _rA.Z < tolerance * _rALengthSquared ||
newRB.X * _rB.X + newRB.Y * _rB.Y + newRB.Z * _rB.Z < tolerance * _rBLengthSquared)
{
_rA = newRA;
_rB = newRB;
_penetrationConstraint.Prepare(BodyA, BodyB, -_normal, -Vector3F.Cross(_rA, _normal), _normal, Vector3F.Cross(_rB, _normal));
_frictionConstraint0.Prepare(BodyA, BodyB, -_t0, -Vector3F.Cross(_rA, _t0), _t0, Vector3F.Cross(_rB, _t0));
_frictionConstraint1.Prepare(BodyA, BodyB, -_t1, -Vector3F.Cross(_rA, _t1), _t1, Vector3F.Cross(_rB, _t1));
}
}
// Get relative velocity.
Vector3F vRel;
if (_surfaceMotionAEnabled || _surfaceMotionBEnabled)
{
vRel = RelativeVelocity;
}
else
{
//Vector3F velA = BodyA._linearVelocity + Vector3F.Cross(BodyA._angularVelocity, _rA);
//Vector3F velB = BodyB._linearVelocity + Vector3F.Cross(BodyB._angularVelocity, _rB);
//Vector3F vRel = velB - velA;
Vector3F linearVelocityA = BodyA._linearVelocity;
Vector3F linearVelocityB = BodyB._linearVelocity;
Vector3F angularVelocityA = BodyA._angularVelocity;
Vector3F angularVelocityB = BodyB._angularVelocity;
float velAX = linearVelocityA.X + angularVelocityA.Y * _rA.Z - angularVelocityA.Z * _rA.Y;
float velAY = linearVelocityA.Y - angularVelocityA.X * _rA.Z + angularVelocityA.Z * _rA.X;
float velAZ = linearVelocityA.Z + angularVelocityA.X * _rA.Y - angularVelocityA.Y * _rA.X;
float velBX = linearVelocityB.X + angularVelocityB.Y * _rB.Z - angularVelocityB.Z * _rB.Y;
float velBY = linearVelocityB.Y - angularVelocityB.X * _rB.Z + angularVelocityB.Z * _rB.X;
float velBZ = linearVelocityB.Z + angularVelocityB.X * _rB.Y - angularVelocityB.Y * _rB.X;
vRel = new Vector3F(velBX - velAX, velBY - velAY, velBZ - velAZ);
}
// Compute target velocity for restitution.
//float vRelN = Vector3F.Dot(vRel, n);
//if (vRelN < 0)
// _penetrationConstraint.TargetRelativeVelocity = -_restitution * vRelN; // Objects coming closer --> Bounce
//else
// _penetrationConstraint.TargetRelativeVelocity = 0; // Objects separating. Don't create bounce.
if (_restitution > 0)
{
float vRelN = vRel.X * _normal.X + vRel.Y * _normal.Y + vRel.Z * _normal.Z;
if (vRelN < 0)
{
_penetrationConstraint.TargetRelativeVelocity = -_restitution * vRelN; // Objects coming closer --> Bounce
}
else
{
_penetrationConstraint.TargetRelativeVelocity = 0; // Objects separating. Don't create bounce.
}
}
else
{
_penetrationConstraint.TargetRelativeVelocity = 0; // Objects separating. Don't create bounce.
}
// Compute velocity for error reduction.
_splitImpulse = 0;
_splitImpulseTargetVelocity = 0;
if (Contact.PenetrationDepth > _simulation.Settings.Constraints.AllowedPenetration)
{
float deviationLength = Contact.PenetrationDepth - _simulation.Settings.Constraints.AllowedPenetration;
float deltaTime = _simulation.Settings.Timing.FixedTimeStep;
float errorCorrectionVelocity = deviationLength / deltaTime * ErrorReduction;
errorCorrectionVelocity = Math.Min(errorCorrectionVelocity, _simulation.Settings.Constraints.MaxPenetrationCorrectionVelocity);
// Part of the error is corrected with split impulses. Part is corrected with Baumgarte.
_splitImpulseTargetVelocity = (1 - _simulation.Settings.Constraints.BaumgarteRatio) * errorCorrectionVelocity;
_penetrationConstraint.TargetRelativeVelocity = Math.Max(_simulation.Settings.Constraints.BaumgarteRatio * errorCorrectionVelocity, _penetrationConstraint.TargetRelativeVelocity);
}
// Warmstarting:
// We warmstart penetration constraints.
// No warmstarting for friction constraints.
// In our experiments, warmstarting of friction constraints was not very helpful in stacks.
_penetrationConstraint.Warmstart(BodyA, BodyB);
_frictionConstraint0.ConstraintImpulse = 0;
_frictionConstraint1.ConstraintImpulse = 0;
// In this frame we apply the bounce. If the contact persists until the next
// frame, we treat the contact as resting contact to avoid jiggle.
_restitution = 0;
}
