//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <inheritdoc/>
protected override void OnSetup()
{
var deltaTime = Simulation.Settings.Timing.FixedTimeStep;
float errorReduction = ConstraintHelper.ComputeErrorReduction(deltaTime, SpringConstant, DampingConstant);
float softness = ConstraintHelper.ComputeSoftness(deltaTime, SpringConstant, DampingConstant);
// Get anchor orientations in world space.
Matrix33F anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal;
Matrix33F anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal;
Matrix33F relativeOrientation = anchorOrientationA.Transposed * anchorOrientationB;
Vector3F angles = ConstraintHelper.GetEulerAngles(relativeOrientation);
// The constraint axes: See OneNote for a detailed derivation of these non-intuitive axes.
var xA = anchorOrientationA.GetColumn(0); // Anchor x-axis on A.
var zB = anchorOrientationB.GetColumn(2); // Anchor z-axis on B.
Vector3F constraintAxisY = Vector3F.Cross(zB, xA);
Vector3F constraintAxisX = Vector3F.Cross(constraintAxisY, zB);
Vector3F constraintAxisZ = Vector3F.Cross(xA, constraintAxisY);
SetupConstraint(0, angles[0], TargetAngles[0], constraintAxisX, deltaTime, errorReduction, softness);
SetupConstraint(1, angles[1], TargetAngles[1], constraintAxisY, deltaTime, errorReduction, softness);
SetupConstraint(2, angles[2], TargetAngles[2], constraintAxisZ, deltaTime, errorReduction, softness);
// No warmstarting.
_constraints[0].ConstraintImpulse = 0;
_constraints[1].ConstraintImpulse = 0;
_constraints[2].ConstraintImpulse = 0;
}
/// <inheritdoc/>
protected override void OnSetup()
{
// Get anchor orientations in world space.
Matrix33F anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal;
Matrix33F anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal;
// Get anchor orientation of B relative to A.
Matrix33F relativeOrientation = anchorOrientationA.Transposed * anchorOrientationB;
Vector3F angles = ConstraintHelper.GetEulerAngles(relativeOrientation);
// The constraint axes: See OneNote for a detailed derivation of these non-intuitive axes.
var xA = anchorOrientationA.GetColumn(0); // Anchor x-axis on A.
var zB = anchorOrientationB.GetColumn(2); // Anchor z-axis on B.
Vector3F constraintAxisY = Vector3F.Cross(zB, xA);
Vector3F constraintAxisX = Vector3F.Cross(constraintAxisY, zB);
Vector3F constraintAxisZ = Vector3F.Cross(xA, constraintAxisY);
// Remember old states.
LimitState oldXLimitState = _limitStates[0];
LimitState oldYLimitState = _limitStates[1];
LimitState oldZLimitState = _limitStates[2];
SetupConstraint(0, angles.X, constraintAxisX);
SetupConstraint(1, angles.Y, constraintAxisY);
SetupConstraint(2, angles.Z, constraintAxisZ);
Warmstart(0, oldXLimitState);
Warmstart(1, oldYLimitState);
Warmstart(2, oldZLimitState);
}
