2 Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
3 Copyright (C) 2006, 2009 Sony Computer Entertainment Inc.
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
16 ///The 3 following lines include the CPU implementation of the kernels, keep them in this order.
17 #include "BulletMultiThreaded/btGpuDefines.h"
18 #include "BulletMultiThreaded/btGpuUtilsSharedDefs.h"
19 #include "BulletMultiThreaded/btGpuUtilsSharedCode.h"
23 #include "LinearMath/btAlignedAllocator.h"
24 #include "LinearMath/btQuickprof.h"
25 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
29 #include "btGpuDefines.h"
30 #include "btGpuUtilsSharedDefs.h"
32 #include "btGpu3DGridBroadphaseSharedDefs.h"
34 #include "btGpu3DGridBroadphase.h"
35 #include <string.h> //for memset
42 static bt3DGridBroadphaseParams s3DGridBroadphaseParams;
46 btGpu3DGridBroadphase::btGpu3DGridBroadphase( const btVector3& worldAabbMin,const btVector3& worldAabbMax,
47 int gridSizeX, int gridSizeY, int gridSizeZ,
48 int maxSmallProxies, int maxLargeProxies, int maxPairsPerBody,
50 btScalar cellFactorAABB) :
51 btSimpleBroadphase(maxSmallProxies,
52 // new (btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16)) btSortedOverlappingPairCache),
53 new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache),
54 m_bInitialized(false),
57 _initialize(worldAabbMin, worldAabbMax, gridSizeX, gridSizeY, gridSizeZ,
58 maxSmallProxies, maxLargeProxies, maxPairsPerBody,
59 maxBodiesPerCell, cellFactorAABB);
64 btGpu3DGridBroadphase::btGpu3DGridBroadphase( btOverlappingPairCache* overlappingPairCache,
65 const btVector3& worldAabbMin,const btVector3& worldAabbMax,
66 int gridSizeX, int gridSizeY, int gridSizeZ,
67 int maxSmallProxies, int maxLargeProxies, int maxPairsPerBody,
69 btScalar cellFactorAABB) :
70 btSimpleBroadphase(maxSmallProxies, overlappingPairCache),
71 m_bInitialized(false),
74 _initialize(worldAabbMin, worldAabbMax, gridSizeX, gridSizeY, gridSizeZ,
75 maxSmallProxies, maxLargeProxies, maxPairsPerBody,
76 maxBodiesPerCell, cellFactorAABB);
81 btGpu3DGridBroadphase::~btGpu3DGridBroadphase()
83 //btSimpleBroadphase will free memory of btSortedOverlappingPairCache, because m_ownsPairCache
84 btAssert(m_bInitialized);
90 void btGpu3DGridBroadphase::_initialize( const btVector3& worldAabbMin,const btVector3& worldAabbMax,
91 int gridSizeX, int gridSizeY, int gridSizeZ,
92 int maxSmallProxies, int maxLargeProxies, int maxPairsPerBody,
94 btScalar cellFactorAABB)
96 // set various paramerers
97 m_ownsPairCache = true;
98 m_params.m_gridSizeX = gridSizeX;
99 m_params.m_gridSizeY = gridSizeY;
100 m_params.m_gridSizeZ = gridSizeZ;
101 m_params.m_numCells = m_params.m_gridSizeX * m_params.m_gridSizeY * m_params.m_gridSizeZ;
102 btVector3 w_org = worldAabbMin;
103 m_params.m_worldOriginX = w_org.getX();
104 m_params.m_worldOriginY = w_org.getY();
105 m_params.m_worldOriginZ = w_org.getZ();
106 btVector3 w_size = worldAabbMax - worldAabbMin;
107 m_params.m_cellSizeX = w_size.getX() / m_params.m_gridSizeX;
108 m_params.m_cellSizeY = w_size.getY() / m_params.m_gridSizeY;
109 m_params.m_cellSizeZ = w_size.getZ() / m_params.m_gridSizeZ;
110 m_maxRadius = btMin(btMin(m_params.m_cellSizeX, m_params.m_cellSizeY), m_params.m_cellSizeZ);
111 m_maxRadius *= btScalar(0.5f);
112 m_params.m_numBodies = m_numBodies;
113 m_params.m_maxBodiesPerCell = maxBodiesPerCell;
115 m_numLargeHandles = 0;
116 m_maxLargeHandles = maxLargeProxies;
118 m_maxPairsPerBody = maxPairsPerBody;
120 m_cellFactorAABB = cellFactorAABB;
122 m_LastLargeHandleIndex = -1;
124 btAssert(!m_bInitialized);
125 // allocate host storage
126 m_hBodiesHash = new unsigned int[m_maxHandles * 2];
127 memset(m_hBodiesHash, 0x00, m_maxHandles*2*sizeof(unsigned int));
129 m_hCellStart = new unsigned int[m_params.m_numCells];
130 memset(m_hCellStart, 0x00, m_params.m_numCells * sizeof(unsigned int));
132 m_hPairBuffStartCurr = new unsigned int[m_maxHandles * 2 + 2];
133 // --------------- for now, init with m_maxPairsPerBody for each body
134 m_hPairBuffStartCurr[0] = 0;
135 m_hPairBuffStartCurr[1] = 0;
136 for(int i = 1; i <= m_maxHandles; i++)
138 m_hPairBuffStartCurr[i * 2] = m_hPairBuffStartCurr[(i-1) * 2] + m_maxPairsPerBody;
139 m_hPairBuffStartCurr[i * 2 + 1] = 0;
142 unsigned int numAABB = m_maxHandles + m_maxLargeHandles;
143 m_hAABB = new bt3DGrid3F1U[numAABB * 2]; // AABB Min & Max
145 m_hPairBuff = new unsigned int[m_maxHandles * m_maxPairsPerBody];
146 memset(m_hPairBuff, 0x00, m_maxHandles * m_maxPairsPerBody * sizeof(unsigned int)); // needed?
148 m_hPairScan = new unsigned int[m_maxHandles + 1];
150 m_hPairOut = new unsigned int[m_maxHandles * m_maxPairsPerBody];
154 // allocate handles buffer and put all handles on free list
155 m_pLargeHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy) * m_maxLargeHandles, 16);
156 m_pLargeHandles = new(m_pLargeHandlesRawPtr) btSimpleBroadphaseProxy[m_maxLargeHandles];
157 m_firstFreeLargeHandle = 0;
159 for (int i = m_firstFreeLargeHandle; i < m_maxLargeHandles; i++)
161 m_pLargeHandles[i].SetNextFree(i + 1);
162 m_pLargeHandles[i].m_uniqueId = m_maxHandles+2+i;
164 m_pLargeHandles[m_maxLargeHandles - 1].SetNextFree(0);
171 m_bInitialized = true;
176 void btGpu3DGridBroadphase::_finalize()
178 btAssert(m_bInitialized);
179 delete [] m_hBodiesHash;
180 delete [] m_hCellStart;
181 delete [] m_hPairBuffStartCurr;
183 delete [] m_hPairBuff;
184 delete [] m_hPairScan;
185 delete [] m_hPairOut;
186 btAlignedFree(m_pLargeHandlesRawPtr);
187 m_bInitialized = false;
192 void btGpu3DGridBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
194 if(m_numHandles <= 0)
196 BT_PROFILE("addLarge2LargePairsToCache");
197 addLarge2LargePairsToCache(dispatcher);
201 setParameters(&m_params);
202 // prepare AABB array
206 // sort bodies based on hash
208 // find start of each cell
210 // findOverlappingPairs (small/small)
211 findOverlappingPairs();
212 // findOverlappingPairs (small/large)
214 // add pairs to CPU cache
215 computePairCacheChanges();
216 scanOverlappingPairBuff();
217 squeezeOverlappingPairBuff();
218 addPairsToCache(dispatcher);
219 // find and add large/large pairs to CPU cache
220 addLarge2LargePairsToCache(dispatcher);
226 void btGpu3DGridBroadphase::addPairsToCache(btDispatcher* dispatcher)
229 m_numPairsRemoved = 0;
230 for(int i = 0; i < m_numHandles; i++)
232 unsigned int num = m_hPairScan[i+1] - m_hPairScan[i];
237 unsigned int* pInp = m_hPairOut + m_hPairScan[i];
238 unsigned int index0 = m_hAABB[i * 2].uw;
239 btSimpleBroadphaseProxy* proxy0 = &m_pHandles[index0];
240 for(unsigned int j = 0; j < num; j++)
242 unsigned int indx1_s = pInp[j];
243 unsigned int index1 = indx1_s & (~BT_3DGRID_PAIR_ANY_FLG);
244 btSimpleBroadphaseProxy* proxy1;
245 if(index1 < (unsigned int)m_maxHandles)
247 proxy1 = &m_pHandles[index1];
251 index1 -= m_maxHandles;
252 btAssert((index1 >= 0) && (index1 < (unsigned int)m_maxLargeHandles));
253 proxy1 = &m_pLargeHandles[index1];
255 if(indx1_s & BT_3DGRID_PAIR_NEW_FLG)
257 m_pairCache->addOverlappingPair(proxy0,proxy1);
262 m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
271 btBroadphaseProxy* btGpu3DGridBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy)
273 btBroadphaseProxy* proxy;
274 bool bIsLarge = isLargeProxy(aabbMin, aabbMax);
277 if (m_numLargeHandles >= m_maxLargeHandles)
279 ///you have to increase the cell size, so 'large' proxies become 'small' proxies (fitting a cell)
281 return 0; //should never happen, but don't let the game crash ;-)
283 btAssert((aabbMin[0]<= aabbMax[0]) && (aabbMin[1]<= aabbMax[1]) && (aabbMin[2]<= aabbMax[2]));
284 int newHandleIndex = allocLargeHandle();
285 proxy = new (&m_pLargeHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
289 proxy = btSimpleBroadphase::createProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher, multiSapProxy);
296 void btGpu3DGridBroadphase::destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
298 bool bIsLarge = isLargeProxy(proxy);
302 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy);
303 freeLargeHandle(proxy0);
304 m_pairCache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
308 btSimpleBroadphase::destroyProxy(proxy, dispatcher);
315 void btGpu3DGridBroadphase::resetPool(btDispatcher* dispatcher)
317 m_hPairBuffStartCurr[0] = 0;
318 m_hPairBuffStartCurr[1] = 0;
319 for(int i = 1; i <= m_maxHandles; i++)
321 m_hPairBuffStartCurr[i * 2] = m_hPairBuffStartCurr[(i-1) * 2] + m_maxPairsPerBody;
322 m_hPairBuffStartCurr[i * 2 + 1] = 0;
328 bool btGpu3DGridBroadphase::isLargeProxy(const btVector3& aabbMin, const btVector3& aabbMax)
330 btVector3 diag = aabbMax - aabbMin;
332 ///use the bounding sphere radius of this bounding box, to include rotation
333 btScalar radius = diag.length() * btScalar(0.5f);
334 radius *= m_cellFactorAABB; // user-defined factor
336 return (radius > m_maxRadius);
341 bool btGpu3DGridBroadphase::isLargeProxy(btBroadphaseProxy* proxy)
343 return (proxy->getUid() >= (m_maxHandles+2));
348 void btGpu3DGridBroadphase::addLarge2LargePairsToCache(btDispatcher* dispatcher)
351 if (m_numLargeHandles <= 0)
355 int new_largest_index = -1;
356 for(i = 0; i <= m_LastLargeHandleIndex; i++)
358 btSimpleBroadphaseProxy* proxy0 = &m_pLargeHandles[i];
359 if(!proxy0->m_clientObject)
363 new_largest_index = i;
364 for(j = i + 1; j <= m_LastLargeHandleIndex; j++)
366 btSimpleBroadphaseProxy* proxy1 = &m_pLargeHandles[j];
367 if(!proxy1->m_clientObject)
371 btAssert(proxy0 != proxy1);
372 btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
373 btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
374 if(aabbOverlap(p0,p1))
376 if (!m_pairCache->findPair(proxy0,proxy1))
378 m_pairCache->addOverlappingPair(proxy0,proxy1);
383 if(m_pairCache->findPair(proxy0,proxy1))
385 m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
390 m_LastLargeHandleIndex = new_largest_index;
396 void btGpu3DGridBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
398 btSimpleBroadphase::rayTest(rayFrom, rayTo, rayCallback);
399 for (int i=0; i <= m_LastLargeHandleIndex; i++)
401 btSimpleBroadphaseProxy* proxy = &m_pLargeHandles[i];
402 if(!proxy->m_clientObject)
406 rayCallback.process(proxy);
413 // overrides for CPU version
418 void btGpu3DGridBroadphase::prepareAABB()
420 BT_PROFILE("prepareAABB");
421 bt3DGrid3F1U* pBB = m_hAABB;
423 int new_largest_index = -1;
424 unsigned int num_small = 0;
425 for(i = 0; i <= m_LastHandleIndex; i++)
427 btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
428 if(!proxy0->m_clientObject)
432 new_largest_index = i;
433 pBB->fx = proxy0->m_aabbMin.getX();
434 pBB->fy = proxy0->m_aabbMin.getY();
435 pBB->fz = proxy0->m_aabbMin.getZ();
438 pBB->fx = proxy0->m_aabbMax.getX();
439 pBB->fy = proxy0->m_aabbMax.getY();
440 pBB->fz = proxy0->m_aabbMax.getZ();
445 m_LastHandleIndex = new_largest_index;
446 new_largest_index = -1;
447 unsigned int num_large = 0;
448 for(i = 0; i <= m_LastLargeHandleIndex; i++)
450 btSimpleBroadphaseProxy* proxy0 = &m_pLargeHandles[i];
451 if(!proxy0->m_clientObject)
455 new_largest_index = i;
456 pBB->fx = proxy0->m_aabbMin.getX();
457 pBB->fy = proxy0->m_aabbMin.getY();
458 pBB->fz = proxy0->m_aabbMin.getZ();
459 pBB->uw = i + m_maxHandles;
461 pBB->fx = proxy0->m_aabbMax.getX();
462 pBB->fy = proxy0->m_aabbMax.getY();
463 pBB->fz = proxy0->m_aabbMax.getZ();
464 pBB->uw = num_large + m_maxHandles;
468 m_LastLargeHandleIndex = new_largest_index;
470 btAssert(num_small == m_numHandles);
471 btAssert(num_large == m_numLargeHandles);
477 void btGpu3DGridBroadphase::setParameters(bt3DGridBroadphaseParams* hostParams)
479 s3DGridBroadphaseParams = *hostParams;
485 void btGpu3DGridBroadphase::calcHashAABB()
487 BT_PROFILE("bt3DGrid_calcHashAABB");
488 btGpu_calcHashAABB(m_hAABB, m_hBodiesHash, m_numHandles);
494 void btGpu3DGridBroadphase::sortHash()
496 class bt3DGridHashKey
501 void quickSort(bt3DGridHashKey* pData, int lo, int hi)
504 bt3DGridHashKey x = pData[(lo+hi)/2];
507 while(pData[i].hash > x.hash) i++;
508 while(x.hash > pData[j].hash) j--;
511 bt3DGridHashKey t = pData[i];
517 if(lo < j) pData->quickSort(pData, lo, j);
518 if(i < hi) pData->quickSort(pData, i, hi);
521 BT_PROFILE("bt3DGrid_sortHash");
522 bt3DGridHashKey* pHash = (bt3DGridHashKey*)m_hBodiesHash;
523 pHash->quickSort(pHash, 0, m_numHandles - 1);
529 void btGpu3DGridBroadphase::findCellStart()
531 BT_PROFILE("bt3DGrid_findCellStart");
532 btGpu_findCellStart(m_hBodiesHash, m_hCellStart, m_numHandles, m_params.m_numCells);
538 void btGpu3DGridBroadphase::findOverlappingPairs()
540 BT_PROFILE("bt3DGrid_findOverlappingPairs");
541 btGpu_findOverlappingPairs(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr, m_numHandles);
547 void btGpu3DGridBroadphase::findPairsLarge()
549 BT_PROFILE("bt3DGrid_findPairsLarge");
550 btGpu_findPairsLarge(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr, m_numHandles, m_numLargeHandles);
556 void btGpu3DGridBroadphase::computePairCacheChanges()
558 BT_PROFILE("bt3DGrid_computePairCacheChanges");
559 btGpu_computePairCacheChanges(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hAABB, m_numHandles);
565 void btGpu3DGridBroadphase::scanOverlappingPairBuff()
567 BT_PROFILE("bt3DGrid_scanOverlappingPairBuff");
569 for(int i = 1; i <= m_numHandles; i++)
571 unsigned int delta = m_hPairScan[i];
572 m_hPairScan[i] = m_hPairScan[i-1] + delta;
579 void btGpu3DGridBroadphase::squeezeOverlappingPairBuff()
581 BT_PROFILE("bt3DGrid_squeezeOverlappingPairBuff");
582 btGpu_squeezeOverlappingPairBuff(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hPairOut, m_hAABB, m_numHandles);
588 #include "btGpu3DGridBroadphaseSharedCode.h"