//--------------------------------------------------------------
#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);
}
//--------------------------------------------------------------
#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 relativeOrientationMatrix = anchorOrientationA.Transposed * anchorOrientationB;
QuaternionF relativeOrientation = QuaternionF.CreateRotation(relativeOrientationMatrix);
QuaternionF deltaRotation = TargetOrientation * relativeOrientation.Conjugated;
float angle = deltaRotation.Angle;
if (angle > ConstantsF.Pi)
{
// Quaternion should be the shortest arc quaternion (angle < 180°).
deltaRotation = -deltaRotation;
angle = ConstantsF.TwoPi - angle;
Debug.Assert(Numeric.AreEqual(angle, deltaRotation.Angle));
}
Vector3F axis = new Vector3F(deltaRotation.X, deltaRotation.Y, deltaRotation.Z);
if (!axis.TryNormalize())
{
// Do nothing.
_minImpulseLimits[0] = 0;
_minImpulseLimits[1] = 0;
_minImpulseLimits[2] = 0;
}
else
{
// Axis is in local space of anchor A.
// Convert axis to world space.
axis = anchorOrientationA * axis;
// Main axis is the quaternion axis.
bool isActive = !Numeric.IsZero(angle);
SetupConstraint(0, -angle, 0, axis, deltaTime, errorReduction, softness, isActive);
if (!UseSingleAxisMode)
{
// In multi-axes-mode mode: constrain rotation on 2 orthogonal axes.
SetupConstraint(1, 0, 0, axis.Orthonormal1, deltaTime, errorReduction, softness, isActive);
SetupConstraint(2, 0, 0, axis.Orthonormal2, deltaTime, errorReduction, softness, isActive);
}
}
// No warmstarting.
_constraints[0].ConstraintImpulse = 0;
_constraints[1].ConstraintImpulse = 0;
_constraints[2].ConstraintImpulse = 0;
}
//--------------------------------------------------------------
#region Creation & Cleanup
//--------------------------------------------------------------
#endregion
//--------------------------------------------------------------
#region Methods
//--------------------------------------------------------------
/// <inheritdoc/>
protected override void OnSetup()
{
// Anchor pose/position in world space.
Pose anchorPoseA = BodyA.Pose * AnchorPoseALocal;
Vector3F anchorPositionB = BodyB.Pose.ToWorldPosition(AnchorPositionBLocal);
// Compute anchor pose of B relative to anchor pose of A.
Vector3F relativePosition = anchorPoseA.ToLocalPosition(anchorPositionB);
// The linear constraint axes are the fixed anchor axes of A!
Matrix33F anchorOrientation = anchorPoseA.Orientation;
Vector3F rA = anchorPoseA.Position - BodyA.PoseCenterOfMass.Position;
Vector3F rB = anchorPositionB - BodyB.PoseCenterOfMass.Position;
var deltaTime = Simulation.Settings.Timing.FixedTimeStep;
float errorReduction = ConstraintHelper.ComputeErrorReduction(deltaTime, SpringConstant, DampingConstant);
float softness = ConstraintHelper.ComputeSoftness(deltaTime, SpringConstant, DampingConstant);
if (!UseSingleAxisMode)
{
SetupConstraint(0, relativePosition.X, TargetPosition.X, anchorOrientation.GetColumn(0), rA, rB, deltaTime, errorReduction, softness);
SetupConstraint(1, relativePosition.Y, TargetPosition.Y, anchorOrientation.GetColumn(1), rA, rB, deltaTime, errorReduction, softness);
SetupConstraint(2, relativePosition.Z, TargetPosition.Z, anchorOrientation.GetColumn(2), rA, rB, deltaTime, errorReduction, softness);
}
else
{
var axis = TargetPosition - relativePosition;
var deviation = axis.Length;
if (Numeric.IsZero(deviation))
{
axis = Vector3F.UnitX;
}
else
{
axis.Normalize();
}
SetupConstraint(0, -deviation, 0, axis, rA, rB, deltaTime, errorReduction, softness);
}
// No warmstarting.
_constraints[0].ConstraintImpulse = 0;
_constraints[1].ConstraintImpulse = 0;
_constraints[2].ConstraintImpulse = 0;
}
