public static Vector3d GetFixedAngularError(
SoftShapePoint objectA,
SoftShapePoint objectB,
Quaternion relativeOrientation)
{
Quaternion currentRelativeOrientation = objectB.RotationStatus.Inverse() *
objectA.RotationStatus;
Quaternion relativeOrientationError = relativeOrientation.Inverse() *
currentRelativeOrientation;
var angularError = new Vector3d(
relativeOrientationError.b,
relativeOrientationError.c,
relativeOrientationError.d);
if (relativeOrientationError.a < 0.0)
{
angularError = new Vector3d(
-angularError.x,
-angularError.y,
-angularError.z);
}
return(objectA.RotationMatrix * angularError);
}
public static JacobianConstraint GetDOF(
Vector3d angularComponentA,
Vector3d angularComponentB,
SoftShapePoint softShapePointA,
SoftShapePoint softShapePointB,
double constraintValue,
double correctionValue,
double cfm,
double constraintLimit,
ConstraintType type)
{
double jacobianVelocityValue = angularComponentA.Dot(softShapePointA.AngularVelocity) +
angularComponentB.Dot(softShapePointB.AngularVelocity);
return(new JacobianConstraint(
softShapePointA,
softShapePointB,
angularComponentA,
angularComponentB,
type,
constraintValue,
jacobianVelocityValue,
correctionValue,
cfm,
constraintLimit));
}
public SoftConstraint(
SoftShapePoint pointA,
SoftShapePoint pointB,
ISoftShape shape,
double errorReductionParam,
double springCoefficient)
: this(pointA, pointB, shape, errorReductionParam, springCoefficient, 0.0, 0.0)
{
}
private void UpdatePointLinearVelocity(
SoftShapePoint point,
double timeStep)
{
var externalForce = point.ForceValue * point.MassInfo.InverseMass +
timeStep * EngineParameters.ExternalForce;
point.SetLinearVelocity(point.LinearVelocity + externalForce);
}
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 static double GetSoftBodyPointDistanceSum(
Vector3d collisionPoint,
ISoftShape softShape,
int?[] linkedID)
{
double distanceSum = 0.0;
for (int i = 0; i < linkedID.Length; i++)
{
SoftShapePoint softShapePoint = softShape.ShapePoints.FirstOrDefault(x => x.ID == linkedID[i]);
if (softShapePoint == null)
{
continue;
}
distanceSum += Vector3d.Length(softShapePoint.Position - collisionPoint);
}
return(distanceSum);
}
public SoftConstraint(
SoftShapePoint pointA,
SoftShapePoint pointB,
ISoftShape shape,
double errorReductionParam,
double springCoefficient,
double angularErrorReductionParam,
double angularSpringCoeff)
{
PointA = pointA;
PointB = pointB;
KeyIndex = GetHashCode();
SpringCoeff = springCoefficient;
AngularSpringCoeff = angularSpringCoeff;
ErrorReductionParam = errorReductionParam;
AngularErrorReductionParam = angularErrorReductionParam;
Shape = shape;
ActivateAngularConstraint = false;
if (angularErrorReductionParam != 0.0 &&
angularSpringCoeff != 0.0)
{
ActivateAngularConstraint = true;
}
StartAnchorPoint = (PointB.StartPosition + PointA.StartPosition) * 0.5;
Vector3d relativePos = PointA.RotationMatrix * (StartAnchorPoint - PointA.StartPosition);
AnchorPoint = relativePos + PointA.Position;
StartErrorAxis1 = PointA.RotationMatrix.Transpose() *
(AnchorPoint - PointA.Position);
StartErrorAxis2 = PointB.RotationMatrix.Transpose() *
(AnchorPoint - PointB.Position);
RelativeOrientation = pointB.RotationStatus.Inverse() *
pointA.RotationStatus;
}
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);
}