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