00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00017 #include "BulletMultiThreaded/btGpuDefines.h"
00018 #include "BulletMultiThreaded/btGpuUtilsSharedDefs.h"
00019 #include "BulletMultiThreaded/btGpuUtilsSharedCode.h"
00020
00021
00022
00023 #include "LinearMath/btAlignedAllocator.h"
00024 #include "LinearMath/btQuickprof.h"
00025 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
00026
00027
00028
00029 #include "btGpuDefines.h"
00030 #include "btGpuUtilsSharedDefs.h"
00031
00032 #include "btGpu3DGridBroadphaseSharedDefs.h"
00033
00034 #include "btGpu3DGridBroadphase.h"
00035 #include <string.h>
00036
00037
00038 #include <stdio.h>
00039
00040
00041
00042 static bt3DGridBroadphaseParams s3DGridBroadphaseParams;
00043
00044
00045
00046 btGpu3DGridBroadphase::btGpu3DGridBroadphase( const btVector3& worldAabbMin,const btVector3& worldAabbMax,
00047 int gridSizeX, int gridSizeY, int gridSizeZ,
00048 int maxSmallProxies, int maxLargeProxies, int maxPairsPerBody,
00049 int maxBodiesPerCell,
00050 btScalar cellFactorAABB) :
00051 btSimpleBroadphase(maxSmallProxies,
00052
00053 new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache),
00054 m_bInitialized(false),
00055 m_numBodies(0)
00056 {
00057 _initialize(worldAabbMin, worldAabbMax, gridSizeX, gridSizeY, gridSizeZ,
00058 maxSmallProxies, maxLargeProxies, maxPairsPerBody,
00059 maxBodiesPerCell, cellFactorAABB);
00060 }
00061
00062
00063
00064 btGpu3DGridBroadphase::btGpu3DGridBroadphase( btOverlappingPairCache* overlappingPairCache,
00065 const btVector3& worldAabbMin,const btVector3& worldAabbMax,
00066 int gridSizeX, int gridSizeY, int gridSizeZ,
00067 int maxSmallProxies, int maxLargeProxies, int maxPairsPerBody,
00068 int maxBodiesPerCell,
00069 btScalar cellFactorAABB) :
00070 btSimpleBroadphase(maxSmallProxies, overlappingPairCache),
00071 m_bInitialized(false),
00072 m_numBodies(0)
00073 {
00074 _initialize(worldAabbMin, worldAabbMax, gridSizeX, gridSizeY, gridSizeZ,
00075 maxSmallProxies, maxLargeProxies, maxPairsPerBody,
00076 maxBodiesPerCell, cellFactorAABB);
00077 }
00078
00079
00080
00081 btGpu3DGridBroadphase::~btGpu3DGridBroadphase()
00082 {
00083
00084 btAssert(m_bInitialized);
00085 _finalize();
00086 }
00087
00088
00089
00090 void btGpu3DGridBroadphase::_initialize( const btVector3& worldAabbMin,const btVector3& worldAabbMax,
00091 int gridSizeX, int gridSizeY, int gridSizeZ,
00092 int maxSmallProxies, int maxLargeProxies, int maxPairsPerBody,
00093 int maxBodiesPerCell,
00094 btScalar cellFactorAABB)
00095 {
00096
00097 m_ownsPairCache = true;
00098 m_params.m_gridSizeX = gridSizeX;
00099 m_params.m_gridSizeY = gridSizeY;
00100 m_params.m_gridSizeZ = gridSizeZ;
00101 m_params.m_numCells = m_params.m_gridSizeX * m_params.m_gridSizeY * m_params.m_gridSizeZ;
00102 btVector3 w_org = worldAabbMin;
00103 m_params.m_worldOriginX = w_org.getX();
00104 m_params.m_worldOriginY = w_org.getY();
00105 m_params.m_worldOriginZ = w_org.getZ();
00106 btVector3 w_size = worldAabbMax - worldAabbMin;
00107 m_params.m_cellSizeX = w_size.getX() / m_params.m_gridSizeX;
00108 m_params.m_cellSizeY = w_size.getY() / m_params.m_gridSizeY;
00109 m_params.m_cellSizeZ = w_size.getZ() / m_params.m_gridSizeZ;
00110 m_maxRadius = btMin(btMin(m_params.m_cellSizeX, m_params.m_cellSizeY), m_params.m_cellSizeZ);
00111 m_maxRadius *= btScalar(0.5f);
00112 m_params.m_numBodies = m_numBodies;
00113 m_params.m_maxBodiesPerCell = maxBodiesPerCell;
00114
00115 m_numLargeHandles = 0;
00116 m_maxLargeHandles = maxLargeProxies;
00117
00118 m_maxPairsPerBody = maxPairsPerBody;
00119
00120 m_cellFactorAABB = cellFactorAABB;
00121
00122 m_LastLargeHandleIndex = -1;
00123
00124 btAssert(!m_bInitialized);
00125
00126 m_hBodiesHash = new unsigned int[m_maxHandles * 2];
00127 memset(m_hBodiesHash, 0x00, m_maxHandles*2*sizeof(unsigned int));
00128
00129 m_hCellStart = new unsigned int[m_params.m_numCells];
00130 memset(m_hCellStart, 0x00, m_params.m_numCells * sizeof(unsigned int));
00131
00132 m_hPairBuffStartCurr = new unsigned int[m_maxHandles * 2 + 2];
00133
00134 m_hPairBuffStartCurr[0] = 0;
00135 m_hPairBuffStartCurr[1] = 0;
00136 for(int i = 1; i <= m_maxHandles; i++)
00137 {
00138 m_hPairBuffStartCurr[i * 2] = m_hPairBuffStartCurr[(i-1) * 2] + m_maxPairsPerBody;
00139 m_hPairBuffStartCurr[i * 2 + 1] = 0;
00140 }
00141
00142 unsigned int numAABB = m_maxHandles + m_maxLargeHandles;
00143 m_hAABB = new bt3DGrid3F1U[numAABB * 2];
00144
00145 m_hPairBuff = new unsigned int[m_maxHandles * m_maxPairsPerBody];
00146 memset(m_hPairBuff, 0x00, m_maxHandles * m_maxPairsPerBody * sizeof(unsigned int));
00147
00148 m_hPairScan = new unsigned int[m_maxHandles + 1];
00149
00150 m_hPairOut = new unsigned int[m_maxHandles * m_maxPairsPerBody];
00151
00152
00153
00154
00155 m_pLargeHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy) * m_maxLargeHandles, 16);
00156 m_pLargeHandles = new(m_pLargeHandlesRawPtr) btSimpleBroadphaseProxy[m_maxLargeHandles];
00157 m_firstFreeLargeHandle = 0;
00158 {
00159 for (int i = m_firstFreeLargeHandle; i < m_maxLargeHandles; i++)
00160 {
00161 m_pLargeHandles[i].SetNextFree(i + 1);
00162 m_pLargeHandles[i].m_uniqueId = m_maxHandles+2+i;
00163 }
00164 m_pLargeHandles[m_maxLargeHandles - 1].SetNextFree(0);
00165 }
00166
00167
00168 m_numPairsAdded = 0;
00169 m_numOverflows = 0;
00170
00171 m_bInitialized = true;
00172 }
00173
00174
00175
00176 void btGpu3DGridBroadphase::_finalize()
00177 {
00178 btAssert(m_bInitialized);
00179 delete [] m_hBodiesHash;
00180 delete [] m_hCellStart;
00181 delete [] m_hPairBuffStartCurr;
00182 delete [] m_hAABB;
00183 delete [] m_hPairBuff;
00184 delete [] m_hPairScan;
00185 delete [] m_hPairOut;
00186 btAlignedFree(m_pLargeHandlesRawPtr);
00187 m_bInitialized = false;
00188 }
00189
00190
00191
00192 void btGpu3DGridBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
00193 {
00194 if(m_numHandles <= 0)
00195 {
00196 BT_PROFILE("addLarge2LargePairsToCache");
00197 addLarge2LargePairsToCache(dispatcher);
00198 return;
00199 }
00200
00201 setParameters(&m_params);
00202
00203 prepareAABB();
00204
00205 calcHashAABB();
00206
00207 sortHash();
00208
00209 findCellStart();
00210
00211 findOverlappingPairs();
00212
00213 findPairsLarge();
00214
00215 computePairCacheChanges();
00216 scanOverlappingPairBuff();
00217 squeezeOverlappingPairBuff();
00218 addPairsToCache(dispatcher);
00219
00220 addLarge2LargePairsToCache(dispatcher);
00221 return;
00222 }
00223
00224
00225
00226 void btGpu3DGridBroadphase::addPairsToCache(btDispatcher* dispatcher)
00227 {
00228 m_numPairsAdded = 0;
00229 m_numPairsRemoved = 0;
00230 for(int i = 0; i < m_numHandles; i++)
00231 {
00232 unsigned int num = m_hPairScan[i+1] - m_hPairScan[i];
00233 if(!num)
00234 {
00235 continue;
00236 }
00237 unsigned int* pInp = m_hPairOut + m_hPairScan[i];
00238 unsigned int index0 = m_hAABB[i * 2].uw;
00239 btSimpleBroadphaseProxy* proxy0 = &m_pHandles[index0];
00240 for(unsigned int j = 0; j < num; j++)
00241 {
00242 unsigned int indx1_s = pInp[j];
00243 unsigned int index1 = indx1_s & (~BT_3DGRID_PAIR_ANY_FLG);
00244 btSimpleBroadphaseProxy* proxy1;
00245 if(index1 < (unsigned int)m_maxHandles)
00246 {
00247 proxy1 = &m_pHandles[index1];
00248 }
00249 else
00250 {
00251 index1 -= m_maxHandles;
00252 btAssert((index1 >= 0) && (index1 < (unsigned int)m_maxLargeHandles));
00253 proxy1 = &m_pLargeHandles[index1];
00254 }
00255 if(indx1_s & BT_3DGRID_PAIR_NEW_FLG)
00256 {
00257 m_pairCache->addOverlappingPair(proxy0,proxy1);
00258 m_numPairsAdded++;
00259 }
00260 else
00261 {
00262 m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
00263 m_numPairsRemoved++;
00264 }
00265 }
00266 }
00267 }
00268
00269
00270
00271 btBroadphaseProxy* btGpu3DGridBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy)
00272 {
00273 btBroadphaseProxy* proxy;
00274 bool bIsLarge = isLargeProxy(aabbMin, aabbMax);
00275 if(bIsLarge)
00276 {
00277 if (m_numLargeHandles >= m_maxLargeHandles)
00278 {
00280 btAssert(0);
00281 return 0;
00282 }
00283 btAssert((aabbMin[0]<= aabbMax[0]) && (aabbMin[1]<= aabbMax[1]) && (aabbMin[2]<= aabbMax[2]));
00284 int newHandleIndex = allocLargeHandle();
00285 proxy = new (&m_pLargeHandles[newHandleIndex])btSimpleBroadphaseProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,multiSapProxy);
00286 }
00287 else
00288 {
00289 proxy = btSimpleBroadphase::createProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher, multiSapProxy);
00290 }
00291 return proxy;
00292 }
00293
00294
00295
00296 void btGpu3DGridBroadphase::destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
00297 {
00298 bool bIsLarge = isLargeProxy(proxy);
00299 if(bIsLarge)
00300 {
00301
00302 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy);
00303 freeLargeHandle(proxy0);
00304 m_pairCache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
00305 }
00306 else
00307 {
00308 btSimpleBroadphase::destroyProxy(proxy, dispatcher);
00309 }
00310 return;
00311 }
00312
00313
00314
00315 void btGpu3DGridBroadphase::resetPool(btDispatcher* dispatcher)
00316 {
00317 m_hPairBuffStartCurr[0] = 0;
00318 m_hPairBuffStartCurr[1] = 0;
00319 for(int i = 1; i <= m_maxHandles; i++)
00320 {
00321 m_hPairBuffStartCurr[i * 2] = m_hPairBuffStartCurr[(i-1) * 2] + m_maxPairsPerBody;
00322 m_hPairBuffStartCurr[i * 2 + 1] = 0;
00323 }
00324 }
00325
00326
00327
00328 bool btGpu3DGridBroadphase::isLargeProxy(const btVector3& aabbMin, const btVector3& aabbMax)
00329 {
00330 btVector3 diag = aabbMax - aabbMin;
00331
00333 btScalar radius = diag.length() * btScalar(0.5f);
00334 radius *= m_cellFactorAABB;
00335
00336 return (radius > m_maxRadius);
00337 }
00338
00339
00340
00341 bool btGpu3DGridBroadphase::isLargeProxy(btBroadphaseProxy* proxy)
00342 {
00343 return (proxy->getUid() >= (m_maxHandles+2));
00344 }
00345
00346
00347
00348 void btGpu3DGridBroadphase::addLarge2LargePairsToCache(btDispatcher* dispatcher)
00349 {
00350 int i,j;
00351 if (m_numLargeHandles <= 0)
00352 {
00353 return;
00354 }
00355 int new_largest_index = -1;
00356 for(i = 0; i <= m_LastLargeHandleIndex; i++)
00357 {
00358 btSimpleBroadphaseProxy* proxy0 = &m_pLargeHandles[i];
00359 if(!proxy0->m_clientObject)
00360 {
00361 continue;
00362 }
00363 new_largest_index = i;
00364 for(j = i + 1; j <= m_LastLargeHandleIndex; j++)
00365 {
00366 btSimpleBroadphaseProxy* proxy1 = &m_pLargeHandles[j];
00367 if(!proxy1->m_clientObject)
00368 {
00369 continue;
00370 }
00371 btAssert(proxy0 != proxy1);
00372 btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
00373 btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
00374 if(aabbOverlap(p0,p1))
00375 {
00376 if (!m_pairCache->findPair(proxy0,proxy1))
00377 {
00378 m_pairCache->addOverlappingPair(proxy0,proxy1);
00379 }
00380 }
00381 else
00382 {
00383 if(m_pairCache->findPair(proxy0,proxy1))
00384 {
00385 m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
00386 }
00387 }
00388 }
00389 }
00390 m_LastLargeHandleIndex = new_largest_index;
00391 return;
00392 }
00393
00394
00395
00396 void btGpu3DGridBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
00397 {
00398 btSimpleBroadphase::rayTest(rayFrom, rayTo, rayCallback);
00399 for (int i=0; i <= m_LastLargeHandleIndex; i++)
00400 {
00401 btSimpleBroadphaseProxy* proxy = &m_pLargeHandles[i];
00402 if(!proxy->m_clientObject)
00403 {
00404 continue;
00405 }
00406 rayCallback.process(proxy);
00407 }
00408 }
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418 void btGpu3DGridBroadphase::prepareAABB()
00419 {
00420 BT_PROFILE("prepareAABB");
00421 bt3DGrid3F1U* pBB = m_hAABB;
00422 int i;
00423 int new_largest_index = -1;
00424 unsigned int num_small = 0;
00425 for(i = 0; i <= m_LastHandleIndex; i++)
00426 {
00427 btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
00428 if(!proxy0->m_clientObject)
00429 {
00430 continue;
00431 }
00432 new_largest_index = i;
00433 pBB->fx = proxy0->m_aabbMin.getX();
00434 pBB->fy = proxy0->m_aabbMin.getY();
00435 pBB->fz = proxy0->m_aabbMin.getZ();
00436 pBB->uw = i;
00437 pBB++;
00438 pBB->fx = proxy0->m_aabbMax.getX();
00439 pBB->fy = proxy0->m_aabbMax.getY();
00440 pBB->fz = proxy0->m_aabbMax.getZ();
00441 pBB->uw = num_small;
00442 pBB++;
00443 num_small++;
00444 }
00445 m_LastHandleIndex = new_largest_index;
00446 new_largest_index = -1;
00447 unsigned int num_large = 0;
00448 for(i = 0; i <= m_LastLargeHandleIndex; i++)
00449 {
00450 btSimpleBroadphaseProxy* proxy0 = &m_pLargeHandles[i];
00451 if(!proxy0->m_clientObject)
00452 {
00453 continue;
00454 }
00455 new_largest_index = i;
00456 pBB->fx = proxy0->m_aabbMin.getX();
00457 pBB->fy = proxy0->m_aabbMin.getY();
00458 pBB->fz = proxy0->m_aabbMin.getZ();
00459 pBB->uw = i + m_maxHandles;
00460 pBB++;
00461 pBB->fx = proxy0->m_aabbMax.getX();
00462 pBB->fy = proxy0->m_aabbMax.getY();
00463 pBB->fz = proxy0->m_aabbMax.getZ();
00464 pBB->uw = num_large + m_maxHandles;
00465 pBB++;
00466 num_large++;
00467 }
00468 m_LastLargeHandleIndex = new_largest_index;
00469
00470 btAssert(num_small == m_numHandles);
00471 btAssert(num_large == m_numLargeHandles);
00472 return;
00473 }
00474
00475
00476
00477 void btGpu3DGridBroadphase::setParameters(bt3DGridBroadphaseParams* hostParams)
00478 {
00479 s3DGridBroadphaseParams = *hostParams;
00480 return;
00481 }
00482
00483
00484
00485 void btGpu3DGridBroadphase::calcHashAABB()
00486 {
00487 BT_PROFILE("bt3DGrid_calcHashAABB");
00488 btGpu_calcHashAABB(m_hAABB, m_hBodiesHash, m_numHandles);
00489 return;
00490 }
00491
00492
00493
00494 void btGpu3DGridBroadphase::sortHash()
00495 {
00496 class bt3DGridHashKey
00497 {
00498 public:
00499 unsigned int hash;
00500 unsigned int index;
00501 void quickSort(bt3DGridHashKey* pData, int lo, int hi)
00502 {
00503 int i=lo, j=hi;
00504 bt3DGridHashKey x = pData[(lo+hi)/2];
00505 do
00506 {
00507 while(pData[i].hash > x.hash) i++;
00508 while(x.hash > pData[j].hash) j--;
00509 if(i <= j)
00510 {
00511 bt3DGridHashKey t = pData[i];
00512 pData[i] = pData[j];
00513 pData[j] = t;
00514 i++; j--;
00515 }
00516 } while(i <= j);
00517 if(lo < j) pData->quickSort(pData, lo, j);
00518 if(i < hi) pData->quickSort(pData, i, hi);
00519 }
00520 };
00521 BT_PROFILE("bt3DGrid_sortHash");
00522 bt3DGridHashKey* pHash = (bt3DGridHashKey*)m_hBodiesHash;
00523 pHash->quickSort(pHash, 0, m_numHandles - 1);
00524 return;
00525 }
00526
00527
00528
00529 void btGpu3DGridBroadphase::findCellStart()
00530 {
00531 BT_PROFILE("bt3DGrid_findCellStart");
00532 btGpu_findCellStart(m_hBodiesHash, m_hCellStart, m_numHandles, m_params.m_numCells);
00533 return;
00534 }
00535
00536
00537
00538 void btGpu3DGridBroadphase::findOverlappingPairs()
00539 {
00540 BT_PROFILE("bt3DGrid_findOverlappingPairs");
00541 btGpu_findOverlappingPairs(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr, m_numHandles);
00542 return;
00543 }
00544
00545
00546
00547 void btGpu3DGridBroadphase::findPairsLarge()
00548 {
00549 BT_PROFILE("bt3DGrid_findPairsLarge");
00550 btGpu_findPairsLarge(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr, m_numHandles, m_numLargeHandles);
00551 return;
00552 }
00553
00554
00555
00556 void btGpu3DGridBroadphase::computePairCacheChanges()
00557 {
00558 BT_PROFILE("bt3DGrid_computePairCacheChanges");
00559 btGpu_computePairCacheChanges(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hAABB, m_numHandles);
00560 return;
00561 }
00562
00563
00564
00565 void btGpu3DGridBroadphase::scanOverlappingPairBuff()
00566 {
00567 BT_PROFILE("bt3DGrid_scanOverlappingPairBuff");
00568 m_hPairScan[0] = 0;
00569 for(int i = 1; i <= m_numHandles; i++)
00570 {
00571 unsigned int delta = m_hPairScan[i];
00572 m_hPairScan[i] = m_hPairScan[i-1] + delta;
00573 }
00574 return;
00575 }
00576
00577
00578
00579 void btGpu3DGridBroadphase::squeezeOverlappingPairBuff()
00580 {
00581 BT_PROFILE("bt3DGrid_squeezeOverlappingPairBuff");
00582 btGpu_squeezeOverlappingPairBuff(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hPairOut, m_hAABB, m_numHandles);
00583 return;
00584 }
00585
00586
00587
00588 #include "btGpu3DGridBroadphaseSharedCode.h"
00589
00590