public static CollisionPointStructure Find( CollisionPointStructure[] collisionPoints, ContactIndex contactIndex) { foreach (CollisionPointStructure cps in collisionPoints) { if (cps.ObjectA == contactIndex.IndexA && cps.ObjectB == contactIndex.IndexB) { return cps; } } return null; }
public static List<JacobianContact> BuildJoints( CollisionPointStructure[] collisionPointsStruct, SimulationObject[] simulationObjs, SimulationParameters simulationParameters) { var contactConstraints = new List<JacobianContact> (); for (int i = 0; i < collisionPointsStruct.Length; i++) { CollisionPointStructure collisionPointStr = collisionPointsStruct [i]; int indexA = collisionPointStr.ObjectA; int indexB = collisionPointStr.ObjectB; SimulationObject objectA = simulationObjs[indexA]; SimulationObject objectB = simulationObjs[indexB]; double restitutionCoefficient = (simulationObjs[indexA].RestitutionCoeff + simulationObjs[indexB].RestitutionCoeff) * 0.5; double baumgarteStabilizationValue = (simulationObjs[indexA].BaumgarteStabilizationCoeff + simulationObjs[indexB].BaumgarteStabilizationCoeff) * 0.5; for (int k = 0; k < collisionPointStr.CollisionPoints.Length; k++) { Vector3 ra = collisionPointStr.CollisionPoints[k].CollisionPointA - objectA.Position; Vector3 rb = collisionPointStr.CollisionPoints[k].CollisionPointB - objectB.Position; Vector3 linearComponentA = (-1.0 * collisionPointStr.CollisionPoints[k].CollisionNormal).Normalize(); Vector3 linearComponentB = -1.0 * linearComponentA; Vector3 angularComponentA = ra.Cross(linearComponentA); Vector3 angularComponentB = -1.0 * rb.Cross(linearComponentA); Vector3 velocityA = objectA.LinearVelocity + objectA.AngularVelocity.Cross(ra); Vector3 velocityB = objectB.LinearVelocity + objectB.AngularVelocity.Cross(rb); Vector3 relativeVelocity = velocityB - velocityA; if (relativeVelocity.Length() < 1E-12 && collisionPointStr.Intersection && collisionPointStr.ObjectDistance < 1E-10) continue; #region Normal direction contact double linearComponent = linearComponentA.Dot(relativeVelocity); double uCollision = restitutionCoefficient * Math.Max(0.0, linearComponent - simulationParameters.VelocityTolerance); double correctionParameter = 0.0; // Console.WriteLine("coll " + linearComponent); if (collisionPointStr.Intersection) { //Limit the Baum stabilization jitter effect correctionParameter = Math.Max(Math.Max(collisionPointStr.ObjectDistance - simulationParameters.CompenetrationTolerance, 0.0) * baumgarteStabilizationValue - uCollision, 0.0); } double correctedBounce = uCollision; JacobianContact normalContact = JacobianCommon.GetDOF( indexA, indexB, linearComponentA, linearComponentB, angularComponentA, angularComponentB, objectA, objectB, correctedBounce, correctionParameter, simulationParameters.NormalCFM, 0.0, ConstraintType.Collision, null, collisionPointStr.CollisionPoints[k].StartImpulseValue[0]); #endregion #region Friction Contact JacobianContact[] frictionContact = addFriction( objectA, objectB, simulationParameters, indexA, indexB, linearComponentA, relativeVelocity, ra, rb, collisionPointStr.CollisionPoints[k].StartImpulseValue); #endregion contactConstraints.Add(normalContact); int normalIndex = contactConstraints.Count - 1; foreach (JacobianContact jc in frictionContact) { jc.SetContactReference(normalIndex); contactConstraints.Add(jc); } } } return contactConstraints; }
public List<JacobianContact> GetJacobianConstraint( CollisionPointStructure[] collisionPointsStruct, List<IConstraint> simulationJointList, SimulationObject[] simulationObjs, SimulationParameters simulationParameters) { var constraint = new List<JacobianContact>(); #region Collision Contact constraint.AddRange( ContactConstraint.BuildJoints( collisionPointsStruct, simulationObjs, simulationParameters)); #endregion #region Joint foreach (IConstraintBuilder constraintItem in simulationJointList) constraint.AddRange(constraintItem.BuildJacobian(simulationObjs)); #endregion return constraint; }
public List<SpatialPartition> CalculateSpatialPartitioning( CollisionPointStructure[] collisionPoints, List<IConstraint> simulationJoints, SimulationObject[] simulationObjects) { if (collisionPoints.Length > 0 || simulationJoints.Count > 0) { var partitions = new List<SpatialPartition> (); var contactIndex = new List<ContactIndex> (); // Add contacts int keyIndex = 0; foreach (CollisionPointStructure cps in collisionPoints) { contactIndex.Add (new ContactIndex ( cps.ObjectA, cps.ObjectB, ContactGroupType.Collision, keyIndex)); keyIndex++; } // Add joints foreach (IConstraint smj in simulationJoints) { contactIndex.Add (new ContactIndex ( smj.GetObjectIndexA(), smj.GetObjectIndexB(), ContactGroupType.Joint, smj.GetKeyIndex())); } while (contactIndex.Count != 0) { var partition = new List<ContactIndex> (); partition.Add (contactIndex [0]); recursiveSearch ( contactIndex [0], contactIndex, partition, simulationObjects); foreach(ContactIndex cIndex in partition) { var index = new List<int> (); int indexVealue = 0; foreach(ContactIndex cntIndex in contactIndex) { if (cntIndex.IndexA == cIndex.IndexA && cntIndex.IndexB == cIndex.IndexB && cntIndex.Type == cIndex.Type) { index.Add (indexVealue); } indexVealue++; } index.Sort((a, b) => -1 * a.CompareTo(b)); for (int j = 0; j < index.Count; j++) { contactIndex.RemoveAt (index [j]); } } partitions.Add (new SpatialPartition (partition)); } return partitions; } return null; }