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]++;