dali-physics/third-party/bullet3/src/Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.cpp

   1
   2 #include "b3GpuGridBroadphase.h"
   3 #include "Bullet3Geometry/b3AabbUtil.h"
   4 #include "kernels/gridBroadphaseKernels.h"
   5 #include "kernels/sapKernels.h"
   6 //#include "kernels/gridBroadphase.cl"
   7
   8 #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
   9 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
  10
  11 #define B3_BROADPHASE_SAP_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl"
  12 #define B3_GRID_BROADPHASE_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/gridBroadphase.cl"
  13
  14 cl_kernel kCalcHashAABB;
  15 cl_kernel kClearCellStart;
  16 cl_kernel kFindCellStart;
  17 cl_kernel kFindOverlappingPairs;
  18 cl_kernel m_copyAabbsKernel;
  19 cl_kernel m_sap2Kernel;
  20
  21 //int maxPairsPerBody = 64;
  22 int maxBodiesPerCell = 256;  //??
  23
  24 b3GpuGridBroadphase::b3GpuGridBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q)
  25         : m_context(ctx),
  26           m_device(device),
  27           m_queue(q),
  28           m_allAabbsGPU1(ctx, q),
  29           m_smallAabbsMappingGPU(ctx, q),
  30           m_largeAabbsMappingGPU(ctx, q),
  31           m_gpuPairs(ctx, q),
  32
  33           m_hashGpu(ctx, q),
  34
  35           m_cellStartGpu(ctx, q),
  36           m_paramsGPU(ctx, q)
  37 {
  38         b3Vector3 gridSize = b3MakeVector3(3, 3, 3);
  39         b3Vector3 invGridSize = b3MakeVector3(1.f / gridSize[0], 1.f / gridSize[1], 1.f / gridSize[2]);
  40
  41         m_paramsCPU.m_gridSize[0] = 128;
  42         m_paramsCPU.m_gridSize[1] = 128;
  43         m_paramsCPU.m_gridSize[2] = 128;
  44         m_paramsCPU.m_gridSize[3] = maxBodiesPerCell;
  45         m_paramsCPU.setMaxBodiesPerCell(maxBodiesPerCell);
  46         m_paramsCPU.m_invCellSize[0] = invGridSize[0];
  47         m_paramsCPU.m_invCellSize[1] = invGridSize[1];
  48         m_paramsCPU.m_invCellSize[2] = invGridSize[2];
  49         m_paramsCPU.m_invCellSize[3] = 0.f;
  50         m_paramsGPU.push_back(m_paramsCPU);
  51
  52         cl_int errNum = 0;
  53
  54         {
  55                 const char* sapSrc = sapCL;
  56                 cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, sapSrc, &errNum, "", B3_BROADPHASE_SAP_PATH);
  57                 b3Assert(errNum == CL_SUCCESS);
  58                 m_copyAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "copyAabbsKernel", &errNum, sapProg);
  59                 m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelTwoArrays", &errNum, sapProg);
  60                 b3Assert(errNum == CL_SUCCESS);
  61         }
  62
  63         {
  64                 cl_program gridProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, gridBroadphaseCL, &errNum, "", B3_GRID_BROADPHASE_PATH);
  65                 b3Assert(errNum == CL_SUCCESS);
  66
  67                 kCalcHashAABB = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kCalcHashAABB", &errNum, gridProg);
  68                 b3Assert(errNum == CL_SUCCESS);
  69
  70                 kClearCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kClearCellStart", &errNum, gridProg);
  71                 b3Assert(errNum == CL_SUCCESS);
  72
  73                 kFindCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindCellStart", &errNum, gridProg);
  74                 b3Assert(errNum == CL_SUCCESS);
  75
  76                 kFindOverlappingPairs = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindOverlappingPairs", &errNum, gridProg);
  77                 b3Assert(errNum == CL_SUCCESS);
  78         }
  79
  80         m_sorter = new b3RadixSort32CL(m_context, m_device, m_queue);
  81 }
  82 b3GpuGridBroadphase::~b3GpuGridBroadphase()
  83 {
  84         clReleaseKernel(kCalcHashAABB);
  85         clReleaseKernel(kClearCellStart);
  86         clReleaseKernel(kFindCellStart);
  87         clReleaseKernel(kFindOverlappingPairs);
  88         clReleaseKernel(m_sap2Kernel);
  89         clReleaseKernel(m_copyAabbsKernel);
  90
  91         delete m_sorter;
  92 }
  93
  94 void b3GpuGridBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
  95 {
  96         b3SapAabb aabb;
  97         aabb.m_minVec = aabbMin;
  98         aabb.m_maxVec = aabbMax;
  99         aabb.m_minIndices[3] = userPtr;
 100         aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();  //NOT userPtr;
 101         m_smallAabbsMappingCPU.push_back(m_allAabbsCPU1.size());
 102
 103         m_allAabbsCPU1.push_back(aabb);
 104 }
 105 void b3GpuGridBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
 106 {
 107         b3SapAabb aabb;
 108         aabb.m_minVec = aabbMin;
 109         aabb.m_maxVec = aabbMax;
 110         aabb.m_minIndices[3] = userPtr;
 111         aabb.m_signedMaxIndices[3] = m_allAabbsCPU1.size();  //NOT userPtr;
 112         m_largeAabbsMappingCPU.push_back(m_allAabbsCPU1.size());
 113
 114         m_allAabbsCPU1.push_back(aabb);
 115 }
 116
 117 void b3GpuGridBroadphase::calculateOverlappingPairs(int maxPairs)
 118 {
 119         B3_PROFILE("b3GpuGridBroadphase::calculateOverlappingPairs");
 120
 121         if (0)
 122         {
 123                 calculateOverlappingPairsHost(maxPairs);
 124                 /*
 125                 b3AlignedObjectArray<b3Int4> cpuPairs;
 126                 m_gpuPairs.copyToHost(cpuPairs);
 127                 printf("host m_gpuPairs.size()=%d\n",m_gpuPairs.size());
 128                 for (int i=0;i<m_gpuPairs.size();i++)
 129                 {
 130                         printf("host pair %d = %d,%d\n",i,cpuPairs[i].x,cpuPairs[i].y);
 131                 }
 132                 */
 133                 return;
 134         }
 135
 136         int numSmallAabbs = m_smallAabbsMappingGPU.size();
 137
 138         b3OpenCLArray<int> pairCount(m_context, m_queue);
 139         pairCount.push_back(0);
 140         m_gpuPairs.resize(maxPairs);  //numSmallAabbs*maxPairsPerBody);
 141
 142         {
 143                 int numLargeAabbs = m_largeAabbsMappingGPU.size();
 144                 if (numLargeAabbs && numSmallAabbs)
 145                 {
 146                         B3_PROFILE("sap2Kernel");
 147                         b3BufferInfoCL bInfo[] = {
 148                                 b3BufferInfoCL(m_allAabbsGPU1.getBufferCL()),
 149                                 b3BufferInfoCL(m_largeAabbsMappingGPU.getBufferCL()),
 150                                 b3BufferInfoCL(m_smallAabbsMappingGPU.getBufferCL()),
 151                                 b3BufferInfoCL(m_gpuPairs.getBufferCL()),
 152                                 b3BufferInfoCL(pairCount.getBufferCL())};
 153                         b3LauncherCL launcher(m_queue, m_sap2Kernel, "m_sap2Kernel");
 154                         launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
 155                         launcher.setConst(numLargeAabbs);
 156                         launcher.setConst(numSmallAabbs);
 157                         launcher.setConst(0);  //axis is not used
 158                         launcher.setConst(maxPairs);
 159                         //@todo: use actual maximum work item sizes of the device instead of hardcoded values
 160                         launcher.launch2D(numLargeAabbs, numSmallAabbs, 4, 64);
 161
 162                         int numPairs = pairCount.at(0);
 163
 164                         if (numPairs > maxPairs)
 165                         {
 166                                 b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
 167                                 numPairs = maxPairs;
 168                         }
 169                 }
 170         }
 171
 172         if (numSmallAabbs)
 173         {
 174                 B3_PROFILE("gridKernel");
 175                 m_hashGpu.resize(numSmallAabbs);
 176                 {
 177                         B3_PROFILE("kCalcHashAABB");
 178                         b3LauncherCL launch(m_queue, kCalcHashAABB, "kCalcHashAABB");
 179                         launch.setConst(numSmallAabbs);
 180                         launch.setBuffer(m_allAabbsGPU1.getBufferCL());
 181                         launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
 182                         launch.setBuffer(m_hashGpu.getBufferCL());
 183                         launch.setBuffer(this->m_paramsGPU.getBufferCL());
 184                         launch.launch1D(numSmallAabbs);
 185                 }
 186
 187                 m_sorter->execute(m_hashGpu);
 188
 189                 int numCells = this->m_paramsCPU.m_gridSize[0] * this->m_paramsCPU.m_gridSize[1] * this->m_paramsCPU.m_gridSize[2];
 190                 m_cellStartGpu.resize(numCells);
 191                 //b3AlignedObjectArray<int >                    cellStartCpu;
 192
 193                 {
 194                         B3_PROFILE("kClearCellStart");
 195                         b3LauncherCL launch(m_queue, kClearCellStart, "kClearCellStart");
 196                         launch.setConst(numCells);
 197                         launch.setBuffer(m_cellStartGpu.getBufferCL());
 198                         launch.launch1D(numCells);
 199                         //m_cellStartGpu.copyToHost(cellStartCpu);
 200                         //printf("??\n");
 201                 }
 202
 203                 {
 204                         B3_PROFILE("kFindCellStart");
 205                         b3LauncherCL launch(m_queue, kFindCellStart, "kFindCellStart");
 206                         launch.setConst(numSmallAabbs);
 207                         launch.setBuffer(m_hashGpu.getBufferCL());
 208                         launch.setBuffer(m_cellStartGpu.getBufferCL());
 209                         launch.launch1D(numSmallAabbs);
 210                         //m_cellStartGpu.copyToHost(cellStartCpu);
 211                         //printf("??\n");
 212                 }
 213
 214                 {
 215                         B3_PROFILE("kFindOverlappingPairs");
 216
 217                         b3LauncherCL launch(m_queue, kFindOverlappingPairs, "kFindOverlappingPairs");
 218                         launch.setConst(numSmallAabbs);
 219                         launch.setBuffer(m_allAabbsGPU1.getBufferCL());
 220                         launch.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
 221                         launch.setBuffer(m_hashGpu.getBufferCL());
 222                         launch.setBuffer(m_cellStartGpu.getBufferCL());
 223
 224                         launch.setBuffer(m_paramsGPU.getBufferCL());
 225                         //launch.setBuffer(0);
 226                         launch.setBuffer(pairCount.getBufferCL());
 227                         launch.setBuffer(m_gpuPairs.getBufferCL());
 228
 229                         launch.setConst(maxPairs);
 230                         launch.launch1D(numSmallAabbs);
 231
 232                         int numPairs = pairCount.at(0);
 233                         if (numPairs > maxPairs)
 234                         {
 235                                 b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
 236                                 numPairs = maxPairs;
 237                         }
 238
 239                         m_gpuPairs.resize(numPairs);
 240
 241                         if (0)
 242                         {
 243                                 b3AlignedObjectArray<b3Int4> pairsCpu;
 244                                 m_gpuPairs.copyToHost(pairsCpu);
 245
 246                                 int sz = m_gpuPairs.size();
 247                                 printf("m_gpuPairs.size()=%d\n", sz);
 248                                 for (int i = 0; i < m_gpuPairs.size(); i++)
 249                                 {
 250                                         printf("pair %d = %d,%d\n", i, pairsCpu[i].x, pairsCpu[i].y);
 251                                 }
 252
 253                                 printf("?!?\n");
 254                         }
 255                 }
 256         }
 257
 258         //calculateOverlappingPairsHost(maxPairs);
 259 }
 260 void b3GpuGridBroadphase::calculateOverlappingPairsHost(int maxPairs)
 261 {
 262         m_hostPairs.resize(0);
 263         m_allAabbsGPU1.copyToHost(m_allAabbsCPU1);
 264         for (int i = 0; i < m_allAabbsCPU1.size(); i++)
 265         {
 266                 for (int j = i + 1; j < m_allAabbsCPU1.size(); j++)
 267                 {
 268                         if (b3TestAabbAgainstAabb2(m_allAabbsCPU1[i].m_minVec, m_allAabbsCPU1[i].m_maxVec,
 269                                                                            m_allAabbsCPU1[j].m_minVec, m_allAabbsCPU1[j].m_maxVec))
 270                         {
 271                                 b3Int4 pair;
 272                                 int a = m_allAabbsCPU1[j].m_minIndices[3];
 273                                 int b = m_allAabbsCPU1[i].m_minIndices[3];
 274                                 if (a <= b)
 275                                 {
 276                                         pair.x = a;
 277                                         pair.y = b;  //store the original index in the unsorted aabb array
 278                                 }
 279                                 else
 280                                 {
 281                                         pair.x = b;
 282                                         pair.y = a;  //store the original index in the unsorted aabb array
 283                                 }
 284
 285                                 if (m_hostPairs.size() < maxPairs)
 286                                 {
 287                                         m_hostPairs.push_back(pair);
 288                                 }
 289                         }
 290                 }
 291         }
 292
 293         m_gpuPairs.copyFromHost(m_hostPairs);
 294 }
 295
 296 //call writeAabbsToGpu after done making all changes (createProxy etc)
 297 void b3GpuGridBroadphase::writeAabbsToGpu()
 298 {
 299         m_allAabbsGPU1.copyFromHost(m_allAabbsCPU1);
 300         m_smallAabbsMappingGPU.copyFromHost(m_smallAabbsMappingCPU);
 301         m_largeAabbsMappingGPU.copyFromHost(m_largeAabbsMappingCPU);
 302 }
 303
 304 cl_mem b3GpuGridBroadphase::getAabbBufferWS()
 305 {
 306         return this->m_allAabbsGPU1.getBufferCL();
 307 }
 308 int b3GpuGridBroadphase::getNumOverlap()
 309 {
 310         return m_gpuPairs.size();
 311 }
 312 cl_mem b3GpuGridBroadphase::getOverlappingPairBuffer()
 313 {
 314         return m_gpuPairs.getBufferCL();
 315 }
 316
 317 b3OpenCLArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsGPU()
 318 {
 319         return m_allAabbsGPU1;
 320 }
 321
 322 b3AlignedObjectArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsCPU()
 323 {
 324         return m_allAabbsCPU1;
 325 }
 326
 327 b3OpenCLArray<b3Int4>& b3GpuGridBroadphase::getOverlappingPairsGPU()
 328 {
 329         return m_gpuPairs;
 330 }
 331 b3OpenCLArray<int>& b3GpuGridBroadphase::getSmallAabbIndicesGPU()
 332 {
 333         return m_smallAabbsMappingGPU;
 334 }
 335 b3OpenCLArray<int>& b3GpuGridBroadphase::getLargeAabbIndicesGPU()
 336 {
 337         return m_largeAabbsMappingGPU;
 338 }