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 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 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)); }
/// <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); }
/// <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, -Vector3.Cross(_rA, _normal), _normal, Vector3.Cross(_rB, _normal)); _frictionConstraint0.Prepare(BodyA, BodyB, -_t0, -Vector3.Cross(_rA, _t0), _t0, Vector3.Cross(_rB, _t0)); _frictionConstraint1.Prepare(BodyA, BodyB, -_t1, -Vector3.Cross(_rA, _t1), _t1, Vector3.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, -Vector3.Cross(_rA, _normal), _normal, Vector3.Cross(_rB, _normal)); _frictionConstraint0.Prepare(BodyA, BodyB, -_t0, -Vector3.Cross(_rA, _t0), _t0, Vector3.Cross(_rB, _t0)); _frictionConstraint1.Prepare(BodyA, BodyB, -_t1, -Vector3.Cross(_rA, _t1), _t1, Vector3.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 (Vector3.Dot(newRA, _rA) < tolerance * _rALengthSquared || Vector3.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, -Vector3.Cross(_rA, _normal), _normal, Vector3.Cross(_rB, _normal)); _frictionConstraint0.Prepare(BodyA, BodyB, -_t0, -Vector3.Cross(_rA, _t0), _t0, Vector3.Cross(_rB, _t0)); _frictionConstraint1.Prepare(BodyA, BodyB, -_t1, -Vector3.Cross(_rA, _t1), _t1, Vector3.Cross(_rB, _t1)); } } // Get relative velocity. Vector3 vRel; if (_surfaceMotionAEnabled || _surfaceMotionBEnabled) { vRel = RelativeVelocity; } else { //Vector3 velA = BodyA._linearVelocity + Vector3.Cross(BodyA._angularVelocity, _rA); //Vector3 velB = BodyB._linearVelocity + Vector3.Cross(BodyB._angularVelocity, _rB); //Vector3 vRel = velB - velA; Vector3 linearVelocityA = BodyA._linearVelocity; Vector3 linearVelocityB = BodyB._linearVelocity; Vector3 angularVelocityA = BodyA._angularVelocity; Vector3 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 Vector3(velBX - velAX, velBY - velAY, velBZ - velAZ); } // Compute target velocity for restitution. //float vRelN = Vector3.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; }