- Added serialization to btBvhTriangleMeshShape/btOptimizedBvh. See ConcaveDemo for example usage.
- added bt32BitAxisSweep3, which co-exists without recompilation, using template class. This broadphase is recommended for large worlds with many objects (> 16384), until btMultiSwap is finished. - Fixed some recent issues in Bullet 2.57 related to compound (thanks Proctoid) and memory allocations
This commit is contained in:
ejcoumans
@@ -21,660 +21,18 @@
|
||||
|
||||
#include <assert.h>
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
#include <stdio.h>
|
||||
void btAxisSweep3::debugPrintAxis(int axis, bool checkCardinality)
|
||||
btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles, btOverlappingPairCache* pairCache)
|
||||
:btAxisSweep3Internal(worldAabbMin,worldAabbMax,0xfffe,0xffff,maxHandles,pairCache)
|
||||
{
|
||||
int numEdges = m_pHandles[0].m_maxEdges[axis];
|
||||
printf("SAP Axis %d, numEdges=%d\n",axis,numEdges);
|
||||
|
||||
int i;
|
||||
for (i=0;i<numEdges+1;i++)
|
||||
{
|
||||
Edge* pEdge = m_pEdges[axis] + i;
|
||||
Handle* pHandlePrev = getHandle(pEdge->m_handle);
|
||||
int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
|
||||
char beginOrEnd;
|
||||
beginOrEnd=pEdge->IsMax()?'E':'B';
|
||||
printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex);
|
||||
}
|
||||
|
||||
if (checkCardinality)
|
||||
assert(numEdges == m_numHandles*2+1);
|
||||
}
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
|
||||
btBroadphaseProxy* btAxisSweep3::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
|
||||
{
|
||||
(void)shapeType;
|
||||
BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask);
|
||||
|
||||
Handle* handle = getHandle(handleId);
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
void btAxisSweep3::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
|
||||
{
|
||||
Handle* handle = static_cast<Handle*>(proxy);
|
||||
removeHandle(handle->m_handleId,dispatcher);
|
||||
}
|
||||
|
||||
void btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
|
||||
{
|
||||
Handle* handle = static_cast<Handle*>(proxy);
|
||||
updateHandle(handle->m_handleId,aabbMin,aabbMax);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
btAxisSweep3::btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles, btOverlappingPairCache* pairCache)
|
||||
:m_invalidPair(0),
|
||||
m_pairCache(pairCache),
|
||||
m_ownsPairCache(false)
|
||||
{
|
||||
if (!m_pairCache)
|
||||
{
|
||||
m_pairCache = new btOverlappingPairCache();
|
||||
m_ownsPairCache = true;
|
||||
}
|
||||
|
||||
//assert(bounds.HasVolume());
|
||||
|
||||
// 1 handle is reserved as sentinel
|
||||
btAssert(maxHandles > 1 && maxHandles < BP_MAX_HANDLES);
|
||||
|
||||
// init bounds
|
||||
m_worldAabbMin = worldAabbMin;
|
||||
m_worldAabbMax = worldAabbMax;
|
||||
|
||||
btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
|
||||
|
||||
BP_FP_INT_TYPE maxInt = BP_HANDLE_SENTINEL;
|
||||
|
||||
m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
|
||||
|
||||
// allocate handles buffer and put all handles on free list
|
||||
m_pHandles = new Handle[maxHandles];
|
||||
m_maxHandles = maxHandles;
|
||||
m_numHandles = 0;
|
||||
|
||||
// handle 0 is reserved as the null index, and is also used as the sentinel
|
||||
m_firstFreeHandle = 1;
|
||||
{
|
||||
for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
|
||||
m_pHandles[i].SetNextFree(i + 1);
|
||||
m_pHandles[maxHandles - 1].SetNextFree(0);
|
||||
}
|
||||
|
||||
{
|
||||
// allocate edge buffers
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_pEdges[i] = new Edge[maxHandles * 2];
|
||||
}
|
||||
//removed overlap management
|
||||
|
||||
// make boundary sentinels
|
||||
|
||||
m_pHandles[0].m_clientObject = 0;
|
||||
|
||||
for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
m_pHandles[0].m_minEdges[axis] = 0;
|
||||
m_pHandles[0].m_maxEdges[axis] = 1;
|
||||
|
||||
m_pEdges[axis][0].m_pos = 0;
|
||||
m_pEdges[axis][0].m_handle = 0;
|
||||
m_pEdges[axis][1].m_pos = BP_HANDLE_SENTINEL;
|
||||
m_pEdges[axis][1].m_handle = 0;
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
}
|
||||
btAssert(maxHandles > 1 && maxHandles < 32767);
|
||||
|
||||
}
|
||||
|
||||
btAxisSweep3::~btAxisSweep3()
|
||||
|
||||
bt32BitAxisSweep3::bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles , btOverlappingPairCache* pairCache )
|
||||
:btAxisSweep3Internal(worldAabbMin,worldAabbMax,0xfffffffe,0x7fffffff,maxHandles,pairCache)
|
||||
{
|
||||
|
||||
for (int i = 2; i >= 0; i--)
|
||||
delete[] m_pEdges[i];
|
||||
delete[] m_pHandles;
|
||||
|
||||
if (m_ownsPairCache)
|
||||
{
|
||||
delete m_pairCache;
|
||||
}
|
||||
// 1 handle is reserved as sentinel
|
||||
btAssert(maxHandles > 1 && maxHandles < 2147483647);
|
||||
}
|
||||
|
||||
void btAxisSweep3::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
|
||||
{
|
||||
btPoint3 clampedPoint(point);
|
||||
|
||||
|
||||
|
||||
clampedPoint.setMax(m_worldAabbMin);
|
||||
clampedPoint.setMin(m_worldAabbMax);
|
||||
|
||||
btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
|
||||
out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & BP_HANDLE_MASK) | isMax);
|
||||
out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & BP_HANDLE_MASK) | isMax);
|
||||
out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & BP_HANDLE_MASK) | isMax);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
BP_FP_INT_TYPE btAxisSweep3::allocHandle()
|
||||
{
|
||||
assert(m_firstFreeHandle);
|
||||
|
||||
BP_FP_INT_TYPE handle = m_firstFreeHandle;
|
||||
m_firstFreeHandle = getHandle(handle)->GetNextFree();
|
||||
m_numHandles++;
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
void btAxisSweep3::freeHandle(BP_FP_INT_TYPE handle)
|
||||
{
|
||||
assert(handle > 0 && handle < m_maxHandles);
|
||||
|
||||
getHandle(handle)->SetNextFree(m_firstFreeHandle);
|
||||
m_firstFreeHandle = handle;
|
||||
|
||||
m_numHandles--;
|
||||
}
|
||||
|
||||
|
||||
|
||||
BP_FP_INT_TYPE btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
|
||||
{
|
||||
// quantize the bounds
|
||||
BP_FP_INT_TYPE min[3], max[3];
|
||||
quantize(min, aabbMin, 0);
|
||||
quantize(max, aabbMax, 1);
|
||||
|
||||
// allocate a handle
|
||||
BP_FP_INT_TYPE handle = allocHandle();
|
||||
assert(handle!= 0xcdcd);
|
||||
|
||||
Handle* pHandle = getHandle(handle);
|
||||
|
||||
pHandle->m_handleId = handle;
|
||||
//pHandle->m_pOverlaps = 0;
|
||||
pHandle->m_clientObject = pOwner;
|
||||
pHandle->m_collisionFilterGroup = collisionFilterGroup;
|
||||
pHandle->m_collisionFilterMask = collisionFilterMask;
|
||||
|
||||
// compute current limit of edge arrays
|
||||
BP_FP_INT_TYPE limit = m_numHandles * 2;
|
||||
|
||||
|
||||
// insert new edges just inside the max boundary edge
|
||||
for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
|
||||
{
|
||||
|
||||
m_pHandles[0].m_maxEdges[axis] += 2;
|
||||
|
||||
m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
|
||||
|
||||
m_pEdges[axis][limit - 1].m_pos = min[axis];
|
||||
m_pEdges[axis][limit - 1].m_handle = handle;
|
||||
|
||||
m_pEdges[axis][limit].m_pos = max[axis];
|
||||
m_pEdges[axis][limit].m_handle = handle;
|
||||
|
||||
pHandle->m_minEdges[axis] = limit - 1;
|
||||
pHandle->m_maxEdges[axis] = limit;
|
||||
}
|
||||
|
||||
// now sort the new edges to their correct position
|
||||
sortMinDown(0, pHandle->m_minEdges[0], false);
|
||||
sortMaxDown(0, pHandle->m_maxEdges[0], false);
|
||||
sortMinDown(1, pHandle->m_minEdges[1], false);
|
||||
sortMaxDown(1, pHandle->m_maxEdges[1], false);
|
||||
sortMinDown(2, pHandle->m_minEdges[2], true);
|
||||
sortMaxDown(2, pHandle->m_maxEdges[2], true);
|
||||
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher)
|
||||
{
|
||||
|
||||
Handle* pHandle = getHandle(handle);
|
||||
|
||||
//explicitly remove the pairs containing the proxy
|
||||
//we could do it also in the sortMinUp (passing true)
|
||||
//todo: compare performance
|
||||
m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher);
|
||||
|
||||
|
||||
// compute current limit of edge arrays
|
||||
int limit = m_numHandles * 2;
|
||||
|
||||
int axis;
|
||||
|
||||
for (axis = 0;axis<3;axis++)
|
||||
{
|
||||
m_pHandles[0].m_maxEdges[axis] -= 2;
|
||||
}
|
||||
|
||||
// remove the edges by sorting them up to the end of the list
|
||||
for ( axis = 0; axis < 3; axis++)
|
||||
{
|
||||
Edge* pEdges = m_pEdges[axis];
|
||||
BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
|
||||
pEdges[max].m_pos = BP_HANDLE_SENTINEL;
|
||||
|
||||
sortMaxUp(axis,max,false);
|
||||
|
||||
|
||||
BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
|
||||
pEdges[i].m_pos = BP_HANDLE_SENTINEL;
|
||||
|
||||
|
||||
sortMinUp(axis,i,false);
|
||||
|
||||
pEdges[limit-1].m_handle = 0;
|
||||
pEdges[limit-1].m_pos = BP_HANDLE_SENTINEL;
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis,false);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
// free the handle
|
||||
freeHandle(handle);
|
||||
|
||||
|
||||
}
|
||||
|
||||
extern int gOverlappingPairs;
|
||||
|
||||
|
||||
void btAxisSweep3::calculateOverlappingPairs(btDispatcher* dispatcher)
|
||||
{
|
||||
|
||||
if (m_ownsPairCache)
|
||||
{
|
||||
btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
|
||||
|
||||
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
|
||||
overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
|
||||
|
||||
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
|
||||
m_invalidPair = 0;
|
||||
|
||||
|
||||
int i;
|
||||
|
||||
btBroadphasePair previousPair;
|
||||
previousPair.m_pProxy0 = 0;
|
||||
previousPair.m_pProxy1 = 0;
|
||||
previousPair.m_algorithm = 0;
|
||||
|
||||
|
||||
for (i=0;i<overlappingPairArray.size();i++)
|
||||
{
|
||||
|
||||
btBroadphasePair& pair = overlappingPairArray[i];
|
||||
|
||||
bool isDuplicate = (pair == previousPair);
|
||||
|
||||
previousPair = pair;
|
||||
|
||||
bool needsRemoval = false;
|
||||
|
||||
if (!isDuplicate)
|
||||
{
|
||||
bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
|
||||
|
||||
if (hasOverlap)
|
||||
{
|
||||
needsRemoval = false;//callback->processOverlap(pair);
|
||||
} else
|
||||
{
|
||||
needsRemoval = true;
|
||||
}
|
||||
} else
|
||||
{
|
||||
//remove duplicate
|
||||
needsRemoval = true;
|
||||
//should have no algorithm
|
||||
btAssert(!pair.m_algorithm);
|
||||
}
|
||||
|
||||
if (needsRemoval)
|
||||
{
|
||||
m_pairCache->cleanOverlappingPair(pair,dispatcher);
|
||||
|
||||
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
|
||||
// m_overlappingPairArray.pop_back();
|
||||
pair.m_pProxy0 = 0;
|
||||
pair.m_pProxy1 = 0;
|
||||
m_invalidPair++;
|
||||
gOverlappingPairs--;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
///if you don't like to skip the invalid pairs in the array, execute following code:
|
||||
#define CLEAN_INVALID_PAIRS 1
|
||||
#ifdef CLEAN_INVALID_PAIRS
|
||||
|
||||
//perform a sort, to sort 'invalid' pairs to the end
|
||||
overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
|
||||
|
||||
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
|
||||
m_invalidPair = 0;
|
||||
#endif//CLEAN_INVALID_PAIRS
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool btAxisSweep3::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
|
||||
{
|
||||
const Handle* pHandleA = static_cast<Handle*>(proxy0);
|
||||
const Handle* pHandleB = static_cast<Handle*>(proxy1);
|
||||
|
||||
//optimization 1: check the array index (memory address), instead of the m_pos
|
||||
|
||||
for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
|
||||
pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool btAxisSweep3::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB)
|
||||
{
|
||||
//optimization 1: check the array index (memory address), instead of the m_pos
|
||||
|
||||
for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
if (axis != ignoreAxis)
|
||||
{
|
||||
if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
|
||||
pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization)
|
||||
|
||||
/*for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos ||
|
||||
m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void btAxisSweep3::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax)
|
||||
{
|
||||
// assert(bounds.IsFinite());
|
||||
//assert(bounds.HasVolume());
|
||||
|
||||
Handle* pHandle = getHandle(handle);
|
||||
|
||||
// quantize the new bounds
|
||||
BP_FP_INT_TYPE min[3], max[3];
|
||||
quantize(min, aabbMin, 0);
|
||||
quantize(max, aabbMax, 1);
|
||||
|
||||
// update changed edges
|
||||
for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
|
||||
BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
|
||||
|
||||
int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
|
||||
int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
|
||||
|
||||
m_pEdges[axis][emin].m_pos = min[axis];
|
||||
m_pEdges[axis][emax].m_pos = max[axis];
|
||||
|
||||
// expand (only adds overlaps)
|
||||
if (dmin < 0)
|
||||
sortMinDown(axis, emin);
|
||||
|
||||
if (dmax > 0)
|
||||
sortMaxUp(axis, emax);
|
||||
|
||||
// shrink (only removes overlaps)
|
||||
if (dmin > 0)
|
||||
sortMinUp(axis, emin);
|
||||
|
||||
if (dmax < 0)
|
||||
sortMaxDown(axis, emax);
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// sorting a min edge downwards can only ever *add* overlaps
|
||||
void btAxisSweep3::sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
|
||||
{
|
||||
|
||||
Edge* pEdge = m_pEdges[axis] + edge;
|
||||
Edge* pPrev = pEdge - 1;
|
||||
Handle* pHandleEdge = getHandle(pEdge->m_handle);
|
||||
|
||||
while (pEdge->m_pos < pPrev->m_pos)
|
||||
{
|
||||
Handle* pHandlePrev = getHandle(pPrev->m_handle);
|
||||
|
||||
if (pPrev->IsMax())
|
||||
{
|
||||
// if previous edge is a maximum check the bounds and add an overlap if necessary
|
||||
if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev))
|
||||
{
|
||||
m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
|
||||
|
||||
//AddOverlap(pEdge->m_handle, pPrev->m_handle);
|
||||
|
||||
}
|
||||
|
||||
// update edge reference in other handle
|
||||
pHandlePrev->m_maxEdges[axis]++;
|
||||
}
|
||||
else
|
||||
pHandlePrev->m_minEdges[axis]++;
|
||||
|
||||
pHandleEdge->m_minEdges[axis]--;
|
||||
|
||||
// swap the edges
|
||||
Edge swap = *pEdge;
|
||||
*pEdge = *pPrev;
|
||||
*pPrev = swap;
|
||||
|
||||
// decrement
|
||||
pEdge--;
|
||||
pPrev--;
|
||||
}
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
}
|
||||
|
||||
// sorting a min edge upwards can only ever *remove* overlaps
|
||||
void btAxisSweep3::sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
|
||||
{
|
||||
Edge* pEdge = m_pEdges[axis] + edge;
|
||||
Edge* pNext = pEdge + 1;
|
||||
Handle* pHandleEdge = getHandle(pEdge->m_handle);
|
||||
|
||||
while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
|
||||
{
|
||||
Handle* pHandleNext = getHandle(pNext->m_handle);
|
||||
|
||||
if (pNext->IsMax())
|
||||
{
|
||||
// if next edge is maximum remove any overlap between the two handles
|
||||
if (updateOverlaps)
|
||||
{
|
||||
/*
|
||||
Handle* handle0 = getHandle(pEdge->m_handle);
|
||||
Handle* handle1 = getHandle(pNext->m_handle);
|
||||
btBroadphasePair tmpPair(*handle0,*handle1);
|
||||
removeOverlappingPair(tmpPair);
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
// update edge reference in other handle
|
||||
pHandleNext->m_maxEdges[axis]--;
|
||||
}
|
||||
else
|
||||
pHandleNext->m_minEdges[axis]--;
|
||||
|
||||
pHandleEdge->m_minEdges[axis]++;
|
||||
|
||||
// swap the edges
|
||||
Edge swap = *pEdge;
|
||||
*pEdge = *pNext;
|
||||
*pNext = swap;
|
||||
|
||||
// increment
|
||||
pEdge++;
|
||||
pNext++;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// sorting a max edge downwards can only ever *remove* overlaps
|
||||
void btAxisSweep3::sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
|
||||
{
|
||||
|
||||
Edge* pEdge = m_pEdges[axis] + edge;
|
||||
Edge* pPrev = pEdge - 1;
|
||||
Handle* pHandleEdge = getHandle(pEdge->m_handle);
|
||||
|
||||
while (pEdge->m_pos < pPrev->m_pos)
|
||||
{
|
||||
Handle* pHandlePrev = getHandle(pPrev->m_handle);
|
||||
|
||||
if (!pPrev->IsMax())
|
||||
{
|
||||
// if previous edge was a minimum remove any overlap between the two handles
|
||||
if (updateOverlaps)
|
||||
{
|
||||
//this is done during the overlappingpairarray iteration/narrowphase collision
|
||||
/*
|
||||
Handle* handle0 = getHandle(pEdge->m_handle);
|
||||
Handle* handle1 = getHandle(pPrev->m_handle);
|
||||
btBroadphasePair* pair = findPair(handle0,handle1);
|
||||
//assert(pair);
|
||||
|
||||
if (pair)
|
||||
{
|
||||
removeOverlappingPair(*pair);
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
// update edge reference in other handle
|
||||
pHandlePrev->m_minEdges[axis]++;;
|
||||
}
|
||||
else
|
||||
pHandlePrev->m_maxEdges[axis]++;
|
||||
|
||||
pHandleEdge->m_maxEdges[axis]--;
|
||||
|
||||
// swap the edges
|
||||
Edge swap = *pEdge;
|
||||
*pEdge = *pPrev;
|
||||
*pPrev = swap;
|
||||
|
||||
// decrement
|
||||
pEdge--;
|
||||
pPrev--;
|
||||
}
|
||||
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
}
|
||||
|
||||
// sorting a max edge upwards can only ever *add* overlaps
|
||||
void btAxisSweep3::sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
|
||||
{
|
||||
Edge* pEdge = m_pEdges[axis] + edge;
|
||||
Edge* pNext = pEdge + 1;
|
||||
Handle* pHandleEdge = getHandle(pEdge->m_handle);
|
||||
|
||||
while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
|
||||
{
|
||||
Handle* pHandleNext = getHandle(pNext->m_handle);
|
||||
|
||||
if (!pNext->IsMax())
|
||||
{
|
||||
// if next edge is a minimum check the bounds and add an overlap if necessary
|
||||
if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext))
|
||||
{
|
||||
Handle* handle0 = getHandle(pEdge->m_handle);
|
||||
Handle* handle1 = getHandle(pNext->m_handle);
|
||||
m_pairCache->addOverlappingPair(handle0,handle1);
|
||||
}
|
||||
|
||||
// update edge reference in other handle
|
||||
pHandleNext->m_minEdges[axis]--;
|
||||
}
|
||||
else
|
||||
pHandleNext->m_maxEdges[axis]--;
|
||||
|
||||
pHandleEdge->m_maxEdges[axis]++;
|
||||
|
||||
// swap the edges
|
||||
Edge swap = *pEdge;
|
||||
*pEdge = *pNext;
|
||||
*pNext = swap;
|
||||
|
||||
// increment
|
||||
pEdge++;
|
||||
pNext++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -26,28 +26,17 @@
|
||||
#include "btBroadphaseProxy.h"
|
||||
|
||||
|
||||
//Enable BP_USE_FIXEDPOINT_INT_32 if you need more then 32767 objects
|
||||
//#define BP_USE_FIXEDPOINT_INT_32 1
|
||||
|
||||
#ifdef BP_USE_FIXEDPOINT_INT_32
|
||||
#define BP_FP_INT_TYPE unsigned int
|
||||
#define BP_MAX_HANDLES 1500000 //arbitrary maximum number of handles
|
||||
#define BP_HANDLE_SENTINEL 0x7fffffff
|
||||
#define BP_HANDLE_MASK 0xfffffffe
|
||||
#else
|
||||
#define BP_FP_INT_TYPE unsigned short int
|
||||
#define BP_MAX_HANDLES 32767
|
||||
#define BP_HANDLE_SENTINEL 0xffff
|
||||
#define BP_HANDLE_MASK 0xfffe
|
||||
#endif //BP_USE_FIXEDPOINT_INT_32
|
||||
|
||||
//#define DEBUG_BROADPHASE 1
|
||||
|
||||
/// btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
|
||||
/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using integer coordinates instead of floats.
|
||||
/// The testOverlap check is optimized to check the array index, rather then the actual AABB coordinates/pos
|
||||
class btAxisSweep3 : public btBroadphaseInterface
|
||||
/// btAxisSweep3Internal is an internal template class that implements sweep and prune.
|
||||
/// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead.
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
class btAxisSweep3Internal : public btBroadphaseInterface
|
||||
{
|
||||
protected:
|
||||
|
||||
BP_FP_INT_TYPE m_bpHandleMask;
|
||||
BP_FP_INT_TYPE m_handleSentinel;
|
||||
|
||||
public:
|
||||
|
||||
@@ -85,7 +74,7 @@ protected:
|
||||
btVector3 m_quantize; // scaling factor for quantization
|
||||
|
||||
BP_FP_INT_TYPE m_numHandles; // number of active handles
|
||||
int m_maxHandles; // max number of handles
|
||||
BP_FP_INT_TYPE m_maxHandles; // max number of handles
|
||||
Handle* m_pHandles; // handles pool
|
||||
BP_FP_INT_TYPE m_firstFreeHandle; // free handles list
|
||||
|
||||
@@ -118,8 +107,11 @@ protected:
|
||||
void sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps = true);
|
||||
|
||||
public:
|
||||
btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384, btOverlappingPairCache* pairCache=0);
|
||||
virtual ~btAxisSweep3();
|
||||
|
||||
btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0);
|
||||
|
||||
virtual ~btAxisSweep3Internal();
|
||||
|
||||
|
||||
virtual void calculateOverlappingPairs(btDispatcher* dispatcher);
|
||||
|
||||
@@ -149,5 +141,708 @@ public:
|
||||
|
||||
};
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
#include <stdio.h>
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality)
|
||||
{
|
||||
int numEdges = m_pHandles[0].m_maxEdges[axis];
|
||||
printf("SAP Axis %d, numEdges=%d\n",axis,numEdges);
|
||||
|
||||
int i;
|
||||
for (i=0;i<numEdges+1;i++)
|
||||
{
|
||||
Edge* pEdge = m_pEdges[axis] + i;
|
||||
Handle* pHandlePrev = getHandle(pEdge->m_handle);
|
||||
int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
|
||||
char beginOrEnd;
|
||||
beginOrEnd=pEdge->IsMax()?'E':'B';
|
||||
printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex);
|
||||
}
|
||||
|
||||
if (checkCardinality)
|
||||
assert(numEdges == m_numHandles*2+1);
|
||||
}
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
btBroadphaseProxy* btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask)
|
||||
{
|
||||
(void)shapeType;
|
||||
BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask);
|
||||
|
||||
Handle* handle = getHandle(handleId);
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)
|
||||
{
|
||||
Handle* handle = static_cast<Handle*>(proxy);
|
||||
removeHandle(handle->m_handleId,dispatcher);
|
||||
}
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)
|
||||
{
|
||||
Handle* handle = static_cast<Handle*>(proxy);
|
||||
updateHandle(handle->m_handleId,aabbMin,aabbMax);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE maxHandles, btOverlappingPairCache* pairCache )
|
||||
:m_invalidPair(0),
|
||||
m_pairCache(pairCache),
|
||||
m_ownsPairCache(false),
|
||||
m_bpHandleMask(handleMask),
|
||||
m_handleSentinel(handleSentinel)
|
||||
{
|
||||
if (!m_pairCache)
|
||||
{
|
||||
m_pairCache = new btOverlappingPairCache();
|
||||
m_ownsPairCache = true;
|
||||
}
|
||||
|
||||
//assert(bounds.HasVolume());
|
||||
|
||||
// init bounds
|
||||
m_worldAabbMin = worldAabbMin;
|
||||
m_worldAabbMax = worldAabbMax;
|
||||
|
||||
btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin;
|
||||
|
||||
BP_FP_INT_TYPE maxInt = m_handleSentinel;
|
||||
|
||||
m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize;
|
||||
|
||||
// allocate handles buffer and put all handles on free list
|
||||
m_pHandles = new Handle[maxHandles];
|
||||
m_maxHandles = maxHandles;
|
||||
m_numHandles = 0;
|
||||
|
||||
// handle 0 is reserved as the null index, and is also used as the sentinel
|
||||
m_firstFreeHandle = 1;
|
||||
{
|
||||
for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++)
|
||||
m_pHandles[i].SetNextFree(i + 1);
|
||||
m_pHandles[maxHandles - 1].SetNextFree(0);
|
||||
}
|
||||
|
||||
{
|
||||
// allocate edge buffers
|
||||
for (int i = 0; i < 3; i++)
|
||||
m_pEdges[i] = new Edge[maxHandles * 2];
|
||||
}
|
||||
//removed overlap management
|
||||
|
||||
// make boundary sentinels
|
||||
|
||||
m_pHandles[0].m_clientObject = 0;
|
||||
|
||||
for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
m_pHandles[0].m_minEdges[axis] = 0;
|
||||
m_pHandles[0].m_maxEdges[axis] = 1;
|
||||
|
||||
m_pEdges[axis][0].m_pos = 0;
|
||||
m_pEdges[axis][0].m_handle = 0;
|
||||
m_pEdges[axis][1].m_pos = m_handleSentinel;
|
||||
m_pEdges[axis][1].m_handle = 0;
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal()
|
||||
{
|
||||
|
||||
for (int i = 2; i >= 0; i--)
|
||||
delete[] m_pEdges[i];
|
||||
delete[] m_pHandles;
|
||||
|
||||
if (m_ownsPairCache)
|
||||
{
|
||||
delete m_pairCache;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btPoint3& point, int isMax) const
|
||||
{
|
||||
btPoint3 clampedPoint(point);
|
||||
|
||||
|
||||
|
||||
clampedPoint.setMax(m_worldAabbMin);
|
||||
clampedPoint.setMin(m_worldAabbMax);
|
||||
|
||||
btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
|
||||
out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax);
|
||||
out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax);
|
||||
out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax);
|
||||
|
||||
}
|
||||
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle()
|
||||
{
|
||||
assert(m_firstFreeHandle);
|
||||
|
||||
BP_FP_INT_TYPE handle = m_firstFreeHandle;
|
||||
m_firstFreeHandle = getHandle(handle)->GetNextFree();
|
||||
m_numHandles++;
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle)
|
||||
{
|
||||
assert(handle > 0 && handle < m_maxHandles);
|
||||
|
||||
getHandle(handle)->SetNextFree(m_firstFreeHandle);
|
||||
m_firstFreeHandle = handle;
|
||||
|
||||
m_numHandles--;
|
||||
}
|
||||
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask)
|
||||
{
|
||||
// quantize the bounds
|
||||
BP_FP_INT_TYPE min[3], max[3];
|
||||
quantize(min, aabbMin, 0);
|
||||
quantize(max, aabbMax, 1);
|
||||
|
||||
// allocate a handle
|
||||
BP_FP_INT_TYPE handle = allocHandle();
|
||||
assert(handle!= 0xcdcd);
|
||||
|
||||
Handle* pHandle = getHandle(handle);
|
||||
|
||||
pHandle->m_handleId = handle;
|
||||
//pHandle->m_pOverlaps = 0;
|
||||
pHandle->m_clientObject = pOwner;
|
||||
pHandle->m_collisionFilterGroup = collisionFilterGroup;
|
||||
pHandle->m_collisionFilterMask = collisionFilterMask;
|
||||
|
||||
// compute current limit of edge arrays
|
||||
BP_FP_INT_TYPE limit = m_numHandles * 2;
|
||||
|
||||
|
||||
// insert new edges just inside the max boundary edge
|
||||
for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
|
||||
{
|
||||
|
||||
m_pHandles[0].m_maxEdges[axis] += 2;
|
||||
|
||||
m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
|
||||
|
||||
m_pEdges[axis][limit - 1].m_pos = min[axis];
|
||||
m_pEdges[axis][limit - 1].m_handle = handle;
|
||||
|
||||
m_pEdges[axis][limit].m_pos = max[axis];
|
||||
m_pEdges[axis][limit].m_handle = handle;
|
||||
|
||||
pHandle->m_minEdges[axis] = limit - 1;
|
||||
pHandle->m_maxEdges[axis] = limit;
|
||||
}
|
||||
|
||||
// now sort the new edges to their correct position
|
||||
sortMinDown(0, pHandle->m_minEdges[0], false);
|
||||
sortMaxDown(0, pHandle->m_maxEdges[0], false);
|
||||
sortMinDown(1, pHandle->m_minEdges[1], false);
|
||||
sortMaxDown(1, pHandle->m_maxEdges[1], false);
|
||||
sortMinDown(2, pHandle->m_minEdges[2], true);
|
||||
sortMaxDown(2, pHandle->m_maxEdges[2], true);
|
||||
|
||||
|
||||
return handle;
|
||||
}
|
||||
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher)
|
||||
{
|
||||
|
||||
Handle* pHandle = getHandle(handle);
|
||||
|
||||
//explicitly remove the pairs containing the proxy
|
||||
//we could do it also in the sortMinUp (passing true)
|
||||
//todo: compare performance
|
||||
m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher);
|
||||
|
||||
|
||||
// compute current limit of edge arrays
|
||||
int limit = m_numHandles * 2;
|
||||
|
||||
int axis;
|
||||
|
||||
for (axis = 0;axis<3;axis++)
|
||||
{
|
||||
m_pHandles[0].m_maxEdges[axis] -= 2;
|
||||
}
|
||||
|
||||
// remove the edges by sorting them up to the end of the list
|
||||
for ( axis = 0; axis < 3; axis++)
|
||||
{
|
||||
Edge* pEdges = m_pEdges[axis];
|
||||
BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
|
||||
pEdges[max].m_pos = m_handleSentinel;
|
||||
|
||||
sortMaxUp(axis,max,false);
|
||||
|
||||
|
||||
BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
|
||||
pEdges[i].m_pos = m_handleSentinel;
|
||||
|
||||
|
||||
sortMinUp(axis,i,false);
|
||||
|
||||
pEdges[limit-1].m_handle = 0;
|
||||
pEdges[limit-1].m_pos = m_handleSentinel;
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis,false);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
// free the handle
|
||||
freeHandle(handle);
|
||||
|
||||
|
||||
}
|
||||
|
||||
extern int gOverlappingPairs;
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher)
|
||||
{
|
||||
|
||||
if (m_ownsPairCache)
|
||||
{
|
||||
btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
|
||||
|
||||
//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
|
||||
overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
|
||||
|
||||
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
|
||||
m_invalidPair = 0;
|
||||
|
||||
|
||||
int i;
|
||||
|
||||
btBroadphasePair previousPair;
|
||||
previousPair.m_pProxy0 = 0;
|
||||
previousPair.m_pProxy1 = 0;
|
||||
previousPair.m_algorithm = 0;
|
||||
|
||||
|
||||
for (i=0;i<overlappingPairArray.size();i++)
|
||||
{
|
||||
|
||||
btBroadphasePair& pair = overlappingPairArray[i];
|
||||
|
||||
bool isDuplicate = (pair == previousPair);
|
||||
|
||||
previousPair = pair;
|
||||
|
||||
bool needsRemoval = false;
|
||||
|
||||
if (!isDuplicate)
|
||||
{
|
||||
bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1);
|
||||
|
||||
if (hasOverlap)
|
||||
{
|
||||
needsRemoval = false;//callback->processOverlap(pair);
|
||||
} else
|
||||
{
|
||||
needsRemoval = true;
|
||||
}
|
||||
} else
|
||||
{
|
||||
//remove duplicate
|
||||
needsRemoval = true;
|
||||
//should have no algorithm
|
||||
btAssert(!pair.m_algorithm);
|
||||
}
|
||||
|
||||
if (needsRemoval)
|
||||
{
|
||||
m_pairCache->cleanOverlappingPair(pair,dispatcher);
|
||||
|
||||
// m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
|
||||
// m_overlappingPairArray.pop_back();
|
||||
pair.m_pProxy0 = 0;
|
||||
pair.m_pProxy1 = 0;
|
||||
m_invalidPair++;
|
||||
gOverlappingPairs--;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
///if you don't like to skip the invalid pairs in the array, execute following code:
|
||||
#define CLEAN_INVALID_PAIRS 1
|
||||
#ifdef CLEAN_INVALID_PAIRS
|
||||
|
||||
//perform a sort, to sort 'invalid' pairs to the end
|
||||
overlappingPairArray.heapSort(btBroadphasePairSortPredicate());
|
||||
|
||||
overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
|
||||
m_invalidPair = 0;
|
||||
#endif//CLEAN_INVALID_PAIRS
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1)
|
||||
{
|
||||
const Handle* pHandleA = static_cast<Handle*>(proxy0);
|
||||
const Handle* pHandleB = static_cast<Handle*>(proxy1);
|
||||
|
||||
//optimization 1: check the array index (memory address), instead of the m_pos
|
||||
|
||||
for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
|
||||
pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap(int ignoreAxis,const Handle* pHandleA, const Handle* pHandleB)
|
||||
{
|
||||
//optimization 1: check the array index (memory address), instead of the m_pos
|
||||
|
||||
for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
if (axis != ignoreAxis)
|
||||
{
|
||||
if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
|
||||
pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//optimization 2: only 2 axis need to be tested (conflicts with 'delayed removal' optimization)
|
||||
|
||||
/*for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
if (m_pEdges[axis][pHandleA->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleB->m_minEdges[axis]].m_pos ||
|
||||
m_pEdges[axis][pHandleB->m_maxEdges[axis]].m_pos < m_pEdges[axis][pHandleA->m_minEdges[axis]].m_pos)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax)
|
||||
{
|
||||
// assert(bounds.IsFinite());
|
||||
//assert(bounds.HasVolume());
|
||||
|
||||
Handle* pHandle = getHandle(handle);
|
||||
|
||||
// quantize the new bounds
|
||||
BP_FP_INT_TYPE min[3], max[3];
|
||||
quantize(min, aabbMin, 0);
|
||||
quantize(max, aabbMax, 1);
|
||||
|
||||
// update changed edges
|
||||
for (int axis = 0; axis < 3; axis++)
|
||||
{
|
||||
BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis];
|
||||
BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis];
|
||||
|
||||
int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos;
|
||||
int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos;
|
||||
|
||||
m_pEdges[axis][emin].m_pos = min[axis];
|
||||
m_pEdges[axis][emax].m_pos = max[axis];
|
||||
|
||||
// expand (only adds overlaps)
|
||||
if (dmin < 0)
|
||||
sortMinDown(axis, emin);
|
||||
|
||||
if (dmax > 0)
|
||||
sortMaxUp(axis, emax);
|
||||
|
||||
// shrink (only removes overlaps)
|
||||
if (dmin > 0)
|
||||
sortMinUp(axis, emin);
|
||||
|
||||
if (dmax < 0)
|
||||
sortMaxDown(axis, emax);
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// sorting a min edge downwards can only ever *add* overlaps
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
|
||||
{
|
||||
|
||||
Edge* pEdge = m_pEdges[axis] + edge;
|
||||
Edge* pPrev = pEdge - 1;
|
||||
Handle* pHandleEdge = getHandle(pEdge->m_handle);
|
||||
|
||||
while (pEdge->m_pos < pPrev->m_pos)
|
||||
{
|
||||
Handle* pHandlePrev = getHandle(pPrev->m_handle);
|
||||
|
||||
if (pPrev->IsMax())
|
||||
{
|
||||
// if previous edge is a maximum check the bounds and add an overlap if necessary
|
||||
if (updateOverlaps && testOverlap(axis,pHandleEdge, pHandlePrev))
|
||||
{
|
||||
m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev);
|
||||
|
||||
//AddOverlap(pEdge->m_handle, pPrev->m_handle);
|
||||
|
||||
}
|
||||
|
||||
// update edge reference in other handle
|
||||
pHandlePrev->m_maxEdges[axis]++;
|
||||
}
|
||||
else
|
||||
pHandlePrev->m_minEdges[axis]++;
|
||||
|
||||
pHandleEdge->m_minEdges[axis]--;
|
||||
|
||||
// swap the edges
|
||||
Edge swap = *pEdge;
|
||||
*pEdge = *pPrev;
|
||||
*pPrev = swap;
|
||||
|
||||
// decrement
|
||||
pEdge--;
|
||||
pPrev--;
|
||||
}
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
}
|
||||
|
||||
// sorting a min edge upwards can only ever *remove* overlaps
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
|
||||
{
|
||||
Edge* pEdge = m_pEdges[axis] + edge;
|
||||
Edge* pNext = pEdge + 1;
|
||||
Handle* pHandleEdge = getHandle(pEdge->m_handle);
|
||||
|
||||
while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
|
||||
{
|
||||
Handle* pHandleNext = getHandle(pNext->m_handle);
|
||||
|
||||
if (pNext->IsMax())
|
||||
{
|
||||
// if next edge is maximum remove any overlap between the two handles
|
||||
if (updateOverlaps)
|
||||
{
|
||||
/*
|
||||
Handle* handle0 = getHandle(pEdge->m_handle);
|
||||
Handle* handle1 = getHandle(pNext->m_handle);
|
||||
btBroadphasePair tmpPair(*handle0,*handle1);
|
||||
removeOverlappingPair(tmpPair);
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
// update edge reference in other handle
|
||||
pHandleNext->m_maxEdges[axis]--;
|
||||
}
|
||||
else
|
||||
pHandleNext->m_minEdges[axis]--;
|
||||
|
||||
pHandleEdge->m_minEdges[axis]++;
|
||||
|
||||
// swap the edges
|
||||
Edge swap = *pEdge;
|
||||
*pEdge = *pNext;
|
||||
*pNext = swap;
|
||||
|
||||
// increment
|
||||
pEdge++;
|
||||
pNext++;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// sorting a max edge downwards can only ever *remove* overlaps
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
|
||||
{
|
||||
|
||||
Edge* pEdge = m_pEdges[axis] + edge;
|
||||
Edge* pPrev = pEdge - 1;
|
||||
Handle* pHandleEdge = getHandle(pEdge->m_handle);
|
||||
|
||||
while (pEdge->m_pos < pPrev->m_pos)
|
||||
{
|
||||
Handle* pHandlePrev = getHandle(pPrev->m_handle);
|
||||
|
||||
if (!pPrev->IsMax())
|
||||
{
|
||||
// if previous edge was a minimum remove any overlap between the two handles
|
||||
if (updateOverlaps)
|
||||
{
|
||||
//this is done during the overlappingpairarray iteration/narrowphase collision
|
||||
/*
|
||||
Handle* handle0 = getHandle(pEdge->m_handle);
|
||||
Handle* handle1 = getHandle(pPrev->m_handle);
|
||||
btBroadphasePair* pair = findPair(handle0,handle1);
|
||||
//assert(pair);
|
||||
|
||||
if (pair)
|
||||
{
|
||||
removeOverlappingPair(*pair);
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
|
||||
// update edge reference in other handle
|
||||
pHandlePrev->m_minEdges[axis]++;;
|
||||
}
|
||||
else
|
||||
pHandlePrev->m_maxEdges[axis]++;
|
||||
|
||||
pHandleEdge->m_maxEdges[axis]--;
|
||||
|
||||
// swap the edges
|
||||
Edge swap = *pEdge;
|
||||
*pEdge = *pPrev;
|
||||
*pPrev = swap;
|
||||
|
||||
// decrement
|
||||
pEdge--;
|
||||
pPrev--;
|
||||
}
|
||||
|
||||
|
||||
#ifdef DEBUG_BROADPHASE
|
||||
debugPrintAxis(axis);
|
||||
#endif //DEBUG_BROADPHASE
|
||||
|
||||
}
|
||||
|
||||
// sorting a max edge upwards can only ever *add* overlaps
|
||||
template <typename BP_FP_INT_TYPE>
|
||||
void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, bool updateOverlaps)
|
||||
{
|
||||
Edge* pEdge = m_pEdges[axis] + edge;
|
||||
Edge* pNext = pEdge + 1;
|
||||
Handle* pHandleEdge = getHandle(pEdge->m_handle);
|
||||
|
||||
while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
|
||||
{
|
||||
Handle* pHandleNext = getHandle(pNext->m_handle);
|
||||
|
||||
if (!pNext->IsMax())
|
||||
{
|
||||
// if next edge is a minimum check the bounds and add an overlap if necessary
|
||||
if (updateOverlaps && testOverlap(axis, pHandleEdge, pHandleNext))
|
||||
{
|
||||
Handle* handle0 = getHandle(pEdge->m_handle);
|
||||
Handle* handle1 = getHandle(pNext->m_handle);
|
||||
m_pairCache->addOverlappingPair(handle0,handle1);
|
||||
}
|
||||
|
||||
// update edge reference in other handle
|
||||
pHandleNext->m_minEdges[axis]--;
|
||||
}
|
||||
else
|
||||
pHandleNext->m_maxEdges[axis]--;
|
||||
|
||||
pHandleEdge->m_maxEdges[axis]++;
|
||||
|
||||
// swap the edges
|
||||
Edge swap = *pEdge;
|
||||
*pEdge = *pNext;
|
||||
*pNext = swap;
|
||||
|
||||
// increment
|
||||
pEdge++;
|
||||
pNext++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
/// btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
|
||||
/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats.
|
||||
/// For large worlds and many objects, use bt32BitAxisSweep3 instead. bt32BitAxisSweep3 has higher precision and allows more then 16384 objects at the cost of more memory and bit of performance.
|
||||
class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int>
|
||||
{
|
||||
public:
|
||||
|
||||
btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0);
|
||||
|
||||
};
|
||||
|
||||
/// bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune.
|
||||
/// This comes at the cost of more memory per handle, and a bit slower performance.
|
||||
/// It uses arrays rather then lists for storage of the 3 axis.
|
||||
class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int>
|
||||
{
|
||||
public:
|
||||
|
||||
bt32BitAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0);
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
Reference in New Issue
Block a user