OSDN Git Service

merge from MikuMikuStudio nativebullet.
[mikumikustudio/libgdx-mikumikustudio.git] / extensions / gdx-bullet / jni / src / bullet / BulletMultiThreaded / btGpu3DGridBroadphase.cpp
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
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"
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                 {
279                         ///you have to increase the cell size, so 'large' proxies become 'small' proxies (fitting a cell)
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         
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
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