/* private int ContactDepthComparison(gxtContact a, gxtContact b) { return b.depth.CompareTo(a.depth); //return } */ /// <summary> /// /// </summary> /// <param name="dt"></param> public void Solve(float dt) { float invDt = 1.0f / dt; gxtContactPair[] contactPairs = new gxtContactPair[narrowphaseContacts.Count]; narrowphaseContacts.Values.CopyTo(contactPairs, 0); //Comparison<gxtContact> depthComparison = new Comparison<gxtContact>( Array.Sort<gxtContactPair>(contactPairs); // prune objects that are static, that have response disabled // sort by contact depth //gxtLog.WriteLineV(gxtVerbosityLevel.INFORMATIONAL, "New..."); int numEnabledContacts = 0; for (int i = 0; i < contactPairs.Length; ++i) { if (contactPairs[i].Enabled) { numEnabledContacts++; contactPairs[i].PreStepImpulse(invDt); } } //gxtLog.WriteLineV(gxtVerbosityLevel.INFORMATIONAL, "Num Enabled Contacts: {0}", numEnabledContacts); // optimal iterations seems to be between 7 and 15 int iterations = 10; for (int i = 0; i < iterations; ++i) { for (int j = 0; j < contactPairs.Length; ++j) { if (contactPairs[j].Enabled) contactPairs[j].ApplyImpulses(); } } }
/// <summary> /// Runs more expensive narrowphase tests to determine object-object intersections /// No need for dt here... /// </summary> public void NarrowPhase() { //if (testPairs == null) // return; HashSet<gxtBroadphaseCollisionPair<gxtGeom>> broadphaseSet = new HashSet<gxtBroadphaseCollisionPair<gxtGeom>>(testPairs); HashSet<gxtBroadphaseCollisionPair<gxtGeom>> seperated = new HashSet<gxtBroadphaseCollisionPair<gxtGeom>>(narrowphaseContacts.Keys); seperated.ExceptWith(broadphaseSet); foreach (gxtBroadphaseCollisionPair<gxtGeom> seperatedPair in seperated) { gxtGeom.FireOnSeperationEvents(seperatedPair.objA, seperatedPair.objB); narrowphaseContacts.Remove(seperatedPair); } foreach (gxtBroadphaseCollisionPair<gxtGeom> pair in broadphaseSet) { gxtCollisionResult collisionResult; if (CanCollide(pair.objA, pair.objB)) { collisionResult = gxtGJKCollider.Collide(pair.objA.Polygon, pair.objA.Position, pair.objB.Polygon, pair.objB.Position); if (collisionResult.Intersection) { // response can be canceled by user if (gxtGeom.FireOnCollisionEvents(pair.objA, pair.objB, collisionResult)) { gxtContactPair c; // If the response isn't enabled, the contact won't be either if (pair.objA.CollisionResponseEnabled && pair.objB.CollisionResponseEnabled) { // either adjust the exisiting contact or create a new one if (narrowphaseContacts.ContainsKey(pair)) { c = narrowphaseContacts[pair]; c.SetupContact(pair.objA, pair.objB, ref collisionResult, gxtFrictionType.AVERAGE_SQRT, true); } else { c = new gxtContactPair(); c.SetupContact(pair.objA, pair.objB, ref collisionResult, gxtFrictionType.AVERAGE_SQRT, true); narrowphaseContacts.Add(pair, c); } } else { // same thing as above, except the contacts are disabled if (narrowphaseContacts.ContainsKey(pair)) { c = narrowphaseContacts[pair]; c.SetupContact(pair.objA, pair.objB, ref collisionResult, gxtFrictionType.AVERAGE_SQRT, false); } else { c = new gxtContactPair(); c.SetupContact(pair.objA, pair.objB, ref collisionResult, gxtFrictionType.AVERAGE_SQRT, false); narrowphaseContacts.Add(pair, c); } } } } else { if (narrowphaseContacts.ContainsKey(pair)) { gxtGeom.FireOnSeperationEvents(pair.objA, pair.objB); narrowphaseContacts.Remove(pair); } } } } // broadphase pairs should be a hashset // construct a new temporary hashset // make new hashset = broapdhasepairs.exceptwith(narrowpahseContacts.keys); // fire on seperation events for each pair in the except hashset // remove every contact from the difference set // now, all previous contacts that aren't in broadphase collision anymore are handled // iterate thru the broadphase pairs // if intersection // fire on collision, make a new contact or update an existing one // else // if in the contact list // fire onseperation event // remove the contact /* gxtDebug.Assert(testPairs != null); //narrowphaseContacts.Clear(); // process all the broadphase pairs, determine if can collide (as a result of geom properties first) for (int i = 0; i < testPairs.Length; i++) { gxtBroadphaseCollisionPair<gxtGeom> pair = testPairs[i]; gxtCollisionResult collisionResult; if (CanCollide(pair.objA, pair.objB)) { collisionResult = gxtGJKCollider.Collide(pair.objA.Polygon, pair.objA.Position, pair.objB.Polygon, pair.objB.Position); if (collisionResult.Intersection) { if (gxtGeom.FireOnCollisionEvents(pair.objA, pair.objB, collisionResult)) { if (pair.objA.CollisionResponseEnabled && pair.objB.CollisionResponseEnabled) { gxtContact c; if (narrowphaseContacts.ContainsKey(pair.GetHashCode())) { c = narrowphaseContacts[pair.GetHashCode()]; c.SetupContact(pair.objA, pair.objB, ref collisionResult, gxtFrictionType.AVERAGE); } else { c = new gxtContact(); c.SetupContact(pair.objA, pair.objB, ref collisionResult, gxtFrictionType.AVERAGE); narrowphaseContacts.Add(pair.GetHashCode(), c); } } // existing contacts preserve values needed for impulses, but collision data is changed } } else { // if no intersection remove the contact int hashCode = pair.GetHashCode(); narrowphaseContacts.Remove(hashCode); } } } // remove contacts that didn't have a broadphase overlap List<int> removalHashCodes = new List<int>(); foreach (int hashCode in narrowphaseContacts.Keys) { bool found = false; for (int i = 0; i < testPairs.Length; ++i) { if (testPairs[i].GetHashCode() == hashCode) { found = true; break; } } if (!found) removalHashCodes.Add(hashCode); } for (int i = 0; i < removalHashCodes.Count; i++) { narrowphaseContacts.Remove(removalHashCodes[i]); } */ }