//-------------------------------------------------------------- #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; }
//-------------------------------------------------------------- #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; }