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"