1 #include "GpuRigidBodyDemo.h"
2 #include "../OpenGLWindow/ShapeData.h"
3 #include "../OpenGLWindow/GLInstancingRenderer.h"
4 #include "Bullet3Common/b3Quaternion.h"
5 #include "../CommonInterfaces/CommonWindowInterface.h"
6 #include "Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h"
7 #include "Bullet3OpenCL/BroadphaseCollision/b3GpuGridBroadphase.h"
8
9 #include "../CommonOpenCL/GpuDemoInternalData.h"
10 #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
11 #include "../OpenGLWindow/OpenGLInclude.h"
12 #include "../OpenGLWindow/GLInstanceRendererInternalData.h"
13 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
14 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
15 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
16 #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
17 #include "GpuRigidBodyDemoInternalData.h"
18 #include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
19 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
20 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h"
21 #include "stb_image/stb_image.h"
22 #include "../OpenGLWindow/GLPrimitiveRenderer.h"
23
24 extern int gPreferredOpenCLDeviceIndex;
25 extern int gPreferredOpenCLPlatformIndex;
26 extern int gGpuArraySizeX;
27 extern int gGpuArraySizeY;
28 extern int gGpuArraySizeZ;
29
30 static b3KeyboardCallback oldCallback = 0;
31 extern bool gReset;
32 bool useUniformGrid = false;
33 bool convertOnCpu = false;
34
35 #define MSTRINGIFY(A) #A
36
37 static const char* s_rigidBodyKernelString = MSTRINGIFY(
38
39 typedef struct
40 {
41 float4 m_pos;
42 float4 m_quat;
43 float4 m_linVel;
44 float4 m_angVel;
45 unsigned int m_collidableIdx;
46 float m_invMass;
47 float m_restituitionCoeff;
48 float m_frictionCoeff;
49 } Body;
50
51 __kernel void
52 copyTransformsToVBOKernel(__global Body* gBodies, __global float4* posOrnColor, const int numNodes) {
53 int nodeID = get_global_id(0);
54 if (nodeID < numNodes)
55 {
56 posOrnColor[nodeID] = (float4)(gBodies[nodeID].m_pos.xyz, 1.0);
57 posOrnColor[nodeID + numNodes] = gBodies[nodeID].m_quat;
58 }
59 });
60
GpuRigidBodyDemo(GUIHelperInterface * helper)61 GpuRigidBodyDemo::GpuRigidBodyDemo(GUIHelperInterface* helper)
62 : CommonOpenCLBase(helper),
63 m_instancingRenderer(0),
64 m_window(0)
65 {
66 if (helper->getRenderInterface()->getInternalData())
67 {
68 m_instancingRenderer = (GLInstancingRenderer*)helper->getRenderInterface();
69 }
70 else
71 {
72 m_instancingRenderer = 0;
73 }
74
75 m_window = helper->getAppInterface()->m_window;
76
77 m_data = new GpuRigidBodyDemoInternalData;
78 m_data->m_guiHelper = helper;
79 }
80
resetCamera()81 void GpuRigidBodyDemo::resetCamera()
82 {
83 float dist = 114;
84 float pitch = -35;
85 float yaw = 52;
86 float targetPos[3] = {0, 0, 0};
87 m_data->m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
88 }
89
~GpuRigidBodyDemo()90 GpuRigidBodyDemo::~GpuRigidBodyDemo()
91 {
92 delete m_data;
93 }
94
PairKeyboardCallback(int key,int state)95 static void PairKeyboardCallback(int key, int state)
96 {
97 if (key == 'R' && state)
98 {
99 //gReset = true;
100 }
101
102 //b3DefaultKeyboardCallback(key,state);
103 oldCallback(key, state);
104 }
105
setupScene()106 void GpuRigidBodyDemo::setupScene()
107 {
108 }
109
initPhysics()110 void GpuRigidBodyDemo::initPhysics()
111 {
112 initCL(gPreferredOpenCLDeviceIndex, gPreferredOpenCLPlatformIndex);
113
114 m_guiHelper->setUpAxis(1);
115
116 if (m_clData->m_clContext)
117 {
118 int errNum = 0;
119
120 cl_program rbProg = 0;
121 m_data->m_copyTransformsToVBOKernel = b3OpenCLUtils::compileCLKernelFromString(m_clData->m_clContext, m_clData->m_clDevice, s_rigidBodyKernelString, "copyTransformsToVBOKernel", &errNum, rbProg);
122
123 m_data->m_config.m_maxConvexBodies = b3Max(m_data->m_config.m_maxConvexBodies, gGpuArraySizeX * gGpuArraySizeY * gGpuArraySizeZ + 10);
124 m_data->m_config.m_maxConvexShapes = m_data->m_config.m_maxConvexBodies;
125 int maxPairsPerBody = 16;
126 m_data->m_config.m_maxBroadphasePairs = maxPairsPerBody * m_data->m_config.m_maxConvexBodies;
127 m_data->m_config.m_maxContactCapacity = m_data->m_config.m_maxBroadphasePairs;
128
129 b3GpuNarrowPhase* np = new b3GpuNarrowPhase(m_clData->m_clContext, m_clData->m_clDevice, m_clData->m_clQueue, m_data->m_config);
130 b3GpuBroadphaseInterface* bp = 0;
131
132 if (useUniformGrid)
133 {
134 bp = new b3GpuGridBroadphase(m_clData->m_clContext, m_clData->m_clDevice, m_clData->m_clQueue);
135 }
136 else
137 {
138 bp = new b3GpuSapBroadphase(m_clData->m_clContext, m_clData->m_clDevice, m_clData->m_clQueue);
139 }
140 m_data->m_np = np;
141 m_data->m_bp = bp;
142 m_data->m_broadphaseDbvt = new b3DynamicBvhBroadphase(m_data->m_config.m_maxConvexBodies);
143
144 m_data->m_rigidBodyPipeline = new b3GpuRigidBodyPipeline(m_clData->m_clContext, m_clData->m_clDevice, m_clData->m_clQueue, np, bp, m_data->m_broadphaseDbvt, m_data->m_config);
145
146 setupScene();
147
148 m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
149 np->writeAllBodiesToGpu();
150 bp->writeAabbsToGpu();
151 }
152
153 m_guiHelper->getRenderInterface()->writeTransforms();
154 }
155
exitPhysics()156 void GpuRigidBodyDemo::exitPhysics()
157 {
158 destroyScene();
159
160 delete m_data->m_instancePosOrnColor;
161 delete m_data->m_rigidBodyPipeline;
162 delete m_data->m_broadphaseDbvt;
163
164 delete m_data->m_np;
165 m_data->m_np = 0;
166 delete m_data->m_bp;
167 m_data->m_bp = 0;
168
169 exitCL();
170 }
171
renderScene()172 void GpuRigidBodyDemo::renderScene()
173 {
174 m_guiHelper->getRenderInterface()->renderScene();
175 }
176
stepSimulation(float deltaTime)177 void GpuRigidBodyDemo::stepSimulation(float deltaTime)
178 {
179 if (!m_instancingRenderer)
180 return;
181
182 bool animate = true;
183 int numObjects = m_data->m_rigidBodyPipeline->getNumBodies();
184 //printf("numObjects=%d\n",numObjects);
185 if (numObjects > m_instancingRenderer->getInstanceCapacity())
186 {
187 static bool once = true;
188 if (once)
189 {
190 once = false;
191 b3Assert(0);
192 b3Error("m_instancingRenderer out-of-memory\n");
193 }
194 numObjects = m_instancingRenderer->getInstanceCapacity();
195 }
196
197 GLint err = glGetError();
198 assert(err == GL_NO_ERROR);
199
200 b3Vector4* positions = 0;
201 if (animate && numObjects)
202 {
203 B3_PROFILE("gl2cl");
204
205 if (!m_data->m_instancePosOrnColor)
206 {
207 GLuint vbo = m_instancingRenderer->getInternalData()->m_vbo;
208 int arraySizeInBytes = numObjects * (3) * sizeof(b3Vector4);
209 glBindBuffer(GL_ARRAY_BUFFER, vbo);
210 // cl_bool blocking= CL_TRUE;
211 positions = (b3Vector4*)glMapBufferRange(GL_ARRAY_BUFFER, m_instancingRenderer->getMaxShapeCapacity(), arraySizeInBytes, GL_MAP_READ_BIT); //GL_READ_WRITE);//GL_WRITE_ONLY
212 GLint err = glGetError();
213 assert(err == GL_NO_ERROR);
214 m_data->m_instancePosOrnColor = new b3OpenCLArray<b3Vector4>(m_clData->m_clContext, m_clData->m_clQueue);
215 m_data->m_instancePosOrnColor->resize(3 * numObjects);
216 m_data->m_instancePosOrnColor->copyFromHostPointer(positions, 3 * numObjects, 0);
217 glUnmapBuffer(GL_ARRAY_BUFFER);
218 err = glGetError();
219 assert(err == GL_NO_ERROR);
220 }
221 }
222
223 {
224 B3_PROFILE("stepSimulation");
225 m_data->m_rigidBodyPipeline->stepSimulation(1. / 60.f);
226 }
227
228 if (numObjects)
229 {
230 if (convertOnCpu)
231 {
232 b3GpuNarrowPhaseInternalData* npData = m_data->m_np->getInternalData();
233 npData->m_bodyBufferGPU->copyToHost(*npData->m_bodyBufferCPU);
234
235 b3AlignedObjectArray<b3Vector4> vboCPU;
236 m_data->m_instancePosOrnColor->copyToHost(vboCPU);
237
238 for (int i = 0; i < numObjects; i++)
239 {
240 b3Vector4 pos = (const b3Vector4&)npData->m_bodyBufferCPU->at(i).m_pos;
241 b3Quat orn = npData->m_bodyBufferCPU->at(i).m_quat;
242 pos.w = 1.f;
243 vboCPU[i] = pos;
244 vboCPU[i + numObjects] = (b3Vector4&)orn;
245 }
246 m_data->m_instancePosOrnColor->copyFromHost(vboCPU);
247 }
248 else
249 {
250 B3_PROFILE("cl2gl_convert");
251 int ciErrNum = 0;
252 cl_mem bodies = m_data->m_rigidBodyPipeline->getBodyBuffer();
253 b3LauncherCL launch(m_clData->m_clQueue, m_data->m_copyTransformsToVBOKernel, "m_copyTransformsToVBOKernel");
254 launch.setBuffer(bodies);
255 launch.setBuffer(m_data->m_instancePosOrnColor->getBufferCL());
256 launch.setConst(numObjects);
257 launch.launch1D(numObjects);
258 oclCHECKERROR(ciErrNum, CL_SUCCESS);
259 }
260 }
261
262 if (animate && numObjects)
263 {
264 B3_PROFILE("cl2gl_upload");
265 GLint err = glGetError();
266 assert(err == GL_NO_ERROR);
267 GLuint vbo = m_instancingRenderer->getInternalData()->m_vbo;
268
269 int arraySizeInBytes = numObjects * (3) * sizeof(b3Vector4);
270
271 glBindBuffer(GL_ARRAY_BUFFER, vbo);
272 // cl_bool blocking= CL_TRUE;
273 positions = (b3Vector4*)glMapBufferRange(GL_ARRAY_BUFFER, m_instancingRenderer->getMaxShapeCapacity(), arraySizeInBytes, GL_MAP_WRITE_BIT); //GL_READ_WRITE);//GL_WRITE_ONLY
274 err = glGetError();
275 assert(err == GL_NO_ERROR);
276 m_data->m_instancePosOrnColor->copyToHostPointer(positions, 3 * numObjects, 0);
277 glUnmapBuffer(GL_ARRAY_BUFFER);
278 err = glGetError();
279 assert(err == GL_NO_ERROR);
280 }
281 }
282
getRayTo(int x,int y)283 b3Vector3 GpuRigidBodyDemo::getRayTo(int x, int y)
284 {
285 if (!m_instancingRenderer)
286 return b3MakeVector3(0, 0, 0);
287
288 float top = 1.f;
289 float bottom = -1.f;
290 float nearPlane = 1.f;
291 float tanFov = (top - bottom) * 0.5f / nearPlane;
292 float fov = b3Scalar(2.0) * b3Atan(tanFov);
293
294 b3Vector3 camPos, camTarget;
295 m_instancingRenderer->getActiveCamera()->getCameraPosition(camPos);
296 m_instancingRenderer->getActiveCamera()->getCameraTargetPosition(camTarget);
297
298 b3Vector3 rayFrom = camPos;
299 b3Vector3 rayForward = (camTarget - camPos);
300 rayForward.normalize();
301 float farPlane = 10000.f;
302 rayForward *= farPlane;
303
304 // b3Vector3 rightOffset;
305 b3Vector3 m_cameraUp = b3MakeVector3(0, 1, 0);
306 b3Vector3 vertical = m_cameraUp;
307
308 b3Vector3 hor;
309 hor = rayForward.cross(vertical);
310 hor.normalize();
311 vertical = hor.cross(rayForward);
312 vertical.normalize();
313
314 float tanfov = tanf(0.5f * fov);
315
316 hor *= 2.f * farPlane * tanfov;
317 vertical *= 2.f * farPlane * tanfov;
318
319 b3Scalar aspect;
320 float width = m_instancingRenderer->getScreenWidth();
321 float height = m_instancingRenderer->getScreenHeight();
322
323 aspect = width / height;
324
325 hor *= aspect;
326
327 b3Vector3 rayToCenter = rayFrom + rayForward;
328 b3Vector3 dHor = hor * 1.f / width;
329 b3Vector3 dVert = vertical * 1.f / height;
330
331 b3Vector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
332 rayTo += b3Scalar(x) * dHor;
333 rayTo -= b3Scalar(y) * dVert;
334 return rayTo;
335 }
336
loadImage(const char * fileName,int & width,int & height,int & n)337 unsigned char* GpuRigidBodyDemo::loadImage(const char* fileName, int& width, int& height, int& n)
338 {
339 unsigned char* data = stbi_load(fileName, &width, &height, &n, 3);
340 return data;
341 }
342
keyboardCallback(int key,int state)343 bool GpuRigidBodyDemo::keyboardCallback(int key, int state)
344 {
345 if (m_data)
346 {
347 if (key == B3G_ALT)
348 {
349 m_data->m_altPressed = state;
350 }
351 if (key == B3G_CONTROL)
352 {
353 m_data->m_controlPressed = state;
354 }
355 }
356 return false;
357 }
358
mouseMoveCallback(float x,float y)359 bool GpuRigidBodyDemo::mouseMoveCallback(float x, float y)
360 {
361 if (!m_instancingRenderer)
362 return false;
363
364 if (m_data->m_altPressed != 0 || m_data->m_controlPressed != 0)
365 return false;
366
367 if (m_data->m_pickBody >= 0 && m_data->m_pickConstraint >= 0)
368 {
369 m_data->m_rigidBodyPipeline->removeConstraintByUid(m_data->m_pickConstraint);
370 b3Vector3 newRayTo = getRayTo(x, y);
371 b3Vector3 rayFrom;
372 // b3Vector3 oldPivotInB = m_data->m_pickPivotInB;
373 b3Vector3 newPivotB;
374 m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPosition(rayFrom);
375 b3Vector3 dir = newRayTo - rayFrom;
376 dir.normalize();
377 dir *= m_data->m_pickDistance;
378 newPivotB = rayFrom + dir;
379 m_data->m_pickPivotInB = newPivotB;
380 m_data->m_rigidBodyPipeline->copyConstraintsToHost();
381 m_data->m_pickConstraint = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(m_data->m_pickBody, m_data->m_pickFixedBody, m_data->m_pickPivotInA, m_data->m_pickPivotInB, 1e30);
382 m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
383 return true;
384 }
385 return false;
386 }
mouseButtonCallback(int button,int state,float x,float y)387 bool GpuRigidBodyDemo::mouseButtonCallback(int button, int state, float x, float y)
388 {
389 if (!m_instancingRenderer)
390 return false;
391
392 if (state == 1)
393 {
394 if (button == 0 && (m_data->m_altPressed == 0 && m_data->m_controlPressed == 0))
395 {
396 b3AlignedObjectArray<b3RayInfo> rays;
397 b3AlignedObjectArray<b3RayHit> hitResults;
398 b3Vector3 camPos;
399 m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPosition(camPos);
400
401 b3RayInfo ray;
402 ray.m_from = camPos;
403 ray.m_to = getRayTo(x, y);
404 rays.push_back(ray);
405 b3RayHit hit;
406 hit.m_hitFraction = 1.f;
407 hitResults.push_back(hit);
408 m_data->m_rigidBodyPipeline->castRays(rays, hitResults);
409 if (hitResults[0].m_hitFraction < 1.f)
410 {
411 int hitBodyA = hitResults[0].m_hitBody;
412 if (m_data->m_np->getBodiesCpu()[hitBodyA].m_invMass)
413 {
414 //printf("hit!\n");
415 m_data->m_np->readbackAllBodiesToCpu();
416 m_data->m_pickBody = hitBodyA;
417
418 //pivotInA
419 b3Vector3 pivotInB;
420 pivotInB.setInterpolate3(ray.m_from, ray.m_to, hitResults[0].m_hitFraction);
421 b3Vector3 posA;
422 b3Quaternion ornA;
423 m_data->m_np->getObjectTransformFromCpu(posA, ornA, hitBodyA);
424 b3Transform tr;
425 tr.setOrigin(posA);
426 tr.setRotation(ornA);
427 b3Vector3 pivotInA = tr.inverse() * pivotInB;
428 if (m_data->m_pickFixedBody < 0)
429 {
430 b3Vector3 pos = b3MakeVector3(0, 0, 0);
431 b3Quaternion orn(0, 0, 0, 1);
432 int fixedSphere = m_data->m_np->registerConvexHullShape(0, 0, 0, 0); //>registerSphereShape(0.1);
433 m_data->m_pickFixedBody = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0, pos, orn, fixedSphere, 0, false);
434 m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
435 m_data->m_bp->writeAabbsToGpu();
436
437 if (m_data->m_pickGraphicsShapeIndex < 0)
438 {
439 int strideInBytes = 9 * sizeof(float);
440 int numVertices = sizeof(point_sphere_vertices) / strideInBytes;
441 int numIndices = sizeof(point_sphere_indices) / sizeof(int);
442 m_data->m_pickGraphicsShapeIndex = m_guiHelper->getRenderInterface()->registerShape(&point_sphere_vertices[0], numVertices, point_sphere_indices, numIndices, B3_GL_POINTS);
443
444 float color[4] = {1, 0, 0, 1};
445 float scaling[4] = {1, 1, 1, 1};
446
447 m_data->m_pickGraphicsShapeInstance = m_guiHelper->getRenderInterface()->registerGraphicsInstance(m_data->m_pickGraphicsShapeIndex, pivotInB, orn, color, scaling);
448 m_guiHelper->getRenderInterface()->writeTransforms();
449 delete m_data->m_instancePosOrnColor;
450 m_data->m_instancePosOrnColor = 0;
451 }
452 else
453 {
454 m_guiHelper->getRenderInterface()->writeSingleInstanceTransformToCPU(pivotInB, orn, m_data->m_pickGraphicsShapeInstance);
455 if (this->m_instancingRenderer)
456 m_instancingRenderer->writeSingleInstanceTransformToGPU(pivotInB, orn, m_data->m_pickGraphicsShapeInstance);
457 m_data->m_np->setObjectTransformCpu(pos, orn, m_data->m_pickFixedBody);
458 }
459 }
460 pivotInB.w = 0.f;
461 m_data->m_pickPivotInA = pivotInA;
462 m_data->m_pickPivotInB = pivotInB;
463 m_data->m_rigidBodyPipeline->copyConstraintsToHost();
464 m_data->m_pickConstraint = m_data->m_rigidBodyPipeline->createPoint2PointConstraint(hitBodyA, m_data->m_pickFixedBody, pivotInA, pivotInB, 1e30); //hitResults[0].m_hitResult0
465 m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
466 m_data->m_np->writeAllBodiesToGpu();
467 m_data->m_pickDistance = (pivotInB - camPos).length();
468
469 return true;
470 }
471 }
472 }
473 }
474 else
475 {
476 if (button == 0)
477 {
478 if (m_data->m_pickConstraint >= 0)
479 {
480 m_data->m_rigidBodyPipeline->removeConstraintByUid(m_data->m_pickConstraint);
481 m_data->m_pickConstraint = -1;
482 }
483 }
484 }
485
486 //printf("button=%d, state=%d\n",button,state);
487 return false;
488 }
489