/*
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]);
}
*/
}
