/// <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; }
/// <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 relativeOrientationMatrix = anchorOrientationA.Transposed * anchorOrientationB; Quaternion relativeOrientation = Quaternion.CreateFromRotationMatrix(relativeOrientationMatrix); Quaternion 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)); } Vector3 axis = new Vector3(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; }
/// <inheritdoc/> protected override void OnSetup() { // Anchor pose/position in world space. Pose anchorPoseA = BodyA.Pose * AnchorPoseALocal; Vector3 anchorPositionB = BodyB.Pose.ToWorldPosition(AnchorPositionBLocal); // Compute anchor pose of B relative to anchor pose of A. Vector3 relativePosition = anchorPoseA.ToLocalPosition(anchorPositionB); // The linear constraint axes are the fixed anchor axes of A! Matrix anchorOrientation = anchorPoseA.Orientation; Vector3 rA = anchorPoseA.Position - BodyA.PoseCenterOfMass.Position; Vector3 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 = Vector3.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; }