From 87df3d0f3233c045ad6703c23337d95c2432c7c8 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Sat, 8 Sep 2007 05:40:01 +0000 Subject: [PATCH] Based on feedback from another professional game company, there are several improvements, including some API change... Some dynamic memory allocations have been replace by pool allocation or stack allocations. quantized aabb versus quantized aabb overlap check is made branch-free (helps a lot on consoles PS3/XBox 360) Collision algorithms are now created through a new btDefaultCollisionConfiguration, to decouple dependency (this is the API change): Example: btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); m_dispatcher = new btCollisionDispatcher(collisionConfiguration); --- Demos/BasicDemo/BasicDemo.cpp | 22 +- Demos/BspDemo/BspDemo.cpp | 3 +- Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp | 3 +- Demos/ColladaDemo/ColladaDemo.cpp | 3 +- .../CollisionInterfaceDemo.cpp | 3 +- Demos/ConcaveDemo/ConcavePhysicsDemo.cpp | 5 +- Demos/ConstraintDemo/ConstraintDemo.cpp | 3 +- .../ConvexDecompositionDemo.cpp | 6 +- .../DoublePrecisionDemo.cpp | 3 +- .../MovingConcaveDemo/ConcavePhysicsDemo.cpp | 5 +- Demos/OpenGL/DemoApplication.cpp | 2 +- Demos/RagdollDemo/RagdollDemo.cpp | 774 +++++++++--------- .../UserCollisionAlgorithm.cpp | 4 +- Demos/VehicleDemo/VehicleDemo.cpp | 5 +- Extras/BulletMultiThreaded/Jamfile | 2 +- .../SpuContactManifoldCollisionAlgorithm.h | 5 +- .../SpuGatheringCollisionDispatcher.cpp | 17 +- .../SpuGatheringCollisionDispatcher.h | 4 +- .../BulletMultiThreaded/SpuParallelSolver.cpp | 2 +- .../BulletMultiThreaded/SpuParallelSolver.h | 2 +- .../Bullet/btGImpactCollisionAlgorithm.h | 6 +- .../Bullet/btGImpactCollisionAlgorithm.cpp | 4 +- Extras/quickstep/OdeConstraintSolver.cpp | 6 +- Extras/quickstep/OdeConstraintSolver.h | 5 +- .../BroadphaseCollision/btAxisSweep3.cpp | 14 +- .../BroadphaseCollision/btAxisSweep3.h | 6 +- .../btBroadphaseInterface.h | 4 +- .../btCollisionAlgorithm.cpp | 2 +- .../btCollisionAlgorithm.h | 6 +- .../BroadphaseCollision/btDispatcher.h | 8 +- .../btMultiSapBroadphase.cpp | 8 +- .../btMultiSapBroadphase.h | 4 +- .../btOverlappingPairCache.cpp | 31 +- .../btOverlappingPairCache.h | 12 +- .../btSimpleBroadphase.cpp | 8 +- .../BroadphaseCollision/btSimpleBroadphase.h | 4 +- src/BulletCollision/CMakeLists.txt | 1 + .../btCollisionConfiguration.h | 16 +- .../btCollisionDispatcher.cpp | 125 +-- .../CollisionDispatch/btCollisionDispatcher.h | 32 +- .../CollisionDispatch/btCollisionWorld.cpp | 25 +- .../CollisionDispatch/btCollisionWorld.h | 2 +- .../btCompoundCollisionAlgorithm.cpp | 5 +- .../btCompoundCollisionAlgorithm.h | 7 +- .../btConvexConcaveCollisionAlgorithm.cpp | 9 +- .../btConvexConcaveCollisionAlgorithm.h | 6 +- .../btConvexConvexAlgorithm.h | 4 +- .../btDefaultCollisionConfiguration.cpp | 76 +- .../btDefaultCollisionConfiguration.h | 33 +- .../btEmptyCollisionAlgorithm.h | 4 +- .../CollisionDispatch/btManifoldResult.cpp | 19 +- .../btSimulationIslandManager.cpp | 40 +- .../btSphereBoxCollisionAlgorithm.h | 7 +- .../btSphereSphereCollisionAlgorithm.h | 5 +- .../btSphereTriangleCollisionAlgorithm.h | 5 +- .../CollisionShapes/btOptimizedBvh.cpp | 34 +- .../CollisionShapes/btOptimizedBvh.h | 10 +- .../btPersistentManifold.h | 2 +- .../ConstraintSolver/btConstraintSolver.h | 4 +- .../btSequentialImpulseConstraintSolver.cpp | 463 ++++++----- .../btSequentialImpulseConstraintSolver.h | 4 +- src/BulletDynamics/Dynamics/Bullet-C-API.cpp | 3 +- .../Dynamics/btDiscreteDynamicsWorld.cpp | 56 +- .../Dynamics/btSimpleDynamicsWorld.cpp | 2 +- src/LinearMath/btAlignedAllocator.cpp | 5 +- src/LinearMath/btScalar.h | 82 +- src/btBulletCollisionCommon.h | 1 + 67 files changed, 1116 insertions(+), 972 deletions(-) diff --git a/Demos/BasicDemo/BasicDemo.cpp b/Demos/BasicDemo/BasicDemo.cpp index b085ded32..aeac6128b 100644 --- a/Demos/BasicDemo/BasicDemo.cpp +++ b/Demos/BasicDemo/BasicDemo.cpp @@ -129,6 +129,7 @@ void BasicDemo::displayCallback(void) { void BasicDemo::initPhysics() { + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); #ifdef USE_PARALLEL_DISPATCHER @@ -149,9 +150,9 @@ void BasicDemo::initPhysics() #endif - m_dispatcher = new SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks); + m_dispatcher = new SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,collisionConfiguration); #else - m_dispatcher = new btCollisionDispatcher(true); + m_dispatcher = new btCollisionDispatcher(collisionConfiguration); #endif //USE_PARALLEL_DISPATCHER #define USE_SWEEP_AND_PRUNE 1 @@ -167,18 +168,8 @@ void BasicDemo::initPhysics() m_overlappingPairCache = new btSimpleBroadphase; #endif //USE_SWEEP_AND_PRUNE -#ifndef USE_PARALLEL_DISPATCHER - m_sphereSphereCF = new btSphereSphereCollisionAlgorithm::CreateFunc; - m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,m_sphereSphereCF); - m_sphereBoxCF = new btSphereBoxCollisionAlgorithm::CreateFunc; - m_boxSphereCF = new btSphereBoxCollisionAlgorithm::CreateFunc; - m_boxSphereCF->m_swapped = true; - m_dispatcher->registerCollisionCreateFunc(SPHERE_SHAPE_PROXYTYPE,BOX_SHAPE_PROXYTYPE,m_sphereBoxCF); - m_dispatcher->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE,SPHERE_SHAPE_PROXYTYPE,m_boxSphereCF); -#endif //USE_PARALLEL_DISPATCHER - btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; m_solver = sol; @@ -267,13 +258,6 @@ void BasicDemo::exitPhysics() //delete dynamics world delete m_dynamicsWorld; - //delete collision algorithms creation functions - delete m_sphereSphereCF; - - - delete m_sphereBoxCF; - delete m_boxSphereCF; - //delete solver delete m_solver; diff --git a/Demos/BspDemo/BspDemo.cpp b/Demos/BspDemo/BspDemo.cpp index 692491917..af1d041a5 100644 --- a/Demos/BspDemo/BspDemo.cpp +++ b/Demos/BspDemo/BspDemo.cpp @@ -134,8 +134,9 @@ void BspDemo::initPhysics(char* bspfilename) ///Setup a Physics Simulation Environment + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); // btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50)); - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); btVector3 worldMin(-1000,-1000,-1000); btVector3 worldMax(1000,1000,1000); btBroadphaseInterface* pairCache = new btAxisSweep3(worldMin,worldMax); diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index 03de70952..c169725be 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -388,7 +388,8 @@ int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks dispatcher = new SpuGatheringCollisionDispatcher(threadSupportCollision,maxNumOutstandingTasks); // dispatcher = new btCollisionDispatcher(); #else - dispatcher = new btCollisionDispatcher(); + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + dispatcher = new btCollisionDispatcher(collisionConfiguration); #endif //USE_PARALLEL_DISPATCHER #ifdef USE_CUSTOM_NEAR_CALLBACK diff --git a/Demos/ColladaDemo/ColladaDemo.cpp b/Demos/ColladaDemo/ColladaDemo.cpp index 97eedd2fe..db1eeb000 100644 --- a/Demos/ColladaDemo/ColladaDemo.cpp +++ b/Demos/ColladaDemo/ColladaDemo.cpp @@ -170,7 +170,8 @@ void ColladaDemo::initPhysics(const char* filename) m_ele = 60; m_forwardAxis = 1; - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); btVector3 worldMin(-1000,-1000,-1000); btVector3 worldMax(1000,1000,1000); btBroadphaseInterface* pairCache = new btAxisSweep3(worldMin,worldMax); diff --git a/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp b/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp index 97b0d493e..65270eb1b 100644 --- a/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp +++ b/Demos/CollisionInterfaceDemo/CollisionInterfaceDemo.cpp @@ -81,7 +81,8 @@ void CollisionInterfaceDemo::initPhysics() objects[0].setCollisionShape(boxA);//&hullA; objects[1].setCollisionShape(boxB);//&hullB; - btCollisionDispatcher* dispatcher = new btCollisionDispatcher; + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); btVector3 worldAabbMin(-1000,-1000,-1000); btVector3 worldAabbMax(1000,1000,1000); diff --git a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp index 8337e2167..adbc353c0 100644 --- a/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp +++ b/Demos/ConcaveDemo/ConcavePhysicsDemo.cpp @@ -217,7 +217,8 @@ void ConcaveDemo::initPhysics() btCollisionDispatcher* dispatcher = new SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks); #else - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); #endif//USE_PARALLEL_DISPATCHER @@ -276,7 +277,7 @@ void ConcaveDemo::clientMoveAndDisplay() trimeshShape->refitTree(); //clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation. - m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle()); + m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher()); } m_dynamicsWorld->stepSimulation(dt); diff --git a/Demos/ConstraintDemo/ConstraintDemo.cpp b/Demos/ConstraintDemo/ConstraintDemo.cpp index 2725b9fd7..7defb7c7f 100644 --- a/Demos/ConstraintDemo/ConstraintDemo.cpp +++ b/Demos/ConstraintDemo/ConstraintDemo.cpp @@ -76,7 +76,8 @@ void drawLimit() void ConstraintDemo::initPhysics() { // btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50)); - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); btVector3 worldMin(-1000,-1000,-1000); btVector3 worldMax(1000,1000,1000); btBroadphaseInterface* pairCache = new btAxisSweep3(worldMin,worldMax); diff --git a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp index 82d89bf56..8b4693660 100644 --- a/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp +++ b/Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp @@ -95,6 +95,8 @@ void ConvexDecompositionDemo::initPhysics(const char* filename) tcount = wo.loadObj("../../file.obj"); } + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + #ifdef USE_PARALLEL_DISPATCHER #ifdef USE_WIN32_THREADING @@ -112,9 +114,9 @@ void ConvexDecompositionDemo::initPhysics(const char* filename) ///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface #endif - btCollisionDispatcher* dispatcher = new SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks); + btCollisionDispatcher* dispatcher = new SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,collisionConfiguration); #else - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); #endif//USE_PARALLEL_DISPATCHER diff --git a/Demos/DoublePrecisionDemo/DoublePrecisionDemo.cpp b/Demos/DoublePrecisionDemo/DoublePrecisionDemo.cpp index 7ad4c12bd..7eb0b12ef 100644 --- a/Demos/DoublePrecisionDemo/DoublePrecisionDemo.cpp +++ b/Demos/DoublePrecisionDemo/DoublePrecisionDemo.cpp @@ -82,7 +82,8 @@ void DoublePrecisionDemo::initPhysics() objects[0].setCollisionShape(boxA);//&hullA; objects[1].setCollisionShape(boxB);//&hullB; - btCollisionDispatcher* dispatcher = new btCollisionDispatcher; + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); btVector3 worldAabbMin(80000,80000,80000); btVector3 worldAabbMax(120000,120000,120000); diff --git a/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp b/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp index 4b6b4acd0..d94dd9221 100644 --- a/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp +++ b/Demos/MovingConcaveDemo/ConcavePhysicsDemo.cpp @@ -1643,10 +1643,11 @@ void ConcaveDemo::initGImpactCollision() void ConcaveDemo::initPhysics() { - + + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); //btConstraintSolver* solver = new btSequentialImpulseConstraintSolver; - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); //btOverlappingPairCache* broadphase = new btSimpleBroadphase(); btBroadphaseInterface* broadphase = new btSimpleBroadphase(); diff --git a/Demos/OpenGL/DemoApplication.cpp b/Demos/OpenGL/DemoApplication.cpp index 1308462a4..46f2138a8 100644 --- a/Demos/OpenGL/DemoApplication.cpp +++ b/Demos/OpenGL/DemoApplication.cpp @@ -932,7 +932,7 @@ void DemoApplication::clientResetScene() colObj->activate(); } //removed cached contact points - m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle()); + m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(colObj->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher()); btRigidBody* body = btRigidBody::upcast(colObj); if (body && !body->isStaticObject()) diff --git a/Demos/RagdollDemo/RagdollDemo.cpp b/Demos/RagdollDemo/RagdollDemo.cpp index eb10bef36..03ed8aebe 100644 --- a/Demos/RagdollDemo/RagdollDemo.cpp +++ b/Demos/RagdollDemo/RagdollDemo.cpp @@ -1,389 +1,391 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Ragdoll Demo -Copyright (c) 2007 Starbreeze Studios - -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. - -Written by: Marten Svanfeldt -*/ - - -#include "btBulletDynamicsCommon.h" -#include "GlutStuff.h" -#include "GL_ShapeDrawer.h" - +/* +Bullet Continuous Collision Detection and Physics Library +Ragdoll Demo +Copyright (c) 2007 Starbreeze Studios + +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. + +Written by: Marten Svanfeldt +*/ + + +#include "btBulletDynamicsCommon.h" +#include "GlutStuff.h" +#include "GL_ShapeDrawer.h" + #include "LinearMath/btIDebugDraw.h" -#include "GLDebugDrawer.h" -#include "RagdollDemo.h" - -GLDebugDrawer debugDrawer; - -#define M_PI 3.14159265358979323846 -#define M_PI_2 1.57079632679489661923 -#define M_PI_4 0.785398163397448309616 - - -class RagDoll -{ - enum - { - BODYPART_PELVIS = 0, - BODYPART_SPINE, - BODYPART_HEAD, - - BODYPART_LEFT_UPPER_LEG, - BODYPART_LEFT_LOWER_LEG, - - BODYPART_RIGHT_UPPER_LEG, - BODYPART_RIGHT_LOWER_LEG, - - BODYPART_LEFT_UPPER_ARM, - BODYPART_LEFT_LOWER_ARM, - - BODYPART_RIGHT_UPPER_ARM, - BODYPART_RIGHT_LOWER_ARM, - - BODYPART_COUNT - }; - - enum - { - JOINT_PELVIS_SPINE = 0, - JOINT_SPINE_HEAD, - - JOINT_LEFT_HIP, - JOINT_LEFT_KNEE, - - JOINT_RIGHT_HIP, - JOINT_RIGHT_KNEE, - - JOINT_LEFT_SHOULDER, - JOINT_LEFT_ELBOW, - - JOINT_RIGHT_SHOULDER, - JOINT_RIGHT_ELBOW, - - JOINT_COUNT - }; - - btDynamicsWorld* m_ownerWorld; - btCollisionShape* m_shapes[BODYPART_COUNT]; - btRigidBody* m_bodies[BODYPART_COUNT]; - btTypedConstraint* m_joints[JOINT_COUNT]; - - btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape) - { - bool isDynamic = (mass != 0.f); - - btVector3 localInertia(0,0,0); - if (isDynamic) - shape->calculateLocalInertia(mass,localInertia); - - btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); - btRigidBody* body = new btRigidBody(mass,myMotionState,shape,localInertia); - - m_ownerWorld->addRigidBody(body); - - return body; - } - -public: - RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset) - : m_ownerWorld (ownerWorld) - { - // Setup the geometry - m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(0.15), btScalar(0.20)); - m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(0.15), btScalar(0.28)); - m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(0.10), btScalar(0.05)); - m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45)); - m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37)); - m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45)); - m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37)); - m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33)); - m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25)); - m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33)); - m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25)); - - // Setup all the rigid bodies - btTransform offset; offset.setIdentity(); - offset.setOrigin(positionOffset); - - btTransform transform; - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(0.), btScalar(1.), btScalar(0.))); - m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(0.), btScalar(1.2), btScalar(0.))); - m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(0.), btScalar(1.6), btScalar(0.))); - m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.65), btScalar(0.))); - m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.2), btScalar(0.))); - m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.65), btScalar(0.))); - m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.2), btScalar(0.))); - m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(-0.35), btScalar(1.45), btScalar(0.))); - transform.getBasis().setEulerZYX(0,0,M_PI_2); - m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(-0.7), btScalar(1.45), btScalar(0.))); - transform.getBasis().setEulerZYX(0,0,M_PI_2); - m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(0.35), btScalar(1.45), btScalar(0.))); - transform.getBasis().setEulerZYX(0,0,-M_PI_2); - m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]); - - transform.setIdentity(); - transform.setOrigin(btVector3(btScalar(0.7), btScalar(1.45), btScalar(0.))); - transform.getBasis().setEulerZYX(0,0,-M_PI_2); - m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]); - - // Setup some damping on the m_bodies - for (int i = 0; i < BODYPART_COUNT; ++i) - { - m_bodies[i]->setDamping(0.05, 0.85); - m_bodies[i]->setDeactivationTime(0.8); - m_bodies[i]->setSleepingThresholds(1.6, 2.5); - } - - // Now setup the constraints - btHingeConstraint* hingeC; - btConeTwistConstraint* coneC; - - btTransform localA, localB; - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15), btScalar(0.))); - localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15), btScalar(0.))); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB); - hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2)); - m_joints[JOINT_PELVIS_SPINE] = hingeC; - m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true); - - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB); - coneC->setLimit(M_PI_4, M_PI_4, M_PI_2); - m_joints[JOINT_SPINE_HEAD] = coneC; - m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true); - - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,M_PI_4*5); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB); - coneC->setLimit(M_PI_4, M_PI_4, 0); - m_joints[JOINT_LEFT_HIP] = coneC; - m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true); - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.))); - localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.))); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB); - hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); - m_joints[JOINT_LEFT_KNEE] = hingeC; - m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true); - - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_4); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB); - coneC->setLimit(M_PI_4, M_PI_4, 0); - m_joints[JOINT_RIGHT_HIP] = coneC; - m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true); - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.))); - localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.))); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB); - hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); - m_joints[JOINT_RIGHT_KNEE] = hingeC; - m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true); - - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB); - coneC->setLimit(M_PI_2, M_PI_2, 0); - m_joints[JOINT_LEFT_SHOULDER] = coneC; - m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true); - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.))); - localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB); - hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); - m_joints[JOINT_LEFT_ELBOW] = hingeC; - m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true); - - - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.))); - localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); - coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB); - coneC->setLimit(M_PI_2, M_PI_2, 0); - m_joints[JOINT_RIGHT_SHOULDER] = coneC; - m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true); - - localA.setIdentity(); localB.setIdentity(); - localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.))); - localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); - hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB); - hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); - m_joints[JOINT_RIGHT_ELBOW] = hingeC; - m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true); - } - - ~RagDoll () - { - // Remove all constraints - for (int i = 0; i < JOINT_COUNT; ++i) - { - m_ownerWorld->removeConstraint(m_joints[i]); - delete m_joints[i]; m_joints[i] = 0; - } - - // Remove all bodies and shapes - for (int i = 0; i < BODYPART_COUNT; ++i) - { - m_ownerWorld->removeRigidBody(m_bodies[i]); - - delete m_bodies[i]->getMotionState(); - - delete m_bodies[i]; m_bodies[i] = 0; - delete m_shapes[i]; m_shapes[i] = 0; - } - } -}; - - - -int main(int argc,char* argv[]) -{ - RagdollDemo demoApp; - - demoApp.initPhysics(); - demoApp.setCameraDistance(btScalar(10.)); - - return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bullet.sf.net",&demoApp); -} - - -void RagdollDemo::initPhysics() -{ - // Setup the basic world - - btCollisionDispatcher* dispatcher = new btCollisionDispatcher; - - btPoint3 worldAabbMin(-10000,-10000,-10000); - btPoint3 worldAabbMax(10000,10000,10000); - btBroadphaseInterface* overlappingPairCache = new btAxisSweep3 (worldAabbMin, worldAabbMax); - - btConstraintSolver* solver = new btSequentialImpulseConstraintSolver; - - m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver); - +#include "GLDebugDrawer.h" +#include "RagdollDemo.h" + +GLDebugDrawer debugDrawer; + +#define M_PI 3.14159265358979323846 +#define M_PI_2 1.57079632679489661923 +#define M_PI_4 0.785398163397448309616 + + +class RagDoll +{ + enum + { + BODYPART_PELVIS = 0, + BODYPART_SPINE, + BODYPART_HEAD, + + BODYPART_LEFT_UPPER_LEG, + BODYPART_LEFT_LOWER_LEG, + + BODYPART_RIGHT_UPPER_LEG, + BODYPART_RIGHT_LOWER_LEG, + + BODYPART_LEFT_UPPER_ARM, + BODYPART_LEFT_LOWER_ARM, + + BODYPART_RIGHT_UPPER_ARM, + BODYPART_RIGHT_LOWER_ARM, + + BODYPART_COUNT + }; + + enum + { + JOINT_PELVIS_SPINE = 0, + JOINT_SPINE_HEAD, + + JOINT_LEFT_HIP, + JOINT_LEFT_KNEE, + + JOINT_RIGHT_HIP, + JOINT_RIGHT_KNEE, + + JOINT_LEFT_SHOULDER, + JOINT_LEFT_ELBOW, + + JOINT_RIGHT_SHOULDER, + JOINT_RIGHT_ELBOW, + + JOINT_COUNT + }; + + btDynamicsWorld* m_ownerWorld; + btCollisionShape* m_shapes[BODYPART_COUNT]; + btRigidBody* m_bodies[BODYPART_COUNT]; + btTypedConstraint* m_joints[JOINT_COUNT]; + + btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape) + { + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + shape->calculateLocalInertia(mass,localInertia); + + btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform); + btRigidBody* body = new btRigidBody(mass,myMotionState,shape,localInertia); + + m_ownerWorld->addRigidBody(body); + + return body; + } + +public: + RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset) + : m_ownerWorld (ownerWorld) + { + // Setup the geometry + m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(0.15), btScalar(0.20)); + m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(0.15), btScalar(0.28)); + m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(0.10), btScalar(0.05)); + m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45)); + m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37)); + m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07), btScalar(0.45)); + m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05), btScalar(0.37)); + m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33)); + m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25)); + m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05), btScalar(0.33)); + m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04), btScalar(0.25)); + + // Setup all the rigid bodies + btTransform offset; offset.setIdentity(); + offset.setOrigin(positionOffset); + + btTransform transform; + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(0.), btScalar(1.), btScalar(0.))); + m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(0.), btScalar(1.2), btScalar(0.))); + m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(0.), btScalar(1.6), btScalar(0.))); + m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.65), btScalar(0.))); + m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(-0.18), btScalar(0.2), btScalar(0.))); + m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.65), btScalar(0.))); + m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(0.18), btScalar(0.2), btScalar(0.))); + m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(-0.35), btScalar(1.45), btScalar(0.))); + transform.getBasis().setEulerZYX(0,0,M_PI_2); + m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(-0.7), btScalar(1.45), btScalar(0.))); + transform.getBasis().setEulerZYX(0,0,M_PI_2); + m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(0.35), btScalar(1.45), btScalar(0.))); + transform.getBasis().setEulerZYX(0,0,-M_PI_2); + m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]); + + transform.setIdentity(); + transform.setOrigin(btVector3(btScalar(0.7), btScalar(1.45), btScalar(0.))); + transform.getBasis().setEulerZYX(0,0,-M_PI_2); + m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]); + + // Setup some damping on the m_bodies + for (int i = 0; i < BODYPART_COUNT; ++i) + { + m_bodies[i]->setDamping(0.05, 0.85); + m_bodies[i]->setDeactivationTime(0.8); + m_bodies[i]->setSleepingThresholds(1.6, 2.5); + } + + // Now setup the constraints + btHingeConstraint* hingeC; + btConeTwistConstraint* coneC; + + btTransform localA, localB; + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.15), btScalar(0.))); + localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.15), btScalar(0.))); + hingeC = new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB); + hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2)); + m_joints[JOINT_PELVIS_SPINE] = hingeC; + m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true); + + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.30), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); + coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB); + coneC->setLimit(M_PI_4, M_PI_4, M_PI_2); + m_joints[JOINT_SPINE_HEAD] = coneC; + m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true); + + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,M_PI_4*5); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); + coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB); + coneC->setLimit(M_PI_4, M_PI_4, 0); + m_joints[JOINT_LEFT_HIP] = coneC; + m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true); + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.))); + localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.))); + hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB); + hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); + m_joints[JOINT_LEFT_KNEE] = hingeC; + m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true); + + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,-M_PI_4); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.225), btScalar(0.))); + coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB); + coneC->setLimit(M_PI_4, M_PI_4, 0); + m_joints[JOINT_RIGHT_HIP] = coneC; + m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true); + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.))); + localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(0.185), btScalar(0.))); + hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB); + hingeC->setLimit(btScalar(0), btScalar(M_PI_2)); + m_joints[JOINT_RIGHT_KNEE] = hingeC; + m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true); + + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); + coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB); + coneC->setLimit(M_PI_2, M_PI_2, 0); + m_joints[JOINT_LEFT_SHOULDER] = coneC; + m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true); + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.))); + localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); + hingeC = new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB); + hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); + m_joints[JOINT_LEFT_ELBOW] = hingeC; + m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true); + + + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.))); + localB.getBasis().setEulerZYX(0,0,-M_PI_2); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.))); + coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB); + coneC->setLimit(M_PI_2, M_PI_2, 0); + m_joints[JOINT_RIGHT_SHOULDER] = coneC; + m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true); + + localA.setIdentity(); localB.setIdentity(); + localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(btVector3(btScalar(0.), btScalar(0.18), btScalar(0.))); + localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.))); + hingeC = new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB); + hingeC->setLimit(btScalar(-M_PI_2), btScalar(0)); + m_joints[JOINT_RIGHT_ELBOW] = hingeC; + m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true); + } + + ~RagDoll () + { + // Remove all constraints + for (int i = 0; i < JOINT_COUNT; ++i) + { + m_ownerWorld->removeConstraint(m_joints[i]); + delete m_joints[i]; m_joints[i] = 0; + } + + // Remove all bodies and shapes + for (int i = 0; i < BODYPART_COUNT; ++i) + { + m_ownerWorld->removeRigidBody(m_bodies[i]); + + delete m_bodies[i]->getMotionState(); + + delete m_bodies[i]; m_bodies[i] = 0; + delete m_shapes[i]; m_shapes[i] = 0; + } + } +}; + + + +int main(int argc,char* argv[]) +{ + RagdollDemo demoApp; + + demoApp.initPhysics(); + demoApp.setCameraDistance(btScalar(10.)); + + return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://bullet.sf.net",&demoApp); +} + + +void RagdollDemo::initPhysics() +{ + // Setup the basic world + + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); + + btPoint3 worldAabbMin(-10000,-10000,-10000); + btPoint3 worldAabbMax(10000,10000,10000); + btBroadphaseInterface* overlappingPairCache = new btAxisSweep3 (worldAabbMin, worldAabbMax); + + btConstraintSolver* solver = new btSequentialImpulseConstraintSolver; + + m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver); + m_dynamicsWorld->setDebugDrawer(&debugDrawer); - - // Setup a big ground box - { - btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.))); - btTransform groundTransform; - groundTransform.setIdentity(); - groundTransform.setOrigin(btVector3(0,-10,0)); - localCreateRigidBody(btScalar(0.),groundTransform,groundShape); - } - - // Spawn one ragdoll - spawnRagdoll(); - - clientResetScene(); -} - -void RagdollDemo::spawnRagdoll(bool random) -{ - RagDoll* ragDoll = new RagDoll (m_dynamicsWorld, btVector3 (0,1,0)); - m_ragdolls.push_back(ragDoll); -} - -void RagdollDemo::clientMoveAndDisplay() -{ - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - - //simple dynamics world doesn't handle fixed-time-stepping - float ms = m_clock.getTimeMicroseconds(); - m_clock.reset(); - float minFPS = 1000000.f/60.f; - if (ms > minFPS) - ms = minFPS; - - if (m_dynamicsWorld) - m_dynamicsWorld->stepSimulation(ms / 1000000.f); - - renderme(); - - glFlush(); - - glutSwapBuffers(); -} - -void RagdollDemo::displayCallback() -{ - glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); - - if (m_dynamicsWorld) - m_dynamicsWorld->updateAabbs(); - - renderme(); - - glFlush(); - glutSwapBuffers(); -} - -void RagdollDemo::keyboardCallback(unsigned char key, int x, int y) -{ - switch (key) - { - case 'e': - spawnRagdoll(true); - break; - default: - DemoApplication::keyboardCallback(key, x, y); - } - - -} + + // Setup a big ground box + { + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.))); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-10,0)); + localCreateRigidBody(btScalar(0.),groundTransform,groundShape); + } + + // Spawn one ragdoll + spawnRagdoll(); + + clientResetScene(); +} + +void RagdollDemo::spawnRagdoll(bool random) +{ + RagDoll* ragDoll = new RagDoll (m_dynamicsWorld, btVector3 (0,1,0)); + m_ragdolls.push_back(ragDoll); +} + +void RagdollDemo::clientMoveAndDisplay() +{ + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + //simple dynamics world doesn't handle fixed-time-stepping + float ms = m_clock.getTimeMicroseconds(); + m_clock.reset(); + float minFPS = 1000000.f/60.f; + if (ms > minFPS) + ms = minFPS; + + if (m_dynamicsWorld) + m_dynamicsWorld->stepSimulation(ms / 1000000.f); + + renderme(); + + glFlush(); + + glutSwapBuffers(); +} + +void RagdollDemo::displayCallback() +{ + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + + if (m_dynamicsWorld) + m_dynamicsWorld->updateAabbs(); + + renderme(); + + glFlush(); + glutSwapBuffers(); +} + +void RagdollDemo::keyboardCallback(unsigned char key, int x, int y) +{ + switch (key) + { + case 'e': + spawnRagdoll(true); + break; + default: + DemoApplication::keyboardCallback(key, x, y); + } + + +} diff --git a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp index 3f7786db2..57a4b7a43 100644 --- a/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp +++ b/Demos/UserCollisionAlgorithm/UserCollisionAlgorithm.cpp @@ -104,8 +104,8 @@ void UserCollisionAlgorithm::initPhysics() btCollisionShape* trimeshShape = new btBvhTriangleMeshShape(trimesh,useQuantizedBvhTree); //ConstraintSolver* solver = new btSequentialImpulseConstraintSolver; - - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); btVector3 maxAabb(10000,10000,10000); btBroadphaseInterface* broadphase = new btAxisSweep3(-maxAabb,maxAabb);//SimpleBroadphase(); diff --git a/Demos/VehicleDemo/VehicleDemo.cpp b/Demos/VehicleDemo/VehicleDemo.cpp index 08b97332a..3441eb195 100644 --- a/Demos/VehicleDemo/VehicleDemo.cpp +++ b/Demos/VehicleDemo/VehicleDemo.cpp @@ -118,7 +118,8 @@ void VehicleDemo::setupPhysics() #endif btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50)); - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); btVector3 worldMin(-1000,-1000,-1000); btVector3 worldMax(1000,1000,1000); btBroadphaseInterface* pairCache = new btAxisSweep3(worldMin,worldMax); @@ -490,7 +491,7 @@ void VehicleDemo::clientResetScene() m_carChassis->setCenterOfMassTransform(btTransform::getIdentity()); m_carChassis->setLinearVelocity(btVector3(0,0,0)); m_carChassis->setAngularVelocity(btVector3(0,0,0)); - m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle()); + m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher()); if (m_vehicle) { m_vehicle->resetSuspension(); diff --git a/Extras/BulletMultiThreaded/Jamfile b/Extras/BulletMultiThreaded/Jamfile index 8de542a38..970cb4925 100644 --- a/Extras/BulletMultiThreaded/Jamfile +++ b/Extras/BulletMultiThreaded/Jamfile @@ -2,7 +2,7 @@ SubDir TOP Extras BulletMultiThreaded ; #IncludeDir Extras/BulletMultiThreaded ; -Library bulletmultithreaded : [ Wildcard . : */.h *.cpp ] [ Wildcard SpuNarrowPhaseCollisionTask : *.h *.cpp ] : noinstall ; +Library bulletmultithreaded : [ Wildcard . : */.h *.cpp ] [ Wildcard SpuNarrowPhaseCollisionTask : *.h *.cpp ] [ Wildcard SpuSolverTask : *.h *.cpp ] : noinstall ; CFlags bulletmultithreaded : [ FIncludes $(TOP)/Extras/BulletMultiThreaded ] ; LibDepends bulletmultithreaded : ; diff --git a/Extras/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h b/Extras/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h index 4a4069eb7..8a585a863 100644 --- a/Extras/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h +++ b/Extras/BulletMultiThreaded/SpuContactManifoldCollisionAlgorithm.h @@ -19,6 +19,8 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" + class btPersistentManifold; /// SpuContactManifoldCollisionAlgorithm provides contact manifold and should be processed on SPU. @@ -71,7 +73,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new SpuContactManifoldCollisionAlgorithm(ci,body0,body1); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(SpuContactManifoldCollisionAlgorithm)); + return new(mem) SpuContactManifoldCollisionAlgorithm(ci,body0,body1); } }; diff --git a/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp b/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp index 4d63c0fed..48bc5a2c7 100644 --- a/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp +++ b/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.cpp @@ -26,8 +26,9 @@ subject to the following restrictions: -SpuGatheringCollisionDispatcher::SpuGatheringCollisionDispatcher(class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks) -:m_spuCollisionTaskProcess(0), +SpuGatheringCollisionDispatcher::SpuGatheringCollisionDispatcher(class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration) +:btCollisionDispatcher(collisionConfiguration), +m_spuCollisionTaskProcess(0), m_threadInterface(threadInterface), m_maxNumOutstandingTasks(maxNumOutstandingTasks) { @@ -115,7 +116,7 @@ public: btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject; btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher = m_dispatcher; + ci.m_dispatcher1 = m_dispatcher; ci.m_manifold = 0; if (m_dispatcher->needsCollision(colObj0,colObj1)) @@ -124,7 +125,9 @@ public: int proxyType1 = colObj1->getCollisionShape()->getShapeType(); if (m_dispatcher->supportsDispatchPairOnSpu(proxyType0,proxyType1)) { - collisionPair.m_algorithm = new SpuContactManifoldCollisionAlgorithm(ci,colObj0,colObj1); + int so = sizeof(SpuContactManifoldCollisionAlgorithm); + void* mem = m_dispatcher->allocateCollisionAlgorithm(so); + collisionPair.m_algorithm = new(mem) SpuContactManifoldCollisionAlgorithm(ci,colObj0,colObj1); collisionPair.m_userInfo = (void*) 2; } else { @@ -138,7 +141,7 @@ public: } }; -void SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo) +void SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) { if (dispatchInfo.m_enableSPU) @@ -152,7 +155,7 @@ void SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPai { btSpuCollisionPairCallback collisionCallback(dispatchInfo,this); - pairCache->processAllOverlappingPairs(&collisionCallback); + pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); } //send one big batch @@ -203,7 +206,7 @@ void SpuGatheringCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPai { ///PPU fallback ///!Need to make sure to clear all 'algorithms' when switching between SPU and PPU - btCollisionDispatcher::dispatchAllCollisionPairs(pairCache,dispatchInfo); + btCollisionDispatcher::dispatchAllCollisionPairs(pairCache,dispatchInfo,dispatcher); } } diff --git a/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h b/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h index 33a433c94..497773b61 100644 --- a/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h +++ b/Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h @@ -49,13 +49,13 @@ public: return m_spuCollisionTaskProcess; } - SpuGatheringCollisionDispatcher (class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks); + SpuGatheringCollisionDispatcher (class btThreadSupportInterface* threadInterface, unsigned int maxNumOutstandingTasks,btCollisionConfiguration* collisionConfiguration); virtual ~SpuGatheringCollisionDispatcher(); bool supportsDispatchPairOnSpu(int proxyType0,int proxyType1); - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo); + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher); }; diff --git a/Extras/BulletMultiThreaded/SpuParallelSolver.cpp b/Extras/BulletMultiThreaded/SpuParallelSolver.cpp index 429848a49..5ea559ba4 100644 --- a/Extras/BulletMultiThreaded/SpuParallelSolver.cpp +++ b/Extras/BulletMultiThreaded/SpuParallelSolver.cpp @@ -105,7 +105,7 @@ void btParallelSequentialImpulseSolver::prepareSolve(int numBodies, int numManif m_allObjects.reserve(numBodies); } -btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) +btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) { if (!numManifolds && !numConstraints) return 0; diff --git a/Extras/BulletMultiThreaded/SpuParallelSolver.h b/Extras/BulletMultiThreaded/SpuParallelSolver.h index f71b530ce..9f52ba16c 100644 --- a/Extras/BulletMultiThreaded/SpuParallelSolver.h +++ b/Extras/BulletMultiThreaded/SpuParallelSolver.h @@ -67,7 +67,7 @@ public: virtual ~btParallelSequentialImpulseSolver(); virtual void prepareSolve (int numBodies, int numManifolds); - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); virtual void reset (); }; diff --git a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h index f16605c24..c52259e0e 100755 --- a/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h +++ b/Extras/GIMPACT/include/GIMPACT/Bullet/btGImpactCollisionAlgorithm.h @@ -84,7 +84,8 @@ protected: { if(m_convex_algorithm) { - delete m_convex_algorithm; + m_convex_algorithm->~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm( m_convex_algorithm); m_convex_algorithm = NULL; } } @@ -188,7 +189,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btGImpactCollisionAlgorithm(ci,body0,body1); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btGImpactCollisionAlgorithm)); + return new(mem) btGImpactCollisionAlgorithm(ci,body0,body1); } }; diff --git a/Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp b/Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp index 59fb1fe92..313c7f82a 100755 --- a/Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp +++ b/Extras/GIMPACT/src/Bullet/btGImpactCollisionAlgorithm.cpp @@ -905,9 +905,9 @@ btScalar btGImpactCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* b void btGImpactCollisionAlgorithm::registerAlgorithm(btCollisionDispatcher * dispatcher) { - for (GUINT i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) + for (GUINT i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) { - dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,new btGImpactCollisionAlgorithm::CreateFunc); + dispatcher->registerCollisionCreateFunc(GIMPACT_SHAPE_PROXYTYPE,i ,new btGImpactCollisionAlgorithm::CreateFunc); } for (GUINT i = 0;i < MAX_BROADPHASE_COLLISION_TYPES ;i++ ) diff --git a/Extras/quickstep/OdeConstraintSolver.cpp b/Extras/quickstep/OdeConstraintSolver.cpp index b91ddba08..f1b6acbb5 100644 --- a/Extras/quickstep/OdeConstraintSolver.cpp +++ b/Extras/quickstep/OdeConstraintSolver.cpp @@ -72,7 +72,7 @@ m_erp(0.4f) //iterative lcp and penalty method -btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher) { BEGIN_PROFILE("prepareConstraints"); @@ -291,3 +291,7 @@ void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,BU_Jo //create a new contact constraint }; + +void OdeConstraintSolver::reset() +{ +} diff --git a/Extras/quickstep/OdeConstraintSolver.h b/Extras/quickstep/OdeConstraintSolver.h index a8fd0b81e..4b2e5e60b 100644 --- a/Extras/quickstep/OdeConstraintSolver.h +++ b/Extras/quickstep/OdeConstraintSolver.h @@ -20,6 +20,7 @@ subject to the following restrictions: class btRigidBody; struct OdeSolverBody; +class btDispatcher; class BU_Joint; /// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework @@ -45,7 +46,7 @@ public: virtual ~OdeConstraintSolver() {} - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher); ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' @@ -59,6 +60,8 @@ public: { m_erp = erp; } + + virtual void reset(); }; diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp index 885bc2bb5..87696ea99 100644 --- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp +++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.cpp @@ -55,10 +55,10 @@ btBroadphaseProxy* btAxisSweep3::createProxy( const btVector3& aabbMin, const return handle; } -void btAxisSweep3::destroyProxy(btBroadphaseProxy* proxy) +void btAxisSweep3::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) { Handle* handle = static_cast(proxy); - removeHandle(handle->m_handleId); + removeHandle(handle->m_handleId,dispatcher); } void btAxisSweep3::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax) @@ -248,15 +248,15 @@ BP_FP_INT_TYPE btAxisSweep3::addHandle(const btPoint3& aabbMin,const btPoint3& a } -void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle) +void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher) { - + Handle* pHandle = getHandle(handle); //explicitly remove the pairs containing the proxy //we could do it also in the sortMinUp (passing true) //todo: compare performance - m_pairCache->removeOverlappingPairsContainingProxy(pHandle); + m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher); // compute current limit of edge arrays @@ -305,7 +305,7 @@ void btAxisSweep3::removeHandle(BP_FP_INT_TYPE handle) extern int gOverlappingPairs; -void btAxisSweep3::calculateOverlappingPairs() +void btAxisSweep3::calculateOverlappingPairs(btDispatcher* dispatcher) { if (m_ownsPairCache) @@ -359,7 +359,7 @@ void btAxisSweep3::calculateOverlappingPairs() if (needsRemoval) { - m_pairCache->cleanOverlappingPair(pair); + m_pairCache->cleanOverlappingPair(pair,dispatcher); // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); // m_overlappingPairArray.pop_back(); diff --git a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index 444116284..6e5f85a76 100644 --- a/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -121,10 +121,10 @@ public: btAxisSweep3(const btPoint3& worldAabbMin,const btPoint3& worldAabbMax, int maxHandles = 16384, btOverlappingPairCache* pairCache=0); virtual ~btAxisSweep3(); - virtual void calculateOverlappingPairs(); + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); BP_FP_INT_TYPE addHandle(const btPoint3& aabbMin,const btPoint3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask); - void removeHandle(BP_FP_INT_TYPE handle); + void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher); void updateHandle(BP_FP_INT_TYPE handle, const btPoint3& aabbMin,const btPoint3& aabbMax); inline Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} @@ -132,7 +132,7 @@ public: //Broadphase Interface virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask); - virtual void destroyProxy(btBroadphaseProxy* proxy); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax); bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); diff --git a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index bea2fd621..eba825f1b 100644 --- a/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -32,11 +32,11 @@ public: virtual ~btBroadphaseInterface() {} virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask) =0; - virtual void destroyProxy(btBroadphaseProxy* proxy)=0; + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher)=0; virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax)=0; ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb - virtual void calculateOverlappingPairs()=0; + virtual void calculateOverlappingPairs(btDispatcher* dispatcher)=0; virtual btOverlappingPairCache* getOverlappingPairCache()=0; virtual const btOverlappingPairCache* getOverlappingPairCache() const =0; diff --git a/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp b/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp index 2ad0c86d8..c95d1be0f 100644 --- a/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp +++ b/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.cpp @@ -18,6 +18,6 @@ subject to the following restrictions: btCollisionAlgorithm::btCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) { - m_dispatcher = ci.m_dispatcher; + m_dispatcher = ci.m_dispatcher1; } diff --git a/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index b384abacd..610eab4ce 100644 --- a/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -29,17 +29,17 @@ class btPersistentManifold; struct btCollisionAlgorithmConstructionInfo { btCollisionAlgorithmConstructionInfo() - :m_dispatcher(0), + :m_dispatcher1(0), m_manifold(0) { } btCollisionAlgorithmConstructionInfo(btDispatcher* dispatcher,int temp) - :m_dispatcher(dispatcher) + :m_dispatcher1(dispatcher) { (void)temp; } - btDispatcher* m_dispatcher; + btDispatcher* m_dispatcher1; btPersistentManifold* m_manifold; int getDispatcherId(); diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 4e36293cd..daea11f77 100644 --- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -81,12 +81,18 @@ public: virtual bool needsResponse(btCollisionObject* body0,btCollisionObject* body1)=0; - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo)=0; + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher)=0; virtual int getNumManifolds() const = 0; virtual btPersistentManifold* getManifoldByIndexInternal(int index) = 0; + virtual btPersistentManifold** getInternalManifoldPointer() = 0; + + virtual void* allocateCollisionAlgorithm(int size) = 0; + + virtual void freeCollisionAlgorithm(void* ptr) = 0; + }; diff --git a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp index 98c57acdb..0e50b9749 100644 --- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @@ -76,7 +76,7 @@ btBroadphaseProxy* btMultiSapBroadphase::createProxy( const btVector3& aabbMin, return proxy; } -void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* proxy) +void btMultiSapBroadphase::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) { ///not yet btAssert(0); @@ -97,9 +97,9 @@ void btMultiSapBroadphase::setAabb(btBroadphaseProxy* proxy,const btVector3& aab } ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb -void btMultiSapBroadphase::calculateOverlappingPairs() +void btMultiSapBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { - m_simpleBroadphase->calculateOverlappingPairs(); + m_simpleBroadphase->calculateOverlappingPairs(dispatcher); btBroadphasePairArray& overlappingPairArray = m_overlappingPairs->getOverlappingPairArray(); @@ -149,7 +149,7 @@ void btMultiSapBroadphase::calculateOverlappingPairs() if (needsRemoval) { - m_overlappingPairs->cleanOverlappingPair(pair); + m_overlappingPairs->cleanOverlappingPair(pair,dispatcher); // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); // m_overlappingPairArray.pop_back(); diff --git a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h index 60ebc61ef..a717a932c 100644 --- a/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h +++ b/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.h @@ -96,11 +96,11 @@ public: virtual ~btMultiSapBroadphase(); virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr, short int collisionFilterGroup,short int collisionFilterMask); - virtual void destroyProxy(btBroadphaseProxy* proxy); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax); ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb - virtual void calculateOverlappingPairs(); + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp index a2813a811..5642fac41 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp @@ -37,7 +37,7 @@ btOverlappingPairCache::~btOverlappingPairCache() } -void btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair) +void btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair, btDispatcher* dispatcher ) { int findIndex = m_overlappingPairArray.findLinearSearch(findPair); @@ -45,7 +45,7 @@ void btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair) { gOverlappingPairs--; btBroadphasePair& pair = m_overlappingPairArray[findIndex]; - cleanOverlappingPair(pair); + cleanOverlappingPair(pair,dispatcher); m_overlappingPairArray.swap(findIndex,m_overlappingPairArray.size()-1); m_overlappingPairArray.pop_back(); @@ -53,12 +53,13 @@ void btOverlappingPairCache::removeOverlappingPair(btBroadphasePair& findPair) } -void btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair) +void btOverlappingPairCache::cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher) { if (pair.m_algorithm) { { - delete pair.m_algorithm;; + pair.m_algorithm->~btCollisionAlgorithm(); + dispatcher->freeCollisionAlgorithm(pair.m_algorithm); pair.m_algorithm=0; } } @@ -109,18 +110,20 @@ void btOverlappingPairCache::addOverlappingPair(btBroadphaseProxy* proxy0,btBroa -void btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy) +void btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher) { class CleanPairCallback : public btOverlapCallback { btBroadphaseProxy* m_cleanProxy; btOverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; public: - CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache) + CleanPairCallback(btBroadphaseProxy* cleanProxy,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :m_cleanProxy(cleanProxy), - m_pairCache(pairCache) + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } virtual bool processOverlap(btBroadphasePair& pair) @@ -128,22 +131,22 @@ void btOverlappingPairCache::cleanProxyFromPairs(btBroadphaseProxy* proxy) if ((pair.m_pProxy0 == m_cleanProxy) || (pair.m_pProxy1 == m_cleanProxy)) { - m_pairCache->cleanOverlappingPair(pair); + m_pairCache->cleanOverlappingPair(pair,m_dispatcher); } return false; } }; - CleanPairCallback cleanPairs(proxy,this); + CleanPairCallback cleanPairs(proxy,this,dispatcher); - processAllOverlappingPairs(&cleanPairs); + processAllOverlappingPairs(&cleanPairs,dispatcher); } -void btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy) +void btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) { class RemovePairCallback : public btOverlapCallback @@ -166,12 +169,12 @@ void btOverlappingPairCache::removeOverlappingPairsContainingProxy(btBroadphaseP RemovePairCallback removeCallback(proxy); - processAllOverlappingPairs(&removeCallback); + processAllOverlappingPairs(&removeCallback,dispatcher); } -void btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback) +void btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher) { int i; @@ -182,7 +185,7 @@ void btOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callb btBroadphasePair* pair = &m_overlappingPairArray[i]; if (callback->processOverlap(*pair)) { - cleanOverlappingPair(*pair); + cleanOverlappingPair(*pair,dispatcher); m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); m_overlappingPairArray.pop_back(); diff --git a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index f207db9b8..3cb01e0f8 100644 --- a/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -22,7 +22,7 @@ subject to the following restrictions: #include "btBroadphaseProxy.h" #include "LinearMath/btPoint3.h" #include "LinearMath/btAlignedObjectArray.h" - +class btDispatcher; struct btOverlapCallback { @@ -61,20 +61,20 @@ class btOverlappingPairCache btOverlappingPairCache(); virtual ~btOverlappingPairCache(); - virtual void processAllOverlappingPairs(btOverlapCallback*); + virtual void processAllOverlappingPairs(btOverlapCallback*,btDispatcher* dispatcher); - void removeOverlappingPair(btBroadphasePair& pair); + void removeOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); - void cleanOverlappingPair(btBroadphasePair& pair); + void cleanOverlappingPair(btBroadphasePair& pair,btDispatcher* dispatcher); void addOverlappingPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); btBroadphasePair* findPair(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); - void cleanProxyFromPairs(btBroadphaseProxy* proxy); + void cleanProxyFromPairs(btBroadphaseProxy* proxy,btDispatcher* dispatcher); - void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy); + void removeOverlappingPairsContainingProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); inline bool needsBroadphaseCollision(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) const diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index b3fff7866..ef4cff3d8 100644 --- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -139,7 +139,7 @@ protected: }; }; -void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg) +void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg,btDispatcher* dispatcher) { int i; @@ -151,7 +151,7 @@ void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg) btAssert (index < m_maxProxies); m_freeProxies[--m_firstFreeProxy] = index; - m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg); + m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg,dispatcher); for (i=0;icleanOverlappingPair(pair); + m_pairCache->cleanOverlappingPair(pair,dispatcher); // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); // m_overlappingPairArray.pop_back(); diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index 07b91d254..1cd4bde08 100644 --- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -82,9 +82,9 @@ public: virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask); - virtual void calculateOverlappingPairs(); + virtual void calculateOverlappingPairs(btDispatcher* dispatcher); - virtual void destroyProxy(btBroadphaseProxy* proxy); + virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax); btOverlappingPairCache* getOverlappingPairCache() diff --git a/src/BulletCollision/CMakeLists.txt b/src/BulletCollision/CMakeLists.txt index f4c9b9693..9c6ed5d2f 100644 --- a/src/BulletCollision/CMakeLists.txt +++ b/src/BulletCollision/CMakeLists.txt @@ -16,6 +16,7 @@ ADD_LIBRARY(LibBulletCollision CollisionDispatch/btCollisionWorld.cpp CollisionDispatch/btCompoundCollisionAlgorithm.cpp CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp + CollisionDispatch/btDefaultCollisionConfiguration.cpp CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp CollisionDispatch/btConvexConvexAlgorithm.cpp diff --git a/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index 7471ecf93..807512100 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -15,6 +15,8 @@ subject to the following restrictions: #ifndef BT_COLLISION_CONFIGURATION #define BT_COLLISION_CONFIGURATION +struct btCollisionAlgorithmCreateFunc; + ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator size, default collision algorithms and persistent manifold pool size ///todo: describe the meaning @@ -28,7 +30,7 @@ public: } ///pool size for the persistent contact manifold - virtual int getMaxPersistentManifoldPoolSize() = 0; + virtual int getPersistentManifoldPoolSize() = 0; virtual int getStackAllocatorSize() = 0; @@ -36,17 +38,7 @@ public: virtual int getCollisionAlgorithmMaxElementSize() = 0; - virtual btCollisionAlgorithmCreateFunc* getConvexConvexCollisionCreateFunc() = 0; - - virtual btCollisionAlgorithmCreateFunc* getConvexConcaveCollisionCreateFunc() = 0; - - virtual btCollisionAlgorithmCreateFunc* getSwappedConvexConcaveCollisionCreateFunc() = 0; - - virtual btCollisionAlgorithmCreateFunc* getCompoundCollisionCreateFunc() = 0; - - virtual btCollisionAlgorithmCreateFunc* getSwappedCompoundCollisionCreateFunc() = 0; - - virtual btCollisionAlgorithmCreateFunc* getEmptyCollisionCreateFunc() = 0; + virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; }; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index b535fac65..84ef418e5 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -19,69 +19,40 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" -#include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" + #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h" +#include "LinearMath/btPoolAllocator.h" +#include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h" int gNumManifold = 0; #include - -btCollisionDispatcher::btCollisionDispatcher(bool noDefaultAlgorithms): -m_count(0), -m_useIslands(true), -m_convexConvexCreateFunc(0), -m_convexConcaveCreateFunc(0), -m_swappedConvexConcaveCreateFunc(0), -m_compoundCreateFunc(0), -m_swappedCompoundCreateFunc(0), -m_emptyCreateFunc(0) -{ - (void)noDefaultAlgorithms; - int i; - setNearCallback(defaultNearCallback); - m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc; - for (i=0;igetCollisionAlgorithmMaxElementSize(),m_collisionConfiguration->getCollisionAlgorithmPoolSize()); + + m_persistentManifoldPoolAllocator = new btPoolAllocator(sizeof(btPersistentManifold),m_collisionConfiguration->getPersistentManifoldPoolSize()); for (i=0;igetCollisionAlgorithmCreateFunc(i,j); assert(m_doubleDispatch[i][j]); } } @@ -89,8 +60,6 @@ btCollisionDispatcher::btCollisionDispatcher (): }; -#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION - void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) { @@ -99,12 +68,7 @@ void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int prox btCollisionDispatcher::~btCollisionDispatcher() { - delete m_convexConvexCreateFunc; - delete m_convexConcaveCreateFunc; - delete m_swappedConvexConcaveCreateFunc; - delete m_compoundCreateFunc; - delete m_swappedCompoundCreateFunc; - delete m_emptyCreateFunc; + } btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) @@ -117,7 +81,8 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) btCollisionObject* body0 = (btCollisionObject*)b0; btCollisionObject* body1 = (btCollisionObject*)b1; - btPersistentManifold* manifold = new btPersistentManifold (body0,body1); + void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); + btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0); m_manifoldsPtr.push_back(manifold); return manifold; @@ -143,7 +108,9 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) { m_manifoldsPtr.swap(findIndex,m_manifoldsPtr.size()-1); m_manifoldsPtr.pop_back(); - delete manifold; + + manifold->~btPersistentManifold(); + m_persistentManifoldPoolAllocator->free(manifold); } } @@ -156,7 +123,8 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo #ifdef USE_DISPATCH_REGISTRY_ARRAY btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher = this; + + ci.m_dispatcher1 = this; ci.m_manifold = sharedManifold; btCollisionAlgorithm* algo = m_doubleDispatch[body0->getCollisionShape()->getShapeType()][body1->getCollisionShape()->getShapeType()] ->CreateCollisionAlgorithm(ci,body0,body1); @@ -167,42 +135,6 @@ btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(btCollisionObject* bo } -#ifndef BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION - -btCollisionAlgorithmCreateFunc* btCollisionDispatcher::internalFindCreateFunc(int proxyType0,int proxyType1) -{ - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) - { - return m_convexConvexCreateFunc; - } - - if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) - { - return m_convexConcaveCreateFunc; - } - - if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) - { - return m_swappedConvexConcaveCreateFunc; - } - - if (btBroadphaseProxy::isCompound(proxyType0)) - { - return m_compoundCreateFunc; - } else - { - if (btBroadphaseProxy::isCompound(proxyType1)) - { - return m_swappedCompoundCreateFunc; - } - } - - //failed to find an algorithm - return m_emptyCreateFunc; -} - -#endif //BT_EXCLUDE_DEFAULT_COLLISIONALGORITHM_REGISTRATION #ifndef USE_DISPATCH_REGISTRY_ARRAY @@ -316,13 +248,13 @@ public: }; -void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo) +void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher) { //m_blockedForChanges = true; btCollisionPairCallback collisionCallback(dispatchInfo,this); - pairCache->processAllOverlappingPairs(&collisionCallback); + pairCache->processAllOverlappingPairs(&collisionCallback,dispatcher); //m_blockedForChanges = false; @@ -365,3 +297,14 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, } } + + +void* btCollisionDispatcher::allocateCollisionAlgorithm(int size) +{ + return m_collisionAlgorithmPoolAllocator->allocate(size); +} + +void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr) +{ + m_collisionAlgorithmPoolAllocator->free(ptr); +} \ No newline at end of file diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 600eb90e4..2a92c5dac 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -26,7 +26,8 @@ subject to the following restrictions: class btIDebugDraw; class btOverlappingPairCache; - +class btPoolAllocator; +class btCollisionConfiguration; #include "btCollisionCreateFunc.h" @@ -51,21 +52,15 @@ class btCollisionDispatcher : public btDispatcher btNearCallback m_nearCallback; + btPoolAllocator* m_collisionAlgorithmPoolAllocator; + + btPoolAllocator* m_persistentManifoldPoolAllocator; + btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; - btCollisionAlgorithmCreateFunc* internalFindCreateFunc(int proxyType0,int proxyType1); - //default CreationFunctions, filling the m_doubleDispatch table - btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; - btCollisionAlgorithmCreateFunc* m_convexConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; - btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; - btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; - btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; + btCollisionConfiguration* m_collisionConfiguration; -#ifndef USE_DISPATCH_REGISTRY_ARRAY - btCollisionAlgorithm* internalFindAlgorithm(btCollisionObject* body0,btCollisionObject* body1,btPersistentManifold* sharedManifold = 0); -#endif //USE_DISPATCH_REGISTRY_ARRAY public: @@ -92,11 +87,7 @@ public: return m_manifoldsPtr[index]; } - ///the default constructor creates/register default collision algorithms, for convex, compound and concave shape support - btCollisionDispatcher (); - - ///a special constructor that doesn't create/register the default collision algorithms - btCollisionDispatcher(bool noDefaultAlgorithms); + btCollisionDispatcher (btCollisionConfiguration* collisionConfiguration); virtual ~btCollisionDispatcher(); @@ -114,7 +105,7 @@ public: virtual bool needsResponse(btCollisionObject* body0,btCollisionObject* body1); - virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo); + virtual void dispatchAllCollisionPairs(btOverlappingPairCache* pairCache,btDispatcherInfo& dispatchInfo,btDispatcher* dispatcher); void setNearCallback(btNearCallback nearCallback) { @@ -129,6 +120,11 @@ public: //by default, Bullet will use this near callback static void defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, btDispatcherInfo& dispatchInfo); + virtual void* allocateCollisionAlgorithm(int size); + + virtual void freeCollisionAlgorithm(void* ptr); + + }; #endif //COLLISION__DISPATCHER_H diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index cf85b2367..5e832a6e8 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -66,8 +66,8 @@ btCollisionWorld::~btCollisionWorld() // // only clear the cached algorithms // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp); - getBroadphase()->destroyProxy(bp); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); + getBroadphase()->destroyProxy(bp,m_dispatcher1); } } @@ -137,7 +137,7 @@ void btCollisionWorld::performDiscreteCollisionDetection() m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax); } - m_broadphasePairCache->calculateOverlappingPairs(); + m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); END_PROFILE("perform Broadphase Collision Detection"); @@ -145,7 +145,7 @@ void btCollisionWorld::performDiscreteCollisionDetection() btDispatcher* dispatcher = getDispatcher(); if (dispatcher) - dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo); + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); END_PROFILE("performDiscreteCollisionDetection"); @@ -166,8 +166,8 @@ void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject) // // only clear the cached algorithms // - getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp); - getBroadphase()->destroyProxy(bp); + getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1); + getBroadphase()->destroyProxy(bp,m_dispatcher1); collisionObject->setBroadphaseHandle(0); } } @@ -212,19 +212,28 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt btConvexShape* convexShape = (btConvexShape*) collisionShape; btVoronoiSimplexSolver simplexSolver; +#define USE_SUBSIMPLEX_CONVEX_CAST 1 +#ifdef USE_SUBSIMPLEX_CONVEX_CAST btSubsimplexConvexCast convexCaster(castShape,convexShape,&simplexSolver); +#else //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); - +#endif //#USE_SUBSIMPLEX_CONVEX_CAST + if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { //add hit if (castResult.m_normal.length2() > btScalar(0.0001)) { - castResult.m_normal.normalize(); + if (castResult.m_fraction < resultCallback.m_closestHitFraction) { +#ifdef USE_SUBSIMPLEX_CONVEX_CAST + //rotate normal into worldspace + castResult.m_normal = rayFromTrans.getBasis() * castResult.m_normal; +#endif //USE_SUBSIMPLEX_CONVEX_CAST + castResult.m_normal.normalize(); btCollisionWorld::LocalRayResult localRayResult ( collisionObject, diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index f1aa36ee3..546cefc80 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -98,7 +98,7 @@ protected: public: //this constructor doesn't own the dispatcher and paircache/broadphase - btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, int stackSize = 2*1024*1024); + btCollisionWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphasePairCache, int stackSize = 10*1024*1024); virtual ~btCollisionWorld(); diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 92f4c8b28..a0f19d486 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -35,7 +35,7 @@ btCompoundCollisionAlgorithm::btCompoundCollisionAlgorithm( const btCollisionAlg btCollisionShape* childShape = compoundShape->getChildShape(i); btCollisionShape* orgShape = colObj->getCollisionShape(); colObj->setCollisionShape( childShape ); - m_childCollisionAlgorithms[i] = ci.m_dispatcher->findAlgorithm(colObj,otherObj); + m_childCollisionAlgorithms[i] = ci.m_dispatcher1->findAlgorithm(colObj,otherObj); colObj->setCollisionShape( orgShape ); } } @@ -47,7 +47,8 @@ btCompoundCollisionAlgorithm::~btCompoundCollisionAlgorithm() int i; for (i=0;i~btCollisionAlgorithm(); + m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); } } diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 43b49364e..a381d8b3c 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -25,6 +25,7 @@ class btDispatcher; #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "btCollisionCreateFunc.h" #include "LinearMath/btAlignedObjectArray.h" +class btDispatcher; /// btCompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes /// Place holder, not fully implemented yet @@ -47,7 +48,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btCompoundCollisionAlgorithm(ci,body0,body1,false); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); + return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,false); } }; @@ -55,7 +57,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btCompoundCollisionAlgorithm(ci,body0,body1,true); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btCompoundCollisionAlgorithm)); + return new(mem) btCompoundCollisionAlgorithm(ci,body0,body1,true); } }; diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 24ceacfd4..8d7e81af4 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -29,7 +29,7 @@ subject to the following restrictions: btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1,bool isSwapped) : btCollisionAlgorithm(ci), m_isSwapped(isSwapped), -m_btConvexTriangleCallback(ci.m_dispatcher,body0,body1,isSwapped) +m_btConvexTriangleCallback(ci.m_dispatcher1,body0,body1,isSwapped) { } @@ -79,7 +79,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i //aabb filter is already applied! btCollisionAlgorithmConstructionInfo ci; - ci.m_dispatcher = m_dispatcher; + ci.m_dispatcher1 = m_dispatcher; btCollisionObject* ob = static_cast(m_triBody); @@ -115,7 +115,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i ob->setCollisionShape( &tm ); - btCollisionAlgorithm* colAlgo = ci.m_dispatcher->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); ///this should use the btDispatcher, so the actual registered algorithm is used // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); @@ -123,7 +123,8 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); - delete colAlgo; + colAlgo->~btCollisionAlgorithm(); + ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); ob->setCollisionShape( tmpShape ); } diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index 8df1b0f48..4f7ba99bf 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -94,7 +94,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,false); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); + return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,false); } }; @@ -102,7 +103,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btConvexConcaveCollisionAlgorithm(ci,body0,body1,true); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConcaveCollisionAlgorithm)); + return new(mem) btConvexConcaveCollisionAlgorithm(ci,body0,body1,true); } }; diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index a8789f611..acc1e1cf7 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -22,6 +22,7 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" class btConvexPenetrationDepthSolver; @@ -66,7 +67,8 @@ public: virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); + return new(mem) btConvexConvexAlgorithm(ci.m_manifold,ci,body0,body1,m_simplexSolver,m_pdSolver); } }; diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 0805c31f7..9fb18eb2e 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" btDefaultCollisionConfiguration::btDefaultCollisionConfiguration() :m_persistentManifoldPoolSize(16384), @@ -35,6 +37,11 @@ m_collisionAlgorithmMaxElementSize(0) m_compoundCreateFunc = new btCompoundCollisionAlgorithm::CreateFunc; m_swappedCompoundCreateFunc = new btCompoundCollisionAlgorithm::SwappedCreateFunc; m_emptyCreateFunc = new btEmptyAlgorithm::CreateFunc; + m_sphereSphereCF = new btSphereSphereCollisionAlgorithm::CreateFunc; + m_sphereBoxCF = new btSphereBoxCollisionAlgorithm::CreateFunc; + m_boxSphereCF = new btSphereBoxCollisionAlgorithm::CreateFunc; + m_boxSphereCF->m_swapped = true; + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool int maxSize = sizeof(btConvexConvexAlgorithm); @@ -43,8 +50,8 @@ m_collisionAlgorithmMaxElementSize(0) int maxSize4 = sizeof(btEmptyAlgorithm); m_collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2); - m_collisionAlgorithmMaxElementSize = btMax(elemSize,maxSize3); - m_collisionAlgorithmMaxElementSize = btMax(elemSize,maxSize4); + m_collisionAlgorithmMaxElementSize = btMax(m_collisionAlgorithmMaxElementSize,maxSize3); + m_collisionAlgorithmMaxElementSize = btMax(m_collisionAlgorithmMaxElementSize,maxSize4); } @@ -56,6 +63,9 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() delete m_compoundCreateFunc; delete m_swappedCompoundCreateFunc; delete m_emptyCreateFunc; + delete m_sphereSphereCF; + delete m_sphereBoxCF; + delete m_boxSphereCF; } @@ -81,32 +91,52 @@ int btDefaultCollisionConfiguration::getCollisionAlgorithmMaxElementSize() } -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getConvexConvexCollisionCreateFunc() -{ - return m_convexConvexCreateFunc; -} -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getConvexConcaveCollisionCreateFunc() +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) { - return m_convexConcaveCreateFunc; -} + -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getSwappedConvexConcaveCollisionCreateFunc() -{ - return m_swappedConvexConcaveCreateFunc; -} + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_sphereSphereCF; + } -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCompoundCollisionCreateFunc() -{ - return m_compoundCreateFunc; -} + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; + } -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getSwappedCompoundCollisionCreateFunc() -{ - return m_swappedCompoundCreateFunc; -} + if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_boxSphereCF; + } -btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getEmptyCollisionCreateFunc() -{ + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) + { + return m_convexConvexCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) + { + return m_convexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) + { + return m_swappedConvexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isCompound(proxyType0)) + { + return m_compoundCreateFunc; + } else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } + } + + //failed to find an algorithm return m_emptyCreateFunc; } diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 2377140d0..1cf8f4d81 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -21,16 +21,16 @@ subject to the following restrictions: ///btCollisionConfiguration allows to configure Bullet collision detection ///stack allocator size, default collision algorithms and persistent manifold pool size ///todo: describe the meaning -class btDefaultCollisionConfiguration +class btDefaultCollisionConfiguration : public btCollisionConfiguration { int m_persistentManifoldPoolSize; int m_stackAllocatorSize; - int collisionAlgorithmPoolSize; + int m_collisionAlgorithmPoolSize; - int collisionAlgorithmMaxElementSize; + int m_collisionAlgorithmMaxElementSize; //default CreationFunctions, filling the m_doubleDispatch table btCollisionAlgorithmCreateFunc* m_convexConvexCreateFunc; @@ -38,7 +38,10 @@ class btDefaultCollisionConfiguration btCollisionAlgorithmCreateFunc* m_swappedConvexConcaveCreateFunc; btCollisionAlgorithmCreateFunc* m_compoundCreateFunc; btCollisionAlgorithmCreateFunc* m_swappedCompoundCreateFunc; - btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; + btCollisionAlgorithmCreateFunc* m_emptyCreateFunc; + btCollisionAlgorithmCreateFunc* m_sphereSphereCF; + btCollisionAlgorithmCreateFunc* m_sphereBoxCF; + btCollisionAlgorithmCreateFunc* m_boxSphereCF; public: @@ -55,18 +58,22 @@ public: virtual int getCollisionAlgorithmMaxElementSize(); - virtual btCollisionAlgorithmCreateFunc* getConvexConvexCollisionCreateFunc(); + btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); - virtual btCollisionAlgorithmCreateFunc* getConvexConcaveCollisionCreateFunc(); + void setStackAllocatorSize(int size) + { + m_stackAllocatorSize = size; + } - virtual btCollisionAlgorithmCreateFunc* getSwappedConvexConcaveCollisionCreateFunc(); - - virtual btCollisionAlgorithmCreateFunc* getCompoundCollisionCreateFunc(); - - virtual btCollisionAlgorithmCreateFunc* getSwappedCompoundCollisionCreateFunc(); - - virtual btCollisionAlgorithmCreateFunc* getEmptyCollisionCreateFunc(); + void setPersistentManifoldPoolSize(int size) + { + m_persistentManifoldPoolSize = size; + } + void setCollisionAlgorithmPoolSize(int size) + { + m_collisionAlgorithmPoolSize = size; + } }; #endif //BT_DEFAULT_COLLISION_CONFIGURATION \ No newline at end of file diff --git a/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index fc4aeea19..89e708078 100644 --- a/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -17,6 +17,7 @@ subject to the following restrictions: #define EMPTY_ALGORITH #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" #define ATTRIBUTE_ALIGNED(a) @@ -39,7 +40,8 @@ public: { (void)body0; (void)body1; - return new btEmptyAlgorithm(ci); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btEmptyAlgorithm)); + return new(mem) btEmptyAlgorithm(ci); } }; diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 490acc0b6..3cdef20cc 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -85,6 +85,17 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b newPt.m_combinedFriction = calculateCombinedFriction(m_body0,m_body1); newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0,m_body1); + + ///todo, check this for any side effects + if (insertIndex >= 0) + { + //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); + m_manifoldPtr->replaceContactPoint(newPt,insertIndex); + } else + { + m_manifoldPtr->AddManifoldPoint(newPt); + } + //User can override friction and/or restitution if (gContactAddedCallback && //and if either of the two bodies requires custom material @@ -97,13 +108,5 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b (*gContactAddedCallback)(newPt,obj0,m_partId0,m_index0,obj1,m_partId1,m_index1); } - if (insertIndex >= 0) - { - //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); - m_manifoldPtr->replaceContactPoint(newPt,insertIndex); - } else - { - m_manifoldPtr->AddManifoldPoint(newPt); - } } diff --git a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index 479080278..de73459bf 100644 --- a/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -138,19 +138,6 @@ class btPersistentManifoldSortPredicate void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback) { - - - /*if (0) - { - int maxNumManifolds = dispatcher->getNumManifolds(); - btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher; - btPersistentManifold** manifold = colDis->getInternalManifoldPointer(); - callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0); - return; - } - */ - - BEGIN_PROFILE("islandUnionFindAndHeapSort"); //we are going to sort the unionfind array, and store the element id in the size @@ -247,11 +234,18 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, } } - btAlignedObjectArray islandmanifold; + int i; int maxNumManifolds = dispatcher->getNumManifolds(); - islandmanifold.reserve(maxNumManifolds); +#define SPLIT_ISLANDS 1 +#ifdef SPLIT_ISLANDS + + btAlignedObjectArray islandmanifold; + islandmanifold.reserve(maxNumManifolds); +#endif //SPLIT_ISLANDS + + for (i=0;igetManifoldByIndexInternal(i); @@ -273,19 +267,25 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, { colObj0->activate(); } - - //filtering for response +#ifdef SPLIT_ISLANDS + // //filtering for response if (dispatcher->needsResponse(colObj0,colObj1)) islandmanifold.push_back(manifold); +#endif //SPLIT_ISLANDS } } - int numManifolds = int (islandmanifold.size()); - +#ifndef SPLIT_ISLANDS + btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); + + callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); +#else // Sort manifolds, based on islands // Sort the vector using predicate and std::sort //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + int numManifolds = int (islandmanifold.size()); + //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) islandmanifold.heapSort(btPersistentManifoldSortPredicate()); @@ -352,6 +352,6 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, islandBodies.resize(0); } - +#endif //SPLIT_ISLANDS } diff --git a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 83cf52c43..ffb8cacf0 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -20,6 +20,8 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; +#include "btCollisionDispatcher.h" + #include "LinearMath/btVector3.h" /// btSphereBoxCollisionAlgorithm provides sphere-box collision detection. @@ -48,12 +50,13 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereBoxCollisionAlgorithm)); if (!m_swapped) { - return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false); + return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,false); } else { - return new btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true); + return new(mem) btSphereBoxCollisionAlgorithm(0,ci,body0,body1,true); } } }; diff --git a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 488a159f3..d54c9d5a7 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -19,6 +19,8 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" +#include "btCollisionDispatcher.h" + class btPersistentManifold; /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. @@ -46,7 +48,8 @@ public: { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btSphereSphereCollisionAlgorithm(0,ci,body0,body1); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereSphereCollisionAlgorithm)); + return new(mem) btSphereSphereCollisionAlgorithm(0,ci,body0,body1); } }; diff --git a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 26d9f4d9b..40d1b12e8 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -20,6 +20,7 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" #include "BulletCollision/CollisionDispatch/btCollisionCreateFunc.h" class btPersistentManifold; +#include "btCollisionDispatcher.h" /// btSphereSphereCollisionAlgorithm provides sphere-sphere collision detection. /// Other features are frame-coherency (persistent data) and collision response. @@ -49,7 +50,9 @@ public: virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) { - return new btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped); + void* mem = ci.m_dispatcher1->allocateCollisionAlgorithm(sizeof(btSphereTriangleCollisionAlgorithm)); + + return new(mem) btSphereTriangleCollisionAlgorithm(ci.m_manifold,ci,body0,body1,m_swapped); } }; diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 44438a244..172d664bc 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -19,10 +19,31 @@ subject to the following restrictions: #include "LinearMath/btIDebugDraw.h" +inline bool testQuantizedAabbAgainstQuantizedAabb2(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) +{ + bool overlap = true; + overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; + overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; + overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; + return overlap; +} + + + +///Branch-free version of quantized aabb versus quantized aabb +inline unsigned testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) +{ + return btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) + & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) + & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])), + 1, 0); +} + + btOptimizedBvh::btOptimizedBvh() : m_useQuantization(false), m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY) - //m_traversalMode(TRAVERSAL_STACKLESS) +// m_traversalMode(TRAVERSAL_STACKLESS) //m_traversalMode(TRAVERSAL_RECURSIVE) { @@ -170,6 +191,9 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized subtree.m_rootNodeIndex = 0; subtree.m_subtreeSize = m_quantizedContiguousNodes[0].isLeafNode() ? 1 : m_quantizedContiguousNodes[0].getEscapeIndex(); } + + m_leafNodes.clear(); + m_quantizedLeafNodes.clear(); } @@ -201,7 +225,7 @@ void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const b { btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; - bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + unsigned int overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); if (overlap) { updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i); @@ -668,7 +692,7 @@ void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize { btAssert(m_useQuantization); - bool aabbOverlap, isLeafNode; + unsigned int aabbOverlap, isLeafNode; aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,currentNode->m_quantizedAabbMin,currentNode->m_quantizedAabbMax); isLeafNode = currentNode->isLeafNode(); @@ -707,7 +731,7 @@ void btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallb const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; int escapeIndex; - bool aabbOverlap, isLeafNode; + unsigned int aabbOverlap, isLeafNode; while (curIndex < endNodeIndex) { @@ -768,7 +792,7 @@ void btOptimizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallba { const btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; - bool overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + unsigned int overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); if (overlap) { walkStacklessQuantizedTree(nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax, diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 35f730008..b069c7c5d 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -30,6 +30,7 @@ class btStridingMeshInterface; #define MAX_SUBTREE_SIZE_IN_BYTES 2048 + ///btQuantizedBvhNode is a compressed aabb node, 16 bytes. ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). ATTRIBUTE_ALIGNED16 (struct) btQuantizedBvhNode @@ -275,14 +276,7 @@ protected: void walkRecursiveQuantizedTreeAgainstQuantizedTree(const btQuantizedBvhNode* treeNodeA,const btQuantizedBvhNode* treeNodeB,btNodeOverlapCallback* nodeCallback) const; - inline bool testQuantizedAabbAgainstQuantizedAabb(unsigned short int* aabbMin1,unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2) const - { - bool overlap = true; - overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap; - overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap; - overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap; - return overlap; - } + void updateSubtreeHeaders(int leftChildNodexIndex,int rightChildNodexIndex); diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 4e6e3ac07..48880b4ae 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -59,7 +59,7 @@ public: btPersistentManifold(); - btPersistentManifold(void* body0,void* body1) + btPersistentManifold(void* body0,void* body1,int bla) : m_body0(body0),m_body1(body1),m_cachedPoints(0) { } diff --git a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 6ad58073d..addfb67a8 100644 --- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -26,7 +26,7 @@ struct btContactSolverInfo; struct btBroadphaseProxy; class btIDebugDraw; class btStackAlloc; - +class btDispatcher; /// btConstraintSolver provides solver interface class btConstraintSolver { @@ -38,7 +38,7 @@ public: virtual void prepareSolve (int numBodies, int numManifolds) {;} ///solve a group of constraints - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0; + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0; virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) {;} diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 285361b82..dd15c8deb 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -29,7 +29,7 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "btSolverBody.h" #include "btSolverConstraint.h" - +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "LinearMath/btAlignedObjectArray.h" #ifdef USE_PROFILE @@ -138,6 +138,7 @@ void initSolverBody(btSolverBody* solverBody, btRigidBody* rigidbody) solverBody->m_invMass = rigidbody->getInvMass(); solverBody->m_linearVelocity = rigidbody->getLinearVelocity(); solverBody->m_originalBody = rigidbody; + btAssert(rigidbody); solverBody->m_angularFactor = rigidbody->getAngularFactor(); } @@ -333,12 +334,10 @@ btScalar resolveSingleFrictionCacheFriendly( #endif //NO_FRICTION_TANGENTIALS -btAlignedObjectArray tmpSolverBodyPool; -btAlignedObjectArray tmpSolverConstraintPool; -btAlignedObjectArray tmpSolverFrictionConstraintPool; -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) + +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher) { (void)stackAlloc; (void)debugDrawer; @@ -351,15 +350,36 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio BEGIN_PROFILE("refreshManifolds"); + int numActiveBodies = 0; + + for (int i=0;igetIslandTag() >= 0)) + { + numActiveBodies++; + } + } + + int numActiveManifolds = 0; + int totalContacts = 0; + int i; for (i=0;igetBody0(); btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); - - manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); - + if (((rb0) && rb0->getActivationState() != ISLAND_SLEEPING) || + ((rb1) && rb1->getActivationState() != ISLAND_SLEEPING)) + { + if (dispatcher->needsResponse(rb0,rb1)) + { + manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); + totalContacts += manifold->getNumContacts(); + numActiveManifolds++; + } + } } END_PROFILE("refreshManifolds"); @@ -367,24 +387,33 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio BEGIN_PROFILE("gatherSolverData"); + btBlock* sablock; + sablock = stackAlloc->beginBlock(); + + + int tmpSolverBodyPoolSize = 0; + int mem1 = sizeof(btSolverBody) * numActiveBodies; + btSolverBody* tmpSolverBodyPool = (btSolverBody*) stackAlloc->allocate(sizeof(btSolverBody) * numActiveBodies*2); + int tmpSolverConstraintPoolSize = 0; + int mem2 = sizeof(btSolverConstraint)*numActiveManifolds; + + btSolverConstraint* tmpSolverConstraintPool = (btSolverConstraint*) stackAlloc->allocate(sizeof(btSolverConstraint)*totalContacts); + + int tmpSolverFrictionConstraintPoolSize = 0; + btSolverConstraint* tmpSolverFrictionConstraintPool = (btSolverConstraint*) stackAlloc->allocate(sizeof(btSolverConstraint)*totalContacts*2); + //int sizeofSB = sizeof(btSolverBody); //int sizeofSC = sizeof(btSolverConstraint); //if (1) { - //if m_stackAlloc, try to pack bodies/constraints to speed up solving -// btBlock* sablock; -// sablock = stackAlloc->beginBlock(); - // int memsize = 16; -// unsigned char* stackMemory = stackAlloc->allocate(memsize); //todo: use stack allocator for this temp memory int minReservation = numManifolds*2; - tmpSolverBodyPool.reserve(minReservation); { for (int i=0;igetIslandTag() >= 0)) { btAssert(rb->getCompanionId() < 0); - int solverBodyId = tmpSolverBodyPool.size(); - btSolverBody& solverBody = tmpSolverBodyPool.expand(); + int solverBodyId = tmpSolverBodyPoolSize; + btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++]; initSolverBody(&solverBody,rb); rb->setCompanionId(solverBodyId); } @@ -402,8 +431,6 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio } - tmpSolverConstraintPool.reserve(minReservation); - tmpSolverFrictionConstraintPool.reserve(minReservation); { int i; @@ -413,187 +440,196 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio btRigidBody* rb0 = (btRigidBody*)manifold->getBody0(); btRigidBody* rb1 = (btRigidBody*)manifold->getBody1(); - - int solverBodyIdA=-1; - int solverBodyIdB=-1; - - if (manifold->getNumContacts()) + if (((rb0) && rb0->getActivationState() != ISLAND_SLEEPING) || + ((rb1) && rb1->getActivationState() != ISLAND_SLEEPING)) { + if (dispatcher->needsResponse(rb0,rb1)) + { - - if (rb0->getIslandTag() >= 0) - { - solverBodyIdA = rb0->getCompanionId(); - } else - { - //create a static body - solverBodyIdA = tmpSolverBodyPool.size(); - btSolverBody& solverBody = tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb0); - } + int solverBodyIdA=-1; + int solverBodyIdB=-1; - if (rb1->getIslandTag() >= 0) - { - solverBodyIdB = rb1->getCompanionId(); - } else - { - //create a static body - solverBodyIdB = tmpSolverBodyPool.size(); - btSolverBody& solverBody = tmpSolverBodyPool.expand(); - initSolverBody(&solverBody,rb1); + if (manifold->getNumContacts()) + { + + + + if (rb0->getIslandTag() >= 0) + { + solverBodyIdA = rb0->getCompanionId(); + } else + { + //create a static body + solverBodyIdA = tmpSolverBodyPoolSize; + btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++]; + initSolverBody(&solverBody,rb0); + } + + if (rb1->getIslandTag() >= 0) + { + solverBodyIdB = rb1->getCompanionId(); + } else + { + //create a static body + solverBodyIdB = tmpSolverBodyPoolSize; + btSolverBody& solverBody = tmpSolverBodyPool[tmpSolverBodyPoolSize++]; + initSolverBody(&solverBody,rb1); + } + } + + for (int j=0;jgetNumContacts();j++) + { + + btManifoldPoint& cp = manifold->getContactPoint(j); + + int frictionIndex = tmpSolverConstraintPoolSize; + + if (cp.getDistance() <= btScalar(0.)) + { + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); + btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition(); + + + btScalar relaxation = 1.f; + + { + btSolverConstraint& solverConstraint = tmpSolverConstraintPool[tmpSolverConstraintPoolSize++]; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; + + + + { + //can be optimized, the cross products are already calculated + btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + } + + solverConstraint.m_contactNormal = cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB); + + + btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2); + + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = cp.m_normalWorldOnB.dot(vel); + + + solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations); + solverConstraint.m_friction = cp.m_combinedFriction; + btScalar rest = restitutionCurve(rel_vel, cp.m_combinedRestitution); + if (rest <= btScalar(0.)) + { + rest = 0.f; + }; + + btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep; + if (rest > penVel) + { + rest = btScalar(0.); + } + solverConstraint.m_restitution = rest; + + solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep); + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedVelocityImpulse = 0.f; + + + btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0; + btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1; + } + + //create 2 '1d axis' constraints for 2 tangential friction directions + + //re-calculate friction direction every frame, todo: check if this is really needed + btVector3 frictionTangential0a, frictionTangential1b; + + btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b); + + { + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool[tmpSolverFrictionConstraintPoolSize++]; + solverConstraint.m_contactNormal = frictionTangential0a; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + + { + btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0; + } + { + btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis0; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0; + } + + } + + + { + + btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool[tmpSolverFrictionConstraintPoolSize++]; + solverConstraint.m_contactNormal = frictionTangential1b; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; + solverConstraint.m_frictionIndex = frictionIndex; + + solverConstraint.m_friction = cp.m_combinedFriction; + + solverConstraint.m_appliedImpulse = btScalar(0.); + solverConstraint.m_appliedVelocityImpulse = 0.f; + + btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; + { + btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1; + } + { + btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; + } + } + + } + } } } - for (int j=0;jgetNumContacts();j++) - { - - btManifoldPoint& cp = manifold->getContactPoint(j); - - int frictionIndex = tmpSolverConstraintPool.size(); - - if (cp.getDistance() <= btScalar(0.)) - { - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - btVector3 rel_pos1 = pos1 - rb0->getCenterOfMassPosition(); - btVector3 rel_pos2 = pos2 - rb1->getCenterOfMassPosition(); - - - btScalar relaxation = 1.f; - - { - btSolverConstraint& solverConstraint = tmpSolverConstraintPool.expand(); - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D; - - - - { - //can be optimized, the cross products are already calculated - btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - } - - solverConstraint.m_contactNormal = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB); - - - btVector3 vel1 = rb0->getVelocityInLocalPoint(rel_pos1); - btVector3 vel2 = rb1->getVelocityInLocalPoint(rel_pos2); - - btVector3 vel = vel1 - vel2; - btScalar rel_vel; - rel_vel = cp.m_normalWorldOnB.dot(vel); - - - solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations); - solverConstraint.m_friction = cp.m_combinedFriction; - btScalar rest = restitutionCurve(rel_vel, cp.m_combinedRestitution); - if (rest <= btScalar(0.)) - { - rest = 0.f; - }; - - btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep; - if (rest > penVel) - { - rest = btScalar(0.); - } - solverConstraint.m_restitution = rest; - - solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep); - - solverConstraint.m_appliedImpulse = 0.f; - solverConstraint.m_appliedVelocityImpulse = 0.f; - - - btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*torqueAxis0; - btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB); - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*torqueAxis1; - } - - //create 2 '1d axis' constraints for 2 tangential friction directions - - //re-calculate friction direction every frame, todo: check if this is really needed - btVector3 frictionTangential0a, frictionTangential1b; - - btPlaneSpace1(cp.m_normalWorldOnB,frictionTangential0a,frictionTangential1b); - - { - btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); - solverConstraint.m_contactNormal = frictionTangential0a; - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; - solverConstraint.m_frictionIndex = frictionIndex; - - solverConstraint.m_friction = cp.m_combinedFriction; - - solverConstraint.m_appliedImpulse = btScalar(0.); - solverConstraint.m_appliedVelocityImpulse = 0.f; - - btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - - { - btVector3 ftorqueAxis0 = rel_pos1.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos1CrossNormal = ftorqueAxis0; - solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis0; - } - { - btVector3 ftorqueAxis0 = rel_pos2.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos2CrossNormal = ftorqueAxis0; - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis0; - } - - } - - - { - - btSolverConstraint& solverConstraint = tmpSolverFrictionConstraintPool.expand(); - solverConstraint.m_contactNormal = frictionTangential1b; - - solverConstraint.m_solverBodyIdA = solverBodyIdA; - solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D; - solverConstraint.m_frictionIndex = frictionIndex; - - solverConstraint.m_friction = cp.m_combinedFriction; - - solverConstraint.m_appliedImpulse = btScalar(0.); - solverConstraint.m_appliedVelocityImpulse = 0.f; - - btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; - { - btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentA = rb0->getInvInertiaTensorWorld()*ftorqueAxis1; - } - { - btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal); - solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; - solverConstraint.m_angularComponentB = rb1->getInvInertiaTensorWorld()*ftorqueAxis1; - } - } - - } - } } } } @@ -612,15 +648,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio } } - btAlignedObjectArray gOrderTmpConstraintPool; - btAlignedObjectArray gOrderFrictionConstraintPool; + int numConstraintPool = tmpSolverConstraintPoolSize; + int numFrictionPool = tmpSolverFrictionConstraintPoolSize; - int numConstraintPool = tmpSolverConstraintPool.size(); - int numFrictionPool = tmpSolverFrictionConstraintPool.size(); + ///use the stack allocator for temporarily memory + + int gOrderTmpConstraintPoolSize = numConstraintPool; + int* gOrderTmpConstraintPool = (int*) stackAlloc->allocate(sizeof(int)*numConstraintPool); + int gOrderFrictionConstraintPoolSize = numFrictionPool; + int* gOrderFrictionConstraintPool = (int*) stackAlloc->allocate(sizeof(int)*numFrictionPool); - ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints - gOrderTmpConstraintPool.resize(numConstraintPool); - gOrderFrictionConstraintPool.resize(numFrictionPool); { int i; for (i=0;iendBlock(sablock); return 0.f; } /// btSequentialImpulseConstraintSolver Sequentially applies impulses -btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) +btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher) { if (getSolverMode() & SOLVER_CACHE_FRIENDLY) @@ -759,7 +782,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY btAssert(bodies); btAssert(numBodies); - return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); + return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc,dispatcher); } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 1d7221640..0efcff292 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -70,9 +70,9 @@ public: virtual ~btSequentialImpulseConstraintSolver() {} - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher); - virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher); ///clear internal cached data and reset random seed virtual void reset(); diff --git a/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/src/BulletDynamics/Dynamics/Bullet-C-API.cpp index 42c442c02..8356eeffb 100644 --- a/src/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ b/src/BulletDynamics/Dynamics/Bullet-C-API.cpp @@ -65,7 +65,8 @@ void plDeletePhysicsSdk(plPhysicsSdkHandle physicsSdk) plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle) { btPhysicsSdk* physicsSdk = reinterpret_cast(physicsSdkHandle); - btDispatcher* dispatcher = new btCollisionDispatcher(); + btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration(); + btDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration); btBroadphaseInterface* pairCache = new btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax); btConstraintSolver* constraintSolver = new btSequentialImpulseConstraintSolver(); diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index c3ec67120..81bd2091a 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -452,7 +452,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) int m_numConstraints; btIDebugDraw* m_debugDrawer; btStackAlloc* m_stackAlloc; - + btDispatcher* m_dispatcher; InplaceSolverIslandCallback( btContactSolverInfo& solverInfo, @@ -460,13 +460,15 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer, - btStackAlloc* stackAlloc) + btStackAlloc* stackAlloc, + btDispatcher* dispatcher) :m_solverInfo(solverInfo), m_solver(solver), m_sortedConstraints(sortedConstraints), m_numConstraints(numConstraints), m_debugDrawer(debugDrawer), - m_stackAlloc(stackAlloc) + m_stackAlloc(stackAlloc), + m_dispatcher(dispatcher) { } @@ -479,30 +481,38 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - int numCurConstraints = 0; - int i; - - //find the first constraint for this island - for (i=0;isolveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } else { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + int numCurConstraints = 0; + int i; + + //find the first constraint for this island + for (i=0;isolveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc); + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + + } } }; @@ -524,7 +534,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0; - InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc); + InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1); m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); diff --git a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 29d0b58c3..0af074957 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -75,7 +75,7 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; m_constraintSolver->prepareSolve(0,numManifolds); - m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc); + m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1); m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc); } diff --git a/src/LinearMath/btAlignedAllocator.cpp b/src/LinearMath/btAlignedAllocator.cpp index fec507338..6c81b9b8e 100644 --- a/src/LinearMath/btAlignedAllocator.cpp +++ b/src/LinearMath/btAlignedAllocator.cpp @@ -21,11 +21,14 @@ subject to the following restrictions: #include void* btAlignedAlloc (int size, int alignment) { - return _aligned_malloc(size,alignment); + void* ptr = _aligned_malloc(size,alignment); +// printf("btAlignedAlloc %d, %x\n",size,ptr); + return ptr; } void btAlignedFree (void* ptr) { +// printf("btAlignedFree %x\n",ptr); _aligned_free(ptr); } diff --git a/src/LinearMath/btScalar.h b/src/LinearMath/btScalar.h index 5664dc89e..05df31945 100644 --- a/src/LinearMath/btScalar.h +++ b/src/LinearMath/btScalar.h @@ -23,6 +23,51 @@ subject to the following restrictions: #include #include + +#ifdef WIN32 + +//added __cdecl, thanks Jack + +// default new and delete overrides that guarantee 16 byte alignment and zero allocated memory +void* __cdecl operator new(size_t sz) throw(); +void* __cdecl operator new[](size_t sz) throw(); +void __cdecl operator delete(void* m) throw(); +void __cdecl operator delete[](void* m) throw(); + +#include +#include +#define BULLET_ALIGNED_NEW_AND_DELETE \ +\ +inline void* operator new(size_t sz) throw() \ +{ \ + printf("new %d\n",sz); \ + void* mem = _aligned_malloc(sz + 64, 16); \ + return mem; \ +} \ + \ +inline void* operator new[](size_t sz) throw() \ +{ \ + printf("new[] %d\n",sz); \ + void* mem = _aligned_malloc(sz + 64, 16); \ + return mem; \ +} \ + \ +inline void operator delete(void* m) throw() \ +{ \ +printf("delete %x\n",m); \ + if (m == 0) \ + return; \ + _aligned_free(m); \ +} \ + \ +inline void operator delete[](void* m) throw() \ +{ \ + printf("delete[] %x\n",m); \ +_aligned_free(m); \ +} \ + +#endif + #ifdef WIN32 #if defined(__MINGW32__) || defined(__CYGWIN__) || (defined (_MSC_VER) && _MSC_VER < 1300) @@ -163,14 +208,6 @@ SIMD_FORCE_INLINE bool btGreaterEqual (btScalar a, btScalar eps) { return (!((a) <= eps)); } -/*SIMD_FORCE_INLINE btScalar btCos(btScalar x) { return cosf(x); } -SIMD_FORCE_INLINE btScalar btSin(btScalar x) { return sinf(x); } -SIMD_FORCE_INLINE btScalar btTan(btScalar x) { return tanf(x); } -SIMD_FORCE_INLINE btScalar btAcos(btScalar x) { return acosf(x); } -SIMD_FORCE_INLINE btScalar btAsin(btScalar x) { return asinf(x); } -SIMD_FORCE_INLINE btScalar btAtan(btScalar x) { return atanf(x); } -SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y) { return atan2f(x, y); } -*/ SIMD_FORCE_INLINE int btIsNegative(btScalar x) { return x < btScalar(0.0) ? 1 : 0; @@ -189,4 +226,33 @@ SIMD_FORCE_INLINE btScalar btFsel(btScalar a, btScalar b, btScalar c) #endif #define btFsels(a,b,c) (btScalar)btFsel(a,b,c) + +///btSelect avoids branches, which makes performance much better for consoles like Playstation 3 and XBox 360 +///Thanks Phil Knight. See also http://www.cellperformance.com/articles/2006/04/more_techniques_for_eliminatin_1.html +SIMD_FORCE_INLINE unsigned btSelect(unsigned condition, unsigned valueIfConditionNonZero, unsigned valueIfConditionZero) +{ + // Set testNz to 0xFFFFFFFF if condition is nonzero, 0x00000000 if condition is zero + // Rely on positive value or'ed with its negative having sign bit on + // and zero value or'ed with its negative (which is still zero) having sign bit off + // Use arithmetic shift right, shifting the sign bit through all 32 bits + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +SIMD_FORCE_INLINE int btSelect(unsigned condition, int valueIfConditionNonZero, int valueIfConditionZero) +{ + unsigned testNz = (unsigned)(((int)condition | -(int)condition) >> 31); + unsigned testEqz = ~testNz; + return ((valueIfConditionNonZero & testNz) | (valueIfConditionZero & testEqz)); +} +SIMD_FORCE_INLINE float btSelect(unsigned condition, float valueIfConditionNonZero, float valueIfConditionZero) +{ +#ifdef BT_HAVE_NATIVE_FSEL + return (float)btFsel((btScalar)condition - btScalar(1.0f), valueIfConditionNonZero, valueIfConditionZero); +#else + return (condition != 0) ? valueIfConditionNonZero : valueIfConditionZero; +#endif +} + + #endif //SIMD___SCALAR_H diff --git a/src/btBulletCollisionCommon.h b/src/btBulletCollisionCommon.h index 482f21768..91aaf639f 100644 --- a/src/btBulletCollisionCommon.h +++ b/src/btBulletCollisionCommon.h @@ -44,6 +44,7 @@ subject to the following restrictions: ///Narrowphase Collision Detector #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h" #include "BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h" +#include "BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h" ///Dispatching and generation of collision pairs (broadphase) #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"