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