public virtual void SortOverlappingPairs(IDispatcher dispatcher)
{
ObjectArray <BroadphasePair> tmpPairs = new ObjectArray <BroadphasePair>();
tmpPairs.AddRange(m_overlappingPairArray);
for (int i = 0; i < tmpPairs.Count; i++)
{
RemoveOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1, dispatcher);
}
for (int i = 0; i < m_next.Count; i++)
{
m_next[i] = BT_NULL_PAIR;
}
tmpPairs.Sort();
//tmpPairs.quickSort(btBroadphasePairSortPredicate());
for (int i = 0; i < tmpPairs.Count; i++)
{
AddOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1);
}
}
public virtual void SortOverlappingPairs(IDispatcher dispatcher)
{
ObjectArray<BroadphasePair> tmpPairs = new ObjectArray<BroadphasePair>();
tmpPairs.AddRange(m_overlappingPairArray);
for (int i = 0; i < tmpPairs.Count; i++)
{
RemoveOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1, dispatcher);
}
for (int i = 0; i < m_next.Count; i++)
{
m_next[i] = BT_NULL_PAIR;
}
tmpPairs.Sort();
//tmpPairs.quickSort(btBroadphasePairSortPredicate());
for (int i = 0; i < tmpPairs.Count; i++)
{
AddOverlappingPair(tmpPairs[i].m_pProxy0, tmpPairs[i].m_pProxy1);
}
}
protected virtual void SolveConstraints(ContactSolverInfo solverInfo)
{
//sorted version of all btTypedConstraint, based on islandId
ObjectArray<TypedConstraint> sortedConstraints = new ObjectArray<TypedConstraint>(GetNumConstraints());
if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("solveConstraints");
}
for (int i = 0; i < GetNumConstraints(); i++)
{
sortedConstraints.Add(m_constraints[i]);
}
// btAssert(0);
//sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
sortedConstraints.Sort(new SortConstraintOnIslandPredicate());
ObjectArray<TypedConstraint> constraintsPtr = GetNumConstraints() > 0 ? sortedConstraints : null;
InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, m_constraintSolver, constraintsPtr, sortedConstraints.Count, m_debugDrawer, m_dispatcher1);
if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("prepareSolve");
}
m_constraintSolver.PrepareSolve(GetCollisionWorld().GetNumCollisionObjects(), GetCollisionWorld().GetDispatcher().GetNumManifolds());
if (BulletGlobals.g_streamWriter != null && debugDiscreteDynamicsWorld)
{
BulletGlobals.g_streamWriter.WriteLine("buildAndProcessIsland");
}
/// solve all the constraints for this island
m_islandManager.BuildAndProcessIslands(GetCollisionWorld().GetDispatcher(), GetCollisionWorld(), solverCallback);
solverCallback.ProcessConstraints();
m_constraintSolver.AllSolved(solverInfo, m_debugDrawer);
}
public void MergeContacts(ContactArray contacts, bool normal_contact_average)
{
Clear();
if (contacts.Count == 0)
{
return;
}
if (contacts.Count == 1)
{
Add(contacts[0]);
return;
}
ObjectArray <CONTACT_KEY_TOKEN> keycontacts = new ObjectArray <CONTACT_KEY_TOKEN>();
keycontacts.Capacity = contacts.Count;
//fill key contacts
for (int i = 0; i < contacts.Count; i++)
{
keycontacts.Add(new CONTACT_KEY_TOKEN(contacts[i].CalcKeyContact(), i));
}
//sort keys
keycontacts.Sort(new Comparison <CONTACT_KEY_TOKEN>(CONTACT_KEY_TOKEN.SortPredicate));
// Merge contacts
int coincident_count = 0;
IndexedVector3[] coincident_normals = new IndexedVector3[MAX_COINCIDENT];
uint last_key = keycontacts[0].m_key;
uint key = 0;
Add(contacts[keycontacts[0].m_value]);
GIM_CONTACT pcontact = this[0];
int pcontactIndex = 0;
for (int i = 1; i < keycontacts.Count; i++)
{
key = keycontacts[i].m_key;
GIM_CONTACT scontact = contacts[keycontacts[i].m_value];
if (last_key == key)//same points
{
//merge contact
if (pcontact.m_depth - GIM_CONTACT.CONTACT_DIFF_EPSILON > scontact.m_depth)//)
{
//*pcontact = *scontact;
this[pcontactIndex] = scontact;
coincident_count = 0;
}
else if (normal_contact_average)
{
if (Math.Abs(pcontact.m_depth - scontact.m_depth) < GIM_CONTACT.CONTACT_DIFF_EPSILON)
{
if (coincident_count < MAX_COINCIDENT)
{
coincident_normals[coincident_count] = scontact.m_normal;
coincident_count++;
}
}
}
}
else
{//add new contact
if (normal_contact_average && coincident_count > 0)
{
pcontact.InterpolateNormals(coincident_normals, coincident_count);
coincident_count = 0;
}
Add(scontact);
pcontactIndex = Count - 1;
pcontact = this[pcontactIndex];
}
last_key = key;
}
}
