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;
}
}
//--------------------------------------------------------------
#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;
}
