/// <inheritdoc/> protected override void OnSetup() { // Get anchor orientations in world space. Matrix anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal; Matrix anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal; // Get anchor orientation of B relative to A. Matrix relativeOrientation = anchorOrientationA.Transposed * anchorOrientationB; Vector3 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. Vector3 constraintAxisY = Vector3.Cross(zB, xA); Vector3 constraintAxisX = Vector3.Cross(constraintAxisY, zB); Vector3 constraintAxisZ = Vector3.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); }
/// <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. Matrix anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal; Matrix anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal; Matrix relativeOrientation = anchorOrientationA.Transposed * anchorOrientationB; Vector3 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. Vector3 constraintAxisY = Vector3.Cross(zB, xA); Vector3 constraintAxisX = Vector3.Cross(constraintAxisY, zB); Vector3 constraintAxisZ = Vector3.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; }