private List <JacobianConstraint> GetCollisionConstraints(
CollisionPointStructure[] collisionPointsStruct,
IShape[] simulationObjs)
{
List <JacobianConstraint> jacobianConstraints = new List <JacobianConstraint>();
if (collisionPointsStruct.Length > 0)
{
var rangePartitioner = Partitioner.Create(0, collisionPointsStruct.Length, Convert.ToInt32(collisionPointsStruct.Length / EngineParameters.MaxThreadNumber) + 1);
var sync = new object();
Parallel.ForEach(
rangePartitioner,
new ParallelOptions {
MaxDegreeOfParallelism = EngineParameters.MaxThreadNumber
},
(range, loopState) =>
{
for (int i = range.Item1; i < range.Item2; i++)
{
var item = collisionPointsStruct[i];
IShape objectA = simulationObjs.First(x => x.ID == item.ObjectIndexA);
IShape objectB = simulationObjs.First(x => x.ID == item.ObjectIndexB);
List <JacobianConstraint> constraintsBuf = contactConstraintBuilder.BuildContactConstraint(
item,
objectA,
objectB);
lock (sync)
{
int idx = jacobianConstraints.Count;
if (idx > 0)
{
for (int j = 0; j < constraintsBuf.Count; j++)
{
var constraintBuf = constraintsBuf[j];
if (constraintBuf.Type == ConstraintType.Friction)
{
constraintBuf.ContactReference = constraintBuf.ContactReference + idx;
constraintsBuf[j] = new JacobianConstraint(constraintBuf);
}
}
}
jacobianConstraints.AddRange(constraintsBuf);
}
}
});
}
return(jacobianConstraints);
}
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);
}
public JacobianConstraint(JacobianConstraint jc)
{
ObjectA = jc.ObjectA;
ObjectB = jc.ObjectB;
ContactReference = jc.ContactReference;
LinearComponentA = jc.LinearComponentA;
LinearComponentB = jc.LinearComponentB;
AngularComponentA = jc.AngularComponentA;
AngularComponentB = jc.AngularComponentB;
Type = jc.Type;
JacobianVelocity = jc.JacobianVelocity;
ConstraintValue = jc.ConstraintValue;
JacobianVelocity = jc.JacobianVelocity;
CorrectionValue = jc.CorrectionValue;
CFM = jc.CFM;
ConstraintLimit = jc.ConstraintLimit;
}
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);
}
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);
}
