feat(physics): wire physx sdk into build
This commit is contained in:
911
engine/third_party/physx/source/lowlevelaabb/src/BpBroadPhaseSapAux.cpp
vendored
Normal file
911
engine/third_party/physx/source/lowlevelaabb/src/BpBroadPhaseSapAux.cpp
vendored
Normal file
@@ -0,0 +1,911 @@
|
||||
// Redistribution and use in source and binary forms, with or without
|
||||
// modification, are permitted provided that the following conditions
|
||||
// are met:
|
||||
// * Redistributions of source code must retain the above copyright
|
||||
// notice, this list of conditions and the following disclaimer.
|
||||
// * Redistributions in binary form must reproduce the above copyright
|
||||
// notice, this list of conditions and the following disclaimer in the
|
||||
// documentation and/or other materials provided with the distribution.
|
||||
// * Neither the name of NVIDIA CORPORATION nor the names of its
|
||||
// contributors may be used to endorse or promote products derived
|
||||
// from this software without specific prior written permission.
|
||||
//
|
||||
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY
|
||||
// EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
|
||||
// PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
|
||||
// CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
|
||||
// EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
|
||||
// PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
|
||||
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
|
||||
// OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
//
|
||||
// Copyright (c) 2008-2025 NVIDIA Corporation. All rights reserved.
|
||||
// Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved.
|
||||
// Copyright (c) 2001-2004 NovodeX AG. All rights reserved.
|
||||
|
||||
#include "BpBroadPhaseSapAux.h"
|
||||
#include "PxcScratchAllocator.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
|
||||
namespace Bp
|
||||
{
|
||||
|
||||
PX_FORCE_INLINE void PxBpHandleSwap(BpHandle& a, BpHandle& b)
|
||||
{
|
||||
const BpHandle c = a; a = b; b = c;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE void Sort(BpHandle& id0, BpHandle& id1)
|
||||
{
|
||||
if(id0>id1) PxBpHandleSwap(id0, id1);
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE bool DifferentPair(const BroadPhasePair& p, BpHandle id0, BpHandle id1)
|
||||
{
|
||||
return (id0!=p.mVolA) || (id1!=p.mVolB);
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE int Hash32Bits_1(int key)
|
||||
{
|
||||
key += ~(key << 15);
|
||||
key ^= (key >> 10);
|
||||
key += (key << 3);
|
||||
key ^= (key >> 6);
|
||||
key += ~(key << 11);
|
||||
key ^= (key >> 16);
|
||||
return key;
|
||||
}
|
||||
|
||||
PX_FORCE_INLINE PxU32 Hash(BpHandle id0, BpHandle id1)
|
||||
{
|
||||
return PxU32(Hash32Bits_1( int(PxU32(id0)|(PxU32(id1)<<16)) ));
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
SapPairManager::SapPairManager() :
|
||||
mHashTable (NULL),
|
||||
mNext (NULL),
|
||||
mHashSize (0),
|
||||
mHashCapacity (0),
|
||||
mMinAllowedHashCapacity (0),
|
||||
mActivePairs (NULL),
|
||||
mActivePairStates (NULL),
|
||||
mNbActivePairs (0),
|
||||
mActivePairsCapacity (0),
|
||||
mMask (0)
|
||||
{
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
SapPairManager::~SapPairManager()
|
||||
{
|
||||
PX_ASSERT(NULL==mHashTable);
|
||||
PX_ASSERT(NULL==mNext);
|
||||
PX_ASSERT(NULL==mActivePairs);
|
||||
PX_ASSERT(NULL==mActivePairStates);
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
void SapPairManager::init(const PxU32 size)
|
||||
{
|
||||
mHashTable=reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16(sizeof(BpHandle)*size), "BpHandle"));
|
||||
mNext=reinterpret_cast<BpHandle*>(PX_ALLOC(ALIGN_SIZE_16(sizeof(BpHandle)*size), "BpHandle"));
|
||||
mActivePairs=reinterpret_cast<BroadPhasePair*>(PX_ALLOC(ALIGN_SIZE_16(sizeof(BroadPhasePair)*size), "BroadPhasePair"));
|
||||
mActivePairStates=reinterpret_cast<PxU8*>(PX_ALLOC(ALIGN_SIZE_16(sizeof(PxU8)*size), "BroadPhaseContextSap ActivePairStates"));
|
||||
mHashCapacity=size;
|
||||
mMinAllowedHashCapacity = size;
|
||||
mActivePairsCapacity=size;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
void SapPairManager::release()
|
||||
{
|
||||
PX_FREE(mHashTable);
|
||||
PX_FREE(mNext);
|
||||
PX_FREE(mActivePairs);
|
||||
PX_FREE(mActivePairStates);
|
||||
mHashSize = 0;
|
||||
mHashCapacity = 0;
|
||||
mMinAllowedHashCapacity = 0;
|
||||
mNbActivePairs = 0;
|
||||
mActivePairsCapacity = 0;
|
||||
mMask = 0;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
const BroadPhasePair* SapPairManager::FindPair(BpHandle id0, BpHandle id1) const
|
||||
{
|
||||
if(0==mHashSize) return NULL; // Nothing has been allocated yet
|
||||
|
||||
// Order the ids
|
||||
Sort(id0, id1);
|
||||
|
||||
// Compute hash value for this pair
|
||||
PxU32 HashValue = Hash(id0, id1) & mMask;
|
||||
PX_ASSERT(HashValue<mHashCapacity);
|
||||
|
||||
// Look for it in the table
|
||||
PX_ASSERT(HashValue<mHashCapacity);
|
||||
PxU32 Offset = mHashTable[HashValue];
|
||||
PX_ASSERT(BP_INVALID_BP_HANDLE==Offset || Offset<mActivePairsCapacity);
|
||||
while(Offset!=BP_INVALID_BP_HANDLE && DifferentPair(mActivePairs[Offset], id0, id1))
|
||||
{
|
||||
PX_ASSERT(mActivePairs[Offset].mVolA!=BP_INVALID_BP_HANDLE);
|
||||
PX_ASSERT(Offset<mHashCapacity);
|
||||
Offset = mNext[Offset]; // Better to have a separate array for this
|
||||
PX_ASSERT(BP_INVALID_BP_HANDLE==Offset || Offset<mActivePairsCapacity);
|
||||
}
|
||||
if(Offset==BP_INVALID_BP_HANDLE) return NULL;
|
||||
PX_ASSERT(Offset<mNbActivePairs);
|
||||
// Match mActivePairs[Offset] => the pair is persistent
|
||||
PX_ASSERT(Offset<mActivePairsCapacity);
|
||||
return &mActivePairs[Offset];
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// Internal version saving hash computation
|
||||
PX_FORCE_INLINE BroadPhasePair* SapPairManager::FindPair(BpHandle id0, BpHandle id1, PxU32 hash_value) const
|
||||
{
|
||||
if(0==mHashSize) return NULL; // Nothing has been allocated yet
|
||||
|
||||
// Look for it in the table
|
||||
PX_ASSERT(hash_value<mHashCapacity);
|
||||
PxU32 Offset = mHashTable[hash_value];
|
||||
PX_ASSERT(BP_INVALID_BP_HANDLE==Offset || Offset<mActivePairsCapacity);
|
||||
while(Offset!=BP_INVALID_BP_HANDLE && DifferentPair(mActivePairs[Offset], id0, id1))
|
||||
{
|
||||
PX_ASSERT(mActivePairs[Offset].mVolA!=BP_INVALID_BP_HANDLE);
|
||||
PX_ASSERT(Offset<mHashCapacity);
|
||||
Offset = mNext[Offset]; // Better to have a separate array for this
|
||||
PX_ASSERT(BP_INVALID_BP_HANDLE==Offset || Offset<mActivePairsCapacity);
|
||||
}
|
||||
if(Offset==BP_INVALID_BP_HANDLE) return NULL;
|
||||
PX_ASSERT(Offset<mNbActivePairs);
|
||||
// Match mActivePairs[Offset] => the pair is persistent
|
||||
PX_ASSERT(Offset<mActivePairsCapacity);
|
||||
return &mActivePairs[Offset];
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
const BroadPhasePair* SapPairManager::AddPair(BpHandle id0, BpHandle id1, const PxU8 state)
|
||||
{
|
||||
// Order the ids
|
||||
Sort(id0, id1);
|
||||
|
||||
PxU32 HashValue = Hash(id0, id1) & mMask;
|
||||
|
||||
BroadPhasePair* P = FindPair(id0, id1, HashValue);
|
||||
if(P)
|
||||
{
|
||||
return P; // Persistent pair
|
||||
}
|
||||
|
||||
// This is a new pair
|
||||
if(mNbActivePairs >= mHashSize)
|
||||
{
|
||||
// Get more entries
|
||||
mHashSize = PxNextPowerOfTwo(mNbActivePairs+1);
|
||||
mMask = mHashSize-1;
|
||||
|
||||
reallocPairs(mHashSize>mHashCapacity);
|
||||
|
||||
// Recompute hash value with new hash size
|
||||
HashValue = Hash(id0, id1) & mMask;
|
||||
}
|
||||
|
||||
PX_ASSERT(mNbActivePairs<mActivePairsCapacity);
|
||||
BroadPhasePair* p = &mActivePairs[mNbActivePairs];
|
||||
p->mVolA = id0; // ### CMOVs would be nice here
|
||||
p->mVolB = id1;
|
||||
mActivePairStates[mNbActivePairs]=state;
|
||||
|
||||
PX_ASSERT(mNbActivePairs<mHashSize);
|
||||
PX_ASSERT(mNbActivePairs<mHashCapacity);
|
||||
PX_ASSERT(HashValue<mHashCapacity);
|
||||
mNext[mNbActivePairs] = mHashTable[HashValue];
|
||||
mHashTable[HashValue] = BpHandle(mNbActivePairs++);
|
||||
return p;
|
||||
}
|
||||
|
||||
///////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
void SapPairManager::RemovePair(BpHandle /*id0*/, BpHandle /*id1*/, PxU32 hash_value, PxU32 pair_index)
|
||||
{
|
||||
// Walk the hash table to fix mNext
|
||||
{
|
||||
PX_ASSERT(hash_value<mHashCapacity);
|
||||
PxU32 Offset = mHashTable[hash_value];
|
||||
PX_ASSERT(Offset!=BP_INVALID_BP_HANDLE);
|
||||
|
||||
PxU32 Previous=BP_INVALID_BP_HANDLE;
|
||||
while(Offset!=pair_index)
|
||||
{
|
||||
Previous = Offset;
|
||||
PX_ASSERT(Offset<mHashCapacity);
|
||||
Offset = mNext[Offset];
|
||||
}
|
||||
|
||||
// Let us go/jump us
|
||||
if(Previous!=BP_INVALID_BP_HANDLE)
|
||||
{
|
||||
PX_ASSERT(Previous<mHashCapacity);
|
||||
PX_ASSERT(pair_index<mHashCapacity);
|
||||
PX_ASSERT(mNext[Previous]==pair_index);
|
||||
mNext[Previous] = mNext[pair_index];
|
||||
}
|
||||
// else we were the first
|
||||
else
|
||||
{
|
||||
PX_ASSERT(hash_value<mHashCapacity);
|
||||
PX_ASSERT(pair_index<mHashCapacity);
|
||||
mHashTable[hash_value] = mNext[pair_index];
|
||||
}
|
||||
}
|
||||
// we're now free to reuse mNext[PairIndex] without breaking the list
|
||||
|
||||
#if PX_DEBUG
|
||||
PX_ASSERT(pair_index<mHashCapacity);
|
||||
mNext[pair_index]=BP_INVALID_BP_HANDLE;
|
||||
#endif
|
||||
// Invalidate entry
|
||||
|
||||
// Fill holes
|
||||
{
|
||||
// 1) Remove last pair
|
||||
const PxU32 LastPairIndex = mNbActivePairs-1;
|
||||
if(LastPairIndex==pair_index)
|
||||
{
|
||||
mNbActivePairs--;
|
||||
}
|
||||
else
|
||||
{
|
||||
PX_ASSERT(LastPairIndex<mActivePairsCapacity);
|
||||
const BroadPhasePair* Last = &mActivePairs[LastPairIndex];
|
||||
const PxU32 LastHashValue = Hash(Last->mVolA, Last->mVolB) & mMask;
|
||||
|
||||
// Walk the hash table to fix mNext
|
||||
PX_ASSERT(LastHashValue<mHashCapacity);
|
||||
PxU32 Offset = mHashTable[LastHashValue];
|
||||
PX_ASSERT(Offset!=BP_INVALID_BP_HANDLE);
|
||||
|
||||
PxU32 Previous=BP_INVALID_BP_HANDLE;
|
||||
while(Offset!=LastPairIndex)
|
||||
{
|
||||
Previous = Offset;
|
||||
PX_ASSERT(Offset<mHashCapacity);
|
||||
Offset = mNext[Offset];
|
||||
}
|
||||
|
||||
// Let us go/jump us
|
||||
if(Previous!=BP_INVALID_BP_HANDLE)
|
||||
{
|
||||
PX_ASSERT(Previous<mHashCapacity);
|
||||
PX_ASSERT(LastPairIndex<mHashCapacity);
|
||||
PX_ASSERT(mNext[Previous]==LastPairIndex);
|
||||
mNext[Previous] = mNext[LastPairIndex];
|
||||
}
|
||||
// else we were the first
|
||||
else
|
||||
{
|
||||
PX_ASSERT(LastHashValue<mHashCapacity);
|
||||
PX_ASSERT(LastPairIndex<mHashCapacity);
|
||||
mHashTable[LastHashValue] = mNext[LastPairIndex];
|
||||
}
|
||||
// we're now free to reuse mNext[LastPairIndex] without breaking the list
|
||||
|
||||
#if PX_DEBUG
|
||||
PX_ASSERT(LastPairIndex<mHashCapacity);
|
||||
mNext[LastPairIndex]=BP_INVALID_BP_HANDLE;
|
||||
#endif
|
||||
|
||||
// Don't invalidate entry since we're going to shrink the array
|
||||
|
||||
// 2) Re-insert in free slot
|
||||
PX_ASSERT(pair_index<mActivePairsCapacity);
|
||||
PX_ASSERT(LastPairIndex<mActivePairsCapacity);
|
||||
mActivePairs[pair_index] = mActivePairs[LastPairIndex];
|
||||
mActivePairStates[pair_index] = mActivePairStates[LastPairIndex];
|
||||
#if PX_DEBUG
|
||||
PX_ASSERT(pair_index<mHashCapacity);
|
||||
PX_ASSERT(mNext[pair_index]==BP_INVALID_BP_HANDLE);
|
||||
#endif
|
||||
PX_ASSERT(pair_index<mHashCapacity);
|
||||
PX_ASSERT(LastHashValue<mHashCapacity);
|
||||
mNext[pair_index] = mHashTable[LastHashValue];
|
||||
mHashTable[LastHashValue] = BpHandle(pair_index);
|
||||
|
||||
mNbActivePairs--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool SapPairManager::RemovePair(BpHandle id0, BpHandle id1)
|
||||
{
|
||||
// Order the ids
|
||||
Sort(id0, id1);
|
||||
|
||||
const PxU32 HashValue = Hash(id0, id1) & mMask;
|
||||
const BroadPhasePair* P = FindPair(id0, id1, HashValue);
|
||||
if(!P) return false;
|
||||
PX_ASSERT(P->mVolA==id0);
|
||||
PX_ASSERT(P->mVolB==id1);
|
||||
|
||||
RemovePair(id0, id1, HashValue, GetPairIndex(P));
|
||||
|
||||
shrinkMemory();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SapPairManager::RemovePairs(const PxBitMap& removedAABBs)
|
||||
{
|
||||
PxU32 i=0;
|
||||
while(i<mNbActivePairs)
|
||||
{
|
||||
const BpHandle id0 = mActivePairs[i].mVolA;
|
||||
const BpHandle id1 = mActivePairs[i].mVolB;
|
||||
if(removedAABBs.test(id0) || removedAABBs.test(id1))
|
||||
{
|
||||
const PxU32 HashValue = Hash(id0, id1) & mMask;
|
||||
RemovePair(id0, id1, HashValue, i);
|
||||
}
|
||||
else i++;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void SapPairManager::shrinkMemory()
|
||||
{
|
||||
//Compute the hash size given the current number of active pairs.
|
||||
const PxU32 correctHashSize = PxNextPowerOfTwo(mNbActivePairs);
|
||||
|
||||
//If we have the correct hash size then no action required.
|
||||
if(correctHashSize==mHashSize || (correctHashSize < mMinAllowedHashCapacity && mHashSize == mMinAllowedHashCapacity))
|
||||
return;
|
||||
|
||||
//The hash size can be reduced so take action.
|
||||
//Don't let the hash size fall below a threshold value.
|
||||
PxU32 newHashSize = correctHashSize;
|
||||
if(newHashSize < mMinAllowedHashCapacity)
|
||||
{
|
||||
newHashSize = mMinAllowedHashCapacity;
|
||||
}
|
||||
mHashSize = newHashSize;
|
||||
mMask = newHashSize-1;
|
||||
|
||||
reallocPairs( (newHashSize > mMinAllowedHashCapacity) || (mHashSize <= (mHashCapacity >> 2)) || (mHashSize <= (mActivePairsCapacity >> 2)));
|
||||
}
|
||||
|
||||
void SapPairManager::reallocPairs(const bool allocRequired)
|
||||
{
|
||||
if(allocRequired)
|
||||
{
|
||||
PX_FREE(mHashTable);
|
||||
mHashCapacity=mHashSize;
|
||||
mActivePairsCapacity=mHashSize;
|
||||
mHashTable = reinterpret_cast<BpHandle*>(PX_ALLOC(mHashSize*sizeof(BpHandle), "BpHandle"));
|
||||
|
||||
for(PxU32 i=0;i<mHashSize;i++)
|
||||
{
|
||||
mHashTable[i] = BP_INVALID_BP_HANDLE;
|
||||
}
|
||||
|
||||
// Get some bytes for new entries
|
||||
BroadPhasePair* NewPairs = reinterpret_cast<BroadPhasePair*>(PX_ALLOC(mHashSize * sizeof(BroadPhasePair), "BroadPhasePair")); PX_ASSERT(NewPairs);
|
||||
BpHandle* NewNext = reinterpret_cast<BpHandle*>(PX_ALLOC(mHashSize * sizeof(BpHandle), "BpHandle")); PX_ASSERT(NewNext);
|
||||
PxU8* NewPairStates = reinterpret_cast<PxU8*>(PX_ALLOC(mHashSize * sizeof(PxU8), "SapPairStates")); PX_ASSERT(NewPairStates);
|
||||
|
||||
// Copy old data if needed
|
||||
if(mNbActivePairs)
|
||||
{
|
||||
PxMemCopy(NewPairs, mActivePairs, mNbActivePairs*sizeof(BroadPhasePair));
|
||||
PxMemCopy(NewPairStates, mActivePairStates, mNbActivePairs*sizeof(PxU8));
|
||||
}
|
||||
|
||||
// ### check it's actually needed... probably only for pairs whose hash value was cut by the and
|
||||
// yeah, since Hash(id0, id1) is a constant
|
||||
// However it might not be needed to recompute them => only less efficient but still ok
|
||||
for(PxU32 i=0;i<mNbActivePairs;i++)
|
||||
{
|
||||
const PxU32 HashValue = Hash(mActivePairs[i].mVolA, mActivePairs[i].mVolB) & mMask; // New hash value with new mask
|
||||
NewNext[i] = mHashTable[HashValue];
|
||||
PX_ASSERT(HashValue<mHashCapacity);
|
||||
mHashTable[HashValue] = BpHandle(i);
|
||||
}
|
||||
|
||||
// Delete old data
|
||||
PX_FREE(mNext);
|
||||
PX_FREE(mActivePairs);
|
||||
PX_FREE(mActivePairStates);
|
||||
|
||||
// Assign new pointer
|
||||
mActivePairs = NewPairs;
|
||||
mActivePairStates = NewPairStates;
|
||||
mNext = NewNext;
|
||||
}
|
||||
else
|
||||
{
|
||||
for(PxU32 i=0;i<mHashSize;i++)
|
||||
{
|
||||
mHashTable[i] = BP_INVALID_BP_HANDLE;
|
||||
}
|
||||
|
||||
// ### check it's actually needed... probably only for pairs whose hash value was cut by the and
|
||||
// yeah, since Hash(id0, id1) is a constant
|
||||
// However it might not be needed to recompute them => only less efficient but still ok
|
||||
for(PxU32 i=0;i<mNbActivePairs;i++)
|
||||
{
|
||||
const PxU32 HashValue = Hash(mActivePairs[i].mVolA, mActivePairs[i].mVolB) & mMask; // New hash value with new mask
|
||||
mNext[i] = mHashTable[HashValue];
|
||||
PX_ASSERT(HashValue<mHashCapacity);
|
||||
mHashTable[HashValue] = BpHandle(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void resizeCreatedDeleted(BroadPhasePair*& pairs, PxU32& maxNumPairs)
|
||||
{
|
||||
PX_ASSERT(pairs);
|
||||
PX_ASSERT(maxNumPairs>0);
|
||||
const PxU32 newMaxNumPairs=2*maxNumPairs;
|
||||
BroadPhasePair* newPairs=reinterpret_cast<BroadPhasePair*>(PX_ALLOC(sizeof(BroadPhasePair)*newMaxNumPairs, "BroadPhasePair"));
|
||||
PxMemCopy(newPairs, pairs, sizeof(BroadPhasePair)*maxNumPairs);
|
||||
PX_FREE(pairs);
|
||||
pairs=newPairs;
|
||||
maxNumPairs=newMaxNumPairs;
|
||||
}
|
||||
|
||||
void ComputeCreatedDeletedPairsLists
|
||||
(const Bp::FilterGroup::Enum* PX_RESTRICT boxGroups,
|
||||
const BpHandle* PX_RESTRICT dataArray, const PxU32 dataArraySize,
|
||||
PxcScratchAllocator* scratchAllocator,
|
||||
BroadPhasePair*& createdPairsList, PxU32& numCreatedPairs, PxU32& maxNumCreatedPairs,
|
||||
BroadPhasePair*& deletedPairsList, PxU32& numDeletedPairs, PxU32& maxNumDeletedPairs,
|
||||
PxU32& numActualDeletedPairs,
|
||||
SapPairManager& pairManager)
|
||||
{
|
||||
#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
|
||||
PX_UNUSED(boxGroups);
|
||||
#endif
|
||||
|
||||
for(PxU32 i=0;i<dataArraySize;i++)
|
||||
{
|
||||
const PxU32 ID = dataArray[i];
|
||||
PX_ASSERT(ID<pairManager.mNbActivePairs);
|
||||
|
||||
const BroadPhasePair* PX_RESTRICT UP = pairManager.mActivePairs + ID;
|
||||
PX_ASSERT(pairManager.IsInArray(UP));
|
||||
|
||||
if(pairManager.IsRemoved(UP))
|
||||
{
|
||||
if(!pairManager.IsNew(UP))
|
||||
{
|
||||
// No need to call "ClearInArray" in this case, since the pair will get removed anyway
|
||||
if(numDeletedPairs==maxNumDeletedPairs)
|
||||
{
|
||||
BroadPhasePair* newDeletedPairsList = reinterpret_cast<BroadPhasePair*>(scratchAllocator->alloc(sizeof(BroadPhasePair)*2*maxNumDeletedPairs, true));
|
||||
PxMemCopy(newDeletedPairsList, deletedPairsList, sizeof(BroadPhasePair)*maxNumDeletedPairs);
|
||||
scratchAllocator->free(deletedPairsList);
|
||||
deletedPairsList = newDeletedPairsList;
|
||||
maxNumDeletedPairs = 2*maxNumDeletedPairs;
|
||||
}
|
||||
|
||||
PX_ASSERT(numDeletedPairs<maxNumDeletedPairs);
|
||||
//PX_ASSERT((uintptr_t)UP->mUserData != 0xcdcdcdcd);
|
||||
deletedPairsList[numDeletedPairs++] = BroadPhasePair(UP->mVolA,UP->mVolB/*, ID*/);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
pairManager.ClearInArray(UP);
|
||||
// Add => already there... Might want to create user data, though
|
||||
if(pairManager.IsNew(UP))
|
||||
{
|
||||
#if !BP_SAP_TEST_GROUP_ID_CREATEUPDATE
|
||||
if(groupFiltering(boxGroups[UP->mVolA], boxGroups[UP->mVolB]))
|
||||
#endif
|
||||
{
|
||||
if(numCreatedPairs==maxNumCreatedPairs)
|
||||
{
|
||||
BroadPhasePair* newCreatedPairsList = reinterpret_cast<BroadPhasePair*>(scratchAllocator->alloc(sizeof(BroadPhasePair)*2*maxNumCreatedPairs, true));
|
||||
PxMemCopy(newCreatedPairsList, createdPairsList, sizeof(BroadPhasePair)*maxNumCreatedPairs);
|
||||
scratchAllocator->free(createdPairsList);
|
||||
createdPairsList = newCreatedPairsList;
|
||||
maxNumCreatedPairs = 2*maxNumCreatedPairs;
|
||||
}
|
||||
|
||||
PX_ASSERT(numCreatedPairs<maxNumCreatedPairs);
|
||||
createdPairsList[numCreatedPairs++] = BroadPhasePair(UP->mVolA,UP->mVolB/*, ID*/);
|
||||
}
|
||||
pairManager.ClearNew(UP);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//Record pairs that are to be deleted because they were simultaneously created and removed
|
||||
//from different axis sorts.
|
||||
numActualDeletedPairs=numDeletedPairs;
|
||||
for(PxU32 i=0;i<dataArraySize;i++)
|
||||
{
|
||||
const PxU32 ID = dataArray[i];
|
||||
PX_ASSERT(ID<pairManager.mNbActivePairs);
|
||||
const BroadPhasePair* PX_RESTRICT UP = pairManager.mActivePairs + ID;
|
||||
if(pairManager.IsRemoved(UP) && pairManager.IsNew(UP))
|
||||
{
|
||||
PX_ASSERT(pairManager.IsInArray(UP));
|
||||
|
||||
if(numActualDeletedPairs==maxNumDeletedPairs)
|
||||
{
|
||||
BroadPhasePair* newDeletedPairsList = reinterpret_cast<BroadPhasePair*>(scratchAllocator->alloc(sizeof(BroadPhasePair)*2*maxNumDeletedPairs, true));
|
||||
PxMemCopy(newDeletedPairsList, deletedPairsList, sizeof(BroadPhasePair)*maxNumDeletedPairs);
|
||||
scratchAllocator->free(deletedPairsList);
|
||||
deletedPairsList = newDeletedPairsList;
|
||||
maxNumDeletedPairs = 2*maxNumDeletedPairs;
|
||||
}
|
||||
|
||||
PX_ASSERT(numActualDeletedPairs<=maxNumDeletedPairs);
|
||||
deletedPairsList[numActualDeletedPairs++] = BroadPhasePair(UP->mVolA,UP->mVolB/*, ID*/); //KS - should we even get here????
|
||||
}
|
||||
}
|
||||
|
||||
// // #### try batch removal here
|
||||
// for(PxU32 i=0;i<numActualDeletedPairs;i++)
|
||||
// {
|
||||
// const BpHandle id0 = deletedPairsList[i].mVolA;
|
||||
// const BpHandle id1 = deletedPairsList[i].mVolB;
|
||||
//#if PX_DEBUG
|
||||
// const bool Status = pairManager.RemovePair(id0, id1);
|
||||
// PX_ASSERT(Status);
|
||||
//#else
|
||||
// pairManager.RemovePair(id0, id1);
|
||||
//#endif
|
||||
// }
|
||||
|
||||
//Only report deleted pairs from different groups.
|
||||
#if !BP_SAP_TEST_GROUP_ID_CREATEUPDATE
|
||||
for(PxU32 i=0;i<numDeletedPairs;i++)
|
||||
{
|
||||
const PxU32 id0 = deletedPairsList[i].mVolA;
|
||||
const PxU32 id1 = deletedPairsList[i].mVolB;
|
||||
if(!groupFiltering(boxGroups[id0], boxGroups[id1]))
|
||||
{
|
||||
while((numDeletedPairs-1) > i && boxGroups[deletedPairsList[numDeletedPairs-1].mVolA] == boxGroups[deletedPairsList[numDeletedPairs-1].mVolB])
|
||||
{
|
||||
numDeletedPairs--;
|
||||
}
|
||||
deletedPairsList[i]=deletedPairsList[numDeletedPairs-1];
|
||||
numDeletedPairs--;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
//#define PRINT_STATS
|
||||
#ifdef PRINT_STATS
|
||||
#include <stdio.h>
|
||||
static PxU32 gNbIter = 0;
|
||||
static PxU32 gNbTests = 0;
|
||||
static PxU32 gNbPairs = 0;
|
||||
#define START_STATS gNbIter = gNbTests = gNbPairs = 0;
|
||||
#define INCREASE_STATS_NB_ITER gNbIter++;
|
||||
#define INCREASE_STATS_NB_TESTS gNbTests++;
|
||||
#define INCREASE_STATS_NB_PAIRS gNbPairs++;
|
||||
#define DUMP_STATS printf("%d %d %d\n", gNbIter, gNbTests, gNbPairs);
|
||||
#else
|
||||
#define START_STATS
|
||||
#define INCREASE_STATS_NB_ITER
|
||||
#define INCREASE_STATS_NB_TESTS
|
||||
#define INCREASE_STATS_NB_PAIRS
|
||||
#define DUMP_STATS
|
||||
#endif
|
||||
|
||||
void DataArray::Resize(PxcScratchAllocator* scratchAllocator)
|
||||
{
|
||||
BpHandle* newDataArray = reinterpret_cast<BpHandle*>(scratchAllocator->alloc(sizeof(BpHandle)*mCapacity*2, true));
|
||||
PxMemCopy(newDataArray, mData, mCapacity*sizeof(BpHandle));
|
||||
scratchAllocator->free(mData);
|
||||
mData = newDataArray;
|
||||
mCapacity *= 2;
|
||||
}
|
||||
|
||||
static PX_FORCE_INLINE int intersect2D(const BoxYZ& a, const BoxYZ& b)
|
||||
{
|
||||
const bool b0 = b.mMaxY < a.mMinY;
|
||||
const bool b1 = a.mMaxY < b.mMinY;
|
||||
const bool b2 = b.mMaxZ < a.mMinZ;
|
||||
const bool b3 = a.mMaxZ < b.mMinZ;
|
||||
// const bool b4 = b0 || b1 || b2 || b3;
|
||||
const bool b4 = b0 | b1 | b2 | b3;
|
||||
return !b4;
|
||||
}
|
||||
|
||||
void addPair(const BpHandle id0, const BpHandle id1, PxcScratchAllocator* scratchAllocator, SapPairManager& pairManager, DataArray& dataArray)
|
||||
{
|
||||
const BroadPhasePair* UP = reinterpret_cast<const BroadPhasePair*>(pairManager.AddPair(id0, id1, SapPairManager::PAIR_UNKNOWN));
|
||||
|
||||
//If the hash table has reached its limit then we're unable to add a new pair.
|
||||
if(NULL==UP)
|
||||
return;
|
||||
|
||||
PX_ASSERT(UP);
|
||||
if(pairManager.IsUnknown(UP))
|
||||
{
|
||||
pairManager.ClearState(UP);
|
||||
pairManager.SetInArray(UP);
|
||||
dataArray.AddData(pairManager.GetPairIndex(UP), scratchAllocator);
|
||||
pairManager.SetNew(UP);
|
||||
}
|
||||
pairManager.ClearRemoved(UP);
|
||||
}
|
||||
|
||||
void removePair(BpHandle id0, BpHandle id1, PxcScratchAllocator* scratchAllocator, SapPairManager& pairManager, DataArray& dataArray)
|
||||
{
|
||||
const BroadPhasePair* UP = reinterpret_cast<const BroadPhasePair*>(pairManager.FindPair(id0, id1));
|
||||
if(UP)
|
||||
{
|
||||
if(!pairManager.IsInArray(UP))
|
||||
{
|
||||
pairManager.SetInArray(UP);
|
||||
dataArray.AddData(pairManager.GetPairIndex(UP), scratchAllocator);
|
||||
}
|
||||
pairManager.SetRemoved(UP);
|
||||
}
|
||||
}
|
||||
|
||||
struct AddPairParams
|
||||
{
|
||||
AddPairParams(const PxU32* remap0, const PxU32* remap1, PxcScratchAllocator* alloc, SapPairManager* pm, DataArray* da) :
|
||||
mRemap0 (remap0),
|
||||
mRemap1 (remap1),
|
||||
mScratchAllocator (alloc),
|
||||
mPairManager (pm),
|
||||
mDataArray (da)
|
||||
{
|
||||
}
|
||||
|
||||
const PxU32* mRemap0;
|
||||
const PxU32* mRemap1;
|
||||
PxcScratchAllocator* mScratchAllocator;
|
||||
SapPairManager* mPairManager;
|
||||
DataArray* mDataArray;
|
||||
};
|
||||
|
||||
static void addPair(const AddPairParams* PX_RESTRICT params, const BpHandle id0_, const BpHandle id1_)
|
||||
{
|
||||
SapPairManager& pairManager = *params->mPairManager;
|
||||
|
||||
const BroadPhasePair* UP = reinterpret_cast<const BroadPhasePair*>(pairManager.AddPair(params->mRemap0[id0_], params->mRemap1[id1_], SapPairManager::PAIR_UNKNOWN));
|
||||
|
||||
//If the hash table has reached its limit then we're unable to add a new pair.
|
||||
if(NULL==UP)
|
||||
return;
|
||||
|
||||
PX_ASSERT(UP);
|
||||
if(pairManager.IsUnknown(UP))
|
||||
{
|
||||
pairManager.ClearState(UP);
|
||||
pairManager.SetInArray(UP);
|
||||
params->mDataArray->AddData(pairManager.GetPairIndex(UP), params->mScratchAllocator);
|
||||
pairManager.SetNew(UP);
|
||||
}
|
||||
pairManager.ClearRemoved(UP);
|
||||
}
|
||||
|
||||
// PT: TODO: use SIMD
|
||||
|
||||
AuxData::AuxData(PxU32 nb, const SapBox1D*const* PX_RESTRICT boxes, const BpHandle* PX_RESTRICT indicesSorted, const Bp::FilterGroup::Enum* PX_RESTRICT groupIds)
|
||||
{
|
||||
// PT: TODO: use scratch allocator / etc
|
||||
BoxX* PX_RESTRICT boxX = reinterpret_cast<BoxX*>(PX_ALLOC(sizeof(BoxX)*(nb+1), "mBoxX"));
|
||||
BoxYZ* PX_RESTRICT boxYZ = reinterpret_cast<BoxYZ*>(PX_ALLOC(sizeof(BoxYZ)*nb, "mBoxYZ"));
|
||||
Bp::FilterGroup::Enum* PX_RESTRICT groups = reinterpret_cast<Bp::FilterGroup::Enum*>(PX_ALLOC(sizeof(Bp::FilterGroup::Enum)*nb, "mGroups"));
|
||||
PxU32* PX_RESTRICT remap = reinterpret_cast<PxU32*>(PX_ALLOC(sizeof(PxU32)*nb, "mRemap"));
|
||||
|
||||
mBoxX = boxX;
|
||||
mBoxYZ = boxYZ;
|
||||
mGroups = groups;
|
||||
mRemap = remap;
|
||||
mNb = nb;
|
||||
|
||||
const PxU32 axis0 = 0;
|
||||
const PxU32 axis1 = 2;
|
||||
const PxU32 axis2 = 1;
|
||||
|
||||
const SapBox1D* PX_RESTRICT boxes0 = boxes[axis0];
|
||||
const SapBox1D* PX_RESTRICT boxes1 = boxes[axis1];
|
||||
const SapBox1D* PX_RESTRICT boxes2 = boxes[axis2];
|
||||
|
||||
for(PxU32 i=0;i<nb;i++)
|
||||
{
|
||||
const PxU32 boxID = indicesSorted[i];
|
||||
groups[i] = groupIds[boxID];
|
||||
remap[i] = boxID;
|
||||
|
||||
const SapBox1D& currentBoxX = boxes0[boxID];
|
||||
boxX[i].mMinX = currentBoxX.mMinMax[0];
|
||||
boxX[i].mMaxX = currentBoxX.mMinMax[1];
|
||||
|
||||
const SapBox1D& currentBoxY = boxes1[boxID];
|
||||
boxYZ[i].mMinY = currentBoxY.mMinMax[0];
|
||||
boxYZ[i].mMaxY = currentBoxY.mMinMax[1];
|
||||
|
||||
const SapBox1D& currentBoxZ = boxes2[boxID];
|
||||
boxYZ[i].mMinZ = currentBoxZ.mMinMax[0];
|
||||
boxYZ[i].mMaxZ = currentBoxZ.mMinMax[1];
|
||||
}
|
||||
boxX[nb].mMinX = 0xffffffff;
|
||||
}
|
||||
|
||||
AuxData::~AuxData()
|
||||
{
|
||||
PX_FREE(mRemap);
|
||||
PX_FREE(mGroups);
|
||||
PX_FREE(mBoxYZ);
|
||||
PX_FREE(mBoxX);
|
||||
}
|
||||
|
||||
void performBoxPruningNewNew( const AuxData* PX_RESTRICT auxData, PxcScratchAllocator* scratchAllocator,
|
||||
const bool* lut, SapPairManager& pairManager, BpHandle*& dataArray, PxU32& dataArraySize, PxU32& dataArrayCapacity)
|
||||
{
|
||||
const PxU32 nb = auxData->mNb;
|
||||
if(!nb)
|
||||
return;
|
||||
|
||||
DataArray da(dataArray, dataArraySize, dataArrayCapacity);
|
||||
|
||||
START_STATS
|
||||
{
|
||||
BoxX* boxX = auxData->mBoxX;
|
||||
BoxYZ* boxYZ = auxData->mBoxYZ;
|
||||
Bp::FilterGroup::Enum* groups = auxData->mGroups;
|
||||
PxU32* remap = auxData->mRemap;
|
||||
|
||||
AddPairParams params(remap, remap, scratchAllocator, &pairManager, &da);
|
||||
|
||||
PxU32 runningIndex = 0;
|
||||
PxU32 index0 = 0;
|
||||
|
||||
while(runningIndex<nb && index0<nb)
|
||||
{
|
||||
#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
|
||||
const Bp::FilterGroup::Enum group0 = groups[index0];
|
||||
#endif
|
||||
const BoxX& boxX0 = boxX[index0];
|
||||
|
||||
const BpHandle minLimit = boxX0.mMinX;
|
||||
while(boxX[runningIndex++].mMinX<minLimit);
|
||||
|
||||
const BpHandle maxLimit = boxX0.mMaxX;
|
||||
PxU32 index1 = runningIndex;
|
||||
while(boxX[index1].mMinX <= maxLimit)
|
||||
{
|
||||
INCREASE_STATS_NB_ITER
|
||||
#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
|
||||
if(groupFiltering(group0, groups[index1], lut))
|
||||
#endif
|
||||
{
|
||||
INCREASE_STATS_NB_TESTS
|
||||
if(intersect2D(boxYZ[index0], boxYZ[index1]))
|
||||
/* __m128i b = _mm_loadu_si128(reinterpret_cast<const __m128i*>(&boxYZ[index0].mMinY));
|
||||
b = _mm_shuffle_epi32(b, 78);
|
||||
const __m128i a = _mm_loadu_si128(reinterpret_cast<const __m128i*>(&boxYZ[index1].mMinY));
|
||||
const __m128i d = _mm_cmpgt_epi32(a, b);
|
||||
const int mask = _mm_movemask_epi8(d);
|
||||
if(mask==0x0000ff00)*/
|
||||
{
|
||||
INCREASE_STATS_NB_PAIRS
|
||||
addPair(¶ms, index0, index1);
|
||||
}
|
||||
}
|
||||
index1++;
|
||||
}
|
||||
index0++;
|
||||
}
|
||||
}
|
||||
DUMP_STATS
|
||||
|
||||
dataArray = da.mData;
|
||||
dataArraySize = da.mSize;
|
||||
dataArrayCapacity = da.mCapacity;
|
||||
}
|
||||
|
||||
template<int codepath>
|
||||
static void bipartitePruning(
|
||||
const PxU32 nb0, const BoxX* PX_RESTRICT boxX0, const BoxYZ* PX_RESTRICT boxYZ0, const PxU32* PX_RESTRICT remap0, const Bp::FilterGroup::Enum* PX_RESTRICT groups0,
|
||||
const PxU32 nb1, const BoxX* PX_RESTRICT boxX1, const BoxYZ* PX_RESTRICT boxYZ1, const PxU32* PX_RESTRICT remap1, const Bp::FilterGroup::Enum* PX_RESTRICT groups1,
|
||||
const bool* lut, PxcScratchAllocator* scratchAllocator, SapPairManager& pairManager, DataArray& dataArray)
|
||||
{
|
||||
AddPairParams params(remap0, remap1, scratchAllocator, &pairManager, &dataArray);
|
||||
|
||||
PxU32 runningIndex = 0;
|
||||
PxU32 index0 = 0;
|
||||
|
||||
while(runningIndex<nb1 && index0<nb0)
|
||||
{
|
||||
#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
|
||||
const Bp::FilterGroup::Enum group0 = groups0[index0];
|
||||
#endif
|
||||
|
||||
const BpHandle minLimit = boxX0[index0].mMinX;
|
||||
if(!codepath)
|
||||
{
|
||||
while(boxX1[runningIndex].mMinX<minLimit)
|
||||
runningIndex++;
|
||||
}
|
||||
else
|
||||
{
|
||||
while(boxX1[runningIndex].mMinX<=minLimit)
|
||||
runningIndex++;
|
||||
}
|
||||
|
||||
const BpHandle maxLimit = boxX0[index0].mMaxX;
|
||||
PxU32 index1 = runningIndex;
|
||||
while(boxX1[index1].mMinX <= maxLimit)
|
||||
{
|
||||
INCREASE_STATS_NB_ITER
|
||||
#if BP_SAP_TEST_GROUP_ID_CREATEUPDATE
|
||||
if(groupFiltering(group0, groups1[index1], lut))
|
||||
#endif
|
||||
{
|
||||
INCREASE_STATS_NB_TESTS
|
||||
if(intersect2D(boxYZ0[index0], boxYZ1[index1]))
|
||||
{
|
||||
INCREASE_STATS_NB_PAIRS
|
||||
addPair(¶ms, index0, index1);
|
||||
}
|
||||
}
|
||||
index1++;
|
||||
}
|
||||
index0++;
|
||||
}
|
||||
}
|
||||
|
||||
void performBoxPruningNewOld( const AuxData* PX_RESTRICT auxData0, const AuxData* PX_RESTRICT auxData1, PxcScratchAllocator* scratchAllocator,
|
||||
const bool* lut, SapPairManager& pairManager, BpHandle*& dataArray, PxU32& dataArraySize, PxU32& dataArrayCapacity)
|
||||
{
|
||||
const PxU32 nb0 = auxData0->mNb;
|
||||
const PxU32 nb1 = auxData1->mNb;
|
||||
|
||||
if(!nb0 || !nb1)
|
||||
return;
|
||||
|
||||
DataArray da(dataArray, dataArraySize, dataArrayCapacity);
|
||||
|
||||
START_STATS
|
||||
{
|
||||
const BoxX* boxX0 = auxData0->mBoxX;
|
||||
const BoxYZ* boxYZ0 = auxData0->mBoxYZ;
|
||||
const Bp::FilterGroup::Enum* groups0 = auxData0->mGroups;
|
||||
const PxU32* remap0 = auxData0->mRemap;
|
||||
|
||||
const BoxX* boxX1 = auxData1->mBoxX;
|
||||
const BoxYZ* boxYZ1 = auxData1->mBoxYZ;
|
||||
const Bp::FilterGroup::Enum* groups1 = auxData1->mGroups;
|
||||
const PxU32* remap1 = auxData1->mRemap;
|
||||
bipartitePruning<0>(nb0, boxX0, boxYZ0, remap0, groups0, nb1, boxX1, boxYZ1, remap1, groups1, lut, scratchAllocator, pairManager, da);
|
||||
bipartitePruning<1>(nb1, boxX1, boxYZ1, remap1, groups1, nb0, boxX0, boxYZ0, remap0, groups0, lut, scratchAllocator, pairManager, da);
|
||||
}
|
||||
DUMP_STATS
|
||||
|
||||
dataArray = da.mData;
|
||||
dataArraySize = da.mSize;
|
||||
dataArrayCapacity = da.mCapacity;
|
||||
}
|
||||
|
||||
} //namespace Bp
|
||||
|
||||
} //namespace physx
|
||||
|
||||
Reference in New Issue
Block a user