예제 #1
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 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;
        }
예제 #2
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);
        }
예제 #3
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;
        }
예제 #4
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;
        }