19 #ifndef BT_AXIS_SWEEP_3_INTERNAL_H
20 #define BT_AXIS_SWEEP_3_INTERNAL_H
30 #define USE_OVERLAP_TEST_ON_REMOVES 1
35 template <
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);
107 #endif //DEBUG_BROADPHASE
191 #ifdef DEBUG_BROADPHASE
194 template <
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);
214 #endif //DEBUG_BROADPHASE
216 template <
typename BP_FP_INT_TYPE>
220 BP_FP_INT_TYPE handleId = addHandle(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher);
222 Handle* handle = getHandle(handleId);
224 if (m_raycastAccelerator)
226 btBroadphaseProxy* rayProxy = m_raycastAccelerator->
createProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher);
227 handle->m_dbvtProxy = rayProxy;
232 template <
typename BP_FP_INT_TYPE>
235 Handle* handle = static_cast<Handle*>(proxy);
236 if (m_raycastAccelerator)
237 m_raycastAccelerator->
destroyProxy(handle->m_dbvtProxy, dispatcher);
238 removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher);
241 template <
typename BP_FP_INT_TYPE>
244 Handle* handle = static_cast<Handle*>(proxy);
245 handle->m_aabbMin = aabbMin;
246 handle->m_aabbMax = aabbMax;
247 updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax, dispatcher);
248 if (m_raycastAccelerator)
249 m_raycastAccelerator->
setAabb(handle->m_dbvtProxy, aabbMin, aabbMax, dispatcher);
252 template <
typename BP_FP_INT_TYPE>
255 if (m_raycastAccelerator)
257 m_raycastAccelerator->
rayTest(rayFrom, rayTo, rayCallback, aabbMin, aabbMax);
262 BP_FP_INT_TYPE axis = 0;
264 for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++)
266 if (m_pEdges[axis][i].IsMax())
268 rayCallback.
process(getHandle(m_pEdges[axis][i].m_handle));
274 template <
typename BP_FP_INT_TYPE>
277 if (m_raycastAccelerator)
279 m_raycastAccelerator->
aabbTest(aabbMin, aabbMax, callback);
284 BP_FP_INT_TYPE axis = 0;
286 for (BP_FP_INT_TYPE i = 1; i < m_numHandles * 2 + 1; i++)
288 if (m_pEdges[axis][i].IsMax())
290 Handle* handle = getHandle(m_pEdges[axis][i].m_handle);
300 template <
typename BP_FP_INT_TYPE>
303 Handle* pHandle = static_cast<Handle*>(proxy);
304 aabbMin = pHandle->m_aabbMin;
305 aabbMax = pHandle->m_aabbMax;
308 template <
typename BP_FP_INT_TYPE>
311 Handle* pHandle = static_cast<Handle*>(proxy);
313 unsigned short vecInMin[3];
314 unsigned short vecInMax[3];
316 vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos;
317 vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos + 1;
318 vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos;
319 vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos + 1;
320 vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos;
321 vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos + 1;
324 aabbMin += m_worldAabbMin;
327 aabbMax += m_worldAabbMin;
330 template <
typename BP_FP_INT_TYPE>
332 : m_bpHandleMask(handleMask),
333 m_handleSentinel(handleSentinel),
334 m_pairCache(pairCache),
335 m_userPairCallback(0),
336 m_ownsPairCache(false),
338 m_raycastAccelerator(0)
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);
407 #endif //DEBUG_BROADPHASE
411 template <
typename BP_FP_INT_TYPE>
414 if (m_raycastAccelerator)
422 for (
int i = 2; i >= 0; i--)
435 template <
typename BP_FP_INT_TYPE>
438 #ifdef OLD_CLAMPING_METHOD
442 clampedPoint.
setMax(m_worldAabbMin);
443 clampedPoint.
setMin(m_worldAabbMax);
444 btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize;
445 out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.
getX() & m_bpHandleMask) | isMax);
446 out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.
getY() & m_bpHandleMask) | isMax);
447 out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.
getZ() & m_bpHandleMask) | isMax);
449 btVector3 v = (point - m_worldAabbMin) * m_quantize;
450 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);
451 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);
452 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);
453 #endif //OLD_CLAMPING_METHOD
456 template <
typename BP_FP_INT_TYPE>
461 BP_FP_INT_TYPE handle = m_firstFreeHandle;
462 m_firstFreeHandle = getHandle(handle)->GetNextFree();
468 template <
typename BP_FP_INT_TYPE>
471 btAssert(handle > 0 && handle < m_maxHandles);
473 getHandle(handle)->SetNextFree(m_firstFreeHandle);
474 m_firstFreeHandle = handle;
479 template <
typename BP_FP_INT_TYPE>
483 BP_FP_INT_TYPE min[3], max[3];
484 quantize(min, aabbMin, 0);
485 quantize(max, aabbMax, 1);
488 BP_FP_INT_TYPE handle = allocHandle();
490 Handle* pHandle = getHandle(handle);
492 pHandle->m_uniqueId = static_cast<int>(handle);
494 pHandle->m_clientObject = pOwner;
495 pHandle->m_collisionFilterGroup = collisionFilterGroup;
496 pHandle->m_collisionFilterMask = collisionFilterMask;
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++)
504 m_pHandles[0].m_maxEdges[axis] += 2;
506 m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1];
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);
515 pHandle->m_maxEdges[axis] = limit;
519 sortMinDown(0, pHandle->m_minEdges[0], dispatcher,
false);
520 sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,
false);
521 sortMinDown(1, pHandle->m_minEdges[1], dispatcher,
false);
522 sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,
false);
523 sortMinDown(2, pHandle->m_minEdges[2], dispatcher,
true);
524 sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,
true);
529 template <
typename BP_FP_INT_TYPE>
532 Handle* pHandle = getHandle(handle);
543 int limit = static_cast<int>(m_numHandles * 2);
547 for (axis = 0; axis < 3; axis++)
549 m_pHandles[0].m_maxEdges[axis] -= 2;
553 for (axis = 0; axis < 3; axis++)
555 Edge* pEdges = m_pEdges[axis];
556 BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis];
557 pEdges[max].m_pos = m_handleSentinel;
559 sortMaxUp(axis, max, dispatcher,
false);
561 BP_FP_INT_TYPE i = pHandle->m_minEdges[axis];
562 pEdges[i].m_pos = m_handleSentinel;
564 sortMinUp(axis, i, dispatcher,
false);
566 pEdges[limit - 1].m_handle = 0;
567 pEdges[limit - 1].m_pos = m_handleSentinel;
569 #ifdef DEBUG_BROADPHASE
570 debugPrintAxis(axis,
false);
571 #endif //DEBUG_BROADPHASE
578 template <
typename BP_FP_INT_TYPE>
581 if (m_numHandles == 0)
583 m_firstFreeHandle = 1;
585 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++)
586 m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1));
587 m_pHandles[m_maxHandles - 1].SetNextFree(0);
594 template <
typename BP_FP_INT_TYPE>
604 overlappingPairArray.
resize(overlappingPairArray.
size() - m_invalidPair);
614 for (i = 0; i < overlappingPairArray.
size(); i++)
618 bool isDuplicate = (pair == previousPair);
622 bool needsRemoval =
false;
631 needsRemoval =
false;
659 #define CLEAN_INVALID_PAIRS 1
660 #ifdef CLEAN_INVALID_PAIRS
665 overlappingPairArray.
resize(overlappingPairArray.
size() - m_invalidPair);
667 #endif //CLEAN_INVALID_PAIRS
673 template <
typename BP_FP_INT_TYPE>
676 const Handle* pHandleA = static_cast<Handle*>(proxy0);
677 const Handle* pHandleB = static_cast<Handle*>(proxy1);
681 for (
int axis = 0; axis < 3; axis++)
683 if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] ||
684 pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis])
692 template <
typename BP_FP_INT_TYPE>
697 if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] ||
698 pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] ||
699 pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] ||
700 pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1])
707 template <
typename BP_FP_INT_TYPE>
713 Handle* pHandle = getHandle(handle);
716 BP_FP_INT_TYPE min[3], max[3];
717 quantize(min, aabbMin, 0);
718 quantize(max, aabbMax, 1);
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];
734 sortMinDown(axis, emin, dispatcher,
true);
737 sortMaxUp(axis, emax, dispatcher,
true);
741 sortMinUp(axis, emin, dispatcher,
true);
744 sortMaxDown(axis, emax, dispatcher,
true);
746 #ifdef DEBUG_BROADPHASE
747 debugPrintAxis(axis);
748 #endif //DEBUG_BROADPHASE
753 template <
typename BP_FP_INT_TYPE>
756 Edge* pEdge = m_pEdges[axis] + edge;
757 Edge* pPrev = pEdge - 1;
758 Handle* pHandleEdge = getHandle(pEdge->m_handle);
760 while (pEdge->m_pos < pPrev->m_pos)
762 Handle* pHandlePrev = getHandle(pPrev->m_handle);
767 const int axis1 = (1 << axis) & 3;
768 const int axis2 = (1 << axis1) & 3;
769 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev, axis1, axis2))
772 if (m_userPairCallback)
779 pHandlePrev->m_maxEdges[axis]++;
782 pHandlePrev->m_minEdges[axis]++;
784 pHandleEdge->m_minEdges[axis]--;
796 #ifdef DEBUG_BROADPHASE
797 debugPrintAxis(axis);
798 #endif //DEBUG_BROADPHASE
802 template <
typename BP_FP_INT_TYPE>
805 Edge* pEdge = m_pEdges[axis] + edge;
806 Edge* pNext = pEdge + 1;
807 Handle* pHandleEdge = getHandle(pEdge->m_handle);
809 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
811 Handle* pHandleNext = getHandle(pNext->m_handle);
815 Handle* handle0 = getHandle(pEdge->m_handle);
816 Handle* handle1 = getHandle(pNext->m_handle);
817 const int axis1 = (1 << axis) & 3;
818 const int axis2 = (1 << axis1) & 3;
823 && testOverlap2D(handle0, handle1, axis1, axis2)
824 #endif //USE_OVERLAP_TEST_ON_REMOVES
828 if (m_userPairCallback)
833 pHandleNext->m_maxEdges[axis]--;
836 pHandleNext->m_minEdges[axis]--;
838 pHandleEdge->m_minEdges[axis]++;
852 template <
typename BP_FP_INT_TYPE>
855 Edge* pEdge = m_pEdges[axis] + edge;
856 Edge* pPrev = pEdge - 1;
857 Handle* pHandleEdge = getHandle(pEdge->m_handle);
859 while (pEdge->m_pos < pPrev->m_pos)
861 Handle* pHandlePrev = getHandle(pPrev->m_handle);
866 Handle* handle0 = getHandle(pEdge->m_handle);
867 Handle* handle1 = getHandle(pPrev->m_handle);
868 const int axis1 = (1 << axis) & 3;
869 const int axis2 = (1 << axis1) & 3;
873 && testOverlap2D(handle0, handle1, axis1, axis2)
874 #endif //USE_OVERLAP_TEST_ON_REMOVES
880 if (m_userPairCallback)
885 pHandlePrev->m_minEdges[axis]++;
889 pHandlePrev->m_maxEdges[axis]++;
891 pHandleEdge->m_maxEdges[axis]--;
903 #ifdef DEBUG_BROADPHASE
904 debugPrintAxis(axis);
905 #endif //DEBUG_BROADPHASE
909 template <
typename BP_FP_INT_TYPE>
912 Edge* pEdge = m_pEdges[axis] + edge;
913 Edge* pNext = pEdge + 1;
914 Handle* pHandleEdge = getHandle(pEdge->m_handle);
916 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos))
918 Handle* pHandleNext = getHandle(pNext->m_handle);
920 const int axis1 = (1 << axis) & 3;
921 const int axis2 = (1 << axis1) & 3;
926 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext, axis1, axis2))
928 Handle* handle0 = getHandle(pEdge->m_handle);
929 Handle* handle1 = getHandle(pNext->m_handle);
931 if (m_userPairCallback)
936 pHandleNext->m_minEdges[axis]--;
939 pHandleNext->m_maxEdges[axis]--;
941 pHandleEdge->m_maxEdges[axis]++;