private List <CollisionPointStructure> SoftBodyCollisionStep(
ISoftShape softShapeA,
ISoftShape softShapeB)
{
var result = new List <CollisionPointStructure>();
var shapeA = (IShape)softShapeA;
var shapeB = (IShape)softShapeB;
var convexDecompositionA = new ConvexDecompositionEngine(
shapeA.AABBox,
Array.ConvertAll(softShapeA.ShapePoints, item => new Vertex3Index(item.Position, item.TriangleIndex, item.ID)),
softShapeA.DecompositionParameter);
var convexDecompositionB = new ConvexDecompositionEngine(
shapeB.AABBox,
Array.ConvertAll(softShapeB.ShapePoints, item => new Vertex3Index(item.Position, item.TriangleIndex, item.ID)),
softShapeB.DecompositionParameter);
List <ShapeDecompositionOutput> decompConvexShapeA = convexDecompositionA.Execute().GetConvexShapeList(true);
List <ShapeDecompositionOutput> decompConvexShapeB = convexDecompositionB.Execute().GetConvexShapeList(true);
AABB[][] boxCollision = new AABB[2][];
boxCollision[0] = Array.ConvertAll(decompConvexShapeA.ToArray(), x => x.Region);
boxCollision[1] = Array.ConvertAll(decompConvexShapeB.ToArray(), x => x.Region);
List <CollisionPair> collisionPair = broadPhaseCollisionEngine.Execute(
boxCollision[0],
boxCollision[1],
parameters.CollisionDistance);
var lockMe = new object();
Parallel.ForEach(
collisionPair,
new ParallelOptions {
MaxDegreeOfParallelism = parameters.MaxThreadNumber
},
pair =>
{
CollisionPointStructure collisionPointStruct = SoftBodyNarrowPhase(
decompConvexShapeA[pair.objectIndexA],
decompConvexShapeB[pair.objectIndexB],
shapeA.ID,
shapeB.ID);
if (collisionPointStruct != null)
{
lock (lockMe)
{
result.Add(collisionPointStruct);
}
}
});
return(result);
}
public SoftConstraint(
SoftShapePoint pointA,
SoftShapePoint pointB,
ISoftShape shape,
double errorReductionParam,
double springCoefficient)
: this(pointA, pointB, shape, errorReductionParam, springCoefficient, 0.0, 0.0)
{
}
private void IntegrateSoftShapePosition(
ISoftShape shape,
double timeStep,
bool updateExtForce)
{
Parallel.ForEach(
shape.ShapePoints,
new ParallelOptions {
MaxDegreeOfParallelism = EngineParameters.MaxThreadNumber
},
point => {
#region Linear Velocity
point.SetPosition(
point.Position +
timeStep *
point.LinearVelocity);
if (updateExtForce)
{
UpdatePointLinearVelocity(point, timeStep);
point.SetForce(new Vector3d());
}
#endregion
#region Angular Velocity
double angularVelocity = point.AngularVelocity.Length();
Vector3d versor = point.AngularVelocity.Normalize();
double rotationAngle = angularVelocity * timeStep;
var rotationQuaternion = new Quaternion(versor, rotationAngle);
point.SetRotationStatus((rotationQuaternion * point.RotationStatus).Normalize());
point.SetRotationMatrix(point.RotationStatus.ConvertToMatrix());
point.SetInverseInertiaTensor(
(point.RotationMatrix * point.MassInfo.InverseBaseInertiaTensor) *
point.RotationMatrix.Transpose());
#endregion
}
);
shape.SetAABB();
}
private List <CollisionPointStructure> GetCollisionPointStructure(
IShape A,
IShape B,
double collisionDistance)
{
List <CollisionPointStructure> collisionPointStructure = new List <CollisionPointStructure>();
ISoftShape softShapeA = A as ISoftShape;
ISoftShape softShapeB = B as ISoftShape;
if (softShapeA == null &&
softShapeB == null)
{
return(RigidBodyCollisionStep(A, B, collisionDistance));
}
else if (softShapeA != null &&
softShapeB != null)
{
//Soft Body Collision Detection
collisionPointStructure.AddRange(SoftBodyCollisionStep(softShapeA, softShapeB));
}
else if (softShapeB != null &&
softShapeA == null)
{
collisionPointStructure.AddRange(Rigid_SoftBodyCollisionDetection(A, softShapeB));
}
else if (softShapeA != null &&
softShapeB == null)
{
collisionPointStructure.AddRange(Rigid_SoftBodyCollisionDetection(B, softShapeA));
}
//Self collision detection
//if (softShapeB != null)
//{
// List<CollisionPointBaseStructure> baseCollisionList = new List<CollisionPointBaseStructure>();
// baseCollisionList.AddRange(softBodyCollisionDetection.SelfSoftBodyCollisionDetect(softShapeA, CollisionDistance));
//}
////Self collision detection
//if (softShapeB != null)
//{
// List<CollisionPointBaseStructure> baseCollisionList = new List<CollisionPointBaseStructure>();
// baseCollisionList.AddRange(softBodyCollisionDetection.SelfSoftBodyCollisionDetect(softShapeB, CollisionDistance));
//}
return(collisionPointStructure);
}
//Manca gestione compuondShape
private List <CollisionPointStructure> Rigid_SoftBodyCollisionDetection(
IShape rigidShape,
ISoftShape softShape)
{
var shapeSoft = (IShape)softShape;
List <CollisionPointStructure> collisionPointStructure = new List <CollisionPointStructure>();
ConvexDecompositionEngine convexDecomposition = new ConvexDecompositionEngine(
shapeSoft.AABBox,
Array.ConvertAll(softShape.ShapePoints, item => new Vertex3Index(item.Position, item.TriangleIndex, item.ID)),
softShape.DecompositionParameter);
List <ShapeDecompositionOutput> shapeOutput = convexDecomposition.Execute().GetIntersectedShape(
rigidShape.AABBox,
parameters.CollisionDistance,
true);
if (shapeOutput != null)
{
IGeometry[] convexShapeGeometry = ShapeDefinition.Helper.GetGeometry(rigidShape);
foreach (var convexGeometry in convexShapeGeometry)
{
VertexProperties[] convexVertexObj = Helper.SetVertexPosition(convexGeometry);
foreach (var softConvexShape in shapeOutput)
{
VertexProperties[] vertexObjSoftShape = Array.ConvertAll(softConvexShape.Vertex3Idx.ToArray(), x => new VertexProperties(x.Vector3, x.ID));
var cps = convexBodyNarrowPhase.Execute(convexVertexObj, vertexObjSoftShape, rigidShape.ID, shapeSoft.ID, parameters.CollisionDistance);
if (cps != null)
{
collisionPointStructure.Add(cps);
}
}
}
}
return(collisionPointStructure);
}
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);
}
