Exemplo n.º 1
0
        List <JacobianConstraint> GetLinearLimit(
            IShape simulationObjectA,
            IShape simulationObjectB,
            Vector3d sliderAxis,
            Vector3d r1,
            Vector3d r2,
            double errorReduction)
        {
            var linearConstraints = new List <JacobianConstraint>();

            if (LinearLimitMin.HasValue &&
                LinearLimitMax.HasValue)
            {
                linearConstraints.Add(
                    JacobianCommon.GetLinearLimit(
                        simulationObjectA,
                        simulationObjectB,
                        sliderAxis,
                        r1,
                        r2,
                        errorReduction,
                        0.0,
                        LinearLimitMin.Value,
                        LinearLimitMax.Value));
            }

            return(linearConstraints);
        }
        /// <summary>
        /// Builds the slider joint.
        /// </summary>
        /// <returns>The slider joint.</returns>
        /// <param name="simulationObjs">Simulation objects.</param>
        public override List <JacobianConstraint> BuildJacobian()
        {
            var sliderConstraints = new List <JacobianConstraint> ();

            IShape simulationObjectA = ShapeA;
            IShape simulationObjectB = ShapeB;

            #region Init Linear

            Vector3d sliderAxis = GetSliderAxis();

            Vector3d t1 = GeometryUtils.GetPerpendicularVector(sliderAxis).Normalize();
            Vector3d t2 = sliderAxis.Cross(t1).Normalize();

            Vector3d r1 = simulationObjectA.RotationMatrix *
                          StartErrorAxis1;

            Vector3d r2 = simulationObjectB.RotationMatrix *
                          StartErrorAxis2;

            Vector3d p1 = simulationObjectA.Position + r1;
            Vector3d p2 = simulationObjectB.Position + r2;

            Vector3d linearError = p2 - p1;

            #endregion

            #region Init Angular

            Vector3d angularError = JacobianCommon.GetFixedAngularError(
                simulationObjectA,
                simulationObjectB,
                RelativeOrientation);

            #endregion

            #region Jacobian Constraint

            double errorReduction    = ErrorReductionParam;
            double springCoefficient = SpringCoefficient;

            #region Base Constraints

            ConstraintType constraintType = ConstraintType.Joint;

            if (SpringCoefficient > 0)
            {
                constraintType = ConstraintType.SoftJoint;
            }

            double constraintLimit = errorReduction * 2.0 * angularError.x;

            //DOF 1

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      xVecNeg,
                                      xVec,
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 2

            constraintLimit = errorReduction * 2.0 * angularError.y;

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      yVecNeg,
                                      yVec,
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 3

            constraintLimit = errorReduction * 2.0 * angularError.z;

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      zVecNeg,
                                      zVec,
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 4

            constraintLimit = errorReduction * t1.Dot(linearError);

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      t1,
                                      -1.0 * t1,
                                      r1.Cross(t1),
                                      -1.0 * r2.Cross(t1),
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            //DOF 5

            constraintLimit = errorReduction * t2.Dot(linearError);

            sliderConstraints.Add(JacobianCommon.GetDOF(
                                      t2,
                                      -1.0 * t2,
                                      r1.Cross(t2),
                                      -1.0 * r2.Cross(t2),
                                      simulationObjectA,
                                      simulationObjectB,
                                      0.0,
                                      constraintLimit,
                                      springCoefficient,
                                      0.0,
                                      constraintType));

            #endregion

            #region Limit Constraints

            // Limit extraction
            if (LinearLimitMin.HasValue &&
                LinearLimitMax.HasValue)
            {
                sliderConstraints.Add(
                    JacobianCommon.GetLinearLimit(
                        simulationObjectA,
                        simulationObjectB,
                        sliderAxis,
                        r1,
                        r2,
                        errorReduction,
                        0.0,
                        LinearLimitMin.Value,
                        LinearLimitMax.Value));
            }

            #endregion

            #region Motor Constraint

            if (ForceLimit.HasValue &&
                SpeedValue.HasValue)
            {
                sliderConstraints.Add(JacobianCommon.GetDOF(
                                          sliderAxis,
                                          -1.0 * sliderAxis,
                                          new Vector3d(),
                                          new Vector3d(),
                                          simulationObjectA,
                                          simulationObjectB,
                                          SpeedValue.Value,
                                          0.0,
                                          0.0,
                                          ForceLimit.Value,
                                          ConstraintType.JointMotor));
            }

            #endregion

            #endregion

            return(sliderConstraints);
        }