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