Bullet Collision Detection & Physics Library
btGpu3DGridBroadphase.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library, http://bulletphysics.org
3 Copyright (C) 2006, 2009 Sony Computer Entertainment Inc.
4 
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:
10 
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.
14 */
15 
17 #include "BulletMultiThreaded/btGpuDefines.h"
18 #include "BulletMultiThreaded/btGpuUtilsSharedDefs.h"
19 #include "BulletMultiThreaded/btGpuUtilsSharedCode.h"
20 
21 
22 
23 #include "LinearMath/btAlignedAllocator.h"
24 #include "LinearMath/btQuickprof.h"
25 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
26 
27 
28 
29 #include "btGpuDefines.h"
30 #include "btGpuUtilsSharedDefs.h"
31 
32 #include "btGpu3DGridBroadphaseSharedDefs.h"
33 
34 #include "btGpu3DGridBroadphase.h"
35 #include <string.h> //for memset
36 
37 
38 #include <stdio.h>
39 
40 
41 
42 static bt3DGridBroadphaseParams s3DGridBroadphaseParams;
43 
44 
45 
46 btGpu3DGridBroadphase::btGpu3DGridBroadphase( const btVector3& worldAabbMin,const btVector3& worldAabbMax,
47  int gridSizeX, int gridSizeY, int gridSizeZ,
48  int maxSmallProxies, int maxLargeProxies, int maxPairsPerBody,
49  int maxBodiesPerCell,
50  btScalar cellFactorAABB) :
51  btSimpleBroadphase(maxSmallProxies,
52 // new (btAlignedAlloc(sizeof(btSortedOverlappingPairCache),16)) btSortedOverlappingPairCache),
53  new (btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache),
54  m_bInitialized(false),
55  m_numBodies(0)
56 {
57  _initialize(worldAabbMin, worldAabbMax, gridSizeX, gridSizeY, gridSizeZ,
58  maxSmallProxies, maxLargeProxies, maxPairsPerBody,
59  maxBodiesPerCell, cellFactorAABB);
60 }
61 
62 
63 
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,
68  int maxBodiesPerCell,
69  btScalar cellFactorAABB) :
70  btSimpleBroadphase(maxSmallProxies, overlappingPairCache),
71  m_bInitialized(false),
72  m_numBodies(0)
73 {
74  _initialize(worldAabbMin, worldAabbMax, gridSizeX, gridSizeY, gridSizeZ,
75  maxSmallProxies, maxLargeProxies, maxPairsPerBody,
76  maxBodiesPerCell, cellFactorAABB);
77 }
78 
79 
80 
81 btGpu3DGridBroadphase::~btGpu3DGridBroadphase()
82 {
83  //btSimpleBroadphase will free memory of btSortedOverlappingPairCache, because m_ownsPairCache
84  btAssert(m_bInitialized);
85  _finalize();
86 }
87 
88 
89 
90 void btGpu3DGridBroadphase::_initialize( const btVector3& worldAabbMin,const btVector3& worldAabbMax,
91  int gridSizeX, int gridSizeY, int gridSizeZ,
92  int maxSmallProxies, int maxLargeProxies, int maxPairsPerBody,
93  int maxBodiesPerCell,
94  btScalar cellFactorAABB)
95 {
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;
114 
115  m_numLargeHandles = 0;
116  m_maxLargeHandles = maxLargeProxies;
117 
118  m_maxPairsPerBody = maxPairsPerBody;
119 
120  m_cellFactorAABB = cellFactorAABB;
121 
122  m_LastLargeHandleIndex = -1;
123 
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));
128 
129  m_hCellStart = new unsigned int[m_params.m_numCells];
130  memset(m_hCellStart, 0x00, m_params.m_numCells * sizeof(unsigned int));
131 
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++)
137  {
138  m_hPairBuffStartCurr[i * 2] = m_hPairBuffStartCurr[(i-1) * 2] + m_maxPairsPerBody;
139  m_hPairBuffStartCurr[i * 2 + 1] = 0;
140  }
141  //----------------
142  unsigned int numAABB = m_maxHandles + m_maxLargeHandles;
143  m_hAABB = new bt3DGrid3F1U[numAABB * 2]; // AABB Min & Max
144 
145  m_hPairBuff = new unsigned int[m_maxHandles * m_maxPairsPerBody];
146  memset(m_hPairBuff, 0x00, m_maxHandles * m_maxPairsPerBody * sizeof(unsigned int)); // needed?
147 
148  m_hPairScan = new unsigned int[m_maxHandles + 1];
149 
150  m_hPairOut = new unsigned int[m_maxHandles * m_maxPairsPerBody];
151 
152 // large proxies
153 
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;
158  {
159  for (int i = m_firstFreeLargeHandle; i < m_maxLargeHandles; i++)
160  {
161  m_pLargeHandles[i].SetNextFree(i + 1);
162  m_pLargeHandles[i].m_uniqueId = m_maxHandles+2+i;
163  }
164  m_pLargeHandles[m_maxLargeHandles - 1].SetNextFree(0);
165  }
166 
167 // debug data
168  m_numPairsAdded = 0;
169  m_numOverflows = 0;
170 
171  m_bInitialized = true;
172 }
173 
174 
175 
176 void btGpu3DGridBroadphase::_finalize()
177 {
178  btAssert(m_bInitialized);
179  delete [] m_hBodiesHash;
180  delete [] m_hCellStart;
181  delete [] m_hPairBuffStartCurr;
182  delete [] m_hAABB;
183  delete [] m_hPairBuff;
184  delete [] m_hPairScan;
185  delete [] m_hPairOut;
186  btAlignedFree(m_pLargeHandlesRawPtr);
187  m_bInitialized = false;
188 }
189 
190 
191 
192 void btGpu3DGridBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
193 {
194  if(m_numHandles <= 0)
195  {
196  BT_PROFILE("addLarge2LargePairsToCache");
197  addLarge2LargePairsToCache(dispatcher);
198  return;
199  }
200  // update constants
201  setParameters(&m_params);
202  // prepare AABB array
203  prepareAABB();
204  // calculate hash
205  calcHashAABB();
206  // sort bodies based on hash
207  sortHash();
208  // find start of each cell
209  findCellStart();
210  // findOverlappingPairs (small/small)
211  findOverlappingPairs();
212  // findOverlappingPairs (small/large)
213  findPairsLarge();
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);
221  return;
222 }
223 
224 
225 
226 void btGpu3DGridBroadphase::addPairsToCache(btDispatcher* dispatcher)
227 {
228  m_numPairsAdded = 0;
229  m_numPairsRemoved = 0;
230  for(int i = 0; i < m_numHandles; i++)
231  {
232  unsigned int num = m_hPairScan[i+1] - m_hPairScan[i];
233  if(!num)
234  {
235  continue;
236  }
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++)
241  {
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)
246  {
247  proxy1 = &m_pHandles[index1];
248  }
249  else
250  {
251  index1 -= m_maxHandles;
252  btAssert((index1 >= 0) && (index1 < (unsigned int)m_maxLargeHandles));
253  proxy1 = &m_pLargeHandles[index1];
254  }
255  if(indx1_s & BT_3DGRID_PAIR_NEW_FLG)
256  {
257  m_pairCache->addOverlappingPair(proxy0,proxy1);
258  m_numPairsAdded++;
259  }
260  else
261  {
262  m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
263  m_numPairsRemoved++;
264  }
265  }
266  }
267 }
268 
269 
270 
271 btBroadphaseProxy* btGpu3DGridBroadphase::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask, btDispatcher* dispatcher,void* multiSapProxy)
272 {
273  btBroadphaseProxy* proxy;
274  bool bIsLarge = isLargeProxy(aabbMin, aabbMax);
275  if(bIsLarge)
276  {
277  if (m_numLargeHandles >= m_maxLargeHandles)
278  {
280  btAssert(0);
281  return 0; //should never happen, but don't let the game crash ;-)
282  }
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);
286  }
287  else
288  {
289  proxy = btSimpleBroadphase::createProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask, dispatcher, multiSapProxy);
290  }
291  return proxy;
292 }
293 
294 
295 
296 void btGpu3DGridBroadphase::destroyProxy(btBroadphaseProxy* proxy, btDispatcher* dispatcher)
297 {
298  bool bIsLarge = isLargeProxy(proxy);
299  if(bIsLarge)
300  {
301 
302  btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxy);
303  freeLargeHandle(proxy0);
304  m_pairCache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
305  }
306  else
307  {
308  btSimpleBroadphase::destroyProxy(proxy, dispatcher);
309  }
310  return;
311 }
312 
313 
314 
315 void btGpu3DGridBroadphase::resetPool(btDispatcher* dispatcher)
316 {
317  m_hPairBuffStartCurr[0] = 0;
318  m_hPairBuffStartCurr[1] = 0;
319  for(int i = 1; i <= m_maxHandles; i++)
320  {
321  m_hPairBuffStartCurr[i * 2] = m_hPairBuffStartCurr[(i-1) * 2] + m_maxPairsPerBody;
322  m_hPairBuffStartCurr[i * 2 + 1] = 0;
323  }
324 }
325 
326 
327 
328 bool btGpu3DGridBroadphase::isLargeProxy(const btVector3& aabbMin, const btVector3& aabbMax)
329 {
330  btVector3 diag = aabbMax - aabbMin;
331 
333  btScalar radius = diag.length() * btScalar(0.5f);
334  radius *= m_cellFactorAABB; // user-defined factor
335 
336  return (radius > m_maxRadius);
337 }
338 
339 
340 
341 bool btGpu3DGridBroadphase::isLargeProxy(btBroadphaseProxy* proxy)
342 {
343  return (proxy->getUid() >= (m_maxHandles+2));
344 }
345 
346 
347 
348 void btGpu3DGridBroadphase::addLarge2LargePairsToCache(btDispatcher* dispatcher)
349 {
350  int i,j;
351  if (m_numLargeHandles <= 0)
352  {
353  return;
354  }
355  int new_largest_index = -1;
356  for(i = 0; i <= m_LastLargeHandleIndex; i++)
357  {
358  btSimpleBroadphaseProxy* proxy0 = &m_pLargeHandles[i];
359  if(!proxy0->m_clientObject)
360  {
361  continue;
362  }
363  new_largest_index = i;
364  for(j = i + 1; j <= m_LastLargeHandleIndex; j++)
365  {
366  btSimpleBroadphaseProxy* proxy1 = &m_pLargeHandles[j];
367  if(!proxy1->m_clientObject)
368  {
369  continue;
370  }
371  btAssert(proxy0 != proxy1);
372  btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
373  btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
374  if(aabbOverlap(p0,p1))
375  {
376  if (!m_pairCache->findPair(proxy0,proxy1))
377  {
378  m_pairCache->addOverlappingPair(proxy0,proxy1);
379  }
380  }
381  else
382  {
383  if(m_pairCache->findPair(proxy0,proxy1))
384  {
385  m_pairCache->removeOverlappingPair(proxy0,proxy1,dispatcher);
386  }
387  }
388  }
389  }
390  m_LastLargeHandleIndex = new_largest_index;
391  return;
392 }
393 
394 
395 
396 void btGpu3DGridBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax)
397 {
398  btSimpleBroadphase::rayTest(rayFrom, rayTo, rayCallback);
399  for (int i=0; i <= m_LastLargeHandleIndex; i++)
400  {
401  btSimpleBroadphaseProxy* proxy = &m_pLargeHandles[i];
402  if(!proxy->m_clientObject)
403  {
404  continue;
405  }
406  rayCallback.process(proxy);
407  }
408 }
409 
410 
411 
412 //
413 // overrides for CPU version
414 //
415 
416 
417 
418 void btGpu3DGridBroadphase::prepareAABB()
419 {
420  BT_PROFILE("prepareAABB");
421  bt3DGrid3F1U* pBB = m_hAABB;
422  int i;
423  int new_largest_index = -1;
424  unsigned int num_small = 0;
425  for(i = 0; i <= m_LastHandleIndex; i++)
426  {
427  btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
428  if(!proxy0->m_clientObject)
429  {
430  continue;
431  }
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();
436  pBB->uw = i;
437  pBB++;
438  pBB->fx = proxy0->m_aabbMax.getX();
439  pBB->fy = proxy0->m_aabbMax.getY();
440  pBB->fz = proxy0->m_aabbMax.getZ();
441  pBB->uw = num_small;
442  pBB++;
443  num_small++;
444  }
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++)
449  {
450  btSimpleBroadphaseProxy* proxy0 = &m_pLargeHandles[i];
451  if(!proxy0->m_clientObject)
452  {
453  continue;
454  }
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;
460  pBB++;
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;
465  pBB++;
466  num_large++;
467  }
468  m_LastLargeHandleIndex = new_largest_index;
469  // paranoid checks
470  btAssert(num_small == m_numHandles);
471  btAssert(num_large == m_numLargeHandles);
472  return;
473 }
474 
475 
476 
477 void btGpu3DGridBroadphase::setParameters(bt3DGridBroadphaseParams* hostParams)
478 {
479  s3DGridBroadphaseParams = *hostParams;
480  return;
481 }
482 
483 
484 
485 void btGpu3DGridBroadphase::calcHashAABB()
486 {
487  BT_PROFILE("bt3DGrid_calcHashAABB");
488  btGpu_calcHashAABB(m_hAABB, m_hBodiesHash, m_numHandles);
489  return;
490 }
491 
492 
493 
494 void btGpu3DGridBroadphase::sortHash()
495 {
496  class bt3DGridHashKey
497  {
498  public:
499  unsigned int hash;
500  unsigned int index;
501  void quickSort(bt3DGridHashKey* pData, int lo, int hi)
502  {
503  int i=lo, j=hi;
504  bt3DGridHashKey x = pData[(lo+hi)/2];
505  do
506  {
507  while(pData[i].hash > x.hash) i++;
508  while(x.hash > pData[j].hash) j--;
509  if(i <= j)
510  {
511  bt3DGridHashKey t = pData[i];
512  pData[i] = pData[j];
513  pData[j] = t;
514  i++; j--;
515  }
516  } while(i <= j);
517  if(lo < j) pData->quickSort(pData, lo, j);
518  if(i < hi) pData->quickSort(pData, i, hi);
519  }
520  };
521  BT_PROFILE("bt3DGrid_sortHash");
522  bt3DGridHashKey* pHash = (bt3DGridHashKey*)m_hBodiesHash;
523  pHash->quickSort(pHash, 0, m_numHandles - 1);
524  return;
525 }
526 
527 
528 
529 void btGpu3DGridBroadphase::findCellStart()
530 {
531  BT_PROFILE("bt3DGrid_findCellStart");
532  btGpu_findCellStart(m_hBodiesHash, m_hCellStart, m_numHandles, m_params.m_numCells);
533  return;
534 }
535 
536 
537 
538 void btGpu3DGridBroadphase::findOverlappingPairs()
539 {
540  BT_PROFILE("bt3DGrid_findOverlappingPairs");
541  btGpu_findOverlappingPairs(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr, m_numHandles);
542  return;
543 }
544 
545 
546 
547 void btGpu3DGridBroadphase::findPairsLarge()
548 {
549  BT_PROFILE("bt3DGrid_findPairsLarge");
550  btGpu_findPairsLarge(m_hAABB, m_hBodiesHash, m_hCellStart, m_hPairBuff, m_hPairBuffStartCurr, m_numHandles, m_numLargeHandles);
551  return;
552 }
553 
554 
555 
556 void btGpu3DGridBroadphase::computePairCacheChanges()
557 {
558  BT_PROFILE("bt3DGrid_computePairCacheChanges");
559  btGpu_computePairCacheChanges(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hAABB, m_numHandles);
560  return;
561 }
562 
563 
564 
565 void btGpu3DGridBroadphase::scanOverlappingPairBuff()
566 {
567  BT_PROFILE("bt3DGrid_scanOverlappingPairBuff");
568  m_hPairScan[0] = 0;
569  for(int i = 1; i <= m_numHandles; i++)
570  {
571  unsigned int delta = m_hPairScan[i];
572  m_hPairScan[i] = m_hPairScan[i-1] + delta;
573  }
574  return;
575 }
576 
577 
578 
579 void btGpu3DGridBroadphase::squeezeOverlappingPairBuff()
580 {
581  BT_PROFILE("bt3DGrid_squeezeOverlappingPairBuff");
582  btGpu_squeezeOverlappingPairBuff(m_hPairBuff, m_hPairBuffStartCurr, m_hPairScan, m_hPairOut, m_hAABB, m_numHandles);
583  return;
584 }
585 
586 
587 
588 #include "btGpu3DGridBroadphaseSharedCode.h"
589 
590 
Generated on Thu Oct 3 2013 18:25:06 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4