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);
}
private JacobianConstraint[] AddFrictionConstraints(
IShape objectA,
IShape objectB,
PhysicsEngineParameters simulationParameters,
Vector3d normal,
Vector3d relativeVelocity,
Vector3d ra,
Vector3d rb,
SoftShapePoint softShapePoint)
{
JacobianConstraint[] friction = new JacobianConstraint[simulationParameters.FrictionDirections];
Vector3d[] frictionDirection = GetFrictionCone(normal, simulationParameters.FrictionDirections);
double constraintLimit = 0.0;
Vector3d tangentialVelocity = relativeVelocity -
normal.Dot(relativeVelocity) *
normal;
if (Vector3d.Length(tangentialVelocity) > simulationParameters.ShiftToStaticFrictionTolerance)
{
constraintLimit = 0.5 * (objectA.DynamicFrictionCoeff + objectB.DynamicFrictionCoeff);
}
else
{
constraintLimit = 0.5 * (objectA.StaticFrictionCoeff + objectB.StaticFrictionCoeff);
}
for (int i = 0; i < simulationParameters.FrictionDirections; i++)
{
var linearComponentA = frictionDirection[i];
var linearComponentB = -1.0 * linearComponentA;
var angularComponentA = ra.Cross(linearComponentA);
var angularComponentB = -1.0 * rb.Cross(linearComponentA);
friction[i] = JacobianCommon.GetDOF(
linearComponentA,
linearComponentB,
angularComponentA,
angularComponentB,
(objectA is ISoftShape) ? (IShapeCommon)softShapePoint: objectA,
(objectB is ISoftShape) ? (IShapeCommon)softShapePoint: objectB,
0.0,
0.0,
simulationParameters.FrictionCFM,
constraintLimit,
ConstraintType.Friction,
null);
}
return(friction);
}
private List <JacobianConstraint> GetMotorConstraint(
IShape simulationObjectA,
IShape simulationObjectB,
Vector3d hingeAxis,
Vector3d rotationAxis)
{
var motorConstraint = new List <JacobianConstraint>();
if (SpeedHingeAxisLimit.HasValue &&
ForceHingeAxisLimit.HasValue)
{
motorConstraint.Add(
JacobianCommon.GetDOF(
-1.0 * hingeAxis,
hingeAxis,
simulationObjectA,
simulationObjectB,
SpeedHingeAxisLimit.Value,
0.0,
0.0,
ForceHingeAxisLimit.Value,
ConstraintType.JointMotor));
}
if (SpeedRotationAxisLimit.HasValue &&
ForceRotationAxisLimit.HasValue)
{
motorConstraint.Add(
JacobianCommon.GetDOF(
-1.0 * rotationAxis,
rotationAxis,
simulationObjectA,
simulationObjectB,
SpeedRotationAxisLimit.Value,
0.0,
0.0,
ForceRotationAxisLimit.Value,
ConstraintType.JointMotor));
}
return(motorConstraint);
}
double GetAngle1(
Vector3d axis1,
Vector3d axis2,
Vector3d startAxis,
Quaternion rotationStatus,
Quaternion startRelativeRotation)
{
Matrix3x3 rotationMatrix = Matrix3x3.GetRotationMatrix(axis1, axis2);
Quaternion rotationQ = Quaternion.GetQuaternion(rotationMatrix);
Quaternion mult1 = Quaternion.Multiply1(rotationStatus, rotationQ);
Quaternion mult2 = Quaternion.Multiply2(mult1, startRelativeRotation);
var quaternionVectorPart = new Vector3d(
mult2.b,
mult2.c,
mult2.d);
return(JacobianCommon.GetRotationAngle(quaternionVectorPart, mult2.a, startAxis));
}
List <JacobianConstraint> GetMotorConstraint(
IShape simulationObjectA,
IShape simulationObjectB,
Vector3d sliderAxis)
{
var motorConstraints = new List <JacobianConstraint>();
if (LinearForceLimit.HasValue &&
LinearSpeedValue.HasValue)
{
motorConstraints.Add(JacobianCommon.GetDOF(
sliderAxis,
-1.0 * sliderAxis,
new Vector3d(),
new Vector3d(),
simulationObjectA,
simulationObjectB,
LinearSpeedValue.Value,
0.0,
0.0,
LinearForceLimit.Value,
ConstraintType.JointMotor));
}
if (AngularForceLimit.HasValue &&
AngularSpeedValue.HasValue)
{
motorConstraints.Add(JacobianCommon.GetDOF(
sliderAxis,
-1.0 * sliderAxis,
simulationObjectA,
simulationObjectB,
AngularSpeedValue.Value,
0.0,
0.0,
AngularForceLimit.Value,
ConstraintType.JointMotor));
}
return(motorConstraints);
}
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);
}
private List <JacobianConstraint> GetSyncConstraintsExternalShape(
Vector3d hingeAxis,
Vector3d rotationAxis)
{
var syncConstraints = new List <JacobianConstraint>();
if (ExternalSyncShape != null)
{
var rotationAxisExt = ExternalSyncShape.RotationMatrix * RotationAxis;
var hingeAxisExt = hingeAxis;
syncConstraints.Add(JacobianCommon.GetDOF(
-1.0 * rotationAxisExt,
rotationAxis,
ExternalSyncShape,
ShapeB,
0.0,
0.0,
SpringCoefficientHingeAxis,
0.0,
ConstraintType.Joint));
syncConstraints.Add(JacobianCommon.GetDOF(
-1.0 * hingeAxisExt,
hingeAxis,
ExternalSyncShape,
ShapeB,
0.0,
0.0,
SpringCoefficientHingeAxis,
0.0,
ConstraintType.Joint));
}
return(syncConstraints);
}
private List <JacobianConstraint> BuildRigidBodyCollisionConstraints(
CollisionPointStructure collisionPointStr,
IShape objectA,
IShape objectB)
{
List <JacobianConstraint> contactConstraints = new List <JacobianConstraint>();
double restitutionCoefficient =
(objectA.RestitutionCoeff +
objectB.RestitutionCoeff) * 0.5;
double baumgarteStabilizationValue =
(objectA.RestoreCoeff +
objectB.RestoreCoeff) * 0.5;
for (int h = 0; h < collisionPointStr.CollisionPointBase.Length; h++)
{
var collisionPointBase = collisionPointStr.CollisionPointBase[h];
for (int k = 0; k < collisionPointBase.CollisionPoints.Length; k++)
{
CollisionPoint collisionPoint = collisionPointBase.CollisionPoints[k];
Vector3d ra = collisionPoint.CollisionPointA.Vertex - objectA.Position;
Vector3d rb = collisionPoint.CollisionPointB.Vertex - objectB.Position;
Vector3d linearComponentA = (-1.0 * collisionPoint.CollisionNormal).Normalize();
Vector3d linearComponentB = -1.0 * linearComponentA;
Vector3d angularComponentA = ra.Cross(linearComponentA);
Vector3d angularComponentB = -1.0 * rb.Cross(linearComponentA);
Vector3d velocityA = objectA.LinearVelocity +
objectA.AngularVelocity.Cross(ra);
Vector3d velocityB = objectB.LinearVelocity +
objectB.AngularVelocity.Cross(rb);
Vector3d relativeVelocity = velocityB - velocityA;
if (relativeVelocity.Length() < 1E-12 &&
collisionPointBase.CollisionPoint.Intersection &&
collisionPointBase.CollisionPoint.Distance < 1E-10)
{
continue;
}
#region Normal direction contact
double linearComponent = linearComponentA.Dot(relativeVelocity);
double uCollision = restitutionCoefficient * Math.Max(0.0, linearComponent);
if (uCollision <= 0.0 &&
!collisionPointBase.CollisionPoint.Intersection)
{
continue;
}
double correctionParameter = 0.0;
if (collisionPointBase.CollisionPoint.Intersection)
{
//Limit the Baum stabilization jitter effect
correctionParameter = Math.Max(Math.Max(collisionPointBase.CollisionPoint.Distance - simulationParameters.CompenetrationTolerance, 0.0) *
baumgarteStabilizationValue, 0.0);
}
double correctedBounce = uCollision;
JacobianConstraint normalContact = JacobianCommon.GetDOF(
linearComponentA,
linearComponentB,
angularComponentA,
angularComponentB,
objectA,
objectB,
correctedBounce,
correctionParameter,
simulationParameters.NormalCFM,
0.0,
ConstraintType.Collision,
null);
#endregion
contactConstraints.Add(normalContact);
#region Friction Contact
JacobianConstraint[] frictionContact =
AddFrictionConstraints(
objectA,
objectB,
simulationParameters,
linearComponentA,
relativeVelocity,
ra,
rb);
#endregion
int normalIndex = contactConstraints.Count - 1;
foreach (JacobianConstraint fjc in frictionContact)
{
fjc.SetContactReference(normalIndex);
contactConstraints.Add(fjc);
}
}
}
return(contactConstraints);
}
/// <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 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);
}
public override List <JacobianConstraint> BuildJacobian()
{
var angularConstraints = new List <JacobianConstraint>();
IShape simulationObjectA = ShapeA;
IShape simulationObjectB = ShapeB;
double errorReduction = ErrorReductionParam;
double springCoefficientHingeAxis = SpringCoefficientHingeAxis;
double springCoefficientRotationAxis = SpringCoefficientRotationAxis;
#region Init Angular
Vector3d hingeAxis = GetHingeAxis();
Vector3d rotationAxis = GetRotationAxis();
double k = hingeAxis.Dot(rotationAxis);
Vector3d tempPerpendicular = rotationAxis - k * hingeAxis;
Vector3d t1 = hingeAxis.Cross(tempPerpendicular).Normalize();
double hingeAngle = GetAngle1(
hingeAxis,
rotationAxis,
HingeAxis,
simulationObjectA.RotationStatus,
RelativeOrientation1);
double twistAngle = GetAngle2(
hingeAxis,
rotationAxis,
RotationAxis,
simulationObjectB.RotationStatus,
RelativeOrientation2);
#endregion
#region Jacobian Constraint
double angularLimit = errorReduction * hingeAngle;
angularConstraints.Add(JacobianCommon.GetDOF(
hingeAxis,
-1.0 * hingeAxis,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficientHingeAxis,
0.0,
ConstraintType.Joint));
angularLimit = errorReduction * twistAngle;
angularConstraints.Add(JacobianCommon.GetDOF(
rotationAxis,
-1.0 * rotationAxis,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficientRotationAxis,
0.0,
ConstraintType.Joint));
#endregion
return(angularConstraints);
}
/// <summary>
/// Builds the Universal joint.
/// </summary>
/// <returns>The Universal joint.</returns>
/// <param name="simulationObjs">Simulation objects.</param>
public override List <JacobianConstraint> BuildJacobian()
{
var hinge2Constraints = 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;
#endregion
#region Init Angular
Vector3d hingeAxis = GetHingeAxis();
Vector3d rotationAxis = GetRotationAxis();
double k = hingeAxis.Dot(rotationAxis);
Vector3d tempPerpendicular = rotationAxis - k * hingeAxis;
Vector3d t1 = hingeAxis.Cross(tempPerpendicular).Normalize();
#endregion
#region Jacobian Constraint
double errorReduction = ErrorReductionParam;
double springCoefficient = SpringCoefficient;
#region Base Constraint
ConstraintType constraintType = ConstraintType.Joint;
if (SpringCoefficient > 0.0)
{
constraintType = ConstraintType.SoftJoint;
}
//DOF 1
double constraintLimit = errorReduction * t1.Dot(linearError);
hinge2Constraints.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 2
constraintLimit = errorReduction * Vector3d.Dot(tempPerpendicular, linearError);
hinge2Constraints.Add(JacobianCommon.GetDOF(
tempPerpendicular,
-1.0 * tempPerpendicular,
r1.Cross(tempPerpendicular),
-1.0 * r2.Cross(tempPerpendicular),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
springCoefficient,
0.0,
constraintType));
//DOF 3
ConstraintType hingeAxisConstraintType = ConstraintType.Joint;
if (SpringCoefficientHingeAxis > 0)
{
hingeAxisConstraintType = ConstraintType.SoftJoint;
}
constraintLimit = errorReduction * hingeAxis.Dot(linearError);
hinge2Constraints.Add(JacobianCommon.GetDOF(
hingeAxis,
-1.0 * hingeAxis,
r1.Cross(hingeAxis),
-1.0 * r2.Cross(hingeAxis),
simulationObjectA,
simulationObjectB,
0.0,
constraintLimit,
SpringCoefficientHingeAxis,
0.0,
hingeAxisConstraintType));
//DOF 4
double angularLimit = errorReduction * (-k);
hinge2Constraints.Add(
JacobianCommon.GetDOF(
t1,
-1.0 * t1,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficient,
0.0,
constraintType));
hinge2Constraints.AddRange(GetSyncConstraintsExternalShape(
hingeAxis,
rotationAxis));
#endregion
#region Limit Constraints
hinge2Constraints.AddRange(GetAngularLimit(
simulationObjectA,
simulationObjectB,
hingeAxis,
rotationAxis,
errorReduction));
#endregion
#region Motor Constraint
hinge2Constraints.AddRange(GetMotorConstraint(
simulationObjectA,
simulationObjectB,
hingeAxis,
rotationAxis));
#endregion
#endregion
return(hinge2Constraints);
}
/// <summary>
/// Builds the piston joint.
/// </summary>
/// <returns>The piston joint.</returns>
/// <param name="simulationObjs">Simulation objects.</param>
public override List <JacobianConstraint> BuildJacobian()
{
var pistonConstraints = 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
Vector3d angularError = sliderAxis.Cross(
(simulationObjectB.RotationMatrix * PistonAxis));
#region Jacobian Constraint
double errorReduction = ErrorReductionParam;
double springCoefficient = SpringCoefficient;
#region Base Constraints
ConstraintType constraintType = ConstraintType.Joint;
if (SpringCoefficient > 0)
{
constraintType = ConstraintType.SoftJoint;
}
//DOF 1
double angularLimit = errorReduction *
t1.Dot(angularError);
pistonConstraints.Add(
JacobianCommon.GetDOF(
t1,
-1.0 * t1,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficient,
0.0,
constraintType));
//DOF 2
angularLimit = errorReduction *
t2.Dot(angularError);
pistonConstraints.Add(
JacobianCommon.GetDOF(
t2,
-1.0 * t2,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficient,
0.0,
constraintType));
//DOF 3
double constraintLimit = errorReduction * t1.Dot(linearError);
pistonConstraints.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 4
constraintLimit = errorReduction * Vector3d.Dot(t2, linearError);
pistonConstraints.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
pistonConstraints.AddRange(GetLinearLimit(
simulationObjectA,
simulationObjectB,
sliderAxis,
r1,
r2,
errorReduction));
pistonConstraints.AddRange(GetAnguarLimit(
simulationObjectA,
simulationObjectB,
sliderAxis,
errorReduction));
#endregion
#region Motor Constraint
pistonConstraints.AddRange(GetMotorConstraint(
simulationObjectA,
simulationObjectB,
sliderAxis));
#endregion
#endregion
return(pistonConstraints);
}
/// <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);
}
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);
}
private List <JacobianConstraint> BuildSoftBodyVSRigidBodyCollisionConstraints(
CollisionPointStructure collisionPointStr,
ISoftShape objectA,
IShape objectB)
{
List <JacobianConstraint> contactConstraints = new List <JacobianConstraint>();
IShape iSoftShape = (IShape)objectA;
double restitutionCoeff = GetRestitutionCoeff(iSoftShape, objectB);
double baumgarteStabValue = GetBaumgarteStabilizationValue(iSoftShape, objectB);
for (int h = 0; h < collisionPointStr.CollisionPointBase.Length; h++)
{
int?[] linkedID = collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionPointA.LinkedID.Distinct().ToArray();
double distanceSum = GetSoftBodyPointDistanceSum(
collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionPointA.Vertex,
objectA,
linkedID);
for (int i = 0; i < linkedID.Length; i++)
{
SoftShapePoint softShapePoint = objectA.ShapePoints.FirstOrDefault(x => x.ID == linkedID[i]);
if (softShapePoint == null)
{
continue;
}
double distanceWeigth = 1.0;
if (distanceSum > 0.0)
{
distanceWeigth = Vector3d.Length(softShapePoint.Position - collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionPointA.Vertex) / distanceSum;
}
if (distanceWeigth < 1E-10)
{
continue;
}
Vector3d ra = Vector3d.Zero();
Vector3d rb = collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionPointB.Vertex - objectB.Position;
////Component
Vector3d linearComponentA = (-1.0 * collisionPointStr.CollisionPointBase[h].CollisionPoint.CollisionNormal).Normalize();
Vector3d linearComponentB = -1.0 * linearComponentA;
Vector3d angularComponentA = Vector3d.Zero();
Vector3d angularComponentB = -1.0 * rb.Cross(linearComponentA);
////Velocity
Vector3d velocityA = softShapePoint.LinearVelocity;
Vector3d velocityB = objectB.LinearVelocity +
objectB.AngularVelocity.Cross(rb);
Vector3d relativeVelocity = velocityB - velocityA;
if (relativeVelocity.Length() < 1E-12 &&
collisionPointStr.CollisionPointBase[h].CollisionPoint.Intersection &&
collisionPointStr.CollisionPointBase[h].CollisionPoint.Distance < 1E-10)
{
continue;
}
#region Normal direction contact
double linearComponent = linearComponentA.Dot(relativeVelocity);
double uCollision = restitutionCoeff * Math.Max(0.0, linearComponent);
if (uCollision <= 0.0 &&
!collisionPointStr.CollisionPointBase[h].CollisionPoint.Intersection)
{
continue;
}
double correctionParameter = 0.0;
if (collisionPointStr.CollisionPointBase[h].CollisionPoint.Intersection)
{
//Limit the Baum stabilization jitter effect
correctionParameter = Math.Max(Math.Max(collisionPointStr.CollisionPointBase[h].CollisionPoint.Distance - simulationParameters.CompenetrationTolerance, 0.0) *
baumgarteStabValue - uCollision, 0.0);
}
double correctedBounce = uCollision * distanceWeigth;
correctionParameter = correctionParameter * distanceWeigth;
JacobianConstraint normalContact = JacobianCommon.GetDOF(
linearComponentA,
linearComponentB,
angularComponentA,
angularComponentB,
softShapePoint,
objectB,
correctedBounce,
correctionParameter,
simulationParameters.NormalCFM,
0.0,
ConstraintType.Collision,
null);
#endregion
#region Friction Contact
JacobianConstraint[] frictionContact =
AddFrictionConstraints(
iSoftShape,
objectB,
simulationParameters,
linearComponentA,
relativeVelocity,
ra,
rb,
softShapePoint);
#endregion
contactConstraints.Add(normalContact);
int normalIndex = contactConstraints.Count - 1;
foreach (JacobianConstraint fjc in frictionContact)
{
fjc.SetContactReference(normalIndex);
contactConstraints.Add(fjc);
}
}
}
return(contactConstraints);
}
/// <summary>
/// Builds the ball socket joint.
/// </summary>
/// <returns>The ball socket joint.</returns>
/// <param name="simulationObjs">Simulation objects.</param>
public override List <JacobianConstraint> BuildJacobian()
{
var ballSocketConstraints = new List <JacobianConstraint>();
IShape simulationObjectA = ShapeA;
IShape simulationObjectB = ShapeB;
#region Init Linear
Vector3d r1 = simulationObjectA.RotationMatrix *
StartErrorAxis1;
Vector3d r2 = simulationObjectB.RotationMatrix *
StartErrorAxis2;
Matrix3x3 skewR1 = r1.GetSkewSymmetricMatrix();
Matrix3x3 skewR2 = r2.GetSkewSymmetricMatrix();
Vector3d p1 = simulationObjectA.Position + r1;
Vector3d p2 = simulationObjectB.Position + r2;
Vector3d linearError = p2 - p1;
#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
ballSocketConstraints.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;
ballSocketConstraints.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;
ballSocketConstraints.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));
#endregion
return(ballSocketConstraints);
}
/// <summary>
/// Builds the Universal joint.
/// </summary>
/// <returns>The Universal joint.</returns>
/// <param name="simulationObjs">Simulation objects.</param>
public override List <JacobianConstraint> BuildJacobian()
{
var universalConstraints = new List <JacobianConstraint> ();
IShape simulationObjectA = ShapeA;
IShape simulationObjectB = ShapeB;
AnchorPoint = (simulationObjectA.RotationMatrix *
(StartAnchorPoint - simulationObjectA.InitCenterOfMass)) +
simulationObjectA.Position;
#region Init Linear
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 hingeAxis = simulationObjectA.RotationMatrix * HingeAxis;
Vector3d rotationAxis = simulationObjectB.RotationMatrix * RotationAxis;
double k = hingeAxis.Dot(rotationAxis);
Vector3d tempPerpendicular = rotationAxis - k * hingeAxis;
Vector3d t1 = hingeAxis.Cross(tempPerpendicular).Normalize();
#endregion
#region Jacobian Constraint
#region Base Constraint
double errorReduction = ErrorReductionParam;
double springCoefficient = SpringCoefficient;
ConstraintType constraintType = ConstraintType.Joint;
if (SpringCoefficient > 0)
{
constraintType = ConstraintType.SoftJoint;
}
//DOF 1
double constraintLimit = errorReduction * linearError.x;
universalConstraints.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;
universalConstraints.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;
universalConstraints.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 * (-k);
universalConstraints.Add(
JacobianCommon.GetDOF(
t1,
-1.0 * t1,
simulationObjectA,
simulationObjectB,
0.0,
angularLimit,
springCoefficient,
0.0,
constraintType));
#endregion
#region Limit Constraints
universalConstraints.AddRange(GetAngularLimit(
simulationObjectA,
simulationObjectB,
hingeAxis,
rotationAxis,
errorReduction));
#endregion
#endregion
return(universalConstraints);
}
