private CollisionPoint GetDistance(
IShape shapeA,
IShape shapeB)
{
CollisionPointStructure collisionPoint = collisionDetectionEngine.Execute(shapeA, shapeB, double.MaxValue);
if (collisionPoint == null)
{
return(null);
}
//TODO Gestire soft body, compound shape, concaveshape
return(collisionPoint.CollisionPointBase[0].CollisionPoint);
}
public List <JacobianConstraint> BuildContactConstraint(
CollisionPointStructure collisionPointStr,
IShape objectA,
IShape objectB)
{
var contactConstraints = new List <JacobianConstraint>();
if (objectA is ISoftShape &&
!(objectB is SimSoftShape))
{
contactConstraints.AddRange(BuildSoftBodyVSRigidBodyCollisionConstraints(collisionPointStr, (ISoftShape)objectA, objectB));
}
else if (objectB is ISoftShape &&
!(objectA is SimSoftShape))
{
contactConstraints.AddRange(BuildSoftBodyVSRigidBodyCollisionConstraints(collisionPointStr, objectA, (ISoftShape)objectB));
}
else
{
contactConstraints.AddRange(BuildRigidBodyCollisionConstraints(collisionPointStr, objectA, objectB));
}
return(contactConstraints);
}
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);
}
public List <CollisionPointStructure> GetWarmStartedCollisionPoints(
IShape[] shapes,
List <CollisionPointStructure> previousCollisionPoints,
List <CollisionPointStructure> actualCollisionPoints,
Dictionary <int, StabilizationValues> previousShapesProperties)
{
UpdatePersistentCollisionCounter(
previousCollisionPoints,
actualCollisionPoints,
previousShapesProperties,
shapes);
Dictionary <WarmStartHashSet, CollisionPointStructure> warmStartedCollisionPoints = new Dictionary <WarmStartHashSet, CollisionPointStructure>();
int warmStartCounter = 15;
foreach (var item in actualCollisionPoints)
{
WarmStartHashSet hashset = new WarmStartHashSet(
item.ObjectIndexA,
item.ObjectIndexB);
if (collisionPointsCounterDictionary.TryGetValue(hashset, out Tuple <bool, int, CollisionPointStructure> value) &&
value.Item2 >= warmStartCounter)
{
warmStartedCollisionPoints.Add(new WarmStartHashSet(item.ObjectIndexA, item.ObjectIndexB), value.Item3);
//}
}
}
var outputCollisionPoints = new List <CollisionPointStructure>(actualCollisionPoints);
for (int i = 0; i < outputCollisionPoints.Count; i++)
{
var hashSet = new WarmStartHashSet(outputCollisionPoints[i].ObjectIndexA, outputCollisionPoints[i].ObjectIndexB);
if (warmStartedCollisionPoints.TryGetValue(hashSet, out CollisionPointStructure cPoint))
{
CollisionPointStructure item = outputCollisionPoints[i];
int k = 0;
foreach (var value in item.CollisionPointBase[0].CollisionPoints)
{
//if (!value.Intersection)
//{
List <StartImpulseProperties> startImpulse = new List <StartImpulseProperties>
{
new StartImpulseProperties(0.0),
new StartImpulseProperties(0.0),
new StartImpulseProperties(0.0)
};
//CollisionPoint val = value;
//Get nearest
double minDistance = double.MaxValue;
foreach (var point in cPoint.CollisionPointBase[0].CollisionPoints)
{
double distAA = Vector3d.Length(point.CollisionPointA.Vertex - value.CollisionPointA.Vertex);
double distBA = Vector3d.Length(point.CollisionPointB.Vertex - value.CollisionPointA.Vertex);
if (distAA < minDistance ||
distBA < minDistance)
{
minDistance = (distAA < distBA) ? distAA : distBA;
//startImpulse = point.StartImpulseValue;
}
/*
* if (
* (CheckCollisionPointDistance(point.CollisionPointA.Vertex, value.CollisionPointA.Vertex, 1E-2) &&
* CheckCollisionPointDistance(point.CollisionPointB.Vertex, value.CollisionPointB.Vertex, 1E-2)) ||
* (CheckCollisionPointDistance(point.CollisionPointA.Vertex, value.CollisionPointB.Vertex, 1E-2) &&
* CheckCollisionPointDistance(point.CollisionPointB.Vertex, value.CollisionPointA.Vertex, 1E-2)))
* {
* startImpulse = point.StartImpulseValue;
* break;
* }
*/
}
k++;
//}
//value.SetNormal(outputCollisionPoints[i].CollisionPointBase[0].CollisionPoint.CollisionNormal);
}
outputCollisionPoints[i] = item;
//}
}
}
return(outputCollisionPoints);
}
public List <Partition> GetPartitions(
CollisionPointStructure[] collisionPoints,
List <IConstraint> joints,
IShape[] shapes,
ISoftShape[] softShapes)
{
List <SpatialPartition> spatialPartitions = CalculateSpatialPartitioning(collisionPoints, joints, shapes);
List <Partition> partitions = new List <Partition>();
if (spatialPartitions != null)
{
HashSet <int> totObjectIndex = new HashSet <int>();
for (int i = 0; i < spatialPartitions.Count; i++)
{
Partition partitionItem = new Partition();
HashSet <int> objectIndex = new HashSet <int>();
for (int j = 0; j < spatialPartitions[i].ObjectList.Count; j++)
{
if (spatialPartitions[i].ObjectList[j].Type == ContactGroupType.Collision)
{
CollisionPointStructure cpStruct = ConstraintHelper.Find(
collisionPoints,
spatialPartitions[i].ObjectList[j]);
if (cpStruct != null)
{
partitionItem.PartitionedCollisionPoints.Add(cpStruct);
}
objectIndex.Add(cpStruct.ObjectIndexA);
objectIndex.Add(cpStruct.ObjectIndexB);
totObjectIndex.Add(cpStruct.ObjectIndexA);
totObjectIndex.Add(cpStruct.ObjectIndexB);
}
else
{
IConstraint smJoint = joints.Find(item =>
item.GetObjectIndexA() == spatialPartitions[i].ObjectList[j].IndexA &&
item.GetObjectIndexB() == spatialPartitions[i].ObjectList[j].IndexB &&
item.GetKeyIndex() == spatialPartitions[i].ObjectList[j].KeyIndex);
partitionItem.PartitionedJoints.Add(smJoint);
objectIndex.Add(smJoint.GetObjectIndexA());
objectIndex.Add(smJoint.GetObjectIndexB());
totObjectIndex.Add(smJoint.GetObjectIndexA());
totObjectIndex.Add(smJoint.GetObjectIndexB());
}
}
////Add Soft Body Constraints if is involved in collision
foreach (var item in objectIndex)
{
if (shapes[item] is ISoftShape softShape)
{
partitionItem.PartitionedJoints.AddRange(softShape.SoftConstraint);
}
}
partitions.Add(partitionItem);
}
////Add Soft Body Constraints if is not involved in collision
foreach (var item in softShapes)
{
if (!totObjectIndex.Contains(((IShape)item).ID))
{
Partition partitionItem = new Partition();
partitionItem.PartitionedJoints.AddRange(item.SoftConstraint);
partitions.Add(partitionItem);
}
}
}
else if (softShapes.Length > 0)
{
foreach (var softShape in softShapes)
{
Partition partitionItem = new Partition();
partitionItem.PartitionedJoints.AddRange(softShape.SoftConstraint);
partitions.Add(partitionItem);
}
}
return(partitions);
}
