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>
51 BP_FP_INT_TYPE
IsMax()
const {
return static_cast<BP_FP_INT_TYPE
>(
m_pos & 1); }
103 bool testOverlap2D(
const Handle* pHandleA,
const Handle* pHandleB,
int axis0,
int axis1);
105#ifdef DEBUG_BROADPHASE
106 void debugPrintAxis(
int axis,
bool checkCardinality =
true);
191#ifdef DEBUG_BROADPHASE
194template <
typename BP_FP_INT_TYPE>
197 int numEdges = m_pHandles[0].
m_maxEdges[axis];
198 printf(
"SAP Axis %d, numEdges=%d\n", axis, numEdges);
201 for (i = 0; i < numEdges + 1; i++)
203 Edge* pEdge = m_pEdges[axis] + i;
204 Handle* pHandlePrev = getHandle(pEdge->m_handle);
205 int handleIndex = pEdge->IsMax() ? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis];
207 beginOrEnd = pEdge->IsMax() ?
'E' :
'B';
208 printf(
" [%c,h=%d,p=%x,i=%d]\n", beginOrEnd, pEdge->m_handle, pEdge->m_pos, handleIndex);
211 if (checkCardinality)
212 btAssert(numEdges == m_numHandles * 2 + 1);
216template <
typename BP_FP_INT_TYPE>
220 BP_FP_INT_TYPE handleId =
addHandle(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher);
232template <
typename BP_FP_INT_TYPE>
241template <
typename BP_FP_INT_TYPE>
252template <
typename BP_FP_INT_TYPE>
262 BP_FP_INT_TYPE axis = 0;
264 for (BP_FP_INT_TYPE i = 1; i <
m_numHandles * 2 + 1; i++)
274template <
typename BP_FP_INT_TYPE>
284 BP_FP_INT_TYPE axis = 0;
286 for (BP_FP_INT_TYPE i = 1; i <
m_numHandles * 2 + 1; i++)
300template <
typename BP_FP_INT_TYPE>
308template <
typename BP_FP_INT_TYPE>
313 unsigned short vecInMin[3];
314 unsigned short vecInMax[3];
330template <
typename BP_FP_INT_TYPE>
340 BP_FP_INT_TYPE maxHandles =
static_cast<BP_FP_INT_TYPE
>(userMaxHandles + 1);
349 if (!disableRaycastAccelerator)
378 m_pHandles[i].SetNextFree(
static_cast<BP_FP_INT_TYPE
>(i + 1));
384 for (
int i = 0; i < 3; i++)
396 for (
int axis = 0; axis < 3; axis++)
405#ifdef DEBUG_BROADPHASE
406 debugPrintAxis(axis);
411template <
typename BP_FP_INT_TYPE>
422 for (
int i = 2; i >= 0; i--)
435template <
typename BP_FP_INT_TYPE>
438#ifdef OLD_CLAMPING_METHOD
456template <
typename BP_FP_INT_TYPE>
468template <
typename BP_FP_INT_TYPE>
479template <
typename BP_FP_INT_TYPE>
483 BP_FP_INT_TYPE min[3], max[3];
492 pHandle->
m_uniqueId =
static_cast<int>(handle);
499 BP_FP_INT_TYPE limit =
static_cast<BP_FP_INT_TYPE
>(
m_numHandles * 2);
502 for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++)
508 m_pEdges[axis][limit - 1].m_pos = min[axis];
509 m_pEdges[axis][limit - 1].m_handle = handle;
511 m_pEdges[axis][limit].m_pos = max[axis];
512 m_pEdges[axis][limit].m_handle = handle;
514 pHandle->
m_minEdges[axis] =
static_cast<BP_FP_INT_TYPE
>(limit - 1);
529template <
typename BP_FP_INT_TYPE>
539 m_pairCache->removeOverlappingPairsContainingProxy(pHandle, dispatcher);
547 for (axis = 0; axis < 3; axis++)
553 for (axis = 0; axis < 3; axis++)
556 BP_FP_INT_TYPE max = pHandle->
m_maxEdges[axis];
569#ifdef DEBUG_BROADPHASE
570 debugPrintAxis(axis,
false);
578template <
typename BP_FP_INT_TYPE>
586 m_pHandles[i].SetNextFree(
static_cast<BP_FP_INT_TYPE
>(i + 1));
594template <
typename BP_FP_INT_TYPE>
614 for (i = 0; i < overlappingPairArray.
size(); i++)
618 bool isDuplicate = (pair == previousPair);
622 bool needsRemoval =
false;
631 needsRemoval =
false;
648 m_pairCache->cleanOverlappingPair(pair, dispatcher);
659#define CLEAN_INVALID_PAIRS 1
660#ifdef CLEAN_INVALID_PAIRS
673template <
typename BP_FP_INT_TYPE>
681 for (
int axis = 0; axis < 3; axis++)
692template <
typename BP_FP_INT_TYPE>
707template <
typename BP_FP_INT_TYPE>
716 BP_FP_INT_TYPE min[3], max[3];
721 for (
int axis = 0; axis < 3; axis++)
723 BP_FP_INT_TYPE emin = pHandle->
m_minEdges[axis];
724 BP_FP_INT_TYPE emax = pHandle->
m_maxEdges[axis];
726 int dmin = (int)min[axis] - (
int)
m_pEdges[axis][emin].m_pos;
727 int dmax = (int)max[axis] - (
int)
m_pEdges[axis][emax].m_pos;
729 m_pEdges[axis][emin].m_pos = min[axis];
730 m_pEdges[axis][emax].m_pos = max[axis];
746#ifdef DEBUG_BROADPHASE
747 debugPrintAxis(axis);
753template <
typename BP_FP_INT_TYPE>
757 Edge* pPrev = pEdge - 1;
767 const int axis1 = (1 << axis) & 3;
768 const int axis2 = (1 << axis1) & 3;
769 if (updateOverlaps &&
testOverlap2D(pHandleEdge, pHandlePrev, axis1, axis2))
771 m_pairCache->addOverlappingPair(pHandleEdge, pHandlePrev);
796#ifdef DEBUG_BROADPHASE
797 debugPrintAxis(axis);
802template <
typename BP_FP_INT_TYPE>
806 Edge* pNext = pEdge + 1;
817 const int axis1 = (1 << axis) & 3;
818 const int axis2 = (1 << axis1) & 3;
827 m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher);
852template <
typename BP_FP_INT_TYPE>
856 Edge* pPrev = pEdge - 1;
868 const int axis1 = (1 << axis) & 3;
869 const int axis2 = (1 << axis1) & 3;
879 m_pairCache->removeOverlappingPair(handle0, handle1, dispatcher);
903#ifdef DEBUG_BROADPHASE
904 debugPrintAxis(axis);
909template <
typename BP_FP_INT_TYPE>
913 Edge* pNext = pEdge + 1;
920 const int axis1 = (1 << axis) & 3;
921 const int axis2 = (1 << axis1) & 3;
926 if (updateOverlaps &&
testOverlap2D(pHandleEdge, pHandleNext, axis1, axis2))
SIMD_FORCE_INLINE 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
btAlignedObjectArray< btBroadphasePair > btBroadphasePairArray
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]
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,...
The btOverlappingPairCallback class is an additional optional broadphase user callback for adding/rem...
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...