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; }