public BroadphasePair InternalFindPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1, int hash)
{
BroadphasePair[] rawPairArray = m_overlappingPairArray.GetRawArray();
int proxyId1 = proxy0.GetUid();
int proxyId2 = proxy1.GetUid();
//#if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
//if (proxyId1 > proxyId2)
// btSwap(proxyId1, proxyId2);
//#endif
int index = m_hashTable[hash];
while (index != BT_NULL_PAIR && EqualsPair(rawPairArray[index], proxyId1, proxyId2) == false)
{
index = m_next[index];
}
if ( index == BT_NULL_PAIR )
{
return null;
}
//btAssert(index < m_overlappingPairArray.size());
return rawPairArray[index];
}
private BroadphasePair InternalAddPair(BroadphaseProxy proxy0,BroadphaseProxy proxy1)
{
if(proxy0.m_uniqueId>proxy1.m_uniqueId)
{
BroadphaseProxy temp = proxy0;
proxy0 = proxy1;
proxy1 = temp;
}
int proxyId1 = proxy0.GetUid();
int proxyId2 = proxy1.GetUid();
int hash = (int)(GetHash((uint)proxyId1,(uint)proxyId2) & (m_overlappingPairArray.Capacity-1)); // New hash value with new mask
BroadphasePair pair = InternalFindPair(proxy0, proxy1, hash);
if (pair != null)
{
return pair;
}
else
{
/*for(int i=0;i<m_overlappingPairArray.size();++i)
{
if( (m_overlappingPairArray[i].m_pProxy0==proxy0)&&
(m_overlappingPairArray[i].m_pProxy1==proxy1))
{
printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
internalFindPair(proxy0, proxy1, hash);
}
}*/
int count = m_overlappingPairArray.Count;
int oldCapacity = m_overlappingPairArray.Capacity;
// MAN - 2.76 - uses expand noninitializing....??
//void* mem = &m_overlappingPairArray.expand();
//this is where we add an actual pair, so also call the 'ghost'
if (m_ghostPairCallback != null)
{
m_ghostPairCallback.AddOverlappingPair(proxy0, proxy1);
}
pair = new BroadphasePair(proxy0, proxy1);
m_overlappingPairArray.Add(pair);
int newCapacity = m_overlappingPairArray.Capacity;
if (oldCapacity < newCapacity)
{
GrowTables();
//hash with new capacity
hash = (int)(GetHash((uint)(proxyId1), (uint)(proxyId2)) & (m_overlappingPairArray.Capacity - 1));
}
m_next[count] = m_hashTable[hash];
m_hashTable[hash] = count;
return pair;
}
}
public virtual Object RemoveOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1, IDispatcher dispatcher)
{
OverlappingPairCacheGlobals.gRemovePairs++;
if(proxy0.m_uniqueId>proxy1.m_uniqueId)
{
BroadphaseProxy temp = proxy0;
proxy0 = proxy1;
proxy1 = temp;
}
int proxyId1 = proxy0.GetUid();
int proxyId2 = proxy1.GetUid();
int hash = (int)(GetHash((uint)(proxyId1),(uint)(proxyId2)) & (m_overlappingPairArray.Capacity-1));
BroadphasePair pair = InternalFindPair(proxy0, proxy1, hash);
if (pair == null)
{
return null;
}
CleanOverlappingPair(pair,dispatcher);
Object userData = pair.m_internalInfo1;
Debug.Assert(pair.m_pProxy0.GetUid() == proxyId1);
Debug.Assert(pair.m_pProxy1.GetUid() == proxyId2);
int pairIndex = m_overlappingPairArray.IndexOf(pair);
Debug.Assert(pairIndex < m_overlappingPairArray.Count);
// Remove the pair from the hash table.
int index = m_hashTable[hash];
Debug.Assert(index != BT_NULL_PAIR);
int previous = BT_NULL_PAIR;
while (index != pairIndex)
{
previous = index;
index = m_next[index];
}
if (previous != BT_NULL_PAIR)
{
Debug.Assert(m_next[previous] == pairIndex);
m_next[previous] = m_next[pairIndex];
}
else
{
m_hashTable[hash] = m_next[pairIndex];
}
// We now move the last pair into spot of the
// pair being removed. We need to fix the hash
// table indices to support the move.
int lastPairIndex = m_overlappingPairArray.Count - 1;
if (m_ghostPairCallback != null)
{
m_ghostPairCallback.RemoveOverlappingPair(proxy0, proxy1,dispatcher);
}
// If the removed pair is the last pair, we are done.
if (lastPairIndex == pairIndex)
{
m_overlappingPairArray.RemoveAt(lastPairIndex);
return userData;
}
// Remove the last pair from the hash table.
BroadphasePair last = m_overlappingPairArray[lastPairIndex];
/* missing swap here too, Nat. */
int lastHash = (int)(GetHash((uint)(last.m_pProxy0.GetUid()), (uint)(last.m_pProxy1.GetUid())) & (m_overlappingPairArray.Capacity-1));
index = m_hashTable[lastHash];
Debug.Assert(index != BT_NULL_PAIR);
previous = BT_NULL_PAIR;
while (index != lastPairIndex)
{
previous = index;
index = m_next[index];
}
if (previous != BT_NULL_PAIR)
{
Debug.Assert(m_next[previous] == lastPairIndex);
m_next[previous] = m_next[lastPairIndex];
}
else
{
m_hashTable[lastHash] = m_next[lastPairIndex];
}
// Copy the last pair into the remove pair's spot.
m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
// Insert the last pair into the hash table
m_next[pairIndex] = m_hashTable[lastHash];
m_hashTable[lastHash] = pairIndex;
m_overlappingPairArray.RemoveAt(lastPairIndex);
return userData;
}
public BroadphasePair FindPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1)
{
OverlappingPairCacheGlobals.gFindPairs++;
if(proxy0.m_uniqueId>proxy1.m_uniqueId)
{
BroadphaseProxy temp;
temp = proxy0;
proxy0 = proxy1;
proxy1 = temp;
}
int proxyId1 = proxy0.GetUid();
int proxyId2 = proxy1.GetUid();
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
int hash = (int)(GetHash((uint)(proxyId1), (uint)(proxyId2)) & (m_overlappingPairArray.Capacity-1));
if (hash >= m_hashTable.Count)
{
return null;
}
int index = m_hashTable[hash];
while (index != BT_NULL_PAIR && EqualsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
{
index = m_next[index];
}
if (index == BT_NULL_PAIR)
{
return null;
}
Debug.Assert(index < m_overlappingPairArray.Count);
return m_overlappingPairArray[index];
}
