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
b3GpuGridBroadphase(cl_context ctx,cl_device_id device,cl_command_queue q)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 }
~b3GpuGridBroadphase()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
createProxy(const b3Vector3 & aabbMin,const b3Vector3 & aabbMax,int userPtr,int collisionFilterGroup,int collisionFilterMask)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 }
createLargeProxy(const b3Vector3 & aabbMin,const b3Vector3 & aabbMax,int userPtr,int collisionFilterGroup,int collisionFilterMask)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
calculateOverlappingPairs(int maxPairs)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 }
calculateOverlappingPairsHost(int maxPairs)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)
writeAabbsToGpu()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
getAabbBufferWS()304 cl_mem b3GpuGridBroadphase::getAabbBufferWS()
305 {
306 return this->m_allAabbsGPU1.getBufferCL();
307 }
getNumOverlap()308 int b3GpuGridBroadphase::getNumOverlap()
309 {
310 return m_gpuPairs.size();
311 }
getOverlappingPairBuffer()312 cl_mem b3GpuGridBroadphase::getOverlappingPairBuffer()
313 {
314 return m_gpuPairs.getBufferCL();
315 }
316
getAllAabbsGPU()317 b3OpenCLArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsGPU()
318 {
319 return m_allAabbsGPU1;
320 }
321
getAllAabbsCPU()322 b3AlignedObjectArray<b3SapAabb>& b3GpuGridBroadphase::getAllAabbsCPU()
323 {
324 return m_allAabbsCPU1;
325 }
326
getOverlappingPairsGPU()327 b3OpenCLArray<b3Int4>& b3GpuGridBroadphase::getOverlappingPairsGPU()
328 {
329 return m_gpuPairs;
330 }
getSmallAabbIndicesGPU()331 b3OpenCLArray<int>& b3GpuGridBroadphase::getSmallAabbIndicesGPU()
332 {
333 return m_smallAabbsMappingGPU;
334 }
getLargeAabbIndicesGPU()335 b3OpenCLArray<int>& b3GpuGridBroadphase::getLargeAabbIndicesGPU()
336 {
337 return m_largeAabbsMappingGPU;
338 }
339