/// <summary> /// Builds the fixed joint. /// </summary> /// <returns>The fixed joint.</returns> /// <param name="simulationObjs">Simulation objects.</param> public override List <JacobianConstraint> BuildJacobian() { var fixedConstraints = new List <JacobianConstraint> (); IShape simulationObjectA = ShapeA; IShape simulationObjectB = ShapeB; #region Init Linear Vector3d r1 = simulationObjectA.RotationMatrix * StartErrorAxis1; Vector3d r2 = simulationObjectB.RotationMatrix * StartErrorAxis2; Vector3d p1 = simulationObjectA.Position + r1; Vector3d p2 = simulationObjectB.Position + r2; Vector3d linearError = p2 - p1; Matrix3x3 skewR1 = r1.GetSkewSymmetricMatrix(); Matrix3x3 skewR2 = r2.GetSkewSymmetricMatrix(); #endregion #region Init Angular Vector3d angularError = JacobianCommon.GetFixedAngularError( simulationObjectA, simulationObjectB, RelativeOrientation); #endregion #region Jacobian Constraint double errorReduction = ErrorReductionParam; double springCoefficient = SpringCoefficient; ConstraintType constraintType = ConstraintType.Joint; if (SpringCoefficient > 0) { constraintType = ConstraintType.SoftJoint; } double restoreCoeff = errorReduction; double constraintLimit = restoreCoeff * linearError.x; //DOF 1 fixedConstraints.Add(JacobianCommon.GetDOF( xVec, xVecNeg, new Vector3d(-skewR1.r1c1, -skewR1.r1c2, -skewR1.r1c3), new Vector3d(skewR2.r1c1, skewR2.r1c2, skewR2.r1c3), simulationObjectA, simulationObjectB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 2 constraintLimit = restoreCoeff * linearError.y; fixedConstraints.Add(JacobianCommon.GetDOF( yVec, yVecNeg, new Vector3d(-skewR1.r2c1, -skewR1.r2c2, -skewR1.r2c3), new Vector3d(skewR2.r2c1, skewR2.r2c2, skewR2.r2c3), simulationObjectA, simulationObjectB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 3 constraintLimit = restoreCoeff * linearError.z; fixedConstraints.Add(JacobianCommon.GetDOF( zVec, zVecNeg, new Vector3d(-skewR1.r3c1, -skewR1.r3c2, -skewR1.r3c3), new Vector3d(skewR2.r3c1, skewR2.r3c2, skewR2.r3c3), simulationObjectA, simulationObjectB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 4 constraintLimit = restoreCoeff * 2.0 * angularError.x; fixedConstraints.Add(JacobianCommon.GetDOF( xVecNeg, xVec, simulationObjectA, simulationObjectB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 5 constraintLimit = restoreCoeff * 2.0 * angularError.y; fixedConstraints.Add(JacobianCommon.GetDOF( yVecNeg, yVec, simulationObjectA, simulationObjectB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 6 constraintLimit = restoreCoeff * 2.0 * angularError.z; fixedConstraints.Add(JacobianCommon.GetDOF( zVecNeg, zVec, simulationObjectA, simulationObjectB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); #endregion return(fixedConstraints); }
/// <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); }
/// <summary> /// Builds the fixed joint. /// </summary> /// <returns>The fixed joint.</returns> /// <param name="simulationObjs">Simulation objects.</param> public List <JacobianConstraint> BuildJacobian() { var softConstraints = new List <JacobianConstraint>(); #region Init Linear Vector3d r1 = PointA.RotationMatrix * StartErrorAxis1; Vector3d r2 = PointB.RotationMatrix * StartErrorAxis2; Vector3d p1 = PointA.Position + r1; Vector3d p2 = PointB.Position + r2; Vector3d linearError = p2 - p1; Matrix3x3 skewR1 = r1.GetSkewSymmetricMatrix(); Matrix3x3 skewR2 = r2.GetSkewSymmetricMatrix(); #endregion #region Init Angular Vector3d angularError = JacobianCommon.GetFixedAngularError( PointA, PointB, RelativeOrientation); #endregion #region Jacobian Constraint double errorReduction = ErrorReductionParam; double springCoefficient = SpringCoeff; ConstraintType constraintType = ConstraintType.SoftJoint; double constraintLimit = errorReduction * linearError.x; //DOF 1 softConstraints.Add(JacobianCommon.GetDOF( xVec, xVecNeg, new Vector3d(-skewR1.r1c1, -skewR1.r1c2, -skewR1.r1c3), new Vector3d(skewR2.r1c1, skewR2.r1c2, skewR2.r1c3), PointA, PointB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 2 constraintLimit = errorReduction * linearError.y; softConstraints.Add(JacobianCommon.GetDOF( yVec, yVecNeg, new Vector3d(-skewR1.r2c1, -skewR1.r2c2, -skewR1.r2c3), new Vector3d(skewR2.r2c1, skewR2.r2c2, skewR2.r2c3), PointA, PointB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); //DOF 3 constraintLimit = errorReduction * linearError.z; softConstraints.Add(JacobianCommon.GetDOF( zVec, zVecNeg, new Vector3d(-skewR1.r3c1, -skewR1.r3c2, -skewR1.r3c3), new Vector3d(skewR2.r3c1, skewR2.r3c2, skewR2.r3c3), PointA, PointB, 0.0, constraintLimit, springCoefficient, 0.0, constraintType)); if (ActivateAngularConstraint) { double angErrorReduction = AngularErrorReductionParam; double angSpringCoefficient = AngularSpringCoeff; //DOF 4 constraintLimit = angErrorReduction * angularError.x; softConstraints.Add(JacobianCommon.GetDOF( xVecNeg, xVec, PointA, PointB, 0.0, constraintLimit, angSpringCoefficient, 0.0, constraintType)); //DOF 5 constraintLimit = angErrorReduction * angularError.y; softConstraints.Add(JacobianCommon.GetDOF( yVecNeg, yVec, PointA, PointB, 0.0, constraintLimit, angSpringCoefficient, 0.0, constraintType)); //DOF 6 constraintLimit = angErrorReduction * angularError.z; softConstraints.Add(JacobianCommon.GetDOF( zVecNeg, zVec, PointA, PointB, 0.0, constraintLimit, angSpringCoefficient, 0.0, constraintType)); } #endregion return(softConstraints); }
/// <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); }