diff --git a/build/premake4.lua b/build/premake4.lua index 53f5b9368..9e9ecc988 100644 --- a/build/premake4.lua +++ b/build/premake4.lua @@ -113,6 +113,6 @@ include "../src/Bullet3Common" include "../src/Bullet3Geometry" include "../src/Bullet3Collision" - + include "../test/b3DynamicBvhBroadphase" end diff --git a/demo/gpudemo/GpuDemo.h b/demo/gpudemo/GpuDemo.h index 4b34bf868..d177762ba 100644 --- a/demo/gpudemo/GpuDemo.h +++ b/demo/gpudemo/GpuDemo.h @@ -38,9 +38,9 @@ public: :useOpenCL(true), preferredOpenCLPlatformIndex(-1), preferredOpenCLDeviceIndex(-1), - arraySizeX(10), - arraySizeY(30), - arraySizeZ(10), + arraySizeX(30), + arraySizeY(20), + arraySizeZ(30), m_useConcaveMesh(false), gapX(14.3), gapY(14.0), diff --git a/demo/gpudemo/main_opengl3core.cpp b/demo/gpudemo/main_opengl3core.cpp index edacb04bf..0d89a851a 100644 --- a/demo/gpudemo/main_opengl3core.cpp +++ b/demo/gpudemo/main_opengl3core.cpp @@ -66,12 +66,13 @@ int selectedDemo = 0; GpuDemo::CreateFunc* allDemos[]= { // ConcaveCompound2Scene::MyCreateFunc, - ConcaveSphereScene::MyCreateFunc, + GpuBoxPlaneScene::MyCreateFunc, GpuConvexPlaneScene::MyCreateFunc, + ConcaveSphereScene::MyCreateFunc, GpuCompoundScene::MyCreateFunc, diff --git a/demo/gpudemo/premake4.lua b/demo/gpudemo/premake4.lua index 961a3a291..28ad3c929 100644 --- a/demo/gpudemo/premake4.lua +++ b/demo/gpudemo/premake4.lua @@ -28,6 +28,7 @@ function createProject(vendor) "gwen", "Bullet3Common", "Bullet3Geometry", + "Bullet3Collision", "Bullet3Dynamics" } diff --git a/demo/gpudemo/rigidbody/ConcaveScene.cpp b/demo/gpudemo/rigidbody/ConcaveScene.cpp index 4e49991b6..913ed260b 100644 --- a/demo/gpudemo/rigidbody/ConcaveScene.cpp +++ b/demo/gpudemo/rigidbody/ConcaveScene.cpp @@ -223,7 +223,7 @@ void ConcaveScene::createConcaveMesh(const ConstructionInfo& ci, const char* fil float mass = 0.f; b3Vector3 position(0,0,0); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } @@ -286,13 +286,15 @@ void ConcaveScene::setupScene(const ConstructionInfo& ci) btVector4 color(0,0,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false); } } createDynamicObjects(ci); + m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); + float camPos[4]={0,0,0,0};//65.5,4.5,65.5,0}; //float camPos[4]={1,12.5,1.5,0}; m_instancingRenderer->setCameraPitch(45); @@ -345,7 +347,7 @@ void ConcaveScene::createDynamicObjects(const ConstructionInfo& ci) curColor&=3; int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } @@ -572,7 +574,7 @@ void ConcaveCompound2Scene::createDynamicObjects(const ConstructionInfo& ci) curColor&=3; btVector4 scaling(1,1,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } @@ -687,7 +689,7 @@ b3Vector3 childPositions[3] = { curColor&=3; btVector4 scaling(1,1,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } @@ -746,7 +748,7 @@ void ConcaveSphereScene::createDynamicObjects(const ConstructionInfo& ci) curColor&=3; btVector4 scaling(radius,radius,radius,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } diff --git a/demo/gpudemo/rigidbody/GpuCompoundScene.cpp b/demo/gpudemo/rigidbody/GpuCompoundScene.cpp index 783dd9a74..2d6cf75e0 100644 --- a/demo/gpudemo/rigidbody/GpuCompoundScene.cpp +++ b/demo/gpudemo/rigidbody/GpuCompoundScene.cpp @@ -122,13 +122,16 @@ void GpuCompoundScene::setupScene(const ConstructionInfo& ci) curColor&=3; btVector4 scaling(1,1,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } } } + m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); + + float camPos[4]={0,0,0};//65.5,4.5,65.5,0}; //float camPos[4]={1,12.5,1.5,0}; m_instancingRenderer->setCameraTargetPosition(camPos); @@ -219,7 +222,7 @@ void GpuCompoundScene::createStaticEnvironment(const ConstructionInfo& ci) curColor&=3; btVector4 scaling(radius,radius,radius,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; @@ -247,5 +250,5 @@ void GpuCompoundPlaneScene::createStaticEnvironment(const ConstructionInfo& ci) int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false); } \ No newline at end of file diff --git a/demo/gpudemo/rigidbody/GpuConvexScene.cpp b/demo/gpudemo/rigidbody/GpuConvexScene.cpp index 05728a23c..246de37d2 100644 --- a/demo/gpudemo/rigidbody/GpuConvexScene.cpp +++ b/demo/gpudemo/rigidbody/GpuConvexScene.cpp @@ -27,6 +27,8 @@ void GpuConvexScene::setupScene(const ConstructionInfo& ci) index+=createDynamicsObjects(ci); + m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); + float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0}; //float camPos[4]={1,12.5,1.5,0}; @@ -101,7 +103,7 @@ int GpuConvexScene::createDynamicsObjects2(const ConstructionInfo& ci, const flo curColor&=3; btVector4 scaling(1,1,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } @@ -132,7 +134,7 @@ void GpuConvexScene::createStaticEnvironment(const ConstructionInfo& ci) btVector4 color(0,0,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false); } } @@ -155,6 +157,6 @@ void GpuConvexPlaneScene::createStaticEnvironment(const ConstructionInfo& ci) int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false); } \ No newline at end of file diff --git a/demo/gpudemo/rigidbody/GpuRigidBodyDemo.cpp b/demo/gpudemo/rigidbody/GpuRigidBodyDemo.cpp index e22bfba74..609961034 100644 --- a/demo/gpudemo/rigidbody/GpuRigidBodyDemo.cpp +++ b/demo/gpudemo/rigidbody/GpuRigidBodyDemo.cpp @@ -14,6 +14,7 @@ #include "gpu_rigidbody/host/b3GpuNarrowPhase.h" #include "gpu_rigidbody/host/b3Config.h" #include "GpuRigidBodyDemoInternalData.h" +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" static btKeyboardCallback oldCallback = 0; extern bool gReset; @@ -111,8 +112,9 @@ void GpuRigidBodyDemo::initPhysics(const ConstructionInfo& ci) b3GpuSapBroadphase* bp = new b3GpuSapBroadphase(m_clData->m_clContext,m_clData->m_clDevice,m_clData->m_clQueue); m_data->m_np = np; m_data->m_bp = bp; + b3DynamicBvhBroadphase* broadphaseDbvt = new b3DynamicBvhBroadphase(config.m_maxConvexBodies); - m_data->m_rigidBodyPipeline = new b3GpuRigidBodyPipeline(m_clData->m_clContext,m_clData->m_clDevice,m_clData->m_clQueue, np, bp); + m_data->m_rigidBodyPipeline = new b3GpuRigidBodyPipeline(m_clData->m_clContext,m_clData->m_clDevice,m_clData->m_clQueue, np, bp,broadphaseDbvt); setupScene(ci); diff --git a/demo/gpudemo/rigidbody/GpuSphereScene.cpp b/demo/gpudemo/rigidbody/GpuSphereScene.cpp index cbcebfdc3..decc39308 100644 --- a/demo/gpudemo/rigidbody/GpuSphereScene.cpp +++ b/demo/gpudemo/rigidbody/GpuSphereScene.cpp @@ -31,7 +31,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) int group=1; int mask=1; int index=0; - + bool writeInstanceToGpu = false; if (0) { @@ -95,7 +95,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) curColor&=3; btVector4 scaling(radius,radius,radius,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index, writeInstanceToGpu); index++; @@ -152,7 +152,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) curColor&=3; btVector4 scaling(radius,radius,radius,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index, writeInstanceToGpu); index++; } @@ -182,12 +182,17 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci) btVector4 color(0,0,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); - int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(1.f,position,orn,colIndex,index); + int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(1.f,position,orn,colIndex,index,false); index++; } } + if (!writeInstanceToGpu) + { + m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); + } + float camPos[4]={ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0}; //float camPos[4]={1,12.5,1.5,0}; m_instancingRenderer->setCameraTargetPosition(camPos); diff --git a/opencl/gpu_rigidbody/host/b3GpuNarrowPhase.cpp b/opencl/gpu_rigidbody/host/b3GpuNarrowPhase.cpp index 448d22d47..2c0fcef66 100644 --- a/opencl/gpu_rigidbody/host/b3GpuNarrowPhase.cpp +++ b/opencl/gpu_rigidbody/host/b3GpuNarrowPhase.cpp @@ -882,6 +882,11 @@ int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const f return m_data->m_numAcceleratedRigidBodies++; } +int b3GpuNarrowPhase::getNumRigidBodies() const +{ + return m_data->m_numAcceleratedRigidBodies; +} + void b3GpuNarrowPhase::writeAllBodiesToGpu() { m_data->m_bodyBufferGPU->resize(m_data->m_numAcceleratedRigidBodies); diff --git a/opencl/gpu_rigidbody/host/b3GpuNarrowPhase.h b/opencl/gpu_rigidbody/host/b3GpuNarrowPhase.h index 467de5278..52488b88e 100644 --- a/opencl/gpu_rigidbody/host/b3GpuNarrowPhase.h +++ b/opencl/gpu_rigidbody/host/b3GpuNarrowPhase.h @@ -72,6 +72,7 @@ public: cl_mem getAabbBufferGpu(); + int getNumRigidBodies() const; int allocateCollidable(); diff --git a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.cpp b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.cpp index 2592756b5..f13610ae7 100644 --- a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.cpp +++ b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.cpp @@ -11,12 +11,16 @@ #include "parallel_primitives/host/btLauncherCL.h" #include "Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h" +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" //#define TEST_OTHER_GPU_SOLVER +bool useDbvt = true; +bool useBullet2CpuSolver = false; +bool dumpContactStats = false; + #ifdef TEST_OTHER_GPU_SOLVER #include "btGpuJacobiSolver.h" -#include "b3PgsJacobiSolver.h" #endif //TEST_OTHER_GPU_SOLVER #include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h" @@ -27,9 +31,9 @@ #include "Bullet3Common/b3Quickprof.h" #include "b3Config.h" -bool dumpContactStats = false; -b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q,class b3GpuNarrowPhase* narrowphase, class b3GpuSapBroadphase* broadphaseSap ) + +b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q,class b3GpuNarrowPhase* narrowphase, class b3GpuSapBroadphase* broadphaseSap , class b3DynamicBvhBroadphase* broadphaseDbvt) { m_data = new b3GpuRigidBodyPipelineInternalData; m_data->m_context = ctx; @@ -37,16 +41,18 @@ b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id devic m_data->m_queue = q; m_data->m_solver = new b3PgsJacobiSolver(); + b3Config config; + m_data->m_allAabbsGPU = new btOpenCLArray(ctx,q,config.m_maxConvexBodies); + m_data->m_overlappingPairsGPU = new btOpenCLArray(ctx,q,config.m_maxBroadphasePairs); - #ifdef TEST_OTHER_GPU_SOLVER m_data->m_solver3 = new btGpuJacobiSolver(ctx,device,q,config.m_maxBroadphasePairs); #endif // TEST_OTHER_GPU_SOLVER - b3Config config; + m_data->m_solver2 = new b3GpuBatchingPgsSolver(ctx,device,q,config.m_maxBroadphasePairs); - + m_data->m_broadphaseDbvt = broadphaseDbvt; m_data->m_broadphaseSap = broadphaseSap; m_data->m_narrowphase = narrowphase; @@ -75,6 +81,8 @@ b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline() clReleaseKernel(m_data->m_integrateTransformsKernel); delete m_data->m_solver; + delete m_data->m_allAabbsGPU; + delete m_data->m_overlappingPairsGPU; #ifdef TEST_OTHER_GPU_SOLVER delete m_data->m_solver3; @@ -94,15 +102,40 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) setupGpuAabbsFull(); } + int numPairs =0; + //compute overlapping pairs { - //m_data->m_broadphaseSap->calculateOverlappingPairsHost(); - m_data->m_broadphaseSap->calculateOverlappingPairs(); + + if (useDbvt) + { + { + BT_PROFILE("setAabb"); + m_data->m_allAabbsGPU->copyToHost(m_data->m_allAabbsCPU); + for (int i=0;im_allAabbsCPU.size();i++) + { + btBroadphaseProxy* proxy = &m_data->m_broadphaseDbvt->m_proxies[i]; + b3Vector3 aabbMin(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(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(proxy,aabbMin,aabbMax,0); + } + } + + { + BT_PROFILE("calculateOverlappingPairs"); + m_data->m_broadphaseDbvt->calculateOverlappingPairs(); + } + numPairs = m_data->m_broadphaseDbvt->getOverlappingPairCache()->getNumOverlappingPairs(); + } else + { + m_data->m_broadphaseSap->calculateOverlappingPairs(); + numPairs = m_data->m_broadphaseSap->getNumOverlap(); + } } //compute contact points - int numPairs = m_data->m_broadphaseSap->getNumOverlap(); + int numContacts = 0; @@ -110,8 +143,19 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) if (numPairs) { - cl_mem pairs = m_data->m_broadphaseSap->getOverlappingPairBuffer(); - cl_mem aabbsWS = m_data->m_broadphaseSap->getAabbBufferWS(); + cl_mem pairs =0; + cl_mem aabbsWS =0; + if (useDbvt) + { + BT_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_narrowphase->computeContacts(pairs,numPairs,aabbsWS,numBodies); @@ -149,7 +193,7 @@ void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime) btOpenCLArray gpuContacts(m_data->m_context,m_data->m_queue,0,true); gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu()); - bool useBullet2CpuSolver = false; + if (useBullet2CpuSolver) { b3AlignedObjectArray hostBodies; @@ -271,7 +315,15 @@ void b3GpuRigidBodyPipeline::setupGpuAabbsFull() launcher.setBuffer(collidables); cl_mem localAabbs = m_data->m_narrowphase->getAabbBufferGpu(); launcher.setBuffer(localAabbs); - cl_mem worldAabbs = m_data->m_broadphaseSap->getAabbBufferWS(); + + cl_mem worldAabbs =0; + if (useDbvt) + { + worldAabbs = m_data->m_allAabbsGPU->getBufferCL(); + } else + { + worldAabbs = m_data->m_broadphaseSap->getAabbBufferWS(); + } launcher.setBuffer(worldAabbs); launcher.launch1D(numBodies); oclCHECKERROR(ciErrNum, CL_SUCCESS); @@ -290,12 +342,19 @@ int b3GpuRigidBodyPipeline::getNumBodies() const } +void b3GpuRigidBodyPipeline::writeAllInstancesToGpu() +{ + m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU); +} - -int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex) +int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex, bool writeInstanceToGpu) { b3Vector3 aabbMin(0,0,0),aabbMax(0,0,0); + + int bodyIndex = m_data->m_narrowphase->getNumRigidBodies(); + + if (collidableIndex>=0) { b3SapAabb localAabb = m_data->m_narrowphase->getLocalSpaceAabb(collidableIndex); @@ -308,21 +367,39 @@ int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po t.setOrigin(b3Vector3(position[0],position[1],position[2])); t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); btTransformAabb(localAabbMin,localAabbMax, margin,t,aabbMin,aabbMax); - if (mass) + if (useDbvt) { - m_data->m_broadphaseSap->createProxy(aabbMin,aabbMax,userIndex,1,1);//m_dispatcher); + 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 { - m_data->m_broadphaseSap->createLargeProxy(aabbMin,aabbMax,userIndex,1,1);//m_dispatcher); + if (mass) + { + m_data->m_broadphaseSap->createProxy(aabbMin,aabbMax,userIndex,1,1);//m_dispatcher); + } else + { + m_data->m_broadphaseSap->createLargeProxy(aabbMin,aabbMax,userIndex,1,1);//m_dispatcher); + } } } + bool writeToGpu = false; - int bodyIndex = -1; - - + bodyIndex = m_data->m_narrowphase->registerRigidBody(collidableIndex,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu); + /* if (mass>0.f) m_numDynamicPhysicsInstances++; diff --git a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.h b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.h index 0225bc2ae..7541619b8 100644 --- a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.h +++ b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.h @@ -12,7 +12,8 @@ protected: public: - b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q , class b3GpuNarrowPhase* narrowphase, class b3GpuSapBroadphase* broadphaseSap); + + b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue q , class b3GpuNarrowPhase* narrowphase, class b3GpuSapBroadphase* broadphaseSap, class b3DynamicBvhBroadphase* broadphaseDbvt); virtual ~b3GpuRigidBodyPipeline(); void stepSimulation(float deltaTime); @@ -29,7 +30,9 @@ public: //int registerCompoundShape(b3AlignedObjectArray* childShapes); - int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData); + int registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData, bool writeInstanceToGpu); + //if you passed "writeInstanceToGpu" false in the registerPhysicsInstance method (for performance) you need to call writeAllInstancesToGpu after all instances are registered + void writeAllInstancesToGpu(); cl_mem getBodyBuffer(); diff --git a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipelineInternalData.h b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipelineInternalData.h index 95988c4d9..17d377003 100644 --- a/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipelineInternalData.h +++ b/opencl/gpu_rigidbody/host/b3GpuRigidBodyPipelineInternalData.h @@ -7,6 +7,12 @@ #include "../../parallel_primitives/host/btOpenCLArray.h" #include "../../gpu_narrowphase/host/b3Collidable.h" +#include "gpu_broadphase/host/b3SapAabb.h" + + + + +#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h" struct b3GpuRigidBodyPipelineInternalData { @@ -23,6 +29,12 @@ struct b3GpuRigidBodyPipelineInternalData class btGpuJacobiSolver* m_solver3; class b3GpuSapBroadphase* m_broadphaseSap; + + class b3DynamicBvhBroadphase* m_broadphaseDbvt; + btOpenCLArray* m_allAabbsGPU; + b3AlignedObjectArray m_allAabbsCPU; + btOpenCLArray* m_overlappingPairsGPU; + class b3GpuNarrowPhase* m_narrowphase; diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h b/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h new file mode 100644 index 000000000..1a288a2ce --- /dev/null +++ b/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h @@ -0,0 +1,26 @@ + +#ifndef B3_BROADPHASE_CALLBACK_H +#define B3_BROADPHASE_CALLBACK_H + +#include "Bullet3Common/b3Vector3.h" +struct btBroadphaseProxy; + + +struct btBroadphaseAabbCallback +{ + virtual ~btBroadphaseAabbCallback() {} + virtual bool process(const btBroadphaseProxy* proxy) = 0; +}; + + +struct btBroadphaseRayCallback : public btBroadphaseAabbCallback +{ + ///added some cached data to accelerate ray-AABB tests + b3Vector3 m_rayDirectionInverse; + unsigned int m_signs[3]; + b3Scalar m_lambda_max; + + virtual ~btBroadphaseRayCallback() {} +}; + +#endif //B3_BROADPHASE_CALLBACK_H diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp new file mode 100644 index 000000000..cb066eb46 --- /dev/null +++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp @@ -0,0 +1,1295 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ +///b3DynamicBvh implementation by Nathanael Presson + +#include "b3DynamicBvh.h" + +// +typedef b3AlignedObjectArray tNodeArray; +typedef b3AlignedObjectArray tConstNodeArray; + +// +struct btDbvtNodeEnumerator : b3DynamicBvh::ICollide +{ + tConstNodeArray nodes; + void Process(const btDbvtNode* n) { nodes.push_back(n); } +}; + +// +static DBVT_INLINE int indexof(const btDbvtNode* node) +{ + return(node->parent->childs[1]==node); +} + +// +static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, + const btDbvtVolume& b) +{ +#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE) + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]); + btDbvtVolume& res=*(btDbvtVolume*)locals; +#else + btDbvtVolume res; +#endif + Merge(a,b,res); + return(res); +} + +// volume+edge lengths +static DBVT_INLINE b3Scalar size(const btDbvtVolume& a) +{ + const b3Vector3 edges=a.Lengths(); + return( edges.x*edges.y*edges.z+ + edges.x+edges.y+edges.z); +} + +// +static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) +{ + if(node->isinternal()) + { + getmaxdepth(node->childs[0],depth+1,maxdepth); + getmaxdepth(node->childs[1],depth+1,maxdepth); + } else maxdepth=btMax(maxdepth,depth); +} + +// +static DBVT_INLINE void deletenode( b3DynamicBvh* pdbvt, + btDbvtNode* node) +{ + btAlignedFree(pdbvt->m_free); + pdbvt->m_free=node; +} + +// +static void recursedeletenode( b3DynamicBvh* pdbvt, + btDbvtNode* node) +{ + if(!node->isleaf()) + { + recursedeletenode(pdbvt,node->childs[0]); + recursedeletenode(pdbvt,node->childs[1]); + } + if(node==pdbvt->m_root) pdbvt->m_root=0; + deletenode(pdbvt,node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( b3DynamicBvh* pdbvt, + btDbvtNode* parent, + void* data) +{ + btDbvtNode* node; + if(pdbvt->m_free) + { node=pdbvt->m_free;pdbvt->m_free=0; } + else + { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); } + node->parent = parent; + node->data = data; + node->childs[1] = 0; + return(node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( b3DynamicBvh* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume, + void* data) +{ + btDbvtNode* node=createnode(pdbvt,parent,data); + node->volume=volume; + return(node); +} + +// +static DBVT_INLINE btDbvtNode* createnode( b3DynamicBvh* pdbvt, + btDbvtNode* parent, + const btDbvtVolume& volume0, + const btDbvtVolume& volume1, + void* data) +{ + btDbvtNode* node=createnode(pdbvt,parent,data); + Merge(volume0,volume1,node->volume); + return(node); +} + +// +static void insertleaf( b3DynamicBvh* pdbvt, + btDbvtNode* root, + btDbvtNode* leaf) +{ + if(!pdbvt->m_root) + { + pdbvt->m_root = leaf; + leaf->parent = 0; + } + else + { + if(!root->isleaf()) + { + do { + root=root->childs[Select( leaf->volume, + root->childs[0]->volume, + root->childs[1]->volume)]; + } while(!root->isleaf()); + } + btDbvtNode* prev=root->parent; + btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); + if(prev) + { + prev->childs[indexof(root)] = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + do { + if(!prev->volume.Contain(node->volume)) + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + else + break; + node=prev; + } while(0!=(prev=node->parent)); + } + else + { + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + pdbvt->m_root = node; + } + } +} + +// +static btDbvtNode* removeleaf( b3DynamicBvh* pdbvt, + btDbvtNode* leaf) +{ + if(leaf==pdbvt->m_root) + { + pdbvt->m_root=0; + return(0); + } + else + { + btDbvtNode* parent=leaf->parent; + btDbvtNode* prev=parent->parent; + btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; + if(prev) + { + prev->childs[indexof(parent)]=sibling; + sibling->parent=prev; + deletenode(pdbvt,parent); + while(prev) + { + const btDbvtVolume pb=prev->volume; + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + if(NotEqual(pb,prev->volume)) + { + prev=prev->parent; + } else break; + } + return(prev?prev:pdbvt->m_root); + } + else + { + pdbvt->m_root=sibling; + sibling->parent=0; + deletenode(pdbvt,parent); + return(pdbvt->m_root); + } + } +} + +// +static void fetchleaves(b3DynamicBvh* pdbvt, + btDbvtNode* root, + tNodeArray& leaves, + int depth=-1) +{ + if(root->isinternal()&&depth) + { + fetchleaves(pdbvt,root->childs[0],leaves,depth-1); + fetchleaves(pdbvt,root->childs[1],leaves,depth-1); + deletenode(pdbvt,root); + } + else + { + leaves.push_back(root); + } +} + +// +static void split( const tNodeArray& leaves, + tNodeArray& left, + tNodeArray& right, + const b3Vector3& org, + const b3Vector3& axis) +{ + left.resize(0); + right.resize(0); + for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) + left.push_back(leaves[i]); + else + right.push_back(leaves[i]); + } +} + +// +static btDbvtVolume bounds( const tNodeArray& leaves) +{ +#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE + ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); + btDbvtVolume& volume=*(btDbvtVolume*)locals; + volume=leaves[0]->volume; +#else + btDbvtVolume volume=leaves[0]->volume; +#endif + for(int i=1,ni=leaves.size();ivolume,volume); + } + return(volume); +} + +// +static void bottomup( b3DynamicBvh* pdbvt, + tNodeArray& leaves) +{ + while(leaves.size()>1) + { + b3Scalar minsize=SIMD_INFINITY; + int minidx[2]={-1,-1}; + for(int i=0;ivolume,leaves[j]->volume)); + if(szvolume,n[1]->volume,0); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves.swap(minidx[1],leaves.size()-1); + leaves.pop_back(); + } +} + +// +static btDbvtNode* topdown(b3DynamicBvh* pdbvt, + tNodeArray& leaves, + int bu_treshold) +{ + static const b3Vector3 axis[]={b3Vector3(1,0,0), + b3Vector3(0,1,0), + b3Vector3(0,0,1)}; + if(leaves.size()>1) + { + if(leaves.size()>bu_treshold) + { + const btDbvtVolume vol=bounds(leaves); + const b3Vector3 org=vol.Center(); + tNodeArray sets[2]; + int bestaxis=-1; + int bestmidp=leaves.size(); + int splitcount[3][2]={{0,0},{0,0},{0,0}}; + int i; + for( i=0;ivolume.Center()-org; + for(int j=0;j<3;++j) + { + ++splitcount[j][btDot(x,axis[j])>0?1:0]; + } + } + for( i=0;i<3;++i) + { + if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) + { + const int midp=(int)btFabs(b3Scalar(splitcount[i][0]-splitcount[i][1])); + if(midp=0) + { + sets[0].reserve(splitcount[bestaxis][0]); + sets[1].reserve(splitcount[bestaxis][1]); + split(leaves,sets[0],sets[1],org,axis[bestaxis]); + } + else + { + sets[0].reserve(leaves.size()/2+1); + sets[1].reserve(leaves.size()/2); + for(int i=0,ni=leaves.size();ichilds[0]=topdown(pdbvt,sets[0],bu_treshold); + node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); + node->childs[0]->parent=node; + node->childs[1]->parent=node; + return(node); + } + else + { + bottomup(pdbvt,leaves); + return(leaves[0]); + } + } + return(leaves[0]); +} + +// +static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) +{ + btDbvtNode* p=n->parent; + btAssert(n->isinternal()); + if(p>n) + { + const int i=indexof(n); + const int j=1-i; + btDbvtNode* s=p->childs[j]; + btDbvtNode* q=p->parent; + btAssert(n==p->childs[i]); + if(q) q->childs[indexof(p)]=n; else r=n; + s->parent=n; + p->parent=n; + n->parent=q; + p->childs[0]=n->childs[0]; + p->childs[1]=n->childs[1]; + n->childs[0]->parent=p; + n->childs[1]->parent=p; + n->childs[i]=p; + n->childs[j]=s; + btSwap(p->volume,n->volume); + return(p); + } + return(n); +} + +#if 0 +static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) +{ + while(n&&(count--)) n=n->parent; + return(n); +} +#endif + +// +// Api +// + +// +b3DynamicBvh::b3DynamicBvh() +{ + m_root = 0; + m_free = 0; + m_lkhd = -1; + m_leaves = 0; + m_opath = 0; +} + +// +b3DynamicBvh::~b3DynamicBvh() +{ + clear(); +} + +// +void b3DynamicBvh::clear() +{ + if(m_root) + recursedeletenode(this,m_root); + btAlignedFree(m_free); + m_free=0; + m_lkhd = -1; + m_stkStack.clear(); + m_opath = 0; + +} + +// +void b3DynamicBvh::optimizeBottomUp() +{ + if(m_root) + { + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + bottomup(this,leaves); + m_root=leaves[0]; + } +} + +// +void b3DynamicBvh::optimizeTopDown(int bu_treshold) +{ + if(m_root) + { + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + m_root=topdown(this,leaves,bu_treshold); + } +} + +// +void b3DynamicBvh::optimizeIncremental(int passes) +{ + if(passes<0) passes=m_leaves; + if(m_root&&(passes>0)) + { + do { + btDbvtNode* node=m_root; + unsigned bit=0; + while(node->isinternal()) + { + node=sort(node,m_root)->childs[(m_opath>>bit)&1]; + bit=(bit+1)&(sizeof(unsigned)*8-1); + } + update(node); + ++m_opath; + } while(--passes); + } +} + +// +btDbvtNode* b3DynamicBvh::insert(const btDbvtVolume& volume,void* data) +{ + btDbvtNode* leaf=createnode(this,0,volume,data); + insertleaf(this,m_root,leaf); + ++m_leaves; + return(leaf); +} + +// +void b3DynamicBvh::update(btDbvtNode* leaf,int lookahead) +{ + btDbvtNode* root=removeleaf(this,leaf); + if(root) + { + if(lookahead>=0) + { + for(int i=0;(iparent;++i) + { + root=root->parent; + } + } else root=m_root; + } + insertleaf(this,root,leaf); +} + +// +void b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume) +{ + btDbvtNode* root=removeleaf(this,leaf); + if(root) + { + if(m_lkhd>=0) + { + for(int i=0;(iparent;++i) + { + root=root->parent; + } + } else root=m_root; + } + leaf->volume=volume; + insertleaf(this,root,leaf); +} + +// +bool b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(b3Vector3(margin,margin,margin)); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); +} + +// +bool b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector3& velocity) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); +} + +// +bool b3DynamicBvh::update(btDbvtNode* leaf,btDbvtVolume& volume,b3Scalar margin) +{ + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(b3Vector3(margin,margin,margin)); + update(leaf,volume); + return(true); +} + +// +void b3DynamicBvh::remove(btDbvtNode* leaf) +{ + removeleaf(this,leaf); + deletenode(this,leaf); + --m_leaves; +} + +// +void b3DynamicBvh::write(IWriter* iwriter) const +{ + btDbvtNodeEnumerator nodes; + nodes.nodes.reserve(m_leaves*2); + enumNodes(m_root,nodes); + iwriter->Prepare(m_root,nodes.nodes.size()); + for(int i=0;iparent) p=nodes.nodes.findLinearSearch(n->parent); + if(n->isinternal()) + { + const int c0=nodes.nodes.findLinearSearch(n->childs[0]); + const int c1=nodes.nodes.findLinearSearch(n->childs[1]); + iwriter->WriteNode(n,i,p,c0,c1); + } + else + { + iwriter->WriteLeaf(n,i,p); + } + } +} + +// +void b3DynamicBvh::clone(b3DynamicBvh& dest,IClone* iclone) const +{ + dest.clear(); + if(m_root!=0) + { + b3AlignedObjectArray stack; + stack.reserve(m_leaves); + stack.push_back(sStkCLN(m_root,0)); + do { + const int i=stack.size()-1; + const sStkCLN e=stack[i]; + btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); + stack.pop_back(); + if(e.parent!=0) + e.parent->childs[i&1]=n; + else + dest.m_root=n; + if(e.node->isinternal()) + { + stack.push_back(sStkCLN(e.node->childs[0],n)); + stack.push_back(sStkCLN(e.node->childs[1],n)); + } + else + { + iclone->CloneLeaf(n); + } + } while(stack.size()>0); + } +} + +// +int b3DynamicBvh::maxdepth(const btDbvtNode* node) +{ + int depth=0; + if(node) getmaxdepth(node,1,depth); + return(depth); +} + +// +int b3DynamicBvh::countLeaves(const btDbvtNode* node) +{ + if(node->isinternal()) + return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); + else + return(1); +} + +// +void b3DynamicBvh::extractLeaves(const btDbvtNode* node,b3AlignedObjectArray& leaves) +{ + if(node->isinternal()) + { + extractLeaves(node->childs[0],leaves); + extractLeaves(node->childs[1],leaves); + } + else + { + leaves.push_back(node); + } +} + +// +#if DBVT_ENABLE_BENCHMARK + +#include +#include +#include "LinearMath/btQuickProf.h" + +/* +q6600,2.4ghz + +/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32" +/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch" +/Fo"..\..\out\release8\build\libbulletcollision\\" +/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb" +/W3 /nologo /c /Wp64 /Zi /errorReport:prompt + +Benchmarking dbvt... +World scale: 100.000000 +Extents base: 1.000000 +Extents range: 4.000000 +Leaves: 8192 +sizeof(btDbvtVolume): 32 bytes +sizeof(btDbvtNode): 44 bytes +[1] btDbvtVolume intersections: 3499 ms (-1%) +[2] btDbvtVolume merges: 1934 ms (0%) +[3] b3DynamicBvh::collideTT: 5485 ms (-21%) +[4] b3DynamicBvh::collideTT self: 2814 ms (-20%) +[5] b3DynamicBvh::collideTT xform: 7379 ms (-1%) +[6] b3DynamicBvh::collideTT xform,self: 7270 ms (-2%) +[7] b3DynamicBvh::rayTest: 6314 ms (0%),(332143 r/s) +[8] insert/remove: 2093 ms (0%),(1001983 ir/s) +[9] updates (teleport): 1879 ms (-3%),(1116100 u/s) +[10] updates (jitter): 1244 ms (-4%),(1685813 u/s) +[11] optimize (incremental): 2514 ms (0%),(1668000 o/s) +[12] btDbvtVolume notequal: 3659 ms (0%) +[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s) +[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s) +[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s) +[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s) +[17] btDbvtVolume select: 3419 ms (0%) +*/ + +struct btDbvtBenchmark +{ + struct NilPolicy : b3DynamicBvh::ICollide + { + NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} + void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*,b3Scalar depth) + { + ++m_pcount; + if(m_checksort) + { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } + } + int m_pcount; + b3Scalar m_depth; + bool m_checksort; + }; + struct P14 : b3DynamicBvh::ICollide + { + struct Node + { + const btDbvtNode* leaf; + b3Scalar depth; + }; + void Process(const btDbvtNode* leaf,b3Scalar depth) + { + Node n; + n.leaf = leaf; + n.depth = depth; + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depthb.depth) return(-1); + return(0); + } + b3AlignedObjectArray m_nodes; + }; + struct P15 : b3DynamicBvh::ICollide + { + struct Node + { + const btDbvtNode* leaf; + b3Scalar depth; + }; + void Process(const btDbvtNode* leaf) + { + Node n; + n.leaf = leaf; + n.depth = dot(leaf->volume.Center(),m_axis); + } + static int sortfnc(const Node& a,const Node& b) + { + if(a.depthb.depth) return(-1); + return(0); + } + b3AlignedObjectArray m_nodes; + b3Vector3 m_axis; + }; + static b3Scalar RandUnit() + { + return(rand()/(b3Scalar)RAND_MAX); + } + static b3Vector3 RandVector3() + { + return(b3Vector3(RandUnit(),RandUnit(),RandUnit())); + } + static b3Vector3 RandVector3(b3Scalar cs) + { + return(RandVector3()*cs-b3Vector3(cs,cs,cs)/2); + } + static btDbvtVolume RandVolume(b3Scalar cs,b3Scalar eb,b3Scalar es) + { + return(btDbvtVolume::FromCE(RandVector3(cs),b3Vector3(eb,eb,eb)+RandVector3()*es)); + } + static b3Transform RandTransform(b3Scalar cs) + { + b3Transform t; + t.setOrigin(RandVector3(cs)); + t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); + return(t); + } + static void RandTree(b3Scalar cs,b3Scalar eb,b3Scalar es,int leaves,b3DynamicBvh& dbvt) + { + dbvt.clear(); + for(int i=0;i volumes; + b3AlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i volumes; + b3AlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for(int i=0;i rayorg; + b3AlignedObjectArray raydir; + btDbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for(int i=0;i leaves; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[9] updates (teleport): "); + wallclock.reset(); + for(int i=0;i(leaves[rand()%cfgLeaves]), + btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); + } + if(cfgBenchmark10_Enable) + {// Benchmark 10 + srand(380843); + b3DynamicBvh dbvt; + b3AlignedObjectArray leaves; + b3AlignedObjectArray vectors; + vectors.resize(cfgBenchmark10_Iterations); + for(int i=0;i(leaves[rand()%cfgLeaves]); + btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); + dbvt.update(l,v); + } + } + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); + } + if(cfgBenchmark11_Enable) + {// Benchmark 11 + srand(380843); + b3DynamicBvh dbvt; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + printf("[11] optimize (incremental): "); + wallclock.reset(); + for(int i=0;i volumes; + b3AlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i vectors; + btDbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for(int i=0;i vectors; + btDbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for(int i=0;i vectors; + btDbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for(int i=0;i batch; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + batch.reserve(cfgBenchmark16_BatchCount); + printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); + wallclock.reset(); + for(int i=0;i volumes; + b3AlignedObjectArray results; + b3AlignedObjectArray indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for(int i=0;i= 1400) +#define DBVT_USE_TEMPLATE 1 +#else +#define DBVT_USE_TEMPLATE 0 +#endif +#else +#define DBVT_USE_TEMPLATE 0 +#endif + +// Use only intrinsics instead of inline asm +#define DBVT_USE_INTRINSIC_SSE 1 + +// Using memmov for collideOCL +#define DBVT_USE_MEMMOVE 1 + +// Enable benchmarking code +#define DBVT_ENABLE_BENCHMARK 0 + +// Inlining +#define DBVT_INLINE SIMD_FORCE_INLINE + +// Specific methods implementation + +//SSE gives errors on a MSVC 7.1 +#if defined (BT_USE_SSE) //&& defined (_WIN32) +#define DBVT_SELECT_IMPL DBVT_IMPL_SSE +#define DBVT_MERGE_IMPL DBVT_IMPL_SSE +#define DBVT_INT0_IMPL DBVT_IMPL_SSE +#else +#define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC +#define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC +#define DBVT_INT0_IMPL DBVT_IMPL_GENERIC +#endif + +#if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \ + (DBVT_INT0_IMPL==DBVT_IMPL_SSE) +#include +#endif + +// +// Auto config and checks +// + +#if DBVT_USE_TEMPLATE +#define DBVT_VIRTUAL +#define DBVT_VIRTUAL_DTOR(a) +#define DBVT_PREFIX template +#define DBVT_IPOLICY T& policy +#define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker; +#else +#define DBVT_VIRTUAL_DTOR(a) virtual ~a() {} +#define DBVT_VIRTUAL virtual +#define DBVT_PREFIX +#define DBVT_IPOLICY ICollide& policy +#define DBVT_CHECKTYPE +#endif + +#if DBVT_USE_MEMMOVE +#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__) +#include +#endif +#include +#endif + +#ifndef DBVT_USE_TEMPLATE +#error "DBVT_USE_TEMPLATE undefined" +#endif + +#ifndef DBVT_USE_MEMMOVE +#error "DBVT_USE_MEMMOVE undefined" +#endif + +#ifndef DBVT_ENABLE_BENCHMARK +#error "DBVT_ENABLE_BENCHMARK undefined" +#endif + +#ifndef DBVT_SELECT_IMPL +#error "DBVT_SELECT_IMPL undefined" +#endif + +#ifndef DBVT_MERGE_IMPL +#error "DBVT_MERGE_IMPL undefined" +#endif + +#ifndef DBVT_INT0_IMPL +#error "DBVT_INT0_IMPL undefined" +#endif + +// +// Defaults volumes +// + +/* btDbvtAabbMm */ +struct btDbvtAabbMm +{ + DBVT_INLINE b3Vector3 Center() const { return((mi+mx)/2); } + DBVT_INLINE b3Vector3 Lengths() const { return(mx-mi); } + DBVT_INLINE b3Vector3 Extents() const { return((mx-mi)/2); } + DBVT_INLINE const b3Vector3& Mins() const { return(mi); } + DBVT_INLINE const b3Vector3& Maxs() const { return(mx); } + static inline btDbvtAabbMm FromCE(const b3Vector3& c,const b3Vector3& e); + static inline btDbvtAabbMm FromCR(const b3Vector3& c,b3Scalar r); + static inline btDbvtAabbMm FromMM(const b3Vector3& mi,const b3Vector3& mx); + static inline btDbvtAabbMm FromPoints(const b3Vector3* pts,int n); + static inline btDbvtAabbMm FromPoints(const b3Vector3** ppts,int n); + DBVT_INLINE void Expand(const b3Vector3& e); + DBVT_INLINE void SignedExpand(const b3Vector3& e); + DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; + DBVT_INLINE int Classify(const b3Vector3& n,b3Scalar o,int s) const; + DBVT_INLINE b3Scalar ProjectMinimum(const b3Vector3& v,unsigned signs) const; + DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + + DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const b3Vector3& b); + + DBVT_INLINE friend b3Scalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend int Select( const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r); + DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + + DBVT_INLINE b3Vector3& tMins() { return(mi); } + DBVT_INLINE b3Vector3& tMaxs() { return(mx); } + +private: + DBVT_INLINE void AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const; +private: + b3Vector3 mi,mx; +}; + +// Types +typedef btDbvtAabbMm btDbvtVolume; + +/* btDbvtNode */ +struct btDbvtNode +{ + btDbvtVolume volume; + btDbvtNode* parent; + DBVT_INLINE bool isleaf() const { return(childs[1]==0); } + DBVT_INLINE bool isinternal() const { return(!isleaf()); } + union + { + btDbvtNode* childs[2]; + void* data; + int dataAsInt; + }; +}; + +///The b3DynamicBvh class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). +///This b3DynamicBvh is used for soft body collision detection and for the b3DynamicBvhBroadphase. It has a fast insert, remove and update of nodes. +///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. +struct b3DynamicBvh +{ + /* Stack element */ + struct sStkNN + { + const btDbvtNode* a; + const btDbvtNode* b; + sStkNN() {} + sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} + }; + struct sStkNP + { + const btDbvtNode* node; + int mask; + sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} + }; + struct sStkNPS + { + const btDbvtNode* node; + int mask; + b3Scalar value; + sStkNPS() {} + sStkNPS(const btDbvtNode* n,unsigned m,b3Scalar v) : node(n),mask(m),value(v) {} + }; + struct sStkCLN + { + const btDbvtNode* node; + btDbvtNode* parent; + sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} + }; + // Policies/Interfaces + + /* ICollide */ + struct ICollide + { + DBVT_VIRTUAL_DTOR(ICollide) + DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode* n,b3Scalar) { Process(n); } + DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } + DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } + }; + /* IWriter */ + struct IWriter + { + virtual ~IWriter() {} + virtual void Prepare(const btDbvtNode* root,int numnodes)=0; + virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; + virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; + }; + /* IClone */ + struct IClone + { + virtual ~IClone() {} + virtual void CloneLeaf(btDbvtNode*) {} + }; + + // Constants + enum { + SIMPLE_STACKSIZE = 64, + DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 + }; + + // Fields + btDbvtNode* m_root; + btDbvtNode* m_free; + int m_lkhd; + int m_leaves; + unsigned m_opath; + + + b3AlignedObjectArray m_stkStack; + mutable b3AlignedObjectArray m_rayTestStack; + + + // Methods + b3DynamicBvh(); + ~b3DynamicBvh(); + void clear(); + bool empty() const { return(0==m_root); } + void optimizeBottomUp(); + void optimizeTopDown(int bu_treshold=128); + void optimizeIncremental(int passes); + btDbvtNode* insert(const btDbvtVolume& box,void* data); + void update(btDbvtNode* leaf,int lookahead=-1); + void update(btDbvtNode* leaf,btDbvtVolume& volume); + bool update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector3& velocity,b3Scalar margin); + bool update(btDbvtNode* leaf,btDbvtVolume& volume,const b3Vector3& velocity); + bool update(btDbvtNode* leaf,btDbvtVolume& volume,b3Scalar margin); + void remove(btDbvtNode* leaf); + void write(IWriter* iwriter) const; + void clone(b3DynamicBvh& dest,IClone* iclone=0) const; + static int maxdepth(const btDbvtNode* node); + static int countLeaves(const btDbvtNode* node); + static void extractLeaves(const btDbvtNode* node,b3AlignedObjectArray& leaves); +#if DBVT_ENABLE_BENCHMARK + static void benchmark(); +#else + static void benchmark(){} +#endif + // DBVT_IPOLICY must support ICollide policy/interface + DBVT_PREFIX + static void enumNodes( const btDbvtNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + static void enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY); + DBVT_PREFIX + void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); + + DBVT_PREFIX + void collideTTpersistentStack( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); +#if 0 + DBVT_PREFIX + void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const b3Transform& xform, + DBVT_IPOLICY); + DBVT_PREFIX + void collideTT( const btDbvtNode* root0, + const b3Transform& xform0, + const btDbvtNode* root1, + const b3Transform& xform1, + DBVT_IPOLICY); +#endif + + DBVT_PREFIX + void collideTV( const btDbvtNode* root, + const btDbvtVolume& volume, + DBVT_IPOLICY) const; + ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc) + ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time + DBVT_PREFIX + static void rayTest( const btDbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + DBVT_IPOLICY); + ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections + ///rayTestInternal is used by b3DynamicBvhBroadphase to accelerate world ray casts + DBVT_PREFIX + void rayTestInternal( const btDbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + const b3Vector3& rayDirectionInverse, + unsigned int signs[3], + b3Scalar lambda_max, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + DBVT_IPOLICY) const; + + DBVT_PREFIX + static void collideKDOP(const btDbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + int count, + DBVT_IPOLICY); + DBVT_PREFIX + static void collideOCL( const btDbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + const b3Vector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fullsort=true); + DBVT_PREFIX + static void collideTU( const btDbvtNode* root, + DBVT_IPOLICY); + // Helpers + static DBVT_INLINE int nearest(const int* i,const b3DynamicBvh::sStkNPS* a,b3Scalar v,int l,int h) + { + int m=0; + while(l>1; + if(a[i[m]].value>=v) l=m+1; else h=m; + } + return(h); + } + static DBVT_INLINE int allocate( b3AlignedObjectArray& ifree, + b3AlignedObjectArray& stock, + const sStkNPS& value) + { + int i; + if(ifree.size()>0) + { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } + else + { i=stock.size();stock.push_back(value); } + return(i); + } + // +private: + b3DynamicBvh(const b3DynamicBvh&) {} +}; + +// +// Inline's +// + +// +inline btDbvtAabbMm btDbvtAabbMm::FromCE(const b3Vector3& c,const b3Vector3& e) +{ + btDbvtAabbMm box; + box.mi=c-e;box.mx=c+e; + return(box); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromCR(const b3Vector3& c,b3Scalar r) +{ + return(FromCE(c,b3Vector3(r,r,r))); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromMM(const b3Vector3& mi,const b3Vector3& mx) +{ + btDbvtAabbMm box; + box.mi=mi;box.mx=mx; + return(box); +} + +// +inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const b3Vector3* pts,int n) +{ + btDbvtAabbMm box; + box.mi=box.mx=pts[0]; + for(int i=1;i0) mx.setX(mx.x+e[0]); else mi.setX(mi.x+e[0]); + if(e.y>0) mx.setY(mx.y+e[1]); else mi.setY(mi.y+e[1]); + if(e.z>0) mx.setZ(mx.z+e[2]); else mi.setZ(mi.z+e[2]); +} + +// +DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const +{ + return( (mi.x<=a.mi.x)&& + (mi.y<=a.mi.y)&& + (mi.z<=a.mi.z)&& + (mx.x>=a.mx.x)&& + (mx.y>=a.mx.y)&& + (mx.z>=a.mx.z)); +} + +// +DBVT_INLINE int btDbvtAabbMm::Classify(const b3Vector3& n,b3Scalar o,int s) const +{ + b3Vector3 pi,px; + switch(s) + { + case (0+0+0): px=b3Vector3(mi.x,mi.y,mi.z); + pi=b3Vector3(mx.x,mx.y,mx.z);break; + case (1+0+0): px=b3Vector3(mx.x,mi.y,mi.z); + pi=b3Vector3(mi.x,mx.y,mx.z);break; + case (0+2+0): px=b3Vector3(mi.x,mx.y,mi.z); + pi=b3Vector3(mx.x,mi.y,mx.z);break; + case (1+2+0): px=b3Vector3(mx.x,mx.y,mi.z); + pi=b3Vector3(mi.x,mi.y,mx.z);break; + case (0+0+4): px=b3Vector3(mi.x,mi.y,mx.z); + pi=b3Vector3(mx.x,mx.y,mi.z);break; + case (1+0+4): px=b3Vector3(mx.x,mi.y,mx.z); + pi=b3Vector3(mi.x,mx.y,mi.z);break; + case (0+2+4): px=b3Vector3(mi.x,mx.y,mx.z); + pi=b3Vector3(mx.x,mi.y,mi.z);break; + case (1+2+4): px=b3Vector3(mx.x,mx.y,mx.z); + pi=b3Vector3(mi.x,mi.y,mi.z);break; + } + if((btDot(n,px)+o)<0) return(-1); + if((btDot(n,pi)+o)>=0) return(+1); + return(0); +} + +// +DBVT_INLINE b3Scalar btDbvtAabbMm::ProjectMinimum(const b3Vector3& v,unsigned signs) const +{ + const b3Vector3* b[]={&mx,&mi}; + const b3Vector3 p( b[(signs>>0)&1]->x, + b[(signs>>1)&1]->y, + b[(signs>>2)&1]->z); + return(btDot(p,v)); +} + +// +DBVT_INLINE void btDbvtAabbMm::AddSpan(const b3Vector3& d,b3Scalar& smi,b3Scalar& smx) const +{ + for(int i=0;i<3;++i) + { + if(d[i]<0) + { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } + else + { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } + } +} + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +#if DBVT_INT0_IMPL == DBVT_IMPL_SSE + const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); +#if defined (_WIN32) + const __int32* pu((const __int32*)&rt); +#else + const int* pu((const int*)&rt); +#endif + return((pu[0]|pu[1]|pu[2])==0); +#else + return( (a.mi.x<=b.mx.x)&& + (a.mx.x>=b.mi.x)&& + (a.mi.y<=b.mx.y)&& + (a.mx.y>=b.mi.y)&& + (a.mi.z<=b.mx.z)&& + (a.mx.z>=b.mi.z)); +#endif +} + + + +// +DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, + const b3Vector3& b) +{ + return( (b.x>=a.mi.x)&& + (b.y>=a.mi.y)&& + (b.z>=a.mi.z)&& + (b.x<=a.mx.x)&& + (b.y<=a.mx.y)&& + (b.z<=a.mx.z)); +} + + + + + +////////////////////////////////////// + + +// +DBVT_INLINE b3Scalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ + const b3Vector3 d=(a.mi+a.mx)-(b.mi+b.mx); + return(btFabs(d.x)+btFabs(d.y)+btFabs(d.z)); +} + + + +// +DBVT_INLINE int Select( const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ +#if DBVT_SELECT_IMPL == DBVT_IMPL_SSE + +#if defined (_WIN32) + static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; +#else + static ATTRIBUTE_ALIGNED16(const unsigned int) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/}; +#endif + ///@todo: the intrinsic version is 11% slower +#if DBVT_USE_INTRINSIC_SSE + + union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory + { + __m128 ssereg; + float floats[4]; + int ints[4]; + }; + + __m128 omi(_mm_load_ps(o.mi)); + omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); + __m128 ami(_mm_load_ps(a.mi)); + ami=_mm_add_ps(ami,_mm_load_ps(a.mx)); + ami=_mm_sub_ps(ami,omi); + ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask)); + __m128 bmi(_mm_load_ps(b.mi)); + bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx)); + bmi=_mm_sub_ps(bmi,omi); + bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask)); + __m128 t0(_mm_movehl_ps(ami,ami)); + ami=_mm_add_ps(ami,t0); + ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); + __m128 t1(_mm_movehl_ps(bmi,bmi)); + bmi=_mm_add_ps(bmi,t1); + bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); + + btSSEUnion tmp; + tmp.ssereg = _mm_cmple_ss(bmi,ami); + return tmp.ints[0]&1; + +#else + ATTRIBUTE_ALIGNED16(__int32 r[1]); + __asm + { + mov eax,o + mov ecx,a + mov edx,b + movaps xmm0,[eax] + movaps xmm5,mask + addps xmm0,[eax+16] + movaps xmm1,[ecx] + movaps xmm2,[edx] + addps xmm1,[ecx+16] + addps xmm2,[edx+16] + subps xmm1,xmm0 + subps xmm2,xmm0 + andps xmm1,xmm5 + andps xmm2,xmm5 + movhlps xmm3,xmm1 + movhlps xmm4,xmm2 + addps xmm1,xmm3 + addps xmm2,xmm4 + pshufd xmm3,xmm1,1 + pshufd xmm4,xmm2,1 + addss xmm1,xmm3 + addss xmm2,xmm4 + cmpless xmm2,xmm1 + movss r,xmm2 + } + return(r[0]&1); +#endif +#else + return(Proximity(o,a)b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + } +#endif +} + +// +DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b) +{ + return( (a.mi.x!=b.mi.x)|| + (a.mi.y!=b.mi.y)|| + (a.mi.z!=b.mi.z)|| + (a.mx.x!=b.mx.x)|| + (a.mx.y!=b.mx.y)|| + (a.mx.z!=b.mx.z)); +} + +// +// Inline's +// + +// +DBVT_PREFIX +inline void b3DynamicBvh::enumNodes( const btDbvtNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + policy.Process(root); + if(root->isinternal()) + { + enumNodes(root->childs[0],policy); + enumNodes(root->childs[1],policy); + } +} + +// +DBVT_PREFIX +inline void b3DynamicBvh::enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root->isinternal()) + { + enumLeaves(root->childs[0],policy); + enumLeaves(root->childs[1],policy); + } + else + { + policy.Process(root); + } +} + +// +DBVT_PREFIX +inline void b3DynamicBvh::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + b3AlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + + + +DBVT_PREFIX +inline void b3DynamicBvh::collideTTpersistentStack( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + + m_stkStack.resize(DOUBLE_STACKSIZE); + m_stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=m_stkStack[--depth]; + if(depth>treshold) + { + m_stkStack.resize(m_stkStack.size()*2); + treshold=m_stkStack.size()-4; + } + if(p.a==p.b) + { + if(p.a->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} + +#if 0 +// +DBVT_PREFIX +inline void b3DynamicBvh::collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const b3Transform& xform, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root0&&root1) + { + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + b3AlignedObjectArray stkStack; + stkStack.resize(DOUBLE_STACKSIZE); + stkStack[0]=sStkNN(root0,root1); + do { + sStkNN p=stkStack[--depth]; + if(Intersect(p.a->volume,p.b->volume,xform)) + { + if(depth>treshold) + { + stkStack.resize(stkStack.size()*2); + treshold=stkStack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stkStack[depth++]=sStkNN(p.a->childs[0],p.b); + stkStack[depth++]=sStkNN(p.a->childs[1],p.b); + } + } + else + { + if(p.b->isinternal()) + { + stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); + stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } + } + } + } while(depth); + } +} +// +DBVT_PREFIX +inline void b3DynamicBvh::collideTT( const btDbvtNode* root0, + const b3Transform& xform0, + const btDbvtNode* root1, + const b3Transform& xform1, + DBVT_IPOLICY) +{ + const b3Transform xform=xform0.inverse()*xform1; + collideTT(root0,root1,xform,policy); +} +#endif + +// +DBVT_PREFIX +inline void b3DynamicBvh::collideTV( const btDbvtNode* root, + const btDbvtVolume& vol, + DBVT_IPOLICY) const +{ + DBVT_CHECKTYPE + if(root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + b3AlignedObjectArray stack; + stack.resize(0); + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(n->volume,volume)) + { + if(n->isinternal()) + { + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); + } + else + { + policy.Process(n); + } + } + } while(stack.size()>0); + } +} + +DBVT_PREFIX +inline void b3DynamicBvh::rayTestInternal( const btDbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + const b3Vector3& rayDirectionInverse, + unsigned int signs[3], + b3Scalar lambda_max, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + DBVT_IPOLICY) const +{ + (void) rayTo; + DBVT_CHECKTYPE + if(root) + { + b3Vector3 resultNormal; + + int depth=1; + int treshold=DOUBLE_STACKSIZE-2; + b3AlignedObjectArray& stack = m_rayTestStack; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=root; + b3Vector3 bounds[2]; + do + { + const btDbvtNode* node=stack[--depth]; + bounds[0] = node->volume.Mins()-aabbMax; + bounds[1] = node->volume.Maxs()-aabbMin; + b3Scalar tmin=1.f,lambda_min=0.f; + unsigned int result1=false; + result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + if(result1) + { + if(node->isinternal()) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; + } + else + { + policy.Process(node); + } + } + } while(depth); + } +} + +// +DBVT_PREFIX +inline void b3DynamicBvh::rayTest( const btDbvtNode* root, + const b3Vector3& rayFrom, + const b3Vector3& rayTo, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root) + { + b3Vector3 rayDir = (rayTo-rayFrom); + rayDir.normalize (); + + ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT + b3Vector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == b3Scalar(0.0) ? b3Scalar(BT_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == b3Scalar(0.0) ? b3Scalar(BT_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == b3Scalar(0.0) ? b3Scalar(BT_LARGE_FLOAT) : b3Scalar(1.0) / rayDir[2]; + unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; + + b3Scalar lambda_max = rayDir.dot(rayTo-rayFrom); + + b3Vector3 resultNormal; + + b3AlignedObjectArray stack; + + int depth=1; + int treshold=DOUBLE_STACKSIZE-2; + + stack.resize(DOUBLE_STACKSIZE); + stack[0]=root; + b3Vector3 bounds[2]; + do { + const btDbvtNode* node=stack[--depth]; + + bounds[0] = node->volume.Mins(); + bounds[1] = node->volume.Maxs(); + + b3Scalar tmin=1.f,lambda_min=0.f; + unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + +#ifdef COMPARE_BTRAY_AABB2 + b3Scalar param=1.f; + bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); + btAssert(result1 == result2); +#endif //TEST_BTRAY_AABB2 + + if(result1) + { + if(node->isinternal()) + { + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-2; + } + stack[depth++]=node->childs[0]; + stack[depth++]=node->childs[1]; + } + else + { + policy.Process(node); + } + } + } while(depth); + + } +} + +// +DBVT_PREFIX +inline void b3DynamicBvh::collideKDOP(const btDbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + int count, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root) + { + const int inside=(1< stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=0)?1:0)+ + ((normals[i].y>=0)?2:0)+ + ((normals[i].z>=0)?4:0); + } + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(sStkNP(root,0)); + do { + sStkNP se=stack[stack.size()-1]; + bool out=false; + stack.pop_back(); + for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(!out) + { + if((se.mask!=inside)&&(se.node->isinternal())) + { + stack.push_back(sStkNP(se.node->childs[0],se.mask)); + stack.push_back(sStkNP(se.node->childs[1],se.mask)); + } + else + { + if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); + } + } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void b3DynamicBvh::collideOCL( const btDbvtNode* root, + const b3Vector3* normals, + const b3Scalar* offsets, + const b3Vector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fsort) +{ + DBVT_CHECKTYPE + if(root) + { + const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ + (sortaxis[1]>=0?2:0)+ + (sortaxis[2]>=0?4:0); + const int inside=(1< stock; + b3AlignedObjectArray ifree; + b3AlignedObjectArray stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=0)?1:0)+ + ((normals[i].y>=0)?2:0)+ + ((normals[i].z>=0)?4:0); + } + stock.reserve(SIMPLE_STACKSIZE); + stack.reserve(SIMPLE_STACKSIZE); + ifree.reserve(SIMPLE_STACKSIZE); + stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); + do { + const int id=stack[stack.size()-1]; + sStkNPS se=stock[id]; + stack.pop_back();ifree.push_back(id); + if(se.mask!=inside) + { + bool out=false; + for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(out) continue; + } + if(policy.Descent(se.node)) + { + if(se.node->isinternal()) + { + const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; + sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), + sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; + const int q=nes[0].value0)) + { + /* Insert 0 */ + j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); + stack.push_back(0); +#if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[q]); + /* Insert 1 */ + j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); + stack.push_back(0); +#if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[1-q]); + } + else + { + stack.push_back(allocate(ifree,stock,nes[q])); + stack.push_back(allocate(ifree,stock,nes[1-q])); + } + } + else + { + policy.Process(se.node,se.value); + } + } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void b3DynamicBvh::collideTU( const btDbvtNode* root, + DBVT_IPOLICY) +{ + DBVT_CHECKTYPE + if(root) + { + b3AlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(policy.Descent(n)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { policy.Process(n); } + } + } while(stack.size()>0); + } +} + +// +// PP Cleanup +// + +#undef DBVT_USE_MEMMOVE +#undef DBVT_USE_TEMPLATE +#undef DBVT_VIRTUAL_DTOR +#undef DBVT_VIRTUAL +#undef DBVT_PREFIX +#undef DBVT_IPOLICY +#undef DBVT_CHECKTYPE +#undef DBVT_IMPL_GENERIC +#undef DBVT_IMPL_SSE +#undef DBVT_USE_INTRINSIC_SSE +#undef DBVT_SELECT_IMPL +#undef DBVT_MERGE_IMPL +#undef DBVT_INT0_IMPL + +#endif diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp new file mode 100644 index 000000000..cb65dc191 --- /dev/null +++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp @@ -0,0 +1,794 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +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. +*/ + +///b3DynamicBvhBroadphase implementation by Nathanael Presson + +#include "b3DynamicBvhBroadphase.h" +#include "b3OverlappingPair.h" + +// +// Profiling +// + +#if DBVT_BP_PROFILE||DBVT_BP_ENABLE_BENCHMARK +#include +#endif + +#if DBVT_BP_PROFILE +struct ProfileScope +{ + __forceinline ProfileScope(btClock& clock,unsigned long& value) : + m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + { + } + __forceinline ~ProfileScope() + { + (*m_value)+=m_clock->getTimeMicroseconds()-m_base; + } + btClock* m_clock; + unsigned long* m_value; + unsigned long m_base; +}; +#define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) +#else +#define SPC(_value_) +#endif + +// +// Helpers +// + +// +template +static inline void listappend(T* item,T*& list) +{ + item->links[0]=0; + item->links[1]=list; + if(list) list->links[0]=item; + list=item; +} + +// +template +static inline void listremove(T* item,T*& list) +{ + if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; + if(item->links[1]) item->links[1]->links[0]=item->links[0]; +} + +// +template +static inline int listcount(T* root) +{ + int n=0; + while(root) { ++n;root=root->links[1]; } + return(n); +} + +// +template +static inline void clear(T& value) +{ + static const struct ZeroDummy : T {} zerodummy; + value=zerodummy; +} + +// +// Colliders +// + +/* Tree collider */ +struct btDbvtTreeCollider : b3DynamicBvh::ICollide +{ + b3DynamicBvhBroadphase* pbp; + btDbvtProxy* proxy; + btDbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {} + void Process(const btDbvtNode* na,const btDbvtNode* nb) + { + if(na!=nb) + { + btDbvtProxy* pa=(btDbvtProxy*)na->data; + btDbvtProxy* pb=(btDbvtProxy*)nb->data; +#if DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + btSwap(pa,pb); +#endif + pbp->m_paircache->addOverlappingPair(pa->getUid(),pb->getUid()); + ++pbp->m_newpairs; + } + } + void Process(const btDbvtNode* n) + { + Process(n,proxy->leaf); + } +}; + +// +// b3DynamicBvhBroadphase +// + +// +b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache) +{ + m_deferedcollide = false; + m_needcleanup = true; + m_releasepaircache = (paircache!=0)?false:true; + m_prediction = 0; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + m_paircache = paircache? paircache : new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); + + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } +#if DBVT_BP_PROFILE + clear(m_profiling); +#endif + m_proxies.resize(proxyCapacity); +} + +// +b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase() +{ + if(m_releasepaircache) + { + m_paircache->~b3OverlappingPairCache(); + btAlignedFree(m_paircache); + } +} + +// +btBroadphaseProxy* b3DynamicBvhBroadphase::createProxy( const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + int objectId, + void* userPtr, + short int collisionFilterGroup, + short int collisionFilterMask) +{ + btDbvtProxy* mem = &m_proxies[objectId]; + btDbvtProxy* proxy=new(mem) btDbvtProxy( aabbMin,aabbMax,userPtr, + collisionFilterGroup, + collisionFilterMask); + + btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + + //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + proxy->stage = m_stageCurrent; + proxy->m_uniqueId = objectId; + proxy->leaf = m_sets[0].insert(aabb,proxy); + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + collider.proxy=proxy; + m_sets[0].collideTV(m_sets[0].m_root,aabb,collider); + m_sets[1].collideTV(m_sets[1].m_root,aabb,collider); + } + return(proxy); +} + +// +void b3DynamicBvhBroadphase::destroyProxy( btBroadphaseProxy* absproxy, + btDispatcher* dispatcher) +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + if(proxy->stage==STAGECOUNT) + m_sets[1].remove(proxy->leaf); + else + m_sets[0].remove(proxy->leaf); + listremove(proxy,m_stageRoots[proxy->stage]); + m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(),dispatcher); + + m_needcleanup=true; +} + +void b3DynamicBvhBroadphase::getAabb(btBroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + aabbMin = proxy->m_aabbMin; + aabbMax = proxy->m_aabbMax; +} + +struct BroadphaseRayTester : b3DynamicBvh::ICollide +{ + btBroadphaseRayCallback& m_rayCallback; + BroadphaseRayTester(btBroadphaseRayCallback& orgCallback) + :m_rayCallback(orgCallback) + { + } + void Process(const btDbvtNode* leaf) + { + btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + m_rayCallback.process(proxy); + } +}; + +void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, btBroadphaseRayCallback& rayCallback,const b3Vector3& aabbMin,const b3Vector3& aabbMax) +{ + BroadphaseRayTester callback(rayCallback); + + m_sets[0].rayTestInternal( m_sets[0].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + callback); + + m_sets[1].rayTestInternal( m_sets[1].m_root, + rayFrom, + rayTo, + rayCallback.m_rayDirectionInverse, + rayCallback.m_signs, + rayCallback.m_lambda_max, + aabbMin, + aabbMax, + callback); + +} + + +struct BroadphaseAabbTester : b3DynamicBvh::ICollide +{ + btBroadphaseAabbCallback& m_aabbCallback; + BroadphaseAabbTester(btBroadphaseAabbCallback& orgCallback) + :m_aabbCallback(orgCallback) + { + } + void Process(const btDbvtNode* leaf) + { + btDbvtProxy* proxy=(btDbvtProxy*)leaf->data; + m_aabbCallback.process(proxy); + } +}; + +void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin,const b3Vector3& aabbMax,btBroadphaseAabbCallback& aabbCallback) +{ + BroadphaseAabbTester callback(aabbCallback); + + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(aabbMin,aabbMax); + //process all children, that overlap with the given AABB bounds + m_sets[0].collideTV(m_sets[0].m_root,bounds,callback); + m_sets[1].collideTV(m_sets[1].m_root,bounds,callback); + +} + + + +// +void b3DynamicBvhBroadphase::setAabb( btBroadphaseProxy* absproxy, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + btDispatcher* /*dispatcher*/) +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); +#if DBVT_BP_PREVENTFALSEUPDATE + if(NotEqual(aabb,proxy->leaf->volume)) +#endif + { + bool docollide=false; + if(proxy->stage==STAGECOUNT) + {/* fixed -> dynamic set */ + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; + } + else + {/* dynamic set */ + ++m_updates_call; + if(Intersect(proxy->leaf->volume,aabb)) + {/* Moving */ + + const b3Vector3 delta=aabbMin-proxy->m_aabbMin; + b3Vector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); + if(delta[0]<0) velocity[0]=-velocity[0]; + if(delta[1]<0) velocity[1]=-velocity[1]; + if(delta[2]<0) velocity[2]=-velocity[2]; + if ( +#ifdef DBVT_BP_MARGIN + m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) +#else + m_sets[0].update(proxy->leaf,aabb,velocity) +#endif + ) + { + ++m_updates_done; + docollide=true; + } + } + else + {/* Teleporting */ + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; + } + } + listremove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + } + } + } +} + + +// +void b3DynamicBvhBroadphase::setAabbForceUpdate( btBroadphaseProxy* absproxy, + const b3Vector3& aabbMin, + const b3Vector3& aabbMax, + btDispatcher* /*dispatcher*/) +{ + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); + bool docollide=false; + if(proxy->stage==STAGECOUNT) + {/* fixed -> dynamic set */ + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; + } + else + {/* dynamic set */ + ++m_updates_call; + /* Teleporting */ + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; + } + listremove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) + { + m_needcleanup=true; + if(!m_deferedcollide) + { + btDbvtTreeCollider collider(this); + m_sets[1].collideTTpersistentStack(m_sets[1].m_root,proxy->leaf,collider); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,proxy->leaf,collider); + } + } +} + +// +void b3DynamicBvhBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) +{ + collide(dispatcher); +#if DBVT_BP_PROFILE + if(0==(m_pid%DBVT_BP_PROFILING_RATE)) + { + printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); + unsigned int total=m_profiling.m_total; + if(total<=0) total=1; + printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); + printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); + printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); + printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); + const unsigned long sum=m_profiling.m_ddcollide+ + m_profiling.m_fdcollide+ + m_profiling.m_cleanup; + printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); + printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE)); + clear(m_profiling); + m_clock.reset(); + } +#endif + + performDeferredRemoval(dispatcher); + +} + +void b3DynamicBvhBroadphase::performDeferredRemoval(btDispatcher* dispatcher) +{ + + if (m_paircache->hasDeferredRemoval()) + { + + btBroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray(); + + //perform a sort, to find duplicates and to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + + int invalidPair = 0; + + + int i; + + btBroadphasePair previousPair(-1,-1); + + + + for (i=0;ileaf->volume,pb->leaf->volume); + + if (hasOverlap) + { + needsRemoval = false; + } else + { + needsRemoval = true; + } + } else + { + //remove duplicate + needsRemoval = true; + //should have no algorithm + } + + if (needsRemoval) + { + m_paircache->cleanOverlappingPair(pair,dispatcher); + + pair.x = -1; + pair.y = -1; + invalidPair++; + } + + } + + //perform a sort, to sort 'invalid' pairs to the end + overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); + overlappingPairArray.resize(overlappingPairArray.size() - invalidPair); + } +} + +// +void b3DynamicBvhBroadphase::collide(btDispatcher* dispatcher) +{ + /*printf("---------------------------------------------------------\n"); + printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves); + printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves); + printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs()); + { + int i; + for (i=0;igetNumOverlappingPairs();i++) + { + printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(), + getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid()); + } + printf("\n"); + } +*/ + + + + SPC(m_profiling.m_total); + /* optimize */ + m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); + if(m_fixedleft) + { + const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; + m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); + m_fixedleft=btMax(0,m_fixedleft-count); + } + /* dynamic -> fixed set */ + m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; + btDbvtProxy* current=m_stageRoots[m_stageCurrent]; + if(current) + { + btDbvtTreeCollider collider(this); + do { + btDbvtProxy* next=current->links[1]; + listremove(current,m_stageRoots[current->stage]); + listappend(current,m_stageRoots[STAGECOUNT]); +#if DBVT_BP_ACCURATESLEEPING + m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); + collider.proxy=current; + b3DynamicBvh::collideTV(m_sets[0].m_root,current->aabb,collider); + b3DynamicBvh::collideTV(m_sets[1].m_root,current->aabb,collider); +#endif + m_sets[0].remove(current->leaf); + ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); + current->leaf = m_sets[1].insert(curAabb,current); + current->stage = STAGECOUNT; + current = next; + } while(current); + m_fixedleft=m_sets[1].m_leaves; + m_needcleanup=true; + } + /* collide dynamics */ + { + btDbvtTreeCollider collider(this); + if(m_deferedcollide) + { + SPC(m_profiling.m_fdcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[1].m_root,collider); + } + if(m_deferedcollide) + { + SPC(m_profiling.m_ddcollide); + m_sets[0].collideTTpersistentStack(m_sets[0].m_root,m_sets[0].m_root,collider); + } + } + /* clean up */ + if(m_needcleanup) + { + SPC(m_profiling.m_cleanup); + btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) + { + + int ni=btMin(pairs.size(),btMax(m_newpairs,(pairs.size()*m_cupdates)/100)); + for(int i=0;ileaf->volume,pb->leaf->volume)) + { +#if DBVT_BP_SORTPAIRS + if(pa->m_uniqueId>pb->m_uniqueId) + btSwap(pa,pb); +#endif + m_paircache->removeOverlappingPair(pa->getUid(),pb->getUid(),dispatcher); + --ni;--i; + } + } + if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; + } + } + ++m_pid; + m_newpairs=1; + m_needcleanup=false; + if(m_updates_call>0) + { m_updates_ratio=m_updates_done/(b3Scalar)m_updates_call; } + else + { m_updates_ratio=0; } + m_updates_done/=2; + m_updates_call/=2; +} + +// +void b3DynamicBvhBroadphase::optimize() +{ + m_sets[0].optimizeTopDown(); + m_sets[1].optimizeTopDown(); +} + +// +b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() +{ + return(m_paircache); +} + +// +const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const +{ + return(m_paircache); +} + +// +void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const +{ + + ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; + + if(!m_sets[0].empty()) + if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, + m_sets[1].m_root->volume,bounds); + else + bounds=m_sets[0].m_root->volume; + else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; + else + bounds=btDbvtVolume::FromCR(b3Vector3(0,0,0),0); + aabbMin=bounds.Mins(); + aabbMax=bounds.Maxs(); +} + +void b3DynamicBvhBroadphase::resetPool(btDispatcher* dispatcher) +{ + + int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves; + if (!totalObjects) + { + //reset internal dynamic tree data structures + m_sets[0].clear(); + m_sets[1].clear(); + + m_deferedcollide = false; + m_needcleanup = true; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) + { + m_stageRoots[i]=0; + } + } +} + +// +void b3DynamicBvhBroadphase::printStats() +{} + +// +#if DBVT_BP_ENABLE_BENCHMARK + +struct btBroadphaseBenchmark +{ + struct Experiment + { + const char* name; + int object_count; + int update_count; + int spawn_count; + int iterations; + b3Scalar speed; + b3Scalar amplitude; + }; + struct Object + { + b3Vector3 center; + b3Vector3 extents; + btBroadphaseProxy* proxy; + b3Scalar time; + void update(b3Scalar speed,b3Scalar amplitude,btBroadphaseInterface* pbi) + { + time += speed; + center[0] = btCos(time*(b3Scalar)2.17)*amplitude+ + btSin(time)*amplitude/2; + center[1] = btCos(time*(b3Scalar)1.38)*amplitude+ + btSin(time)*amplitude; + center[2] = btSin(time*(b3Scalar)0.777)*amplitude; + pbi->setAabb(proxy,center-extents,center+extents,0); + } + }; + static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } + static b3Scalar UnitRand() { return(UnsignedRand(16384)/(b3Scalar)16384); } + static void OutputTime(const char* name,btClock& c,unsigned count=0) + { + const unsigned long us=c.getTimeMicroseconds(); + const unsigned long ms=(us+500)/1000; + const b3Scalar sec=us/(b3Scalar)(1000*1000); + if(count>0) + printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); + else + printf("%s : %u us (%u ms)\r\n",name,us,ms); + } +}; + +void b3DynamicBvhBroadphase::benchmark(btBroadphaseInterface* pbi) +{ + static const btBroadphaseBenchmark::Experiment experiments[]= + { + {"1024o.10%",1024,10,0,8192,(b3Scalar)0.005,(b3Scalar)100}, + /*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100}, + {"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/ + }; + static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); + b3AlignedObjectArray objects; + btClock wallclock; + /* Begin */ + for(int iexp=0;iexpcenter[0]=btBroadphaseBenchmark::UnitRand()*50; + po->center[1]=btBroadphaseBenchmark::UnitRand()*50; + po->center[2]=btBroadphaseBenchmark::UnitRand()*50; + po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; + po->time=btBroadphaseBenchmark::UnitRand()*2000; + po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + objects.push_back(po); + } + btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); + /* First update */ + wallclock.reset(); + for(int i=0;iupdate(speed,amplitude,pbi); + } + btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); + /* Updates */ + wallclock.reset(); + for(int i=0;iupdate(speed,amplitude,pbi); + } + pbi->calculateOverlappingPairs(0); + } + btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); + /* Clean up */ + wallclock.reset(); + for(int i=0;idestroyProxy(objects[i]->proxy,0); + delete objects[i]; + } + objects.resize(0); + btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); + } + +} +#else +/*void b3DynamicBvhBroadphase::benchmark(btBroadphaseInterface*) +{} +*/ +#endif + +#if DBVT_BP_PROFILE +#undef SPC +#endif + diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h new file mode 100644 index 000000000..488a9e2b6 --- /dev/null +++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h @@ -0,0 +1,207 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +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. +*/ + +///b3DynamicBvhBroadphase implementation by Nathanael Presson +#ifndef BT_DBVT_BROADPHASE_H +#define BT_DBVT_BROADPHASE_H + +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h" +#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +#include "b3BroadphaseCallback.h" + +// +// Compile time config +// + +#define DBVT_BP_PROFILE 0 +//#define DBVT_BP_SORTPAIRS 1 +#define DBVT_BP_PREVENTFALSEUPDATE 0 +#define DBVT_BP_ACCURATESLEEPING 0 +#define DBVT_BP_ENABLE_BENCHMARK 0 +#define DBVT_BP_MARGIN (b3Scalar)0.05 + +#if DBVT_BP_PROFILE +#define DBVT_BP_PROFILING_RATE 256 +#include "LinearMath/btQuickprof.h" +#endif + + + + +ATTRIBUTE_ALIGNED16(struct) btBroadphaseProxy +{ + +BT_DECLARE_ALIGNED_ALLOCATOR(); + + ///optional filtering to cull potential collisions + enum CollisionFilterGroups + { + DefaultFilter = 1, + StaticFilter = 2, + KinematicFilter = 4, + DebrisFilter = 8, + SensorTrigger = 16, + CharacterFilter = 32, + AllFilter = -1 //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger + }; + + //Usually the client btCollisionObject or Rigidbody class + void* m_clientObject; + short int m_collisionFilterGroup; + short int m_collisionFilterMask; + void* m_multiSapParentProxy; + int m_uniqueId;//m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. + + b3Vector3 m_aabbMin; + b3Vector3 m_aabbMax; + + SIMD_FORCE_INLINE int getUid() const + { + return m_uniqueId; + } + + //used for memory pools + btBroadphaseProxy() :m_clientObject(0),m_multiSapParentProxy(0) + { + } + + btBroadphaseProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask,void* multiSapParentProxy=0) + :m_clientObject(userPtr), + m_collisionFilterGroup(collisionFilterGroup), + m_collisionFilterMask(collisionFilterMask), + m_aabbMin(aabbMin), + m_aabbMax(aabbMax) + { + m_multiSapParentProxy = multiSapParentProxy; + } +}; + + + + + +// +// btDbvtProxy +// +struct btDbvtProxy : btBroadphaseProxy +{ + /* Fields */ + //btDbvtAabbMm aabb; + btDbvtNode* leaf; + btDbvtProxy* links[2]; + int stage; + /* ctor */ + + explicit btDbvtProxy() {} + btDbvtProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : + btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) + { + links[0]=links[1]=0; + } +}; + +typedef b3AlignedObjectArray btDbvtProxyArray; + +///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh). +///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other. +///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3. +struct b3DynamicBvhBroadphase +{ + /* Config */ + enum { + DYNAMIC_SET = 0, /* Dynamic set index */ + FIXED_SET = 1, /* Fixed set index */ + STAGECOUNT = 2 /* Number of stages */ + }; + /* Fields */ + b3DynamicBvh m_sets[2]; // Dbvt sets + btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list + + b3AlignedObjectArray m_proxies; + b3OverlappingPairCache* m_paircache; // Pair cache + b3Scalar m_prediction; // Velocity prediction + int m_stageCurrent; // Current stage + int m_fupdates; // % of fixed updates per frame + int m_dupdates; // % of dynamic updates per frame + int m_cupdates; // % of cleanup updates per frame + int m_newpairs; // Number of pairs created + int m_fixedleft; // Fixed optimization left + unsigned m_updates_call; // Number of updates call + unsigned m_updates_done; // Number of updates done + b3Scalar m_updates_ratio; // m_updates_done/m_updates_call + int m_pid; // Parse id + int m_cid; // Cleanup index + bool m_releasepaircache; // Release pair cache on delete + bool m_deferedcollide; // Defere dynamic/static collision to collide call + bool m_needcleanup; // Need to run cleanup? +#if DBVT_BP_PROFILE + btClock m_clock; + struct { + unsigned long m_total; + unsigned long m_ddcollide; + unsigned long m_fdcollide; + unsigned long m_cleanup; + unsigned long m_jobcount; + } m_profiling; +#endif + /* Methods */ + b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache=0); + ~b3DynamicBvhBroadphase(); + void collide(btDispatcher* dispatcher); + void optimize(); + + /* btBroadphaseInterface Implementation */ + btBroadphaseProxy* createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + virtual void setAabb(btBroadphaseProxy* proxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,btDispatcher* dispatcher); + virtual void rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, btBroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3Vector3(0,0,0), const b3Vector3& aabbMax = b3Vector3(0,0,0)); + virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, btBroadphaseAabbCallback& callback); + + virtual void getAabb(btBroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const; + virtual void calculateOverlappingPairs(btDispatcher* dispatcher=0); + virtual b3OverlappingPairCache* getOverlappingPairCache(); + virtual const b3OverlappingPairCache* getOverlappingPairCache() const; + virtual void getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const; + virtual void printStats(); + + + ///reset broadphase internal structures, to ensure determinism/reproducability + virtual void resetPool(btDispatcher* dispatcher); + + void performDeferredRemoval(btDispatcher* dispatcher); + + void setVelocityPrediction(b3Scalar prediction) + { + m_prediction = prediction; + } + b3Scalar getVelocityPrediction() const + { + return m_prediction; + } + + ///this setAabbForceUpdate is similar to setAabb but always forces the aabb update. + ///it is not part of the btBroadphaseInterface but specific to b3DynamicBvhBroadphase. + ///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see + ///http://code.google.com/p/bullet/issues/detail?id=223 + void setAabbForceUpdate( btBroadphaseProxy* absproxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,btDispatcher* /*dispatcher*/); + + //static void benchmark(btBroadphaseInterface*); + + +}; + +#endif diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h new file mode 100644 index 000000000..fd82761b4 --- /dev/null +++ b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h @@ -0,0 +1,45 @@ +#ifndef B3_OVERLAPPING_PAIR_H +#define B3_OVERLAPPING_PAIR_H + +#include "Bullet3Common/btInt2.h" + +//typedef btInt2 btBroadphasePair; +struct btBroadphasePair : public btInt2 +{ + explicit btBroadphasePair(){} + btBroadphasePair(int xx,int yy) + { + if (xx < yy) + { + x = xx; + y = yy; + } + else + { + x = yy; + y = xx; + } + } +}; + +class btBroadphasePairSortPredicate +{ + public: + + bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) const + { + const int uidA0 = a.x; + const int uidB0 = b.x; + const int uidA1 = a.y; + const int uidB1 = b.y; + return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1); + } +}; + +SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphasePair& b) +{ + return (a.x == b.x ) && (a.y == b.y ); +} + +#endif //B3_OVERLAPPING_PAIR_H + diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp new file mode 100644 index 000000000..5d9ef1c2a --- /dev/null +++ b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp @@ -0,0 +1,639 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + + + +#include "b3OverlappingPairCache.h" + +//#include "btDispatcher.h" +//#include "btCollisionAlgorithm.h" +#include "Bullet3Geometry/b3AabbUtil.h" + +#include + +int gOverlappingPairs = 0; + +int gRemovePairs =0; +int gAddedPairs =0; +int gFindPairs =0; + + + + +btHashedOverlappingPairCache::btHashedOverlappingPairCache(): + m_overlapFilterCallback(0), + m_blockedForChanges(false) +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); + growTables(); +} + + + + +btHashedOverlappingPairCache::~btHashedOverlappingPairCache() +{ +} + + + +void btHashedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) +{ +/* if (pair.m_algorithm) + { + { + pair.m_algorithm->~btCollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + } + } + */ + +} + + + + +void btHashedOverlappingPairCache::cleanProxyFromPairs(int proxy,btDispatcher* dispatcher) +{ + + class CleanPairCallback : public btOverlapCallback + { + int m_cleanProxy; + b3OverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; + + public: + CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,btDispatcher* dispatcher) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + if ((pair.x == m_cleanProxy) || + (pair.y == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this,dispatcher); + + processAllOverlappingPairs(&cleanPairs,dispatcher); + +} + + + + +void btHashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,btDispatcher* dispatcher) +{ + + class RemovePairCallback : public btOverlapCallback + { + int m_obsoleteProxy; + + public: + RemovePairCallback(int obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + return ((pair.x == m_obsoleteProxy) || + (pair.y == m_obsoleteProxy)); + } + + }; + + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback,dispatcher); +} + + + + + +btBroadphasePair* btHashedOverlappingPairCache::findPair(int proxy0, int proxy1) +{ + gFindPairs++; + if(proxy0 >proxy1) + btSwap(proxy0,proxy1); + int proxyId1 = proxy0; + int proxyId2 = proxy1; + + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + + int hash = static_cast(getHash(static_cast(proxyId1), static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + if (hash >= m_hashTable.size()) + { + return NULL; + } + + int index = m_hashTable[hash]; + while (index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if (index == BT_NULL_PAIR) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; +} + +//#include + +void btHashedOverlappingPairCache::growTables() +{ + + int newCapacity = m_overlappingPairArray.capacity(); + + if (m_hashTable.size() < newCapacity) + { + //grow hashtable and next table + int curHashtableSize = m_hashTable.size(); + + m_hashTable.resize(newCapacity); + m_next.resize(newCapacity); + + + int i; + + for (i= 0; i < newCapacity; ++i) + { + m_hashTable[i] = BT_NULL_PAIR; + } + for (i = 0; i < newCapacity; ++i) + { + m_next[i] = BT_NULL_PAIR; + } + + for(i=0;i proxyId2) + btSwap(proxyId1, proxyId2);*/ + int hashValue = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + m_next[i] = m_hashTable[hashValue]; + m_hashTable[hashValue] = i; + } + + + } +} + +btBroadphasePair* btHashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1) +{ + if(proxy0>proxy1) + btSwap(proxy0,proxy1); + int proxyId1 = proxy0; + int proxyId2 = proxy1; + + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + + int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); // New hash value with new mask + + + btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair != NULL) + { + return pair; + } + /*for(int i=0;i%u\r\n",proxyId1,proxyId2); + internalFindPair(proxy0, proxy1, hash); + } + }*/ + int count = m_overlappingPairArray.size(); + int oldCapacity = m_overlappingPairArray.capacity(); + void* mem = &m_overlappingPairArray.expandNonInitializing(); + + //this is where we add an actual pair, so also call the 'ghost' +// if (m_ghostPairCallback) +// m_ghostPairCallback->addOverlappingPair(proxy0,proxy1); + + int newCapacity = m_overlappingPairArray.capacity(); + + if (oldCapacity < newCapacity) + { + growTables(); + //hash with new capacity + hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + } + + pair = new (mem) btBroadphasePair(proxy0,proxy1); + +// pair->m_pProxy0 = proxy0; +// pair->m_pProxy1 = proxy1; + //pair->m_algorithm = 0; + //pair->m_internalTmpValue = 0; + + + m_next[count] = m_hashTable[hash]; + m_hashTable[hash] = count; + + return pair; +} + + + +void* btHashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1,btDispatcher* dispatcher) +{ + gRemovePairs++; + if(proxy0>proxy1) + btSwap(proxy0,proxy1); + int proxyId1 = proxy0; + int proxyId2 = proxy1; + + /*if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2);*/ + + int hash = static_cast(getHash(static_cast(proxyId1),static_cast(proxyId2)) & (m_overlappingPairArray.capacity()-1)); + + btBroadphasePair* pair = internalFindPair(proxy0, proxy1, hash); + if (pair == NULL) + { + return 0; + } + + cleanOverlappingPair(*pair,dispatcher); + + + + int pairIndex = int(pair - &m_overlappingPairArray[0]); + btAssert(pairIndex < m_overlappingPairArray.size()); + + // Remove the pair from the hash table. + int index = m_hashTable[hash]; + btAssert(index != BT_NULL_PAIR); + + int previous = BT_NULL_PAIR; + while (index != pairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_NULL_PAIR) + { + btAssert(m_next[previous] == pairIndex); + m_next[previous] = m_next[pairIndex]; + } + else + { + m_hashTable[hash] = m_next[pairIndex]; + } + + // We now move the last pair into spot of the + // pair being removed. We need to fix the hash + // table indices to support the move. + + int lastPairIndex = m_overlappingPairArray.size() - 1; + + //if (m_ghostPairCallback) + // m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher); + + // If the removed pair is the last pair, we are done. + if (lastPairIndex == pairIndex) + { + m_overlappingPairArray.pop_back(); + return 0; + } + + // Remove the last pair from the hash table. + const btBroadphasePair* last = &m_overlappingPairArray[lastPairIndex]; + /* missing swap here too, Nat. */ + int lastHash = static_cast(getHash(static_cast(last->x), static_cast(last->y)) & (m_overlappingPairArray.capacity()-1)); + + index = m_hashTable[lastHash]; + btAssert(index != BT_NULL_PAIR); + + previous = BT_NULL_PAIR; + while (index != lastPairIndex) + { + previous = index; + index = m_next[index]; + } + + if (previous != BT_NULL_PAIR) + { + btAssert(m_next[previous] == lastPairIndex); + m_next[previous] = m_next[lastPairIndex]; + } + else + { + m_hashTable[lastHash] = m_next[lastPairIndex]; + } + + // Copy the last pair into the remove pair's spot. + m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex]; + + // Insert the last pair into the hash table + m_next[pairIndex] = m_hashTable[lastHash]; + m_hashTable[lastHash] = pairIndex; + + m_overlappingPairArray.pop_back(); + + return 0; +} +//#include + +void btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +{ + + int i; + +// printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size()); + for (i=0;iprocessOverlap(*pair)) + { + removeOverlappingPair(pair->x,pair->y,dispatcher); + + gOverlappingPairs--; + } else + { + i++; + } + } +} + + + + + +void btHashedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +{ + ///need to keep hashmap in sync with pair address, so rebuild all + btBroadphasePairArray tmpPairs; + int i; + for (i=0;iremoveOverlappingPair(proxy0, proxy1,dispatcher); + + m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.capacity()-1); + m_overlappingPairArray.pop_back(); + return 0; + } + } + + return 0; +} + + + + + + + + +btBroadphasePair* btSortedOverlappingPairCache::addOverlappingPair(int proxy0,int proxy1) +{ + //don't add overlap with own + btAssert(proxy0 != proxy1); + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + void* mem = &m_overlappingPairArray.expandNonInitializing(); + btBroadphasePair* pair = new (mem) btBroadphasePair(proxy0,proxy1); + + + gOverlappingPairs++; + gAddedPairs++; + +// if (m_ghostPairCallback) +// m_ghostPairCallback->addOverlappingPair(proxy0, proxy1); + return pair; + +} + +///this findPair becomes really slow. Either sort the list to speedup the query, or +///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed. +///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address) +///Also we can use a 2D bitmap, which can be useful for a future GPU implementation + btBroadphasePair* btSortedOverlappingPairCache::findPair(int proxy0,int proxy1) +{ + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + btBroadphasePair tmpPair(proxy0,proxy1); + int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair); + + if (findIndex < m_overlappingPairArray.size()) + { + //btAssert(it != m_overlappingPairSet.end()); + btBroadphasePair* pair = &m_overlappingPairArray[findIndex]; + return pair; + } + return 0; +} + + + + + + + + + + +//#include + +void btSortedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) +{ + + int i; + + for (i=0;iprocessOverlap(*pair)) + { + cleanOverlappingPair(*pair,dispatcher); + pair->x = -1; + pair->y = -1; + m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); + m_overlappingPairArray.pop_back(); + gOverlappingPairs--; + } else + { + i++; + } + } +} + + + + +btSortedOverlappingPairCache::btSortedOverlappingPairCache(): + m_blockedForChanges(false), + m_hasDeferredRemoval(true), + m_overlapFilterCallback(0) + +{ + int initialAllocatedSize= 2; + m_overlappingPairArray.reserve(initialAllocatedSize); +} + +btSortedOverlappingPairCache::~btSortedOverlappingPairCache() +{ +} + +void btSortedOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) +{ +/* if (pair.m_algorithm) + { + { + pair.m_algorithm->~btCollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); + pair.m_algorithm=0; + gRemovePairs--; + } + } + */ +} + + +void btSortedOverlappingPairCache::cleanProxyFromPairs(int proxy,btDispatcher* dispatcher) +{ + + class CleanPairCallback : public btOverlapCallback + { + int m_cleanProxy; + b3OverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; + + public: + CleanPairCallback(int cleanProxy,b3OverlappingPairCache* pairCache,btDispatcher* dispatcher) + :m_cleanProxy(cleanProxy), + m_pairCache(pairCache), + m_dispatcher(dispatcher) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + if ((pair.x == m_cleanProxy) || + (pair.y == m_cleanProxy)) + { + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); + } + return false; + } + + }; + + CleanPairCallback cleanPairs(proxy,this,dispatcher); + + processAllOverlappingPairs(&cleanPairs,dispatcher); + +} + + +void btSortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy,btDispatcher* dispatcher) +{ + + class RemovePairCallback : public btOverlapCallback + { + int m_obsoleteProxy; + + public: + RemovePairCallback(int obsoleteProxy) + :m_obsoleteProxy(obsoleteProxy) + { + } + virtual bool processOverlap(btBroadphasePair& pair) + { + return ((pair.x == m_obsoleteProxy) || + (pair.y == m_obsoleteProxy)); + } + + }; + + RemovePairCallback removeCallback(proxy); + + processAllOverlappingPairs(&removeCallback,dispatcher); +} + +void btSortedOverlappingPairCache::sortOverlappingPairs(btDispatcher* dispatcher) +{ + //should already be sorted +} + diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h new file mode 100644 index 000000000..888446d9f --- /dev/null +++ b/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h @@ -0,0 +1,474 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +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. +*/ + +#ifndef BT_OVERLAPPING_PAIR_CACHE_H +#define BT_OVERLAPPING_PAIR_CACHE_H + +#include "Bullet3Common/btInt2.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + +class btDispatcher; +#include "b3OverlappingPair.h" + + + +typedef b3AlignedObjectArray btBroadphasePairArray; + +struct btOverlapCallback +{ + virtual ~btOverlapCallback() + {} + //return true for deletion of the pair + virtual bool processOverlap(btBroadphasePair& pair) = 0; + +}; + +struct btOverlapFilterCallback +{ + virtual ~btOverlapFilterCallback() + {} + // return true when pairs need collision + virtual bool needBroadphaseCollision(int proxy0,int proxy1) const = 0; +}; + + + + + + + +extern int gRemovePairs; +extern int gAddedPairs; +extern int gFindPairs; + +const int BT_NULL_PAIR=0xffffffff; + +///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the btBroadphaseInterface broadphases. +///The btHashedOverlappingPairCache and btSortedOverlappingPairCache classes are two implementations. +class b3OverlappingPairCache +{ +public: + virtual ~b3OverlappingPairCache() {} // this is needed so we can get to the derived class destructor + + virtual btBroadphasePair* getOverlappingPairArrayPtr() = 0; + + virtual const btBroadphasePair* getOverlappingPairArrayPtr() const = 0; + + virtual btBroadphasePairArray& getOverlappingPairArray() = 0; + + virtual void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) = 0; + + virtual int getNumOverlappingPairs() const = 0; + + virtual void cleanProxyFromPairs(int proxy,btDispatcher* dispatcher) = 0; + + virtual void setOverlapFilterCallback(btOverlapFilterCallback* callback) = 0; + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher) = 0; + + virtual btBroadphasePair* findPair(int proxy0, int proxy1) = 0; + + virtual bool hasDeferredRemoval() = 0; + + //virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback)=0; + + virtual btBroadphasePair* addOverlappingPair(int proxy0,int proxy1)=0; + virtual void* removeOverlappingPair(int proxy0,int proxy1,btDispatcher* dispatcher)=0; + virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,btDispatcher* /*dispatcher*/)=0; + + virtual void sortOverlappingPairs(btDispatcher* dispatcher) = 0; + + +}; + +/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com +class btHashedOverlappingPairCache : public b3OverlappingPairCache +{ + btBroadphasePairArray m_overlappingPairArray; + btOverlapFilterCallback* m_overlapFilterCallback; + bool m_blockedForChanges; + + +public: + btHashedOverlappingPairCache(); + virtual ~btHashedOverlappingPairCache(); + + + virtual void removeOverlappingPairsContainingProxy(int proxy,btDispatcher* dispatcher); + + virtual void* removeOverlappingPair(int proxy0,int proxy1,btDispatcher* dispatcher); + + SIMD_FORCE_INLINE bool needsBroadphaseCollision(int proxy0,int proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + + bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + //collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + // Add a pair and return the new pair. If the pair already exists, + // no new pair is created and the old one is returned. + virtual btBroadphasePair* addOverlappingPair(int proxy0,int proxy1) + { + gAddedPairs++; + + if (!needsBroadphaseCollision(proxy0,proxy1)) + return 0; + + return internalAddPair(proxy0,proxy1); + } + + + + void cleanProxyFromPairs(int proxy,btDispatcher* dispatcher); + + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + + virtual btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btBroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); + + + + btBroadphasePair* findPair(int proxy0, int proxy1); + + int GetCount() const { return m_overlappingPairArray.size(); } +// btBroadphasePair* GetPairs() { return m_pairs; } + + btOverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(btOverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } +private: + + btBroadphasePair* internalAddPair(int proxy0,int proxy1); + + void growTables(); + + SIMD_FORCE_INLINE bool equalsPair(const btBroadphasePair& pair, int proxyId1, int proxyId2) + { + return pair.x == proxyId1 && pair.y == proxyId2; + } + + /* + // Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm + // This assumes proxyId1 and proxyId2 are 16-bit. + SIMD_FORCE_INLINE int getHash(int proxyId1, int proxyId2) + { + int key = (proxyId2 << 16) | proxyId1; + key = ~key + (key << 15); + key = key ^ (key >> 12); + key = key + (key << 2); + key = key ^ (key >> 4); + key = key * 2057; + key = key ^ (key >> 16); + return key; + } + */ + + + + SIMD_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2) + { + int key = static_cast(((unsigned int)proxyId1) | (((unsigned int)proxyId2) <<16)); + // Thomas Wang's hash + + key += ~(key << 15); + key ^= (key >> 10); + key += (key << 3); + key ^= (key >> 6); + key += ~(key << 11); + key ^= (key >> 16); + return static_cast(key); + } + + + + + + SIMD_FORCE_INLINE btBroadphasePair* internalFindPair(int proxy0, int proxy1, int hash) + { + int proxyId1 = proxy0; + int proxyId2 = proxy1; + #if 0 // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat. + if (proxyId1 > proxyId2) + btSwap(proxyId1, proxyId2); + #endif + + int index = m_hashTable[hash]; + + while( index != BT_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false) + { + index = m_next[index]; + } + + if ( index == BT_NULL_PAIR ) + { + return NULL; + } + + btAssert(index < m_overlappingPairArray.size()); + + return &m_overlappingPairArray[index]; + } + + virtual bool hasDeferredRemoval() + { + return false; + } + +/* virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + */ + + virtual void sortOverlappingPairs(btDispatcher* dispatcher); + + +protected: + + b3AlignedObjectArray m_hashTable; + b3AlignedObjectArray m_next; +// btOverlappingPairCallback* m_ghostPairCallback; + +}; + + + + +///btSortedOverlappingPairCache maintains the objects with overlapping AABB +///Typically managed by the Broadphase, Axis3Sweep or btSimpleBroadphase +class btSortedOverlappingPairCache : public b3OverlappingPairCache +{ + protected: + //avoid brute-force finding all the time + btBroadphasePairArray m_overlappingPairArray; + + //during the dispatch, check that user doesn't destroy/create proxy + bool m_blockedForChanges; + + ///by default, do the removal during the pair traversal + bool m_hasDeferredRemoval; + + //if set, use the callback instead of the built in filter in needBroadphaseCollision + btOverlapFilterCallback* m_overlapFilterCallback; + +// btOverlappingPairCallback* m_ghostPairCallback; + + public: + + btSortedOverlappingPairCache(); + virtual ~btSortedOverlappingPairCache(); + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); + + void* removeOverlappingPair(int proxy0,int proxy1,btDispatcher* dispatcher); + + void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); + + btBroadphasePair* addOverlappingPair(int proxy0,int proxy1); + + btBroadphasePair* findPair(int proxy0,int proxy1); + + + void cleanProxyFromPairs(int proxy,btDispatcher* dispatcher); + + virtual void removeOverlappingPairsContainingProxy(int proxy,btDispatcher* dispatcher); + + + inline bool needsBroadphaseCollision(int proxy0,int proxy1) const + { + if (m_overlapFilterCallback) + return m_overlapFilterCallback->needBroadphaseCollision(proxy0,proxy1); + + bool collides = true;//(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0; + //collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); + + return collides; + } + + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + const btBroadphasePairArray& getOverlappingPairArray() const + { + return m_overlappingPairArray; + } + + + + + btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + + int getNumOverlappingPairs() const + { + return m_overlappingPairArray.size(); + } + + btOverlapFilterCallback* getOverlapFilterCallback() + { + return m_overlapFilterCallback; + } + + void setOverlapFilterCallback(btOverlapFilterCallback* callback) + { + m_overlapFilterCallback = callback; + } + + virtual bool hasDeferredRemoval() + { + return m_hasDeferredRemoval; + } + +/* virtual void setInternalGhostPairCallback(btOverlappingPairCallback* ghostPairCallback) + { + m_ghostPairCallback = ghostPairCallback; + } + */ + virtual void sortOverlappingPairs(btDispatcher* dispatcher); + + +}; + + + +///btNullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing. +class btNullPairCache : public b3OverlappingPairCache +{ + + btBroadphasePairArray m_overlappingPairArray; + +public: + + virtual btBroadphasePair* getOverlappingPairArrayPtr() + { + return &m_overlappingPairArray[0]; + } + const btBroadphasePair* getOverlappingPairArrayPtr() const + { + return &m_overlappingPairArray[0]; + } + btBroadphasePairArray& getOverlappingPairArray() + { + return m_overlappingPairArray; + } + + virtual void cleanOverlappingPair(btBroadphasePair& /*pair*/,btDispatcher* /*dispatcher*/) + { + + } + + virtual int getNumOverlappingPairs() const + { + return 0; + } + + virtual void cleanProxyFromPairs(int /*proxy*/,btDispatcher* /*dispatcher*/) + { + + } + + virtual void setOverlapFilterCallback(btOverlapFilterCallback* /*callback*/) + { + } + + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* /*dispatcher*/) + { + } + + virtual btBroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/) + { + return 0; + } + + virtual bool hasDeferredRemoval() + { + return true; + } + +// virtual void setInternalGhostPairCallback(btOverlappingPairCallback* /* ghostPairCallback */) +// { +// +// } + + virtual btBroadphasePair* addOverlappingPair(int /*proxy0*/,int /*proxy1*/) + { + return 0; + } + + virtual void* removeOverlappingPair(int /*proxy0*/,int /*proxy1*/,btDispatcher* /*dispatcher*/) + { + return 0; + } + + virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/,btDispatcher* /*dispatcher*/) + { + } + + virtual void sortOverlappingPairs(btDispatcher* dispatcher) + { + (void) dispatcher; + } + + +}; + + +#endif //BT_OVERLAPPING_PAIR_CACHE_H + + diff --git a/src/Bullet3Common/btInt2.h b/src/Bullet3Common/btInt2.h new file mode 100644 index 000000000..a0a2977d8 --- /dev/null +++ b/src/Bullet3Common/btInt2.h @@ -0,0 +1,35 @@ +#ifndef BT_INT2_H +#define BT_INT2_H + +struct btUnsignedInt2 +{ + union + { + struct + { + unsigned int x,y; + }; + struct + { + unsigned int s[2]; + }; + }; +}; + +struct btInt2 +{ + union + { + struct + { + int x,y; + }; + struct + { + int s[2]; + }; + }; +}; + + +#endif \ No newline at end of file diff --git a/test/b3DynamicBvhBroadphase/main.cpp b/test/b3DynamicBvhBroadphase/main.cpp new file mode 100644 index 000000000..ac4342304 --- /dev/null +++ b/test/b3DynamicBvhBroadphase/main.cpp @@ -0,0 +1,82 @@ +/* +Copyright (c) 2012 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. +*/ + + +#include + +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h" +#include "Bullet3Common/b3CommandLineArgs.h" +#include "Bullet3Common/b3MinMax.h" +#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h" + +int g_nPassed = 0; +int g_nFailed = 0; +bool g_testFailed = 0; + +#define TEST_INIT g_testFailed = 0; +#define TEST_ASSERT(x) if( !(x) ){g_testFailed = 1;} +#define TEST_REPORT(testName) printf("[%s] %s\n",(g_testFailed)?"X":"O", testName); if(g_testFailed) g_nFailed++; else g_nPassed++; + + + +inline void broadphaseTest() +{ + TEST_INIT; + + b3DynamicBvhBroadphase* bp = new b3DynamicBvhBroadphase(1); + + int group=1; + int mask=1; + b3Vector3 aabbMin(0,0,0); + b3Vector3 aabbMax(1,1,1); + int userId = 0; + bp->createProxy(aabbMin,aabbMax,userId++,0,group,mask); + + aabbMin.setValue(1,1,1); + aabbMax.setValue(2,2,2); + + + bp->createProxy(aabbMin,aabbMax,userId++,0,group,mask); + + + bp->calculateOverlappingPairs(); + + int numOverlap = bp->getOverlappingPairCache()->getNumOverlappingPairs(); + + + TEST_ASSERT(numOverlap==1); + + delete bp; + + TEST_REPORT( "broadphaseTest" ); +} + +int main(int argc, char** argv) +{ + + + broadphaseTest(); + + printf("%d tests passed\n",g_nPassed, g_nFailed); + if (g_nFailed) + { + printf("%d tests failed\n",g_nFailed); + } + printf("End, press \n"); + + getchar(); + +} + diff --git a/test/b3DynamicBvhBroadphase/premake4.lua b/test/b3DynamicBvhBroadphase/premake4.lua new file mode 100644 index 000000000..d0e9df3cc --- /dev/null +++ b/test/b3DynamicBvhBroadphase/premake4.lua @@ -0,0 +1,16 @@ + + +project ("b3DynamicBvhBroadphase_test") + + language "C++" + + kind "ConsoleApp" + targetdir "../../bin" + includedirs {"../../src"} + + links {"Bullet3Common", "Bullet3Collision"} + + files { + "main.cpp", + } +