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); }