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); }