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