19#ifndef BT_AXIS_SWEEP_3_INTERNAL_H
20#define BT_AXIS_SWEEP_3_INTERNAL_H
30#define USE_OVERLAP_TEST_ON_REMOVES 1
35template <
typename BP_FP_INT_TYPE>
53 BP_FP_INT_TYPE
IsMax()
const {
return static_cast<BP_FP_INT_TYPE
>(
m_pos & 1);}
108 bool testOverlap2D(
const Handle* pHandleA,
const Handle* pHandleB,
int axis0,
int axis1);
110#ifdef DEBUG_BROADPHASE
111 void debugPrintAxis(
int axis,
bool checkCardinality=
true);
205#ifdef DEBUG_BROADPHASE
208template <
typename BP_FP_INT_TYPE>
211 int numEdges = m_pHandles[0].
m_maxEdges[axis];
212 printf(
"SAP Axis %d, numEdges=%d\n",axis,numEdges);
215 for (i=0;i<numEdges+1;i++)
217 Edge* pEdge = m_pEdges[axis] + i;
218 Handle* pHandlePrev = getHandle(pEdge->m_handle);
219 int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
221 beginOrEnd=pEdge->IsMax()?
'E':
'B';
222 printf(
" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex);
225 if (checkCardinality)
226 btAssert(numEdges == m_numHandles*2+1);
230template <
typename BP_FP_INT_TYPE>
234 BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher);
236 Handle* handle = getHandle(handleId);
238 if (m_raycastAccelerator)
240 btBroadphaseProxy* rayProxy = m_raycastAccelerator->
createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher);
248template <
typename BP_FP_INT_TYPE>
252 if (m_raycastAccelerator)
254 removeHandle(
static_cast<BP_FP_INT_TYPE
>(handle->
m_uniqueId), dispatcher);
257template <
typename BP_FP_INT_TYPE>
263 updateHandle(
static_cast<BP_FP_INT_TYPE
>(handle->
m_uniqueId), aabbMin, aabbMax,dispatcher);
264 if (m_raycastAccelerator)
269template <
typename BP_FP_INT_TYPE>
272 if (m_raycastAccelerator)
274 m_raycastAccelerator->
rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax);
278 BP_FP_INT_TYPE axis = 0;
280 for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
282 if (m_pEdges[axis][i].IsMax())
284 rayCallback.
process(getHandle(m_pEdges[axis][i].m_handle));
290template <
typename BP_FP_INT_TYPE>
293 if (m_raycastAccelerator)
295 m_raycastAccelerator->
aabbTest(aabbMin,aabbMax,callback);
299 BP_FP_INT_TYPE axis = 0;
301 for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++)
303 if (m_pEdges[axis][i].IsMax())
305 Handle* handle = getHandle(m_pEdges[axis][i].m_handle);
317template <
typename BP_FP_INT_TYPE>
326template <
typename BP_FP_INT_TYPE>
331 unsigned short vecInMin[3];
332 unsigned short vecInMax[3];
342 aabbMin += m_worldAabbMin;
345 aabbMax += m_worldAabbMin;
351template <
typename BP_FP_INT_TYPE>
353:m_bpHandleMask(handleMask),
354m_handleSentinel(handleSentinel),
355m_pairCache(pairCache),
356m_userPairCallback(0),
357m_ownsPairCache(false),
359m_raycastAccelerator(0)
361 BP_FP_INT_TYPE maxHandles =
static_cast<BP_FP_INT_TYPE
>(userMaxHandles+1);
370 if (!disableRaycastAccelerator)
399 m_pHandles[i].SetNextFree(
static_cast<BP_FP_INT_TYPE
>(i + 1));
405 for (
int i = 0; i < 3; i++)
417 for (
int axis = 0; axis < 3; axis++)
426#ifdef DEBUG_BROADPHASE
427 debugPrintAxis(axis);
434template <
typename BP_FP_INT_TYPE>
437 if (m_raycastAccelerator)
445 for (
int i = 2; i >= 0; i--)
449 delete [] m_pHandles;
458template <
typename BP_FP_INT_TYPE>
461#ifdef OLD_CLAMPING_METHOD
465 clampedPoint.
setMax(m_worldAabbMin);
466 clampedPoint.
setMin(m_worldAabbMax);
467 btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
468 out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.
getX() & m_bpHandleMask) | isMax);
469 out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.
getY() & m_bpHandleMask) | isMax);
470 out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.
getZ() & m_bpHandleMask) | isMax);
472 btVector3 v = (point - m_worldAabbMin) * m_quantize;
473 out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax);
474 out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax);
475 out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax);
480template <
typename BP_FP_INT_TYPE>
485 BP_FP_INT_TYPE handle = m_firstFreeHandle;
486 m_firstFreeHandle = getHandle(handle)->GetNextFree();
492template <
typename BP_FP_INT_TYPE>
495 btAssert(handle > 0 && handle < m_maxHandles);
497 getHandle(handle)->SetNextFree(m_firstFreeHandle);
498 m_firstFreeHandle = handle;
504template <
typename BP_FP_INT_TYPE>
508 BP_FP_INT_TYPE min[3], max[3];
509 quantize(min, aabbMin, 0);
510 quantize(max, aabbMax, 1);
513 BP_FP_INT_TYPE handle = allocHandle();
516 Handle* pHandle = getHandle(handle);
518 pHandle->
m_uniqueId =
static_cast<int>(handle);
525 BP_FP_INT_TYPE limit =
static_cast<BP_FP_INT_TYPE
>(m_numHandles * 2);
529 for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
534 m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
536 m_pEdges[axis][limit - 1].
m_pos = min[axis];
537 m_pEdges[axis][limit - 1].
m_handle = handle;
539 m_pEdges[axis][limit].
m_pos = max[axis];
540 m_pEdges[axis][limit].
m_handle = handle;
542 pHandle->
m_minEdges[axis] =
static_cast<BP_FP_INT_TYPE
>(limit - 1);
547 sortMinDown(0, pHandle->
m_minEdges[0], dispatcher,
false);
548 sortMaxDown(0, pHandle->
m_maxEdges[0], dispatcher,
false);
549 sortMinDown(1, pHandle->
m_minEdges[1], dispatcher,
false);
550 sortMaxDown(1, pHandle->
m_maxEdges[1], dispatcher,
false);
551 sortMinDown(2, pHandle->
m_minEdges[2], dispatcher,
true);
552 sortMaxDown(2, pHandle->
m_maxEdges[2], dispatcher,
true);
559template <
typename BP_FP_INT_TYPE>
563 Handle* pHandle = getHandle(handle);
574 int limit =
static_cast<int>(m_numHandles * 2);
578 for (axis = 0;axis<3;axis++)
584 for ( axis = 0; axis < 3; axis++)
586 Edge* pEdges = m_pEdges[axis];
587 BP_FP_INT_TYPE max = pHandle->
m_maxEdges[axis];
588 pEdges[max].
m_pos = m_handleSentinel;
590 sortMaxUp(axis,max,dispatcher,
false);
594 pEdges[i].
m_pos = m_handleSentinel;
597 sortMinUp(axis,i,dispatcher,
false);
600 pEdges[limit-1].
m_pos = m_handleSentinel;
602#ifdef DEBUG_BROADPHASE
603 debugPrintAxis(axis,
false);
616template <
typename BP_FP_INT_TYPE>
619 if (m_numHandles == 0)
621 m_firstFreeHandle = 1;
623 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
624 m_pHandles[i].SetNextFree(
static_cast<BP_FP_INT_TYPE
>(i + 1));
634template <
typename BP_FP_INT_TYPE>
646 overlappingPairArray.
resize(overlappingPairArray.
size() - m_invalidPair);
658 for (i=0;i<overlappingPairArray.
size();i++)
663 bool isDuplicate = (pair == previousPair);
667 bool needsRemoval =
false;
676 needsRemoval =
false;
704 #define CLEAN_INVALID_PAIRS 1
705 #ifdef CLEAN_INVALID_PAIRS
710 overlappingPairArray.
resize(overlappingPairArray.
size() - m_invalidPair);
720template <
typename BP_FP_INT_TYPE>
728 for (
int axis = 0; axis < 3; axis++)
739template <
typename BP_FP_INT_TYPE>
754template <
typename BP_FP_INT_TYPE>
760 Handle* pHandle = getHandle(handle);
763 BP_FP_INT_TYPE min[3], max[3];
764 quantize(min, aabbMin, 0);
765 quantize(max, aabbMax, 1);
768 for (
int axis = 0; axis < 3; axis++)
770 BP_FP_INT_TYPE emin = pHandle->
m_minEdges[axis];
771 BP_FP_INT_TYPE emax = pHandle->
m_maxEdges[axis];
773 int dmin = (int)min[axis] - (
int)m_pEdges[axis][emin].
m_pos;
774 int dmax = (int)max[axis] - (
int)m_pEdges[axis][emax].
m_pos;
776 m_pEdges[axis][emin].
m_pos = min[axis];
777 m_pEdges[axis][emax].
m_pos = max[axis];
781 sortMinDown(axis, emin,dispatcher,
true);
784 sortMaxUp(axis, emax,dispatcher,
true);
788 sortMinUp(axis, emin,dispatcher,
true);
791 sortMaxDown(axis, emax,dispatcher,
true);
793#ifdef DEBUG_BROADPHASE
794 debugPrintAxis(axis);
805template <
typename BP_FP_INT_TYPE>
809 Edge* pEdge = m_pEdges[axis] + edge;
810 Edge* pPrev = pEdge - 1;
820 const int axis1 = (1 << axis) & 3;
821 const int axis2 = (1 << axis1) & 3;
822 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2))
825 if (m_userPairCallback)
850#ifdef DEBUG_BROADPHASE
851 debugPrintAxis(axis);
857template <
typename BP_FP_INT_TYPE>
860 Edge* pEdge = m_pEdges[axis] + edge;
861 Edge* pNext = pEdge + 1;
872 const int axis1 = (1 << axis) & 3;
873 const int axis2 = (1 << axis1) & 3;
878 && testOverlap2D(handle0,handle1,axis1,axis2)
885 if (m_userPairCallback)
913template <
typename BP_FP_INT_TYPE>
917 Edge* pEdge = m_pEdges[axis] + edge;
918 Edge* pPrev = pEdge - 1;
930 const int axis1 = (1 << axis) & 3;
931 const int axis2 = (1 << axis1) & 3;
935 && testOverlap2D(handle0,handle1,axis1,axis2)
943 if (m_userPairCallback)
969#ifdef DEBUG_BROADPHASE
970 debugPrintAxis(axis);
976template <
typename BP_FP_INT_TYPE>
979 Edge* pEdge = m_pEdges[axis] + edge;
980 Edge* pNext = pEdge + 1;
987 const int axis1 = (1 << axis) & 3;
988 const int axis2 = (1 << axis1) & 3;
993 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2))
998 if (m_userPairCallback)
bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1, const btVector3 &aabbMin2, const btVector3 &aabbMax2)
conservative test for overlap between two aabbs
#define btAlignedFree(ptr)
#define btAlignedAlloc(size, alignment)
#define USE_OVERLAP_TEST_ON_REMOVES
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
#define SIMD_FORCE_INLINE
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void quickSort(const L &CompareFunc)
BP_FP_INT_TYPE IsMax() const
void SetNextFree(BP_FP_INT_TYPE next)
btBroadphaseProxy * m_dbvtProxy
BP_FP_INT_TYPE GetNextFree() const
BP_FP_INT_TYPE m_maxEdges[3]
BT_DECLARE_ALIGNED_ALLOCATOR()
BP_FP_INT_TYPE m_minEdges[3]
The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase.
virtual void getAabb(btBroadphaseProxy *proxy, btVector3 &aabbMin, btVector3 &aabbMax) const
btOverlappingPairCache * m_pairCache
BP_FP_INT_TYPE m_handleSentinel
bool testAabbOverlap(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)
void unQuantize(btBroadphaseProxy *proxy, btVector3 &aabbMin, btVector3 &aabbMax) const
unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result
void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
virtual void rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, btBroadphaseRayCallback &rayCallback, const btVector3 &aabbMin=btVector3(0, 0, 0), const btVector3 &aabbMax=btVector3(0, 0, 0))
BP_FP_INT_TYPE m_bpHandleMask
BP_FP_INT_TYPE allocHandle()
BP_FP_INT_TYPE m_numHandles
bool testOverlap2D(const Handle *pHandleA, const Handle *pHandleB, int axis0, int axis1)
const btOverlappingPairCallback * getOverlappingPairUserCallback() const
BP_FP_INT_TYPE addHandle(const btVector3 &aabbMin, const btVector3 &aabbMax, void *pOwner, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
void removeHandle(BP_FP_INT_TYPE handle, btDispatcher *dispatcher)
virtual btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
virtual void printStats()
Handle * getHandle(BP_FP_INT_TYPE index) const
const btOverlappingPairCache * getOverlappingPairCache() const
void quantize(BP_FP_INT_TYPE *out, const btVector3 &point, int isMax) const
BP_FP_INT_TYPE getNumHandles() const
btDbvtBroadphase * m_raycastAccelerator
additional dynamic aabb structure, used to accelerate ray cast queries.
btOverlappingPairCache * getOverlappingPairCache()
BP_FP_INT_TYPE m_maxHandles
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
virtual void calculateOverlappingPairs(btDispatcher *dispatcher)
calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during th...
void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
void freeHandle(BP_FP_INT_TYPE handle)
virtual void aabbTest(const btVector3 &aabbMin, const btVector3 &aabbMax, btBroadphaseAabbCallback &callback)
void updateHandle(BP_FP_INT_TYPE handle, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)
btAxisSweep3Internal(const btVector3 &worldAabbMin, const btVector3 &worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles=16384, btOverlappingPairCache *pairCache=0, bool disableRaycastAccelerator=false)
virtual ~btAxisSweep3Internal()
virtual void getBroadphaseAabb(btVector3 &aabbMin, btVector3 &aabbMax) const
getAabb returns the axis aligned bounding box in the 'global' coordinate frame will add some transfor...
btOverlappingPairCallback * m_userPairCallback
btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pai...
BT_DECLARE_ALIGNED_ALLOCATOR()
virtual void destroyProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
void processAllOverlappingPairs(btOverlapCallback *callback)
BP_FP_INT_TYPE m_firstFreeHandle
void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
void setOverlappingPairUserCallback(btOverlappingPairCallback *pairCallback)
virtual void resetPool(btDispatcher *dispatcher)
reset broadphase internal structures, to ensure determinism/reproducability
btOverlappingPairCache * m_nullPairCache
void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher *dispatcher, bool updateOverlaps)
The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase.
The btBroadphaseInterface class provides an interface to detect aabb-overlapping object pairs.
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman,...
btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
The btOverlappingPairCache provides an interface for overlapping pair management (add,...
virtual btBroadphasePairArray & getOverlappingPairArray()=0
virtual void cleanOverlappingPair(btBroadphasePair &pair, btDispatcher *dispatcher)=0
virtual ~btOverlappingPairCache()
virtual bool hasDeferredRemoval()=0
The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/rem...
virtual void * removeOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1, btDispatcher *dispatcher)=0
virtual void removeOverlappingPairsContainingProxy(btBroadphaseProxy *proxy0, btDispatcher *dispatcher)=0
virtual btBroadphasePair * addOverlappingPair(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1)=0
btVector3 can be used to represent 3D points and vectors.
const btScalar & getZ() const
Return the z value.
void setMax(const btVector3 &other)
Set each element to the max of the current values and the values of another btVector3.
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
const btScalar & getY() const
Return the y value.
void setMin(const btVector3 &other)
Set each element to the min of the current values and the values of another btVector3.
const btScalar & getX() const
Return the x value.
virtual bool process(const btBroadphaseProxy *proxy)=0
The btBroadphasePair class contains a pair of aabb-overlapping objects.
btBroadphaseProxy * m_pProxy1
btBroadphaseProxy * m_pProxy0
btCollisionAlgorithm * m_algorithm
The btBroadphaseProxy is the main class that can be used with the Bullet broadphases.
int m_collisionFilterMask
int m_collisionFilterGroup
The btDbvtBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees...
virtual void destroyProxy(btBroadphaseProxy *proxy, btDispatcher *dispatcher)
btBroadphaseProxy * createProxy(const btVector3 &aabbMin, const btVector3 &aabbMax, int shapeType, void *userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher *dispatcher)
virtual void rayTest(const btVector3 &rayFrom, const btVector3 &rayTo, btBroadphaseRayCallback &rayCallback, const btVector3 &aabbMin=btVector3(0, 0, 0), const btVector3 &aabbMax=btVector3(0, 0, 0))
virtual void aabbTest(const btVector3 &aabbMin, const btVector3 &aabbMax, btBroadphaseAabbCallback &callback)
virtual void setAabb(btBroadphaseProxy *proxy, const btVector3 &aabbMin, const btVector3 &aabbMax, btDispatcher *dispatcher)