b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
{
if(proxy0>proxy1)
b3Swap(proxy0,proxy1);
int proxyId1 = proxy0;
int proxyId2 = proxy1;
/*if (proxyId1 > proxyId2)
b3Swap(proxyId1, proxyId2);*/
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask
b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
if (pair != NULL)
{
return pair;
}
/*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.size();
int oldCapacity = m_overlappingPairArray.capacity();
void* mem = &m_overlappingPairArray.expandNonInitializing();
//this is where we add an actual pair, so also call the 'ghost'
// if (m_ghostPairCallback)
// m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
int newCapacity = m_overlappingPairArray.capacity();
if (oldCapacity < newCapacity)
{
growTables();
//hash with new capacity
hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
}
pair = new (mem) b3BroadphasePair(proxy0,proxy1);
// pair->m_pProxy0 = proxy0;
// pair->m_pProxy1 = proxy1;
//pair->m_algorithm = 0;
//pair->m_internalTmpValue = 0;
m_next[count] = m_hashTable[hash];
m_hashTable[hash] = count;
return pair;
}
void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1,btDispatcher* dispatcher)
{
gRemovePairs++;
if(proxy0->m_uniqueId>proxy1->m_uniqueId)
btSwap(proxy0,proxy1);
int proxyId1 = proxy0->getUid();
int proxyId2 = proxy1->getUid();
/*if (proxyId1 > proxyId2)
btSwap(proxyId1, proxyId2);*/
int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1),static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity()-1));
btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
if (pair == NULL)
{
return 0;
}
cleanOverlappingPair(*pair,dispatcher);
void* userData = pair->m_internalInfo1;
btAssert(pair->m_pProxy0->getUid() == proxyId1);
btAssert(pair->m_pProxy1->getUid() == proxyId2);
int pairIndex = int(pair - &m_overlappingPairArray[0]);
btAssert(pairIndex < m_overlappingPairArray.size());
// Remove the pair from the hash table.
int index = m_hashTable[hash];
btAssert(index != BT_NULL_PAIR);
int previous = BT_NULL_PAIR;
while (index != pairIndex)
{
previous = index;
index = m_next[index];
}
if (previous != BT_NULL_PAIR)
{
btAssert(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.size() - 1;
if (m_ghostPairCallback)
m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
// If the removed pair is the last pair, we are done.
if (lastPairIndex == pairIndex)
{
m_overlappingPairArray.pop_back();
return userData;
}
// Remove the last pair from the hash table.
const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
/* missing swap here too, Nat. */
int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->m_pProxy0->getUid()), static_cast<unsigned int>(last->m_pProxy1->getUid())) & (m_overlappingPairArray.capacity()-1));
index = m_hashTable[lastHash];
btAssert(index != BT_NULL_PAIR);
previous = BT_NULL_PAIR;
while (index != lastPairIndex)
{
previous = index;
index = m_next[index];
}
if (previous != BT_NULL_PAIR)
{
btAssert(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.pop_back();
return userData;
}
