List <JacobianConstraint> GetAnguarLimit( IShape simulationObjectA, IShape simulationObjectB, Vector3d sliderAxis, double errorReduction) { var angularConstraints = new List <JacobianConstraint>(); if (AngularLimitMin.HasValue && AngularLimitMax.HasValue) { double angle = JacobianCommon.GetAngle( simulationObjectA, simulationObjectB, RelativeOrientation, PistonAxis); JacobianConstraint?jContact = JacobianCommon.GetAngularLimit( angle, errorReduction, 0.0, simulationObjectA, simulationObjectB, sliderAxis, AngularLimitMin.Value, AngularLimitMax.Value); if (jContact != null) { angularConstraints.Add(jContact.Value); } } return(angularConstraints); }
/// <summary> /// Builds the hinge joint. /// </summary> /// <returns>The hinge joint.</returns> /// <param name="simulationObjs">Simulation objects.</param> public override List <JacobianConstraint> BuildJacobian() { var hingeConstraints = new List <JacobianConstraint> (); IShape simulationObjectA = ShapeA; IShape simulationObjectB = ShapeB; #region Init Linear Vector3d axisRotated = GetHingeAxis(); Vector3d t1 = GeometryUtils.GetPerpendicularVector(axisRotated).Normalize(); Vector3d t2 = axisRotated.Cross(t1).Normalize(); Vector3d r1 = simulationObjectA.RotationMatrix * StartErrorAxis1; Vector3d r2 = simulationObjectB.RotationMatrix * StartErrorAxis2; Matrix3x3 skewP1 = Matrix3x3.GetSkewSymmetricMatrix(r1); Matrix3x3 skewP2 = Matrix3x3.GetSkewSymmetricMatrix(r2); 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 Constraint ConstraintType constraintType = ConstraintType.Joint; if (SpringCoefficient > 0) { constraintType = ConstraintType.SoftJoint; } //DOF 1 double constraintLimit = errorReduction * linearError.x; hingeConstraints.Add(JacobianCommon.GetDOF( xVec, xVecNeg, new Vector3d(-skewP1.r1c1, -skewP1.r1c2, -skewP1.r1c3), new Vector3d(skewP2.r1c1, skewP2.r1c2, skewP2.r1c3), simulationObjectA, simulationObjectB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 2 constraintLimit = errorReduction * linearError.y; hingeConstraints.Add(JacobianCommon.GetDOF( yVec, yVecNeg, new Vector3d(-skewP1.r2c1, -skewP1.r2c2, -skewP1.r2c3), new Vector3d(skewP2.r2c1, skewP2.r2c2, skewP2.r2c3), simulationObjectA, simulationObjectB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 3 constraintLimit = errorReduction * linearError.z; hingeConstraints.Add(JacobianCommon.GetDOF( zVec, zVecNeg, new Vector3d(-skewP1.r3c1, -skewP1.r3c2, -skewP1.r3c3), new Vector3d(skewP2.r3c1, skewP2.r3c2, skewP2.r3c3), simulationObjectA, simulationObjectB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 4 double angularLimit = errorReduction * t1.Dot(angularError); hingeConstraints.Add( JacobianCommon.GetDOF( -1.0 * t1, 1.0 * t1, simulationObjectA, simulationObjectB, 0.0, angularLimit, springCoefficient, 0.0, constraintType)); //DOF 5 angularLimit = errorReduction * t2.Dot(angularError); hingeConstraints.Add( JacobianCommon.GetDOF( -1.0 * t2, 1.0 * t2, simulationObjectA, simulationObjectB, 0.0, angularLimit, springCoefficient, 0.0, constraintType)); #endregion #region Limit Constraints if (AngularLimitMin.HasValue && AngularLimitMax.HasValue) { double angle = JacobianCommon.GetAngle( simulationObjectA, simulationObjectB, RelativeOrientation, HingeAxis); JacobianConstraint?jContact = JacobianCommon.GetAngularLimit( angle, errorReduction, 0.0, simulationObjectA, simulationObjectB, axisRotated, AngularLimitMin.Value, AngularLimitMax.Value); if (jContact != null) { hingeConstraints.Add(jContact.Value); } } #endregion #region Motor Contraint if (SpeedValue.HasValue && ForceLimit.HasValue) { hingeConstraints.Add( JacobianCommon.GetDOF( -1.0 * axisRotated, 1.0 * axisRotated, simulationObjectA, simulationObjectB, SpeedValue.Value, 0.0, 0.0, ForceLimit.Value, ConstraintType.JointMotor)); } #endregion #endregion return(hingeConstraints); }
List <JacobianConstraint> GetAngularLimit( IShape simulationObjectA, IShape simulationObjectB, Vector3d hingeAxis, Vector3d rotationAxis, double errorReduction) { var angularConstraint = new List <JacobianConstraint>(); if (AngularLimitMin1.HasValue && AngularLimitMax1.HasValue) { double angle1 = GetAngle1( hingeAxis, rotationAxis, HingeAxis, simulationObjectA.RotationStatus, RelativeOrientation1); JacobianConstraint?jContact = JacobianCommon.GetAngularLimit( angle1, errorReduction, 0.0, simulationObjectA, simulationObjectB, hingeAxis, AngularLimitMin1.Value, AngularLimitMax1.Value); if (jContact != null) { angularConstraint.Add(jContact.Value); } } if (AngularLimitMin2.HasValue && AngularLimitMax2.HasValue) { double angle2 = GetAngle2( hingeAxis, rotationAxis, RotationAxis, simulationObjectB.RotationStatus, RelativeOrientation2); JacobianConstraint?jContact = JacobianCommon.GetAngularLimit( angle2, errorReduction, 0.0, simulationObjectA, simulationObjectB, rotationAxis, AngularLimitMin2.Value, AngularLimitMax2.Value); if (jContact != null) { angularConstraint.Add(jContact.Value); } } return(angularConstraint); }