diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp')
| -rw-r--r-- | thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp | 708 |
1 files changed, 708 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp new file mode 100644 index 0000000000..783e443060 --- /dev/null +++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp @@ -0,0 +1,708 @@ +/* +Copyright (c) 2013 Advanced Micro Devices, Inc. + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +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. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +//Originally written by Erwin Coumans + +#include "b3GpuRigidBodyPipeline.h" +#include "b3GpuRigidBodyPipelineInternalData.h" +#include "kernels/integrateKernel.h" +#include "kernels/updateAabbsKernel.h" + +#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h" +#include "b3GpuNarrowPhase.h" +#include "Bullet3Geometry/b3AabbUtil.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h" +#include "Bullet3OpenCL/BroadphaseCollision/b3GpuBroadphaseInterface.h" +#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h" +#include "Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h" +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" + +//#define TEST_OTHER_GPU_SOLVER + +#define B3_RIGIDBODY_INTEGRATE_PATH "src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl" +#define B3_RIGIDBODY_UPDATEAABB_PATH "src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl" + +bool useBullet2CpuSolver = true; + +//choice of contact solver +bool gUseJacobi = false; +bool gUseDbvt = false; +bool gDumpContactStats = false; +bool gCalcWorldSpaceAabbOnCpu = false; +bool gUseCalculateOverlappingPairsHost = false; +bool gIntegrateOnCpu = false; +bool gClearPairsOnGpu = true; + +#define TEST_OTHER_GPU_SOLVER 1 +#ifdef TEST_OTHER_GPU_SOLVER +#include "b3GpuJacobiContactSolver.h" +#endif //TEST_OTHER_GPU_SOLVER + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h" +#include "Bullet3OpenCL/RigidBody/b3GpuPgsConstraintSolver.h" + +#include "b3GpuPgsContactSolver.h" +#include "b3Solver.h" + +#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h" +#include "Bullet3OpenCL/Raycast/b3GpuRaycast.h" + + +#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h" +#include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h" + +b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q,class b3GpuNarrowPhase* narrowphase, class b3GpuBroadphaseInterface* broadphaseSap , struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config) +{ + m_data = new b3GpuRigidBodyPipelineInternalData; + m_data->m_constraintUid=0; + m_data->m_config = config; + m_data->m_context = ctx; + m_data->m_device = device; + m_data->m_queue = q; + + m_data->m_solver = new b3PgsJacobiSolver(true);//new b3PgsJacobiSolver(true); + m_data->m_gpuSolver = new b3GpuPgsConstraintSolver(ctx,device,q,true);//new b3PgsJacobiSolver(true); + + m_data->m_allAabbsGPU = new b3OpenCLArray<b3SapAabb>(ctx,q,config.m_maxConvexBodies); + m_data->m_overlappingPairsGPU = new b3OpenCLArray<b3BroadphasePair>(ctx,q,config.m_maxBroadphasePairs); + + m_data->m_gpuConstraints = new b3OpenCLArray<b3GpuGenericConstraint>(ctx,q); +#ifdef TEST_OTHER_GPU_SOLVER + m_data->m_solver3 = new b3GpuJacobiContactSolver(ctx,device,q,config.m_maxBroadphasePairs); +#endif // TEST_OTHER_GPU_SOLVER + + m_data->m_solver2 = new b3GpuPgsContactSolver(ctx,device,q,config.m_maxBroadphasePairs); + + m_data->m_raycaster = new b3GpuRaycast(ctx,device,q); + + + m_data->m_broadphaseDbvt = broadphaseDbvt; + m_data->m_broadphaseSap = broadphaseSap; + m_data->m_narrowphase = narrowphase; + m_data->m_gravity.setValue(0.f,-9.8f,0.f); + + cl_int errNum=0; + + { + cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,integrateKernelCL,&errNum,"",B3_RIGIDBODY_INTEGRATE_PATH); + b3Assert(errNum==CL_SUCCESS); + m_data->m_integrateTransformsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,integrateKernelCL, "integrateTransformsKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + clReleaseProgram(prog); + } + { + cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,updateAabbsKernelCL,&errNum,"",B3_RIGIDBODY_UPDATEAABB_PATH); + b3Assert(errNum==CL_SUCCESS); + m_data->m_updateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,updateAabbsKernelCL, "initializeGpuAabbsFull",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + + + m_data->m_clearOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,updateAabbsKernelCL, "clearOverlappingPairsKernel",&errNum,prog); + b3Assert(errNum==CL_SUCCESS); + + clReleaseProgram(prog); + } + + +} + +b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline() +{ + if (m_data->m_integrateTransformsKernel) + clReleaseKernel(m_data->m_integrateTransformsKernel); + + if (m_data->m_updateAabbsKernel) + clReleaseKernel(m_data->m_updateAabbsKernel); + + if (m_data->m_clearOverlappingPairsKernel) + clReleaseKernel(m_data->m_clearOverlappingPairsKernel); + delete m_data->m_raycaster; + delete m_data->m_solver; + delete m_data->m_allAabbsGPU; + delete m_data->m_gpuConstraints; + delete m_data->m_overlappingPairsGPU; + +#ifdef TEST_OTHER_GPU_SOLVER + delete m_data->m_solver3; +#endif //TEST_OTHER_GPU_SOLVER + + delete m_data->m_solver2; + + + delete m_data; +} + +void b3GpuRigidBodyPipeline::reset() +{ + m_data->m_gpuConstraints->resize(0); + m_data->m_cpuConstraints.resize(0); + m_data->m_allAabbsGPU->resize(0); + m_data->m_allAabbsCPU.resize(0); +} + +void b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint) +{ + m_data->m_joints.push_back(constraint); +} + +void b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint) +{ + m_data->m_joints.remove(constraint); +} + + + +void b3GpuRigidBodyPipeline::removeConstraintByUid(int uid) +{ + m_data->m_gpuSolver->recomputeBatches(); + //slow linear search + m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints); + //remove + for (int i=0;i<m_data->m_cpuConstraints.size();i++) + { + if (m_data->m_cpuConstraints[i].m_uid == uid) + { + //m_data->m_cpuConstraints.remove(m_data->m_cpuConstraints[i]); + m_data->m_cpuConstraints.swap(i,m_data->m_cpuConstraints.size()-1); + m_data->m_cpuConstraints.pop_back(); + + break; + } + } + + if (m_data->m_cpuConstraints.size()) + { + m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints); + } else + { + m_data->m_gpuConstraints->resize(0); + } + +} +int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold) +{ + m_data->m_gpuSolver->recomputeBatches(); + b3GpuGenericConstraint c; + c.m_uid = m_data->m_constraintUid; + m_data->m_constraintUid++; + c.m_flags = B3_CONSTRAINT_FLAG_ENABLED; + c.m_rbA = bodyA; + c.m_rbB = bodyB; + c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]); + c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]); + c.m_breakingImpulseThreshold = breakingThreshold; + c.m_constraintType = B3_GPU_POINT2POINT_CONSTRAINT_TYPE; + m_data->m_cpuConstraints.push_back(c); + return c.m_uid; +} +int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB,float breakingThreshold) +{ + m_data->m_gpuSolver->recomputeBatches(); + b3GpuGenericConstraint c; + c.m_uid = m_data->m_constraintUid; + m_data->m_constraintUid++; + c.m_flags = B3_CONSTRAINT_FLAG_ENABLED; + c.m_rbA = bodyA; + c.m_rbB = bodyB; + c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]); + c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]); + c.m_relTargetAB.setValue(relTargetAB[0],relTargetAB[1],relTargetAB[2],relTargetAB[3]); + c.m_breakingImpulseThreshold = breakingThreshold; + c.m_constraintType = B3_GPU_FIXED_CONSTRAINT_TYPE; + + m_data->m_cpuConstraints.push_back(c); + return c.m_uid; +} + + +void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) +{ + + //update worldspace AABBs from local AABB/worldtransform + { + B3_PROFILE("setupGpuAabbs"); + setupGpuAabbsFull(); + } + + int numPairs =0; + + //compute overlapping pairs + { + + if (gUseDbvt) + { + { + B3_PROFILE("setAabb"); + m_data->m_allAabbsGPU->copyToHost(m_data->m_allAabbsCPU); + for (int i=0;i<m_data->m_allAabbsCPU.size();i++) + { + b3Vector3 aabbMin=b3MakeVector3(m_data->m_allAabbsCPU[i].m_min[0],m_data->m_allAabbsCPU[i].m_min[1],m_data->m_allAabbsCPU[i].m_min[2]); + b3Vector3 aabbMax=b3MakeVector3(m_data->m_allAabbsCPU[i].m_max[0],m_data->m_allAabbsCPU[i].m_max[1],m_data->m_allAabbsCPU[i].m_max[2]); + m_data->m_broadphaseDbvt->setAabb(i,aabbMin,aabbMax,0); + } + } + + { + B3_PROFILE("calculateOverlappingPairs"); + m_data->m_broadphaseDbvt->calculateOverlappingPairs(); + } + numPairs = m_data->m_broadphaseDbvt->getOverlappingPairCache()->getNumOverlappingPairs(); + + } else + { + if (gUseCalculateOverlappingPairsHost) + { + m_data->m_broadphaseSap->calculateOverlappingPairsHost(m_data->m_config.m_maxBroadphasePairs); + } else + { + m_data->m_broadphaseSap->calculateOverlappingPairs(m_data->m_config.m_maxBroadphasePairs); + } + numPairs = m_data->m_broadphaseSap->getNumOverlap(); + } + } + + //compute contact points +// printf("numPairs=%d\n",numPairs); + + int numContacts = 0; + + + int numBodies = m_data->m_narrowphase->getNumRigidBodies(); + + if (numPairs) + { + cl_mem pairs =0; + cl_mem aabbsWS =0; + if (gUseDbvt) + { + B3_PROFILE("m_overlappingPairsGPU->copyFromHost"); + m_data->m_overlappingPairsGPU->copyFromHost(m_data->m_broadphaseDbvt->getOverlappingPairCache()->getOverlappingPairArray()); + pairs = m_data->m_overlappingPairsGPU->getBufferCL(); + aabbsWS = m_data->m_allAabbsGPU->getBufferCL(); + } else + { + pairs = m_data->m_broadphaseSap->getOverlappingPairBuffer(); + aabbsWS = m_data->m_broadphaseSap->getAabbBufferWS(); + } + + m_data->m_overlappingPairsGPU->resize(numPairs); + + //mark the contacts for each pair as 'unused' + if (numPairs) + { + b3OpenCLArray<b3BroadphasePair> gpuPairs(this->m_data->m_context,m_data->m_queue); + gpuPairs.setFromOpenCLBuffer(pairs,numPairs); + + if (gClearPairsOnGpu) + { + + + //b3AlignedObjectArray<b3BroadphasePair> hostPairs;//just for debugging + //gpuPairs.copyToHost(hostPairs); + + b3LauncherCL launcher(m_data->m_queue,m_data->m_clearOverlappingPairsKernel,"clearOverlappingPairsKernel"); + launcher.setBuffer(pairs); + launcher.setConst(numPairs); + launcher.launch1D(numPairs); + + + //gpuPairs.copyToHost(hostPairs); + + + } else + { + b3AlignedObjectArray<b3BroadphasePair> hostPairs; + gpuPairs.copyToHost(hostPairs); + + for (int i=0;i<hostPairs.size();i++) + { + hostPairs[i].z = 0xffffffff; + } + + gpuPairs.copyFromHost(hostPairs); + } + } + + m_data->m_narrowphase->computeContacts(pairs,numPairs,aabbsWS,numBodies); + numContacts = m_data->m_narrowphase->getNumContactsGpu(); + + if (gUseDbvt) + { + ///store the cached information (contact locations in the 'z' component) + B3_PROFILE("m_overlappingPairsGPU->copyToHost"); + m_data->m_overlappingPairsGPU->copyToHost(m_data->m_broadphaseDbvt->getOverlappingPairCache()->getOverlappingPairArray()); + } + if (gDumpContactStats && numContacts) + { + m_data->m_narrowphase->getContactsGpu(); + + printf("numContacts = %d\n", numContacts); + + int totalPoints = 0; + const b3Contact4* contacts = m_data->m_narrowphase->getContactsCPU(); + + for (int i=0;i<numContacts;i++) + { + totalPoints += contacts->getNPoints(); + } + printf("totalPoints=%d\n",totalPoints); + + } + } + + + //convert contact points to contact constraints + + //solve constraints + + b3OpenCLArray<b3RigidBodyData> gpuBodies(m_data->m_context,m_data->m_queue,0,true); + gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumRigidBodies()); + b3OpenCLArray<b3InertiaData> gpuInertias(m_data->m_context,m_data->m_queue,0,true); + gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumRigidBodies()); + b3OpenCLArray<b3Contact4> gpuContacts(m_data->m_context,m_data->m_queue,0,true); + gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu()); + + int numJoints = m_data->m_joints.size() ? m_data->m_joints.size() : m_data->m_cpuConstraints.size(); + if (useBullet2CpuSolver && numJoints) + { + + // b3AlignedObjectArray<b3Contact4> hostContacts; + //gpuContacts.copyToHost(hostContacts); + { + bool useGpu = m_data->m_joints.size()==0; + +// b3Contact4* contacts = numContacts? &hostContacts[0]: 0; + //m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,contacts,numJoints, joints); + if (useGpu) + { + m_data->m_gpuSolver->solveJoints(m_data->m_narrowphase->getNumRigidBodies(),&gpuBodies,&gpuInertias,numJoints, m_data->m_gpuConstraints); + } else + { + b3AlignedObjectArray<b3RigidBodyData> hostBodies; + gpuBodies.copyToHost(hostBodies); + b3AlignedObjectArray<b3InertiaData> hostInertias; + gpuInertias.copyToHost(hostInertias); + + b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 0; + m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(),&hostBodies[0],&hostInertias[0],0,0,numJoints, joints); + gpuBodies.copyFromHost(hostBodies); + } + } + } + + if (numContacts) + { + +#ifdef TEST_OTHER_GPU_SOLVER + + if (gUseJacobi) + { + bool useGpu = true; + if (useGpu) + { + + bool forceHost = false; + if (forceHost) + { + b3AlignedObjectArray<b3RigidBodyData> hostBodies; + b3AlignedObjectArray<b3InertiaData> hostInertias; + b3AlignedObjectArray<b3Contact4> hostContacts; + + { + B3_PROFILE("copyToHost"); + gpuBodies.copyToHost(hostBodies); + gpuInertias.copyToHost(hostInertias); + gpuContacts.copyToHost(hostContacts); + } + + { + b3JacobiSolverInfo solverInfo; + m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),solverInfo); + + + } + { + B3_PROFILE("copyFromHost"); + gpuBodies.copyFromHost(hostBodies); + } + } else + + + { + int static0Index = m_data->m_narrowphase->getStatic0Index(); + b3JacobiSolverInfo solverInfo; + //m_data->m_solver3->solveContacts( >solveGroup(&gpuBodies, &gpuInertias, &gpuContacts,solverInfo); + //m_data->m_solver3->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]); + m_data->m_solver3->solveContacts(numBodies, gpuBodies.getBufferCL(),gpuInertias.getBufferCL(),numContacts, gpuContacts.getBufferCL(),m_data->m_config, static0Index); + } + } else + { + b3AlignedObjectArray<b3RigidBodyData> hostBodies; + gpuBodies.copyToHost(hostBodies); + b3AlignedObjectArray<b3InertiaData> hostInertias; + gpuInertias.copyToHost(hostInertias); + b3AlignedObjectArray<b3Contact4> hostContacts; + gpuContacts.copyToHost(hostContacts); + { + //m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]); + } + gpuBodies.copyFromHost(hostBodies); + } + + } else +#endif //TEST_OTHER_GPU_SOLVER + { + + int static0Index = m_data->m_narrowphase->getStatic0Index(); + m_data->m_solver2->solveContacts(numBodies, gpuBodies.getBufferCL(),gpuInertias.getBufferCL(),numContacts, gpuContacts.getBufferCL(),m_data->m_config, static0Index); + + //m_data->m_solver4->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(), gpuBodies.getBufferCL(), gpuInertias.getBufferCL(), numContacts, gpuContacts.getBufferCL()); + + + /*m_data->m_solver3->solveContactConstraintHost( + (b3OpenCLArray<RigidBodyBase::Body>*)&gpuBodies, + (b3OpenCLArray<RigidBodyBase::Inertia>*)&gpuInertias, + (b3OpenCLArray<Constraint4>*) &gpuContacts, + 0,numContacts,256); + */ + } + } + + integrate(deltaTime); + +} + + +void b3GpuRigidBodyPipeline::integrate(float timeStep) +{ + //integrate + int numBodies = m_data->m_narrowphase->getNumRigidBodies(); + float angularDamp = 0.99f; + + if (gIntegrateOnCpu) + { + if(numBodies) + { + b3GpuNarrowPhaseInternalData* npData = m_data->m_narrowphase->getInternalData(); + npData->m_bodyBufferGPU->copyToHost(*npData->m_bodyBufferCPU); + + b3RigidBodyData_t* bodies = &npData->m_bodyBufferCPU->at(0); + + for (int nodeID=0;nodeID<numBodies;nodeID++) + { + integrateSingleTransform( bodies,nodeID, timeStep, angularDamp, m_data->m_gravity); + } + npData->m_bodyBufferGPU->copyFromHost(*npData->m_bodyBufferCPU); + } + } else + { + b3LauncherCL launcher(m_data->m_queue,m_data->m_integrateTransformsKernel,"m_integrateTransformsKernel"); + launcher.setBuffer(m_data->m_narrowphase->getBodiesGpu()); + + launcher.setConst(numBodies); + launcher.setConst(timeStep); + launcher.setConst(angularDamp); + launcher.setConst(m_data->m_gravity); + launcher.launch1D(numBodies); + } +} + + + + +void b3GpuRigidBodyPipeline::setupGpuAabbsFull() +{ + cl_int ciErrNum=0; + + int numBodies = m_data->m_narrowphase->getNumRigidBodies(); + if (!numBodies) + return; + + if (gCalcWorldSpaceAabbOnCpu) + { + + if (numBodies) + { + if (gUseDbvt) + { + m_data->m_allAabbsCPU.resize(numBodies); + m_data->m_narrowphase->readbackAllBodiesToCpu(); + for (int i=0;i<numBodies;i++) + { + b3ComputeWorldAabb( i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(),&m_data->m_allAabbsCPU[0]); + } + m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); + } else + { + m_data->m_broadphaseSap->getAllAabbsCPU().resize(numBodies); + m_data->m_narrowphase->readbackAllBodiesToCpu(); + for (int i=0;i<numBodies;i++) + { + b3ComputeWorldAabb( i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(),&m_data->m_broadphaseSap->getAllAabbsCPU()[0]); + } + m_data->m_broadphaseSap->getAllAabbsGPU().copyFromHost(m_data->m_broadphaseSap->getAllAabbsCPU()); + //m_data->m_broadphaseSap->writeAabbsToGpu(); + } + } + } else + { + //__kernel void initializeGpuAabbsFull( const int numNodes, __global Body* gBodies,__global Collidable* collidables, __global b3AABBCL* plocalShapeAABB, __global b3AABBCL* pAABB) + b3LauncherCL launcher(m_data->m_queue,m_data->m_updateAabbsKernel,"m_updateAabbsKernel"); + launcher.setConst(numBodies); + cl_mem bodies = m_data->m_narrowphase->getBodiesGpu(); + launcher.setBuffer(bodies); + cl_mem collidables = m_data->m_narrowphase->getCollidablesGpu(); + launcher.setBuffer(collidables); + cl_mem localAabbs = m_data->m_narrowphase->getAabbLocalSpaceBufferGpu(); + launcher.setBuffer(localAabbs); + + cl_mem worldAabbs =0; + if (gUseDbvt) + { + worldAabbs = m_data->m_allAabbsGPU->getBufferCL(); + } else + { + worldAabbs = m_data->m_broadphaseSap->getAabbBufferWS(); + } + launcher.setBuffer(worldAabbs); + launcher.launch1D(numBodies); + + oclCHECKERROR(ciErrNum, CL_SUCCESS); + } + + /* + b3AlignedObjectArray<b3SapAabb> aabbs; + m_data->m_broadphaseSap->m_allAabbsGPU.copyToHost(aabbs); + + printf("numAabbs = %d\n", aabbs.size()); + + for (int i=0;i<aabbs.size();i++) + { + printf("aabb[%d].m_min=%f,%f,%f,%d\n",i,aabbs[i].m_minVec[0],aabbs[i].m_minVec[1],aabbs[i].m_minVec[2],aabbs[i].m_minIndices[3]); + printf("aabb[%d].m_max=%f,%f,%f,%d\n",i,aabbs[i].m_maxVec[0],aabbs[i].m_maxVec[1],aabbs[i].m_maxVec[2],aabbs[i].m_signedMaxIndices[3]); + + }; + */ + + + + + +} + + + +cl_mem b3GpuRigidBodyPipeline::getBodyBuffer() +{ + return m_data->m_narrowphase->getBodiesGpu(); +} + +int b3GpuRigidBodyPipeline::getNumBodies() const +{ + return m_data->m_narrowphase->getNumRigidBodies(); +} + +void b3GpuRigidBodyPipeline::setGravity(const float* grav) +{ + m_data->m_gravity.setValue(grav[0],grav[1],grav[2]); +} + +void b3GpuRigidBodyPipeline::copyConstraintsToHost() +{ + m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints); +} + +void b3GpuRigidBodyPipeline::writeAllInstancesToGpu() +{ + m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); + m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints); +} + + +int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex, bool writeInstanceToGpu) +{ + + b3Vector3 aabbMin=b3MakeVector3(0,0,0),aabbMax=b3MakeVector3(0,0,0); + + + if (collidableIndex>=0) + { + b3SapAabb localAabb = m_data->m_narrowphase->getLocalSpaceAabb(collidableIndex); + b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]); + b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]); + + b3Scalar margin = 0.01f; + b3Transform t; + t.setIdentity(); + t.setOrigin(b3MakeVector3(position[0],position[1],position[2])); + t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); + b3TransformAabb(localAabbMin,localAabbMax, margin,t,aabbMin,aabbMax); + } else + { + b3Error("registerPhysicsInstance using invalid collidableIndex\n"); + return -1; + } + + + bool writeToGpu = false; + int bodyIndex = m_data->m_narrowphase->getNumRigidBodies(); + bodyIndex = m_data->m_narrowphase->registerRigidBody(collidableIndex,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu); + + if (bodyIndex>=0) + { + if (gUseDbvt) + { + m_data->m_broadphaseDbvt->createProxy(aabbMin,aabbMax,bodyIndex,0,1,1); + b3SapAabb aabb; + for (int i=0;i<3;i++) + { + aabb.m_min[i] = aabbMin[i]; + aabb.m_max[i] = aabbMax[i]; + aabb.m_minIndices[3] = bodyIndex; + } + m_data->m_allAabbsCPU.push_back(aabb); + if (writeInstanceToGpu) + { + m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); + } + } else + { + if (mass) + { + m_data->m_broadphaseSap->createProxy(aabbMin,aabbMax,bodyIndex,1,1);//m_dispatcher); + } else + { + m_data->m_broadphaseSap->createLargeProxy(aabbMin,aabbMax,bodyIndex,1,1);//m_dispatcher); + } + } + } + + /* + if (mass>0.f) + m_numDynamicPhysicsInstances++; + + m_numPhysicsInstances++; + */ + + return bodyIndex; +} + +void b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults) +{ + this->m_data->m_raycaster->castRays(rays,hitResults, + getNumBodies(),this->m_data->m_narrowphase->getBodiesCpu(), + m_data->m_narrowphase->getNumCollidablesGpu(), m_data->m_narrowphase->getCollidablesCpu(), + m_data->m_narrowphase->getInternalData(), m_data->m_broadphaseSap); +} |