dali-physics/third-party/bullet3/src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl

   1
   2
   3 int getPosHash(int4 gridPos, __global float4* pParams)
   4 {
   5         int4 gridDim = *((__global int4*)(pParams + 1));
   6         gridPos.x &= gridDim.x - 1;
   7         gridPos.y &= gridDim.y - 1;
   8         gridPos.z &= gridDim.z - 1;
   9         int hash = gridPos.z * gridDim.y * gridDim.x + gridPos.y * gridDim.x + gridPos.x;
  10         return hash;
  11 }
  12
  13 int4 getGridPos(float4 worldPos, __global float4* pParams)
  14 {
  15     int4 gridPos;
  16         int4 gridDim = *((__global int4*)(pParams + 1));
  17     gridPos.x = (int)floor(worldPos.x * pParams[0].x) & (gridDim.x - 1);
  18     gridPos.y = (int)floor(worldPos.y * pParams[0].y) & (gridDim.y - 1);
  19     gridPos.z = (int)floor(worldPos.z * pParams[0].z) & (gridDim.z - 1);
  20     return gridPos;
  21 }
  22
  23
  24 // calculate grid hash value for each body using its AABB
  25 __kernel void kCalcHashAABB(int numObjects, __global float4* allpAABB, __global const int* smallAabbMapping, __global int2* pHash, __global float4* pParams )
  26 {
  27     int index = get_global_id(0);
  28     if(index >= numObjects)
  29         {
  30                 return;
  31         }
  32         float4 bbMin = allpAABB[smallAabbMapping[index]*2];
  33         float4 bbMax = allpAABB[smallAabbMapping[index]*2 + 1];
  34         float4 pos;
  35         pos.x = (bbMin.x + bbMax.x) * 0.5f;
  36         pos.y = (bbMin.y + bbMax.y) * 0.5f;
  37         pos.z = (bbMin.z + bbMax.z) * 0.5f;
  38         pos.w = 0.f;
  39     // get address in grid
  40     int4 gridPos = getGridPos(pos, pParams);
  41     int gridHash = getPosHash(gridPos, pParams);
  42     // store grid hash and body index
  43     int2 hashVal;
  44     hashVal.x = gridHash;
  45     hashVal.y = index;
  46     pHash[index] = hashVal;
  47 }
  48
  49 __kernel void kClearCellStart(  int numCells,
  50                                                                 __global int* pCellStart )
  51 {
  52     int index = get_global_id(0);
  53     if(index >= numCells)
  54         {
  55                 return;
  56         }
  57         pCellStart[index] = -1;
  58 }
  59
  60 __kernel void kFindCellStart(int numObjects, __global int2* pHash, __global int* cellStart )
  61 {
  62         __local int sharedHash[513];
  63     int index = get_global_id(0);
  64         int2 sortedData;
  65
  66     if(index < numObjects)
  67         {
  68                 sortedData = pHash[index];
  69                 // Load hash data into shared memory so that we can look
  70                 // at neighboring body's hash value without loading
  71                 // two hash values per thread
  72                 sharedHash[get_local_id(0) + 1] = sortedData.x;
  73                 if((index > 0) && (get_local_id(0) == 0))
  74                 {
  75                         // first thread in block must load neighbor body hash
  76                         sharedHash[0] = pHash[index-1].x;
  77                 }
  78         }
  79     barrier(CLK_LOCAL_MEM_FENCE);
  80     if(index < numObjects)
  81         {
  82                 if((index == 0) || (sortedData.x != sharedHash[get_local_id(0)]))
  83                 {
  84                         cellStart[sortedData.x] = index;
  85                 }
  86         }
  87 }
  88
  89 int testAABBOverlap(float4 min0, float4 max0, float4 min1, float4 max1)
  90 {
  91         return  (min0.x <= max1.x)&& (min1.x <= max0.x) &&
  92                         (min0.y <= max1.y)&& (min1.y <= max0.y) &&
  93                         (min0.z <= max1.z)&& (min1.z <= max0.z);
  94 }
  95
  96
  97
  98
  99 //search for AABB 'index' against other AABBs' in this cell
 100 void findPairsInCell(   int numObjects,
 101                                                 int4    gridPos,
 102                                                 int    index,
 103                                                 __global int2*  pHash,
 104                                                 __global int*   pCellStart,
 105                                                 __global float4* allpAABB,
 106                                                 __global const int* smallAabbMapping,
 107                                                 __global float4* pParams,
 108                                                         volatile  __global int* pairCount,
 109                                                 __global int4*   pPairBuff2,
 110                                                 int maxPairs
 111                                                 )
 112 {
 113         int4 pGridDim = *((__global int4*)(pParams + 1));
 114         int maxBodiesPerCell = pGridDim.w;
 115     int gridHash = getPosHash(gridPos, pParams);
 116     // get start of bucket for this cell
 117     int bucketStart = pCellStart[gridHash];
 118     if (bucketStart == -1)
 119         {
 120         return;   // cell empty
 121         }
 122         // iterate over bodies in this cell
 123     int2 sortedData = pHash[index];
 124         int unsorted_indx = sortedData.y;
 125     float4 min0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0];
 126         float4 max0 = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];
 127         int handleIndex =  as_int(min0.w);
 128
 129         int bucketEnd = bucketStart + maxBodiesPerCell;
 130         bucketEnd = (bucketEnd > numObjects) ? numObjects : bucketEnd;
 131         for(int index2 = bucketStart; index2 < bucketEnd; index2++)
 132         {
 133         int2 cellData = pHash[index2];
 134         if (cellData.x != gridHash)
 135         {
 136                         break;   // no longer in same bucket
 137                 }
 138                 int unsorted_indx2 = cellData.y;
 139         //if (unsorted_indx2 < unsorted_indx) // check not colliding with self
 140                 if (unsorted_indx2 != unsorted_indx) // check not colliding with self
 141         {
 142                         float4 min1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 0];
 143                         float4 max1 = allpAABB[smallAabbMapping[unsorted_indx2]*2 + 1];
 144                         if(testAABBOverlap(min0, max0, min1, max1))
 145                         {
 146                                 if (pairCount)
 147                                 {
 148                                         int handleIndex2 = as_int(min1.w);
 149                                         if (handleIndex<handleIndex2)
 150                                         {
 151                                                 int curPair = atomic_add(pairCount,1);
 152                                                 if (curPair<maxPairs)
 153                                                 {
 154                                                         int4 newpair;
 155                                                         newpair.x = handleIndex;
 156                                                         newpair.y = handleIndex2;
 157                                                         newpair.z = -1;
 158                                                         newpair.w = -1;
 159                                                         pPairBuff2[curPair] = newpair;
 160                                                 }
 161                                         }
 162
 163                                 }
 164                         }
 165                 }
 166         }
 167 }
 168
 169 __kernel void kFindOverlappingPairs(    int numObjects,
 170                                                                                 __global float4* allpAABB,
 171                                                                                 __global const int* smallAabbMapping,
 172                                                                                 __global int2* pHash,
 173                                                                                 __global int* pCellStart,
 174                                                                                 __global float4* pParams ,
 175                                                                                 volatile  __global int* pairCount,
 176                                                                                 __global int4*   pPairBuff2,
 177                                                                                 int maxPairs
 178                                                                                 )
 179
 180 {
 181     int index = get_global_id(0);
 182     if(index >= numObjects)
 183         {
 184                 return;
 185         }
 186     int2 sortedData = pHash[index];
 187         int unsorted_indx = sortedData.y;
 188         float4 bbMin = allpAABB[smallAabbMapping[unsorted_indx]*2 + 0];
 189         float4 bbMax = allpAABB[smallAabbMapping[unsorted_indx]*2 + 1];
 190         float4 pos;
 191         pos.x = (bbMin.x + bbMax.x) * 0.5f;
 192         pos.y = (bbMin.y + bbMax.y) * 0.5f;
 193         pos.z = (bbMin.z + bbMax.z) * 0.5f;
 194     // get address in grid
 195     int4 gridPosA = getGridPos(pos, pParams);
 196     int4 gridPosB;
 197     // examine only neighbouring cells
 198     for(int z=-1; z<=1; z++)
 199     {
 200                 gridPosB.z = gridPosA.z + z;
 201         for(int y=-1; y<=1; y++)
 202         {
 203                         gridPosB.y = gridPosA.y + y;
 204             for(int x=-1; x<=1; x++)
 205             {
 206                                 gridPosB.x = gridPosA.x + x;
 207                 findPairsInCell(numObjects, gridPosB, index, pHash, pCellStart, allpAABB,smallAabbMapping, pParams, pairCount,pPairBuff2, maxPairs);
 208             }
 209         }
 210     }
 211 }
 212
 213
 214
 215
 216