diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Dynamics')
13 files changed, 2461 insertions, 1723 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp deleted file mode 100644 index e1f69afe101..00000000000 --- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ /dev/null @@ -1,469 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/* - Draft high-level generic physics C-API. For low-level access, use the physics SDK native API's. - Work in progress, functionality will be added on demand. - - If possible, use the richer Bullet C++ API, by including <src/btBulletDynamicsCommon.h> -*/ - -#include "Bullet-C-Api.h" -#include "btBulletDynamicsCommon.h" -#include "LinearMath/btAlignedAllocator.h" -#include "LinearMath/btConvexHullComputer.h" - - -#include "LinearMath/btVector3.h" -#include "LinearMath/btScalar.h" -#include "LinearMath/btMatrix3x3.h" -#include "LinearMath/btTransform.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/CollisionShapes/btTriangleShape.h" -#include "BulletCollision/Gimpact/btTriangleShapeEx.h" - -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" - - -/* - Create and Delete a Physics SDK -*/ - -struct btPhysicsSdk -{ - -// btDispatcher* m_dispatcher; -// btOverlappingPairCache* m_pairCache; -// btConstraintSolver* m_constraintSolver - - btVector3 m_worldAabbMin; - btVector3 m_worldAabbMax; - - - //todo: version, hardware/optimization settings etc? - btPhysicsSdk() - :m_worldAabbMin(-1000,-1000,-1000), - m_worldAabbMax(1000,1000,1000) - { - - } - - -}; - -plPhysicsSdkHandle plNewBulletSdk() -{ - void* mem = btAlignedAlloc(sizeof(btPhysicsSdk),16); - return (plPhysicsSdkHandle)new (mem)btPhysicsSdk; -} - -void plDeletePhysicsSdk(plPhysicsSdkHandle physicsSdk) -{ - btPhysicsSdk* phys = reinterpret_cast<btPhysicsSdk*>(physicsSdk); - btAlignedFree(phys); -} - - -/* Dynamics World */ -plDynamicsWorldHandle plCreateDynamicsWorld(plPhysicsSdkHandle physicsSdkHandle) -{ - btPhysicsSdk* physicsSdk = reinterpret_cast<btPhysicsSdk*>(physicsSdkHandle); - void* mem = btAlignedAlloc(sizeof(btDefaultCollisionConfiguration),16); - btDefaultCollisionConfiguration* collisionConfiguration = new (mem)btDefaultCollisionConfiguration(); - mem = btAlignedAlloc(sizeof(btCollisionDispatcher),16); - btDispatcher* dispatcher = new (mem)btCollisionDispatcher(collisionConfiguration); - mem = btAlignedAlloc(sizeof(btAxisSweep3),16); - btBroadphaseInterface* pairCache = new (mem)btAxisSweep3(physicsSdk->m_worldAabbMin,physicsSdk->m_worldAabbMax); - mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); - btConstraintSolver* constraintSolver = new(mem) btSequentialImpulseConstraintSolver(); - - mem = btAlignedAlloc(sizeof(btDiscreteDynamicsWorld),16); - return (plDynamicsWorldHandle) new (mem)btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration); -} -void plDeleteDynamicsWorld(plDynamicsWorldHandle world) -{ - //todo: also clean up the other allocations, axisSweep, pairCache,dispatcher,constraintSolver,collisionConfiguration - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAlignedFree(dynamicsWorld); -} - -void plStepSimulation(plDynamicsWorldHandle world, plReal timeStep) -{ - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAssert(dynamicsWorld); - dynamicsWorld->stepSimulation(timeStep); -} - -void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) -{ - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAssert(dynamicsWorld); - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - - dynamicsWorld->addRigidBody(body); -} - -void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) -{ - btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - btAssert(dynamicsWorld); - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - - dynamicsWorld->removeRigidBody(body); -} - -/* Rigid Body */ - -plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionShapeHandle cshape ) -{ - btTransform trans; - trans.setIdentity(); - btVector3 localInertia(0,0,0); - btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - btAssert(shape); - if (mass) - { - shape->calculateLocalInertia(mass,localInertia); - } - void* mem = btAlignedAlloc(sizeof(btRigidBody),16); - btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia); - btRigidBody* body = new (mem)btRigidBody(rbci); - body->setWorldTransform(trans); - body->setUserPointer(user_data); - return (plRigidBodyHandle) body; -} - -void plDeleteRigidBody(plRigidBodyHandle cbody) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody); - btAssert(body); - btAlignedFree( body); -} - - -/* Collision Shape definition */ - -plCollisionShapeHandle plNewSphereShape(plReal radius) -{ - void* mem = btAlignedAlloc(sizeof(btSphereShape),16); - return (plCollisionShapeHandle) new (mem)btSphereShape(radius); - -} - -plCollisionShapeHandle plNewBoxShape(plReal x, plReal y, plReal z) -{ - void* mem = btAlignedAlloc(sizeof(btBoxShape),16); - return (plCollisionShapeHandle) new (mem)btBoxShape(btVector3(x,y,z)); -} - -plCollisionShapeHandle plNewCapsuleShape(plReal radius, plReal height) -{ - //capsule is convex hull of 2 spheres, so use btMultiSphereShape - - const int numSpheres = 2; - btVector3 positions[numSpheres] = {btVector3(0,height,0),btVector3(0,-height,0)}; - btScalar radi[numSpheres] = {radius,radius}; - void* mem = btAlignedAlloc(sizeof(btMultiSphereShape),16); - return (plCollisionShapeHandle) new (mem)btMultiSphereShape(positions,radi,numSpheres); -} -plCollisionShapeHandle plNewConeShape(plReal radius, plReal height) -{ - void* mem = btAlignedAlloc(sizeof(btConeShape),16); - return (plCollisionShapeHandle) new (mem)btConeShape(radius,height); -} - -plCollisionShapeHandle plNewCylinderShape(plReal radius, plReal height) -{ - void* mem = btAlignedAlloc(sizeof(btCylinderShape),16); - return (plCollisionShapeHandle) new (mem)btCylinderShape(btVector3(radius,height,radius)); -} - -/* Convex Meshes */ -plCollisionShapeHandle plNewConvexHullShape() -{ - void* mem = btAlignedAlloc(sizeof(btConvexHullShape),16); - return (plCollisionShapeHandle) new (mem)btConvexHullShape(); -} - - -/* Concave static triangle meshes */ -plMeshInterfaceHandle plNewMeshInterface() -{ - return 0; -} - -plCollisionShapeHandle plNewCompoundShape() -{ - void* mem = btAlignedAlloc(sizeof(btCompoundShape),16); - return (plCollisionShapeHandle) new (mem)btCompoundShape(); -} - -void plAddChildShape(plCollisionShapeHandle compoundShapeHandle,plCollisionShapeHandle childShapeHandle, plVector3 childPos,plQuaternion childOrn) -{ - btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>(compoundShapeHandle); - btAssert(colShape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE); - btCompoundShape* compoundShape = reinterpret_cast<btCompoundShape*>(colShape); - btCollisionShape* childShape = reinterpret_cast<btCollisionShape*>(childShapeHandle); - btTransform localTrans; - localTrans.setIdentity(); - localTrans.setOrigin(btVector3(childPos[0],childPos[1],childPos[2])); - localTrans.setRotation(btQuaternion(childOrn[0],childOrn[1],childOrn[2],childOrn[3])); - compoundShape->addChildShape(localTrans,childShape); -} - -void plSetEuler(plReal yaw,plReal pitch,plReal roll, plQuaternion orient) -{ - btQuaternion orn; - orn.setEuler(yaw,pitch,roll); - orient[0] = orn.getX(); - orient[1] = orn.getY(); - orient[2] = orn.getZ(); - orient[3] = orn.getW(); - -} - - -// extern void plAddTriangle(plMeshInterfaceHandle meshHandle, plVector3 v0,plVector3 v1,plVector3 v2); -// extern plCollisionShapeHandle plNewStaticTriangleMeshShape(plMeshInterfaceHandle); - - -void plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z) -{ - btCollisionShape* colShape = reinterpret_cast<btCollisionShape*>( cshape); - (void)colShape; - btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE); - btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape); - convexHullShape->addPoint(btVector3(x,y,z)); - -} - -void plDeleteShape(plCollisionShapeHandle cshape) -{ - btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - btAssert(shape); - btAlignedFree(shape); -} -void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling) -{ - btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - btAssert(shape); - btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]); - shape->setLocalScaling(scaling); -} - - - -void plSetPosition(plRigidBodyHandle object, const plVector3 position) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - btVector3 pos(position[0],position[1],position[2]); - btTransform worldTrans = body->getWorldTransform(); - worldTrans.setOrigin(pos); - body->setWorldTransform(worldTrans); -} - -void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]); - btTransform worldTrans = body->getWorldTransform(); - worldTrans.setRotation(orn); - body->setWorldTransform(worldTrans); -} - -void plSetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - btTransform& worldTrans = body->getWorldTransform(); - worldTrans.setFromOpenGLMatrix(matrix); -} - -void plGetOpenGLMatrix(plRigidBodyHandle object, plReal* matrix) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - body->getWorldTransform().getOpenGLMatrix(matrix); - -} - -void plGetPosition(plRigidBodyHandle object,plVector3 position) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - const btVector3& pos = body->getWorldTransform().getOrigin(); - position[0] = pos.getX(); - position[1] = pos.getY(); - position[2] = pos.getZ(); -} - -void plGetOrientation(plRigidBodyHandle object,plQuaternion orientation) -{ - btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - btAssert(body); - const btQuaternion& orn = body->getWorldTransform().getRotation(); - orientation[0] = orn.getX(); - orientation[1] = orn.getY(); - orientation[2] = orn.getZ(); - orientation[3] = orn.getW(); -} - - - -//plRigidBodyHandle plRayCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); - -// extern plRigidBodyHandle plObjectCast(plDynamicsWorldHandle world, const plVector3 rayStart, const plVector3 rayEnd, plVector3 hitpoint, plVector3 normal); - -double plNearestPoints(float p1[3], float p2[3], float p3[3], float q1[3], float q2[3], float q3[3], float *pa, float *pb, float normal[3]) -{ - btVector3 vp(p1[0], p1[1], p1[2]); - btTriangleShapeEx trishapeA(vp, - btVector3(p2[0], p2[1], p2[2]), - btVector3(p3[0], p3[1], p3[2])); - trishapeA.setMargin(0.000001f); - btVector3 vq(q1[0], q1[1], q1[2]); - btTriangleShapeEx trishapeB(vq, - btVector3(q2[0], q2[1], q2[2]), - btVector3(q3[0], q3[1], q3[2])); - trishapeB.setMargin(0.000001f); - - // btVoronoiSimplexSolver sGjkSimplexSolver; - // btGjkEpaPenetrationDepthSolver penSolverPtr; - - /*static*/ btSimplexSolverInterface sGjkSimplexSolver; - sGjkSimplexSolver.reset(); - - /*static*/ btGjkEpaPenetrationDepthSolver Solver0; - /*static*/ btMinkowskiPenetrationDepthSolver Solver1; - - btConvexPenetrationDepthSolver* Solver = NULL; - - Solver = &Solver1; - - btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,Solver); - - convexConvex.m_catchDegeneracies = 1; - - // btGjkPairDetector convexConvex(&trishapeA ,&trishapeB,&sGjkSimplexSolver,0); - - btPointCollector gjkOutput; - btGjkPairDetector::ClosestPointInput input; - - - btTransform tr; - tr.setIdentity(); - - input.m_transformA = tr; - input.m_transformB = tr; - - convexConvex.getClosestPoints(input, gjkOutput, 0); - - - if (gjkOutput.m_hasResult) - { - - pb[0] = pa[0] = gjkOutput.m_pointInWorld[0]; - pb[1] = pa[1] = gjkOutput.m_pointInWorld[1]; - pb[2] = pa[2] = gjkOutput.m_pointInWorld[2]; - - pb[0]+= gjkOutput.m_normalOnBInWorld[0] * gjkOutput.m_distance; - pb[1]+= gjkOutput.m_normalOnBInWorld[1] * gjkOutput.m_distance; - pb[2]+= gjkOutput.m_normalOnBInWorld[2] * gjkOutput.m_distance; - - normal[0] = gjkOutput.m_normalOnBInWorld[0]; - normal[1] = gjkOutput.m_normalOnBInWorld[1]; - normal[2] = gjkOutput.m_normalOnBInWorld[2]; - - return gjkOutput.m_distance; - } - return -1.0f; -} - -// Convex hull -plConvexHull plConvexHullCompute(float (*coords)[3], int count) -{ - btConvexHullComputer *computer = new btConvexHullComputer; - computer->compute(reinterpret_cast< float* >(coords), - sizeof(*coords), count, 0, 0); - return reinterpret_cast<plConvexHull>(computer); -} - -void plConvexHullDelete(plConvexHull hull) -{ - btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull)); - delete computer; -} - -int plConvexHullNumVertices(plConvexHull hull) -{ - btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull)); - return computer->vertices.size(); -} - -int plConvexHullNumFaces(plConvexHull hull) -{ - btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull)); - return computer->faces.size(); -} - -void plConvexHullGetVertex(plConvexHull hull, int n, float coords[3], - int *original_index) -{ - btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull)); - const btVector3 &v(computer->vertices[n]); - coords[0] = v[0]; - coords[1] = v[1]; - coords[2] = v[2]; - (*original_index) = computer->original_vertex_index[n]; -} - -int plConvexHullGetFaceSize(plConvexHull hull, int n) -{ - btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull)); - const btConvexHullComputer::Edge *e_orig, *e; - int count; - - for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0; - count == 0 || e != e_orig; - e = e->getNextEdgeOfFace(), count++); - return count; -} - -void plConvexHullGetFaceVertices(plConvexHull hull, int n, int *vertices) -{ - btConvexHullComputer *computer(reinterpret_cast< btConvexHullComputer* >(hull)); - const btConvexHullComputer::Edge *e_orig, *e; - int count; - - for (e_orig = &computer->edges[computer->faces[n]], e = e_orig, count = 0; - count == 0 || e != e_orig; - e = e->getNextEdgeOfFace(), count++) - { - vertices[count] = e->getTargetVertex(); - } -} diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h b/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h index e1fea3a49c0..b5cac56cdca 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btActionInterface.h @@ -26,21 +26,16 @@ class btCollisionWorld; class btActionInterface { protected: - static btRigidBody& getFixedBody(); - - -public: +public: virtual ~btActionInterface() { } - virtual void updateAction( btCollisionWorld* collisionWorld, btScalar deltaTimeStep)=0; + virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTimeStep) = 0; virtual void debugDraw(btIDebugDraw* debugDrawer) = 0; - }; -#endif //_BT_ACTION_INTERFACE_H - +#endif //_BT_ACTION_INTERFACE_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 361a054ec69..fb15ae31eb5 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btDiscreteDynamicsWorld.h" //collision detection @@ -38,11 +37,9 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" #include "BulletDynamics/ConstraintSolver/btContactConstraint.h" - #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" - #include "BulletDynamics/Dynamics/btActionInterface.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btMotionState.h" @@ -56,57 +53,52 @@ int startHit=2; int firstHit=startHit; #endif -SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) +SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { int islandId; const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); return islandId; - } - class btSortConstraintOnIslandPredicate { - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId(rhs); - lIslandId0 = btGetConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } +public: + bool operator()(const btTypedConstraint* lhs, const btTypedConstraint* rhs) const + { + int rIslandId0, lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } }; struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { - btContactSolverInfo* m_solverInfo; - btConstraintSolver* m_solver; - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btDispatcher* m_dispatcher; + btContactSolverInfo* m_solverInfo; + btConstraintSolver* m_solver; + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; btAlignedObjectArray<btCollisionObject*> m_bodies; btAlignedObjectArray<btPersistentManifold*> m_manifolds; btAlignedObjectArray<btTypedConstraint*> m_constraints; - InplaceSolverIslandCallback( - btConstraintSolver* solver, + btConstraintSolver* solver, btStackAlloc* stackAlloc, btDispatcher* dispatcher) - :m_solverInfo(NULL), - m_solver(solver), - m_sortedConstraints(NULL), - m_numConstraints(0), - m_debugDrawer(NULL), - m_dispatcher(dispatcher) + : m_solverInfo(NULL), + m_solver(solver), + m_sortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_dispatcher(dispatcher) { - } InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) @@ -116,34 +108,34 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal return *this; } - SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer) + SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer) { btAssert(solverInfo); m_solverInfo = solverInfo; m_sortedConstraints = sortedConstraints; m_numConstraints = numConstraints; m_debugDrawer = debugDrawer; - m_bodies.resize (0); - m_manifolds.resize (0); - m_constraints.resize (0); + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); } - - virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + virtual void processIsland(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifolds, int numManifolds, int islandId) { - if (islandId<0) + if (islandId < 0) { ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else + m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, &m_sortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + } + else { - //also add all non-contact constraints/joints for this island + //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;i<m_numConstraints;i++) + for (i = 0; i < m_numConstraints; i++) { if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) { @@ -152,7 +144,7 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal } } //count the number of constraints in this island - for (;i<m_numConstraints;i++) + for (; i < m_numConstraints; i++) { if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) { @@ -160,91 +152,87 @@ struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCal } } - if (m_solverInfo->m_minimumSolverBatchSize<=1) + if (m_solverInfo->m_minimumSolverBatchSize <= 1) { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); - } else + m_solver->solveGroup(bodies, numBodies, manifolds, numManifolds, startConstraint, numCurConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher); + } + else { - - for (i=0;i<numBodies;i++) + for (i = 0; i < numBodies; i++) m_bodies.push_back(bodies[i]); - for (i=0;i<numManifolds;i++) + for (i = 0; i < numManifolds; i++) m_manifolds.push_back(manifolds[i]); - for (i=0;i<numCurConstraints;i++) + for (i = 0; i < numCurConstraints; i++) m_constraints.push_back(startConstraint[i]); - if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) + if ((m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize) { processConstraints(); - } else + } + else { //printf("deferred\n"); } } } } - void processConstraints() + void processConstraints() { + btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0; + btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0; + btTypedConstraint** constraints = m_constraints.size() ? &m_constraints[0] : 0; - btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; - btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; - btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; - - m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_dispatcher); + m_solver->solveGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher); m_bodies.resize(0); m_manifolds.resize(0); m_constraints.resize(0); - } - }; - - -btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_sortedConstraints (), -m_solverIslandCallback ( NULL ), -m_constraintSolver(constraintSolver), -m_gravity(0,-10,0), -m_localTime(0), -m_fixedTimeStep(0), -m_synchronizeAllMotionStates(false), -m_applySpeculativeContactRestitution(false), -m_profileTimings(0), -m_latencyMotionStateInterpolation(true) +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) + : btDynamicsWorld(dispatcher, pairCache, collisionConfiguration), + m_sortedConstraints(), + m_solverIslandCallback(NULL), + m_constraintSolver(constraintSolver), + m_gravity(0, -10, 0), + m_localTime(0), + m_fixedTimeStep(0), + m_synchronizeAllMotionStates(false), + m_applySpeculativeContactRestitution(false), + m_profileTimings(0), + m_latencyMotionStateInterpolation(true) { if (!m_constraintSolver) { - void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); + void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver), 16); m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; m_ownsConstraintSolver = true; - } else + } + else { m_ownsConstraintSolver = false; } { - void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16); + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager), 16); m_islandManager = new (mem) btSimulationIslandManager(); } m_ownsIslandManager = true; { - void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16); - m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (m_constraintSolver, 0, dispatcher); + void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback), 16); + m_solverIslandCallback = new (mem) InplaceSolverIslandCallback(m_constraintSolver, 0, dispatcher); } } - btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() { //only delete it when we created it if (m_ownsIslandManager) { m_islandManager->~btSimulationIslandManager(); - btAlignedFree( m_islandManager); + btAlignedFree(m_islandManager); } if (m_solverIslandCallback) { @@ -253,18 +241,17 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() } if (m_ownsConstraintSolver) { - m_constraintSolver->~btConstraintSolver(); btAlignedFree(m_constraintSolver); } } -void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) +void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) { -///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows -///to switch status _after_ adding kinematic objects to the world -///fix it for Bullet 3.x release - for (int i=0;i<m_collisionObjects.size();i++) + ///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows + ///to switch status _after_ adding kinematic objects to the world + ///fix it for Bullet 3.x release + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -277,10 +264,9 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) } } } - } -void btDiscreteDynamicsWorld::debugDrawWorld() +void btDiscreteDynamicsWorld::debugDrawWorld() { BT_PROFILE("debugDrawWorld"); @@ -290,43 +276,40 @@ void btDiscreteDynamicsWorld::debugDrawWorld() if (getDebugDrawer()) { int mode = getDebugDrawer()->getDebugMode(); - if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) { drawConstraints = true; } } - if(drawConstraints) + if (drawConstraints) { - for(int i = getNumConstraints()-1; i>=0 ;i--) + for (int i = getNumConstraints() - 1; i >= 0; i--) { btTypedConstraint* constraint = getConstraint(i); debugDrawConstraint(constraint); } } - - - if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals))) + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals))) { int i; if (getDebugDrawer() && getDebugDrawer()->getDebugMode()) { - for (i=0;i<m_actions.size();i++) + for (i = 0; i < m_actions.size(); i++) { m_actions[i]->debugDraw(m_debugDrawer); } } } - if (getDebugDrawer()) - getDebugDrawer()->flushLines(); - + if (getDebugDrawer()) + getDebugDrawer()->flushLines(); } -void btDiscreteDynamicsWorld::clearForces() +void btDiscreteDynamicsWorld::clearForces() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; //need to check if next line is ok @@ -336,10 +319,10 @@ void btDiscreteDynamicsWorld::clearForces() } ///apply gravity, call this once per timestep -void btDiscreteDynamicsWorld::applyGravity() +void btDiscreteDynamicsWorld::applyGravity() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; if (body->isActive()) @@ -349,8 +332,7 @@ void btDiscreteDynamicsWorld::applyGravity() } } - -void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) +void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) { btAssert(body); @@ -363,32 +345,32 @@ void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) { btTransform interpolatedTransform; btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(), - (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(), - interpolatedTransform); + body->getInterpolationLinearVelocity(), body->getInterpolationAngularVelocity(), + (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime * body->getHitFraction(), + interpolatedTransform); body->getMotionState()->setWorldTransform(interpolatedTransform); } } } - -void btDiscreteDynamicsWorld::synchronizeMotionStates() +void btDiscreteDynamicsWorld::synchronizeMotionStates() { - BT_PROFILE("synchronizeMotionStates"); + // BT_PROFILE("synchronizeMotionStates"); if (m_synchronizeAllMotionStates) { //iterate over all collision objects - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); if (body) synchronizeSingleMotionState(body); } - } else + } + else { //iterate over all active rigid bodies - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; if (body->isActive()) @@ -397,13 +379,10 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() } } - -int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +int btDiscreteDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { startProfiling(timeStep); - BT_PROFILE("stepSimulation"); - int numSimulationSubSteps = 0; if (maxSubSteps) @@ -413,10 +392,11 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, m_localTime += timeStep; if (m_localTime >= fixedTimeStep) { - numSimulationSubSteps = int( m_localTime / fixedTimeStep); + numSimulationSubSteps = int(m_localTime / fixedTimeStep); m_localTime -= numSimulationSubSteps * fixedTimeStep; } - } else + } + else { //variable timestep fixedTimeStep = timeStep; @@ -426,7 +406,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, { numSimulationSubSteps = 0; maxSubSteps = 0; - } else + } + else { numSimulationSubSteps = 1; maxSubSteps = 1; @@ -436,28 +417,25 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, //process some debugging flags if (getDebugDrawer()) { - btIDebugDraw* debugDrawer = getDebugDrawer (); + btIDebugDraw* debugDrawer = getDebugDrawer(); gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; } if (numSimulationSubSteps) { - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; - saveKinematicState(fixedTimeStep*clampedSimulationSteps); + saveKinematicState(fixedTimeStep * clampedSimulationSteps); applyGravity(); - - - for (int i=0;i<clampedSimulationSteps;i++) + for (int i = 0; i < clampedSimulationSteps; i++) { internalSingleStepSimulation(fixedTimeStep); synchronizeMotionStates(); } - - } else + } + else { synchronizeMotionStates(); } @@ -466,17 +444,17 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, #ifndef BT_NO_PROFILE CProfileManager::Increment_Frame_Counter(); -#endif //BT_NO_PROFILE +#endif //BT_NO_PROFILE return numSimulationSubSteps; } -void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) +void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - BT_PROFILE("internalSingleStepSimulation"); - if(0 != m_internalPreTickCallback) { + if (0 != m_internalPreTickCallback) + { (*m_internalPreTickCallback)(this, timeStep); } @@ -489,19 +467,15 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) dispatchInfo.m_stepCount = 0; dispatchInfo.m_debugDraw = getDebugDrawer(); - - createPredictiveContacts(timeStep); + createPredictiveContacts(timeStep); ///perform collision detection performDiscreteCollisionDetection(); calculateSimulationIslands(); - getSolverInfo().m_timeStep = timeStep; - - ///solve contact and other joint constraints solveConstraints(getSolverInfo()); @@ -514,37 +488,38 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) ///update vehicle simulation updateActions(timeStep); - updateActivationState( timeStep ); + updateActivationState(timeStep); - if(0 != m_internalTickCallback) { + if (0 != m_internalTickCallback) + { (*m_internalTickCallback)(this, timeStep); } } -void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) +void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; - if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + if (body->isActive() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(gravity); } } } -btVector3 btDiscreteDynamicsWorld::getGravity () const +btVector3 btDiscreteDynamicsWorld::getGravity() const { return m_gravity; } -void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask) +void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject, int collisionFilterGroup, int collisionFilterMask) { - btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask); + btCollisionWorld::addCollisionObject(collisionObject, collisionFilterGroup, collisionFilterMask); } -void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { btRigidBody* body = btRigidBody::upcast(collisionObject); if (body) @@ -553,16 +528,15 @@ void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collision btCollisionWorld::removeCollisionObject(collisionObject); } -void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) +void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { m_nonStaticRigidBodies.remove(body); btCollisionWorld::removeCollisionObject(body); } - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) { - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(m_gravity); } @@ -572,22 +546,23 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) if (!body->isStaticObject()) { m_nonStaticRigidBodies.push_back(body); - } else + } + else { body->setActivationState(ISLAND_SLEEPING); } bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); + int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - addCollisionObject(body,collisionFilterGroup,collisionFilterMask); + addCollisionObject(body, collisionFilterGroup, collisionFilterMask); } } -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) { - if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY)) + if (!body->isStaticOrKinematicObject() && !(body->getFlags() & BT_DISABLE_WORLD_GRAVITY)) { body->setGravity(m_gravity); } @@ -598,31 +573,29 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short { m_nonStaticRigidBodies.push_back(body); } - else + else { body->setActivationState(ISLAND_SLEEPING); } - addCollisionObject(body,group,mask); + addCollisionObject(body, group, mask); } } - -void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) +void btDiscreteDynamicsWorld::updateActions(btScalar timeStep) { BT_PROFILE("updateActions"); - for ( int i=0;i<m_actions.size();i++) + for (int i = 0; i < m_actions.size(); i++) { - m_actions[i]->updateAction( this, timeStep); + m_actions[i]->updateAction(this, timeStep); } } - -void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) +void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("updateActivationState"); - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; if (body) @@ -634,32 +607,33 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) if (body->isStaticOrKinematicObject()) { body->setActivationState(ISLAND_SLEEPING); - } else + } + else { if (body->getActivationState() == ACTIVE_TAG) - body->setActivationState( WANTS_DEACTIVATION ); + body->setActivationState(WANTS_DEACTIVATION); if (body->getActivationState() == ISLAND_SLEEPING) { - body->setAngularVelocity(btVector3(0,0,0)); - body->setLinearVelocity(btVector3(0,0,0)); + body->setAngularVelocity(btVector3(0, 0, 0)); + body->setLinearVelocity(btVector3(0, 0, 0)); } - } - } else + } + else { if (body->getActivationState() != DISABLE_DEACTIVATION) - body->setActivationState( ACTIVE_TAG ); + body->setActivationState(ACTIVE_TAG); } } } } -void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) +void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies) { m_constraints.push_back(constraint); - //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?) - btAssert(&constraint->getRigidBodyA()!=&constraint->getRigidBodyB()); - + //Make sure the two bodies of a type constraint are different (possibly add this to the btTypedConstraint constructor?) + btAssert(&constraint->getRigidBodyA() != &constraint->getRigidBodyB()); + if (disableCollisionsBetweenLinkedBodies) { constraint->getRigidBodyA().addConstraintRef(constraint); @@ -667,105 +641,98 @@ void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool d } } -void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) +void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) { m_constraints.remove(constraint); constraint->getRigidBodyA().removeConstraintRef(constraint); constraint->getRigidBodyB().removeConstraintRef(constraint); } -void btDiscreteDynamicsWorld::addAction(btActionInterface* action) +void btDiscreteDynamicsWorld::addAction(btActionInterface* action) { m_actions.push_back(action); } -void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) +void btDiscreteDynamicsWorld::removeAction(btActionInterface* action) { m_actions.remove(action); } - -void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) +void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle) { addAction(vehicle); } -void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) +void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle) { removeAction(vehicle); } -void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) +void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character) { addAction(character); } -void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) +void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) { removeAction(character); } - - - -void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); - m_sortedConstraints.resize( m_constraints.size()); + m_sortedConstraints.resize(m_constraints.size()); int i; - for (i=0;i<getNumConstraints();i++) + for (i = 0; i < getNumConstraints(); i++) { m_sortedConstraints[i] = m_constraints[i]; } -// btAssert(0); - - + // btAssert(0); m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; - m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer()); + m_solverIslandCallback->setup(&solverInfo, constraintsPtr, m_sortedConstraints.size(), getDebugDrawer()); m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback); + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverIslandCallback); m_solverIslandCallback->processConstraints(); m_constraintSolver->allSolved(solverInfo, m_debugDrawer); } - -void btDiscreteDynamicsWorld::calculateSimulationIslands() +void btDiscreteDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + getSimulationIslandManager()->updateActivationState(getCollisionWorld(), getCollisionWorld()->getDispatcher()); - { - //merge islands based on speculative contact manifolds too - for (int i=0;i<this->m_predictiveManifolds.size();i++) - { - btPersistentManifold* manifold = m_predictiveManifolds[i]; + { + //merge islands based on speculative contact manifolds too + for (int i = 0; i < this->m_predictiveManifolds.size(); i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; - const btCollisionObject* colObj0 = manifold->getBody0(); - const btCollisionObject* colObj1 = manifold->getBody1(); + const btCollisionObject* colObj0 = manifold->getBody0(); + const btCollisionObject* colObj1 = manifold->getBody1(); - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) - { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); - } - } - } + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + { + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); + } + } + } { int i; int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) + for (i = 0; i < numConstraints; i++) { btTypedConstraint* constraint = m_constraints[i]; if (constraint->isEnabled()) @@ -776,7 +743,7 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), (colObj1)->getIslandTag()); } } } @@ -784,51 +751,44 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - - } - - - class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { public: - btCollisionObject* m_me; btScalar m_allowedPenetration; btOverlappingPairCache* m_pairCache; btDispatcher* m_dispatcher; public: - btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btCollisionWorld::ClosestConvexResultCallback(fromA,toA), - m_me(me), - m_allowedPenetration(0.0f), - m_pairCache(pairCache), - m_dispatcher(dispatcher) + btClosestNotMeConvexResultCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA, toA), + m_me(me), + m_allowedPenetration(0.0f), + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } - virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult, bool normalInWorldSpace) { if (convexResult.m_hitCollisionObject == m_me) return 1.0f; //ignore result if there is no contact response - if(!convexResult.m_hitCollisionObject->hasContactResponse()) + if (!convexResult.m_hitCollisionObject->hasContactResponse()) return 1.0f; - btVector3 linVelA,linVelB; - linVelA = m_convexToWorld-m_convexFromWorld; - linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); + btVector3 linVelA, linVelB; + linVelA = m_convexToWorld - m_convexFromWorld; + linVelB = btVector3(0, 0, 0); //toB.getOrigin()-fromB.getOrigin(); - btVector3 relativeVelocity = (linVelA-linVelB); + btVector3 relativeVelocity = (linVelA - linVelB); //don't report time of impact for motion away from the contact normal (or causes minor penetration) - if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) + if (convexResult.m_hitNormalLocal.dot(relativeVelocity) >= -m_allowedPenetration) return 1.f; - return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + return ClosestConvexResultCallback::addSingleResult(convexResult, normalInWorldSpace); } virtual bool needsCollision(btBroadphaseProxy* proxy0) const @@ -840,11 +800,22 @@ public: ///don't do CCD when the collision filters are not matching if (!ClosestConvexResultCallback::needsCollision(proxy0)) return false; + if (m_pairCache->getOverlapFilterCallback()) { + btBroadphaseProxy* proxy1 = m_me->getBroadphaseHandle(); + bool collides = m_pairCache->needsBroadphaseCollision(proxy0, proxy1); + if (!collides) + { + return false; + } + } - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; + + if (!m_dispatcher->needsCollision(m_me, otherObj)) + return false; //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 - if (m_dispatcher->needsResponse(m_me,otherObj)) + if (m_dispatcher->needsResponse(m_me, otherObj)) { #if 0 ///don't do CCD when there are already contact points (touching contact/penetration) @@ -870,41 +841,24 @@ public: return false; } - - }; ///internal debugging variable. this value shouldn't be too high -int gNumClampedCcdMotions=0; +int gNumClampedCcdMotions = 0; - -void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) +void btDiscreteDynamicsWorld::createPredictiveContactsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep) { - BT_PROFILE("createPredictiveContacts"); - - { - BT_PROFILE("release predictive contact manifolds"); - - for (int i=0;i<m_predictiveManifolds.size();i++) - { - btPersistentManifold* manifold = m_predictiveManifolds[i]; - this->m_dispatcher1->releaseManifold(manifold); - } - m_predictiveManifolds.clear(); - } - btTransform predictedTrans; - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < numBodies; i++) { - btRigidBody* body = m_nonStaticRigidBodies[i]; + btRigidBody* body = bodies[i]; body->setHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2(); if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { @@ -916,81 +870,97 @@ void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) class StaticOnlyCallback : public btClosestNotMeConvexResultCallback { public: - - StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher) { } - virtual bool needsCollision(btBroadphaseProxy* proxy0) const + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; if (!otherObj->isStaticOrKinematicObject()) return false; return btClosestNotMeConvexResultCallback::needsCollision(proxy0); } }; - StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #else - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #endif //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; + btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); + convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { - - btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction; + btVector3 distVec = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()) * sweepResults.m_closestHitFraction; btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld); - - btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject); + btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body, sweepResults.m_hitCollisionObject); + btMutexLock(&m_predictiveManifoldsMutex); m_predictiveManifolds.push_back(manifold); + btMutexUnlock(&m_predictiveManifoldsMutex); - btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec; - btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB; + btVector3 worldPointB = body->getWorldTransform().getOrigin() + distVec; + btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse() * worldPointB; - btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance); + btManifoldPoint newPoint(btVector3(0, 0, 0), localPointB, sweepResults.m_hitNormalWorld, distance); bool isPredictive = true; int index = manifold->addManifoldPoint(newPoint, isPredictive); btManifoldPoint& pt = manifold->getContactPoint(index); pt.m_combinedRestitution = 0; - pt.m_combinedFriction = btManifoldResult::calculateCombinedFriction(body,sweepResults.m_hitCollisionObject); + pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body, sweepResults.m_hitCollisionObject); pt.m_positionWorldOnA = body->getWorldTransform().getOrigin(); pt.m_positionWorldOnB = worldPointB; - } } } } } } -void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) + +void btDiscreteDynamicsWorld::releasePredictiveContacts() +{ + BT_PROFILE("release predictive contact manifolds"); + + for (int i = 0; i < m_predictiveManifolds.size(); i++) + { + btPersistentManifold* manifold = m_predictiveManifolds[i]; + this->m_dispatcher1->releaseManifold(manifold); + } + m_predictiveManifolds.clear(); +} + +void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep) +{ + BT_PROFILE("createPredictiveContacts"); + releasePredictiveContacts(); + if (m_nonStaticRigidBodies.size() > 0) + { + createPredictiveContactsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); + } +} + +void btDiscreteDynamicsWorld::integrateTransformsInternal(btRigidBody** bodies, int numBodies, btScalar timeStep) { - BT_PROFILE("integrateTransforms"); btTransform predictedTrans; - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < numBodies; i++) { - btRigidBody* body = m_nonStaticRigidBodies[i]; + btRigidBody* body = bodies[i]; body->setHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) { - body->predictIntegratedTransform(timeStep, predictedTrans); - btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - - + btScalar squareMotion = (predictedTrans.getOrigin() - body->getWorldTransform().getOrigin()).length2(); if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { @@ -1002,43 +972,40 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) class StaticOnlyCallback : public btClosestNotMeConvexResultCallback { public: - - StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : - btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + StaticOnlyCallback(btCollisionObject* me, const btVector3& fromA, const btVector3& toA, btOverlappingPairCache* pairCache, btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me, fromA, toA, pairCache, dispatcher) { } - virtual bool needsCollision(btBroadphaseProxy* proxy0) const + virtual bool needsCollision(btBroadphaseProxy* proxy0) const { - btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + btCollisionObject* otherObj = (btCollisionObject*)proxy0->m_clientObject; if (!otherObj->isStaticOrKinematicObject()) return false; return btClosestNotMeConvexResultCallback::needsCollision(proxy0); } }; - StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + StaticOnlyCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #else - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); + btClosestNotMeConvexResultCallback sweepResults(body, body->getWorldTransform().getOrigin(), predictedTrans.getOrigin(), getBroadphase()->getOverlappingPairCache(), getDispatcher()); #endif //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); - sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; + btSphereShape tmpSphere(body->getCcdSweptSphereRadius()); //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + sweepResults.m_allowedPenetration = getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; - sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); + convexSweepTest(&tmpSphere, body->getWorldTransform(), modifiedPredictedTrans, sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { - //printf("clamped integration to hit fraction = %f\n",fraction); body->setHitFraction(sweepResults.m_closestHitFraction); - body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); + body->predictIntegratedTransform(timeStep * body->getHitFraction(), predictedTrans); body->setHitFraction(0.f); - body->proceedToTransform( predictedTrans); + body->proceedToTransform(predictedTrans); #if 0 btVector3 linVel = body->getLinearVelocity(); @@ -1065,40 +1032,45 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) //btScalar depth = 0.f; //appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); - #endif - continue; + continue; } } } - - body->proceedToTransform( predictedTrans); - + body->proceedToTransform(predictedTrans); } + } +} +void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + BT_PROFILE("integrateTransforms"); + if (m_nonStaticRigidBodies.size() > 0) + { + integrateTransformsInternal(&m_nonStaticRigidBodies[0], m_nonStaticRigidBodies.size(), timeStep); } ///this should probably be switched on by default, but it is not well tested yet if (m_applySpeculativeContactRestitution) { BT_PROFILE("apply speculative contact restitution"); - for (int i=0;i<m_predictiveManifolds.size();i++) + for (int i = 0; i < m_predictiveManifolds.size(); i++) { btPersistentManifold* manifold = m_predictiveManifolds[i]; btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0()); btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1()); - for (int p=0;p<manifold->getNumContacts();p++) + for (int p = 0; p < manifold->getNumContacts(); p++) { const btManifoldPoint& pt = manifold->getContactPoint(p); - btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1); + btScalar combinedRestitution = gCalculateCombinedRestitutionCallback(body0, body1); - if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f) + if (combinedRestitution > 0 && pt.m_appliedImpulse != 0.f) //if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f) { - btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution; + btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse * combinedRestitution; const btVector3& pos1 = pt.getPositionWorldOnA(); const btVector3& pos2 = pt.getPositionWorldOnB(); @@ -1107,25 +1079,19 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin(); if (body0) - body0->applyImpulse(imp,rel_pos0); + body0->applyImpulse(imp, rel_pos0); if (body1) - body1->applyImpulse(-imp,rel_pos1); + body1->applyImpulse(-imp, rel_pos1); } } } } - } - - - - - -void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BT_PROFILE("predictUnconstraintMotion"); - for ( int i=0;i<m_nonStaticRigidBodies.size();i++) + for (int i = 0; i < m_nonStaticRigidBodies.size(); i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; if (!body->isStaticOrKinematicObject()) @@ -1134,179 +1100,171 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform()); } } } - -void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) +void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) { (void)timeStep; #ifndef BT_NO_PROFILE CProfileManager::Reset(); -#endif //BT_NO_PROFILE - +#endif //BT_NO_PROFILE } - - - - - void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) { bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0; bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0; btScalar dbgDrawSize = constraint->getDbgDrawSize(); - if(dbgDrawSize <= btScalar(0.f)) + if (dbgDrawSize <= btScalar(0.f)) { return; } - switch(constraint->getConstraintType()) + switch (constraint->getConstraintType()) { case POINT2POINT_CONSTRAINT_TYPE: + { + btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; + btTransform tr; + tr.setIdentity(); + btVector3 pivot = p2pC->getPivotInA(); + pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + getDebugDrawer()->drawTransform(tr, dbgDrawSize); + // that ideally should draw the same frame + pivot = p2pC->getPivotInB(); + pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; + tr.setOrigin(pivot); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + } + break; + case HINGE_CONSTRAINT_TYPE: + { + btHingeConstraint* pHinge = (btHingeConstraint*)constraint; + btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + btScalar minAng = pHinge->getLowerLimit(); + btScalar maxAng = pHinge->getUpperLimit(); + if (minAng == maxAng) { - btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint; - btTransform tr; - tr.setIdentity(); - btVector3 pivot = p2pC->getPivotInA(); - pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - getDebugDrawer()->drawTransform(tr, dbgDrawSize); - // that ideally should draw the same frame - pivot = p2pC->getPivotInB(); - pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; - tr.setOrigin(pivot); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + break; } - break; - case HINGE_CONSTRAINT_TYPE: + bool drawSect = true; + if (!pHinge->hasLimit()) { - btHingeConstraint* pHinge = (btHingeConstraint*)constraint; - btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - btScalar minAng = pHinge->getLowerLimit(); - btScalar maxAng = pHinge->getUpperLimit(); - if(minAng == maxAng) - { - break; - } - bool drawSect = true; - if(!pHinge->hasLimit()) - { - minAng = btScalar(0.f); - maxAng = SIMD_2_PI; - drawSect = false; - } - if(drawLimits) - { - btVector3& center = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect); - } + minAng = btScalar(0.f); + maxAng = SIMD_2_PI; + drawSect = false; } - break; + if (drawLimits) + { + btVector3& center = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0, 0, 0), drawSect); + } + } + break; case CONETWIST_CONSTRAINT_TYPE: + { + btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; + btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) { - btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint; - btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) + //const btScalar length = btScalar(5); + const btScalar length = dbgDrawSize; + static int nSegments = 8 * 4; + btScalar fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)(nSegments - 1) / btScalar(nSegments); + btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); + pPrev = tr * pPrev; + for (int i = 0; i < nSegments; i++) { - //const btScalar length = btScalar(5); - const btScalar length = dbgDrawSize; - static int nSegments = 8*4; - btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments); - btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length); - pPrev = tr * pPrev; - for (int i=0; i<nSegments; i++) - { - fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments); - btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length); - pCur = tr * pCur; - getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0)); + fAngleInRadians = btScalar(2. * 3.1415926) * (btScalar)i / btScalar(nSegments); + btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length); + pCur = tr * pCur; + getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0, 0, 0)); - if (i%(nSegments/8) == 0) - getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0)); - - pPrev = pCur; - } - btScalar tws = pCT->getTwistSpan(); - btScalar twa = pCT->getTwistAngle(); - bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); - if(useFrameB) - { - tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); - } - else - { - tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); - } - btVector3 pivot = tr.getOrigin(); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis1 = tr.getBasis().getColumn(1); - getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true); + if (i % (nSegments / 8) == 0) + getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0, 0, 0)); + pPrev = pCur; } + btScalar tws = pCT->getTwistSpan(); + btScalar twa = pCT->getTwistAngle(); + bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f)); + if (useFrameB) + { + tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame(); + } + else + { + tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame(); + } + btVector3 pivot = tr.getOrigin(); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis1 = tr.getBasis().getColumn(1); + getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa - tws, -twa + tws, btVector3(0, 0, 0), true); } - break; + } + break; case D6_SPRING_CONSTRAINT_TYPE: case D6_CONSTRAINT_TYPE: + { + btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; + btTransform tr = p6DOF->getCalculatedTransformA(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformB(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) { - btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint; - btTransform tr = p6DOF->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = p6DOF->getCalculatedTransformA(); + const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); + btVector3 up = tr.getBasis().getColumn(2); + btVector3 axis = tr.getBasis().getColumn(0); + btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; + btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0)); + axis = tr.getBasis().getColumn(1); + btScalar ay = p6DOF->getAngle(1); + btScalar az = p6DOF->getAngle(2); + btScalar cy = btCos(ay); + btScalar sy = btSin(ay); + btScalar cz = btCos(az); + btScalar sz = btSin(az); + btVector3 ref; + ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2]; + ref[1] = -sz * axis[0] + cz * axis[1]; + ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2]; tr = p6DOF->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) + btVector3 normal = -tr.getBasis().getColumn(0); + btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; + btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; + if (minFi > maxFi) { - tr = p6DOF->getCalculatedTransformA(); - const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin(); - btVector3 up = tr.getBasis().getColumn(2); - btVector3 axis = tr.getBasis().getColumn(0); - btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; - btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; - btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; - btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; - getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0)); - axis = tr.getBasis().getColumn(1); - btScalar ay = p6DOF->getAngle(1); - btScalar az = p6DOF->getAngle(2); - btScalar cy = btCos(ay); - btScalar sy = btSin(ay); - btScalar cz = btCos(az); - btScalar sz = btSin(az); - btVector3 ref; - ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; - ref[1] = -sz*axis[0] + cz*axis[1]; - ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; - tr = p6DOF->getCalculatedTransformB(); - btVector3 normal = -tr.getBasis().getColumn(0); - btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; - btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit; - if(minFi > maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false); - } - else if(minFi < maxFi) - { - getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true); - } - tr = p6DOF->getCalculatedTransformA(); - btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; - btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; - getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0)); + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false); + } + else if (minFi < maxFi) + { + getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true); } + tr = p6DOF->getCalculatedTransformA(); + btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit; + btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit; + getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0)); } - break; + } + break; ///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage case D6_SPRING_2_CONSTRAINT_TYPE: { @@ -1324,9 +1282,12 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) btVector3 axis = tr.getBasis().getColumn(0); btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit; btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit; - btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; - btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; - getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0)); + if (minTh <= maxTh) + { + btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit; + btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit; + getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0)); + } axis = tr.getBasis().getColumn(1); btScalar ay = p6DOF->getAngle(1); btScalar az = p6DOF->getAngle(2); @@ -1335,9 +1296,9 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) btScalar cz = btCos(az); btScalar sz = btSin(az); btVector3 ref; - ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2]; - ref[1] = -sz*axis[0] + cz*axis[1]; - ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2]; + ref[0] = cy * cz * axis[0] + cy * sz * axis[1] - sy * axis[2]; + ref[1] = -sz * axis[0] + cz * axis[1]; + ref[2] = cz * sy * axis[0] + sz * sy * axis[1] + cy * axis[2]; tr = p6DOF->getCalculatedTransformB(); btVector3 normal = -tr.getBasis().getColumn(0); btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit; @@ -1359,42 +1320,38 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint) break; } case SLIDER_CONSTRAINT_TYPE: + { + btSliderConstraint* pSlider = (btSliderConstraint*)constraint; + btTransform tr = pSlider->getCalculatedTransformA(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + tr = pSlider->getCalculatedTransformB(); + if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); + if (drawLimits) { - btSliderConstraint* pSlider = (btSliderConstraint*)constraint; - btTransform tr = pSlider->getCalculatedTransformA(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - tr = pSlider->getCalculatedTransformB(); - if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize); - if(drawLimits) - { - btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB(); - btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); - btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); - getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); - btVector3 normal = tr.getBasis().getColumn(0); - btVector3 axis = tr.getBasis().getColumn(1); - btScalar a_min = pSlider->getLowerAngLimit(); - btScalar a_max = pSlider->getUpperAngLimit(); - const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); - getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true); - } + btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB(); + btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f); + btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f); + getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0)); + btVector3 normal = tr.getBasis().getColumn(0); + btVector3 axis = tr.getBasis().getColumn(1); + btScalar a_min = pSlider->getLowerAngLimit(); + btScalar a_max = pSlider->getUpperAngLimit(); + const btVector3& center = pSlider->getCalculatedTransformB().getOrigin(); + getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0, 0, 0), true); } - break; - default : + } + break; + default: break; } return; } - - - - -void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) { - btAlignedFree( m_constraintSolver); + btAlignedFree(m_constraintSolver); } m_ownsConstraintSolver = false; m_constraintSolver = solver; @@ -1406,8 +1363,7 @@ btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() return m_constraintSolver; } - -int btDiscreteDynamicsWorld::getNumConstraints() const +int btDiscreteDynamicsWorld::getNumConstraints() const { return int(m_constraints.size()); } @@ -1420,90 +1376,85 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const return m_constraints[index]; } - - -void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) +void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer) { int i; //serialize all collision objects - for (i=0;i<m_collisionObjects.size();i++) + for (i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) { int len = colObj->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(len,1); + btChunk* chunk = serializer->allocate(len, 1); const char* structType = colObj->serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj); + serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, colObj); } } - for (i=0;i<m_constraints.size();i++) + for (i = 0; i < m_constraints.size(); i++) { btTypedConstraint* constraint = m_constraints[i]; int size = constraint->calculateSerializeBufferSize(); - btChunk* chunk = serializer->allocate(size,1); - const char* structType = constraint->serialize(chunk->m_oldPtr,serializer); - serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint); + btChunk* chunk = serializer->allocate(size, 1); + const char* structType = constraint->serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk, structType, BT_CONSTRAINT_CODE, constraint); } } - - - -void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer) +void btDiscreteDynamicsWorld::serializeDynamicsWorldInfo(btSerializer* serializer) { #ifdef BT_USE_DOUBLE_PRECISION - int len = sizeof(btDynamicsWorldDoubleData); - btChunk* chunk = serializer->allocate(len,1); - btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr; -#else//BT_USE_DOUBLE_PRECISION - int len = sizeof(btDynamicsWorldFloatData); - btChunk* chunk = serializer->allocate(len,1); - btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr; -#endif//BT_USE_DOUBLE_PRECISION - - memset(worldInfo ,0x00,len); - - m_gravity.serialize(worldInfo->m_gravity); - worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau; - worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping; - worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction; - worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep; - - worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution; - worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction; - worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor; - worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp; - - worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2; - worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm; - worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold; - worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp; - - worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop; - worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor; - worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce; - worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold; - - worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations; - worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode; - worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold; - worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize; - - worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse; - + int len = sizeof(btDynamicsWorldDoubleData); + btChunk* chunk = serializer->allocate(len, 1); + btDynamicsWorldDoubleData* worldInfo = (btDynamicsWorldDoubleData*)chunk->m_oldPtr; +#else //BT_USE_DOUBLE_PRECISION + int len = sizeof(btDynamicsWorldFloatData); + btChunk* chunk = serializer->allocate(len, 1); + btDynamicsWorldFloatData* worldInfo = (btDynamicsWorldFloatData*)chunk->m_oldPtr; +#endif //BT_USE_DOUBLE_PRECISION + + memset(worldInfo, 0x00, len); + + m_gravity.serialize(worldInfo->m_gravity); + worldInfo->m_solverInfo.m_tau = getSolverInfo().m_tau; + worldInfo->m_solverInfo.m_damping = getSolverInfo().m_damping; + worldInfo->m_solverInfo.m_friction = getSolverInfo().m_friction; + worldInfo->m_solverInfo.m_timeStep = getSolverInfo().m_timeStep; + + worldInfo->m_solverInfo.m_restitution = getSolverInfo().m_restitution; + worldInfo->m_solverInfo.m_maxErrorReduction = getSolverInfo().m_maxErrorReduction; + worldInfo->m_solverInfo.m_sor = getSolverInfo().m_sor; + worldInfo->m_solverInfo.m_erp = getSolverInfo().m_erp; + + worldInfo->m_solverInfo.m_erp2 = getSolverInfo().m_erp2; + worldInfo->m_solverInfo.m_globalCfm = getSolverInfo().m_globalCfm; + worldInfo->m_solverInfo.m_splitImpulsePenetrationThreshold = getSolverInfo().m_splitImpulsePenetrationThreshold; + worldInfo->m_solverInfo.m_splitImpulseTurnErp = getSolverInfo().m_splitImpulseTurnErp; + + worldInfo->m_solverInfo.m_linearSlop = getSolverInfo().m_linearSlop; + worldInfo->m_solverInfo.m_warmstartingFactor = getSolverInfo().m_warmstartingFactor; + worldInfo->m_solverInfo.m_maxGyroscopicForce = getSolverInfo().m_maxGyroscopicForce; + worldInfo->m_solverInfo.m_singleAxisRollingFrictionThreshold = getSolverInfo().m_singleAxisRollingFrictionThreshold; + + worldInfo->m_solverInfo.m_numIterations = getSolverInfo().m_numIterations; + worldInfo->m_solverInfo.m_solverMode = getSolverInfo().m_solverMode; + worldInfo->m_solverInfo.m_restingContactRestitutionThreshold = getSolverInfo().m_restingContactRestitutionThreshold; + worldInfo->m_solverInfo.m_minimumSolverBatchSize = getSolverInfo().m_minimumSolverBatchSize; + + worldInfo->m_solverInfo.m_splitImpulse = getSolverInfo().m_splitImpulse; + + #ifdef BT_USE_DOUBLE_PRECISION - const char* structType = "btDynamicsWorldDoubleData"; -#else//BT_USE_DOUBLE_PRECISION - const char* structType = "btDynamicsWorldFloatData"; -#endif//BT_USE_DOUBLE_PRECISION - serializer->finalizeChunk(chunk,structType,BT_DYNAMICSWORLD_CODE,worldInfo); + const char* structType = "btDynamicsWorldDoubleData"; +#else //BT_USE_DOUBLE_PRECISION + const char* structType = "btDynamicsWorldFloatData"; +#endif //BT_USE_DOUBLE_PRECISION + serializer->finalizeChunk(chunk, structType, BT_DYNAMICSWORLD_CODE, worldInfo); } -void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) +void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) { - serializer->startSerialization(); serializeDynamicsWorldInfo(serializer); @@ -1512,6 +1463,7 @@ void btDiscreteDynamicsWorld::serialize(btSerializer* serializer) serializeRigidBodies(serializer); + serializeContactManifolds(serializer); + serializer->finishSerialization(); } - diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index dd3d1c3660c..73607c61fd5 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -13,12 +13,10 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #ifndef BT_DISCRETE_DYNAMICS_WORLD_H #define BT_DISCRETE_DYNAMICS_WORLD_H #include "btDynamicsWorld.h" - class btDispatcher; class btOverlappingPairCache; class btConstraintSolver; @@ -27,159 +25,161 @@ class btTypedConstraint; class btActionInterface; class btPersistentManifold; class btIDebugDraw; + struct InplaceSolverIslandCallback; #include "LinearMath/btAlignedObjectArray.h" - +#include "LinearMath/btThreads.h" ///btDiscreteDynamicsWorld provides discrete rigid body simulation ///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController -ATTRIBUTE_ALIGNED16(class) btDiscreteDynamicsWorld : public btDynamicsWorld +ATTRIBUTE_ALIGNED16(class) +btDiscreteDynamicsWorld : public btDynamicsWorld { protected: - - btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints; - InplaceSolverIslandCallback* m_solverIslandCallback; + btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints; + InplaceSolverIslandCallback* m_solverIslandCallback; - btConstraintSolver* m_constraintSolver; + btConstraintSolver* m_constraintSolver; - btSimulationIslandManager* m_islandManager; + btSimulationIslandManager* m_islandManager; btAlignedObjectArray<btTypedConstraint*> m_constraints; btAlignedObjectArray<btRigidBody*> m_nonStaticRigidBodies; - btVector3 m_gravity; + btVector3 m_gravity; //for variable timesteps - btScalar m_localTime; - btScalar m_fixedTimeStep; + btScalar m_localTime; + btScalar m_fixedTimeStep; //for variable timesteps - bool m_ownsIslandManager; - bool m_ownsConstraintSolver; - bool m_synchronizeAllMotionStates; - bool m_applySpeculativeContactRestitution; + bool m_ownsIslandManager; + bool m_ownsConstraintSolver; + bool m_synchronizeAllMotionStates; + bool m_applySpeculativeContactRestitution; - btAlignedObjectArray<btActionInterface*> m_actions; - - int m_profileTimings; + btAlignedObjectArray<btActionInterface*> m_actions; - bool m_latencyMotionStateInterpolation; + int m_profileTimings; - btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds; + bool m_latencyMotionStateInterpolation; - virtual void predictUnconstraintMotion(btScalar timeStep); - - virtual void integrateTransforms(btScalar timeStep); - - virtual void calculateSimulationIslands(); + btAlignedObjectArray<btPersistentManifold*> m_predictiveManifolds; + btSpinMutex m_predictiveManifoldsMutex; // used to synchronize threads creating predictive contacts - virtual void solveConstraints(btContactSolverInfo& solverInfo); - - virtual void updateActivationState(btScalar timeStep); + virtual void predictUnconstraintMotion(btScalar timeStep); + + void integrateTransformsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep); // can be called in parallel + virtual void integrateTransforms(btScalar timeStep); - void updateActions(btScalar timeStep); + virtual void calculateSimulationIslands(); - void startProfiling(btScalar timeStep); + - virtual void internalSingleStepSimulation( btScalar timeStep); + virtual void updateActivationState(btScalar timeStep); - void createPredictiveContacts(btScalar timeStep); + void updateActions(btScalar timeStep); - virtual void saveKinematicState(btScalar timeStep); + void startProfiling(btScalar timeStep); - void serializeRigidBodies(btSerializer* serializer); + virtual void internalSingleStepSimulation(btScalar timeStep); - void serializeDynamicsWorldInfo(btSerializer* serializer); + void releasePredictiveContacts(); + void createPredictiveContactsInternal(btRigidBody * *bodies, int numBodies, btScalar timeStep); // can be called in parallel + virtual void createPredictiveContacts(btScalar timeStep); -public: + virtual void saveKinematicState(btScalar timeStep); + void serializeRigidBodies(btSerializer * serializer); + void serializeDynamicsWorldInfo(btSerializer * serializer); + +public: BT_DECLARE_ALIGNED_ALLOCATOR(); ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those - btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + btDiscreteDynamicsWorld(btDispatcher * dispatcher, btBroadphaseInterface * pairCache, btConstraintSolver * constraintSolver, btCollisionConfiguration * collisionConfiguration); virtual ~btDiscreteDynamicsWorld(); ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); - - virtual void synchronizeMotionStates(); + virtual void solveConstraints(btContactSolverInfo & solverInfo); + + virtual void synchronizeMotionStates(); ///this can be useful to synchronize a single rigid body -> graphics object - void synchronizeSingleMotionState(btRigidBody* body); + void synchronizeSingleMotionState(btRigidBody * body); - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + virtual void addConstraint(btTypedConstraint * constraint, bool disableCollisionsBetweenLinkedBodies = false); - virtual void removeConstraint(btTypedConstraint* constraint); + virtual void removeConstraint(btTypedConstraint * constraint); - virtual void addAction(btActionInterface*); + virtual void addAction(btActionInterface*); - virtual void removeAction(btActionInterface*); - - btSimulationIslandManager* getSimulationIslandManager() + virtual void removeAction(btActionInterface*); + + btSimulationIslandManager* getSimulationIslandManager() { return m_islandManager; } - const btSimulationIslandManager* getSimulationIslandManager() const + const btSimulationIslandManager* getSimulationIslandManager() const { return m_islandManager; } - btCollisionWorld* getCollisionWorld() + btCollisionWorld* getCollisionWorld() { return this; } - virtual void setGravity(const btVector3& gravity); + virtual void setGravity(const btVector3& gravity); - virtual btVector3 getGravity () const; + virtual btVector3 getGravity() const; - virtual void addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup=btBroadphaseProxy::StaticFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + virtual void addCollisionObject(btCollisionObject * collisionObject, int collisionFilterGroup = btBroadphaseProxy::StaticFilter, int collisionFilterMask = btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - virtual void addRigidBody(btRigidBody* body); + virtual void addRigidBody(btRigidBody * body); - virtual void addRigidBody(btRigidBody* body, short group, short mask); + virtual void addRigidBody(btRigidBody * body, int group, int mask); - virtual void removeRigidBody(btRigidBody* body); + virtual void removeRigidBody(btRigidBody * body); ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); + virtual void removeCollisionObject(btCollisionObject * collisionObject); + virtual void debugDrawConstraint(btTypedConstraint * constraint); - virtual void debugDrawConstraint(btTypedConstraint* constraint); + virtual void debugDrawWorld(); - virtual void debugDrawWorld(); - - virtual void setConstraintSolver(btConstraintSolver* solver); + virtual void setConstraintSolver(btConstraintSolver * solver); virtual btConstraintSolver* getConstraintSolver(); - - virtual int getNumConstraints() const; - virtual btTypedConstraint* getConstraint(int index) ; + virtual int getNumConstraints() const; + + virtual btTypedConstraint* getConstraint(int index); virtual const btTypedConstraint* getConstraint(int index) const; - - virtual btDynamicsWorldType getWorldType() const + virtual btDynamicsWorldType getWorldType() const { return BT_DISCRETE_DYNAMICS_WORLD; } - + ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep. - virtual void clearForces(); + virtual void clearForces(); ///apply gravity, call this once per timestep - virtual void applyGravity(); + virtual void applyGravity(); - virtual void setNumTasks(int numTasks) + virtual void setNumTasks(int numTasks) { - (void) numTasks; + (void)numTasks; } ///obsolete, use updateActions instead @@ -189,15 +189,15 @@ public: } ///obsolete, use addAction instead - virtual void addVehicle(btActionInterface* vehicle); + virtual void addVehicle(btActionInterface * vehicle); ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle); + virtual void removeVehicle(btActionInterface * vehicle); ///obsolete, use addAction instead - virtual void addCharacter(btActionInterface* character); + virtual void addCharacter(btActionInterface * character); ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character); + virtual void removeCharacter(btActionInterface * character); - void setSynchronizeAllMotionStates(bool synchronizeAll) + void setSynchronizeAllMotionStates(bool synchronizeAll) { m_synchronizeAllMotionStates = synchronizeAll; } @@ -210,18 +210,18 @@ public: { m_applySpeculativeContactRestitution = enable; } - + bool getApplySpeculativeContactRestitution() const { return m_applySpeculativeContactRestitution; } ///Preliminary serialization test for Bullet 2.76. Loading those files requires a separate parser (see Bullet/Demos/SerializeDemo) - virtual void serialize(btSerializer* serializer); + virtual void serialize(btSerializer * serializer); ///Interpolate motion state between previous and current transform, instead of current and next transform. ///This can relieve discontinuities in the rendering, due to penetrations - void setLatencyMotionStateInterpolation(bool latencyInterpolation ) + void setLatencyMotionStateInterpolation(bool latencyInterpolation) { m_latencyMotionStateInterpolation = latencyInterpolation; } @@ -229,6 +229,16 @@ public: { return m_latencyMotionStateInterpolation; } + + btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() + { + return m_nonStaticRigidBodies; + } + + const btAlignedObjectArray<btRigidBody*>& getNonStaticRigidBodies() const + { + return m_nonStaticRigidBodies; + } }; -#endif //BT_DISCRETE_DYNAMICS_WORLD_H +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp new file mode 100644 index 00000000000..8207b471356 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.cpp @@ -0,0 +1,263 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "btDiscreteDynamicsWorldMt.h" + +//collision detection +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "btSimulationIslandManagerMt.h" +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btQuickprof.h" + +//rigidbody & constraints +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" +#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h" +#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" + +#include "LinearMath/btIDebugDraw.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" + +#include "BulletDynamics/Dynamics/btActionInterface.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btMotionState.h" + +#include "LinearMath/btSerializer.h" + +/// +/// btConstraintSolverPoolMt +/// + +btConstraintSolverPoolMt::ThreadSolver* btConstraintSolverPoolMt::getAndLockThreadSolver() +{ + int i = 0; +#if BT_THREADSAFE + i = btGetCurrentThreadIndex() % m_solvers.size(); +#endif // #if BT_THREADSAFE + while (true) + { + ThreadSolver& solver = m_solvers[i]; + if (solver.mutex.tryLock()) + { + return &solver; + } + // failed, try the next one + i = (i + 1) % m_solvers.size(); + } + return NULL; +} + +void btConstraintSolverPoolMt::init(btConstraintSolver** solvers, int numSolvers) +{ + m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER; + m_solvers.resize(numSolvers); + for (int i = 0; i < numSolvers; ++i) + { + m_solvers[i].solver = solvers[i]; + } + if (numSolvers > 0) + { + m_solverType = solvers[0]->getSolverType(); + } +} + +// create the solvers for me +btConstraintSolverPoolMt::btConstraintSolverPoolMt(int numSolvers) +{ + btAlignedObjectArray<btConstraintSolver*> solvers; + solvers.reserve(numSolvers); + for (int i = 0; i < numSolvers; ++i) + { + btConstraintSolver* solver = new btSequentialImpulseConstraintSolver(); + solvers.push_back(solver); + } + init(&solvers[0], numSolvers); +} + +// pass in fully constructed solvers (destructor will delete them) +btConstraintSolverPoolMt::btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers) +{ + init(solvers, numSolvers); +} + +btConstraintSolverPoolMt::~btConstraintSolverPoolMt() +{ + // delete all solvers + for (int i = 0; i < m_solvers.size(); ++i) + { + ThreadSolver& solver = m_solvers[i]; + delete solver.solver; + solver.solver = NULL; + } +} + +///solve a group of constraints +btScalar btConstraintSolverPoolMt::solveGroup(btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifolds, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* dispatcher) +{ + ThreadSolver* ts = getAndLockThreadSolver(); + ts->solver->solveGroup(bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher); + ts->mutex.unlock(); + return 0.0f; +} + +void btConstraintSolverPoolMt::reset() +{ + for (int i = 0; i < m_solvers.size(); ++i) + { + ThreadSolver& solver = m_solvers[i]; + solver.mutex.lock(); + solver.solver->reset(); + solver.mutex.unlock(); + } +} + +/// +/// btDiscreteDynamicsWorldMt +/// + +btDiscreteDynamicsWorldMt::btDiscreteDynamicsWorldMt(btDispatcher* dispatcher, + btBroadphaseInterface* pairCache, + btConstraintSolverPoolMt* solverPool, + btConstraintSolver* constraintSolverMt, + btCollisionConfiguration* collisionConfiguration) + : btDiscreteDynamicsWorld(dispatcher, pairCache, solverPool, collisionConfiguration) +{ + if (m_ownsIslandManager) + { + m_islandManager->~btSimulationIslandManager(); + btAlignedFree(m_islandManager); + } + { + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManagerMt), 16); + btSimulationIslandManagerMt* im = new (mem) btSimulationIslandManagerMt(); + im->setMinimumSolverBatchSize(m_solverInfo.m_minimumSolverBatchSize); + m_islandManager = im; + } + m_constraintSolverMt = constraintSolverMt; +} + +btDiscreteDynamicsWorldMt::~btDiscreteDynamicsWorldMt() +{ +} + +void btDiscreteDynamicsWorldMt::solveConstraints(btContactSolverInfo& solverInfo) +{ + BT_PROFILE("solveConstraints"); + + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); + + /// solve all the constraints for this island + btSimulationIslandManagerMt* im = static_cast<btSimulationIslandManagerMt*>(m_islandManager); + btSimulationIslandManagerMt::SolverParams solverParams; + solverParams.m_solverPool = m_constraintSolver; + solverParams.m_solverMt = m_constraintSolverMt; + solverParams.m_solverInfo = &solverInfo; + solverParams.m_debugDrawer = m_debugDrawer; + solverParams.m_dispatcher = getCollisionWorld()->getDispatcher(); + im->buildAndProcessIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_constraints, solverParams); + + m_constraintSolver->allSolved(solverInfo, m_debugDrawer); +} + +struct UpdaterUnconstrainedMotion : public btIParallelForBody +{ + btScalar timeStep; + btRigidBody** rigidBodies; + + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + for (int i = iBegin; i < iEnd; ++i) + { + btRigidBody* body = rigidBodies[i]; + if (!body->isStaticOrKinematicObject()) + { + //don't integrate/update velocities here, it happens in the constraint solver + body->applyDamping(timeStep); + body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform()); + } + } + } +}; + +void btDiscreteDynamicsWorldMt::predictUnconstraintMotion(btScalar timeStep) +{ + BT_PROFILE("predictUnconstraintMotion"); + if (m_nonStaticRigidBodies.size() > 0) + { + UpdaterUnconstrainedMotion update; + update.timeStep = timeStep; + update.rigidBodies = &m_nonStaticRigidBodies[0]; + int grainSize = 50; // num of iterations per task for task scheduler + btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update); + } +} + +void btDiscreteDynamicsWorldMt::createPredictiveContacts(btScalar timeStep) +{ + BT_PROFILE("createPredictiveContacts"); + releasePredictiveContacts(); + if (m_nonStaticRigidBodies.size() > 0) + { + UpdaterCreatePredictiveContacts update; + update.world = this; + update.timeStep = timeStep; + update.rigidBodies = &m_nonStaticRigidBodies[0]; + int grainSize = 50; // num of iterations per task for task scheduler + btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update); + } +} + +void btDiscreteDynamicsWorldMt::integrateTransforms(btScalar timeStep) +{ + BT_PROFILE("integrateTransforms"); + if (m_nonStaticRigidBodies.size() > 0) + { + UpdaterIntegrateTransforms update; + update.world = this; + update.timeStep = timeStep; + update.rigidBodies = &m_nonStaticRigidBodies[0]; + int grainSize = 50; // num of iterations per task for task scheduler + btParallelFor(0, m_nonStaticRigidBodies.size(), grainSize, update); + } +} + +int btDiscreteDynamicsWorldMt::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) +{ + int numSubSteps = btDiscreteDynamicsWorld::stepSimulation(timeStep, maxSubSteps, fixedTimeStep); + if (btITaskScheduler* scheduler = btGetTaskScheduler()) + { + // tell Bullet's threads to sleep, so other threads can run + scheduler->sleepWorkerThreadsHint(); + } + return numSubSteps; +} diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h new file mode 100644 index 00000000000..dccf35d7a7a --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h @@ -0,0 +1,131 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_DISCRETE_DYNAMICS_WORLD_MT_H +#define BT_DISCRETE_DYNAMICS_WORLD_MT_H + +#include "btDiscreteDynamicsWorld.h" +#include "btSimulationIslandManagerMt.h" +#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" + +/// +/// btConstraintSolverPoolMt - masquerades as a constraint solver, but really it is a threadsafe pool of them. +/// +/// Each solver in the pool is protected by a mutex. When solveGroup is called from a thread, +/// the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the +/// call to the solver. +/// So long as there are at least as many solvers as there are hardware threads, it should never need to +/// spin wait. +/// +class btConstraintSolverPoolMt : public btConstraintSolver +{ +public: + // create the solvers for me + explicit btConstraintSolverPoolMt(int numSolvers); + + // pass in fully constructed solvers (destructor will delete them) + btConstraintSolverPoolMt(btConstraintSolver** solvers, int numSolvers); + + virtual ~btConstraintSolverPoolMt(); + + ///solve a group of constraints + virtual btScalar solveGroup(btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifolds, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* dispatcher) BT_OVERRIDE; + + virtual void reset() BT_OVERRIDE; + virtual btConstraintSolverType getSolverType() const BT_OVERRIDE { return m_solverType; } + +private: + const static size_t kCacheLineSize = 128; + struct ThreadSolver + { + btConstraintSolver* solver; + btSpinMutex mutex; + char _cachelinePadding[kCacheLineSize - sizeof(btSpinMutex) - sizeof(void*)]; // keep mutexes from sharing a cache line + }; + btAlignedObjectArray<ThreadSolver> m_solvers; + btConstraintSolverType m_solverType; + + ThreadSolver* getAndLockThreadSolver(); + void init(btConstraintSolver** solvers, int numSolvers); +}; + +/// +/// btDiscreteDynamicsWorldMt -- a version of DiscreteDynamicsWorld with some minor changes to support +/// solving simulation islands on multiple threads. +/// +/// Should function exactly like btDiscreteDynamicsWorld. +/// Also 3 methods that iterate over all of the rigidbodies can run in parallel: +/// - predictUnconstraintMotion +/// - integrateTransforms +/// - createPredictiveContacts +/// +ATTRIBUTE_ALIGNED16(class) +btDiscreteDynamicsWorldMt : public btDiscreteDynamicsWorld +{ +protected: + btConstraintSolver* m_constraintSolverMt; + + virtual void solveConstraints(btContactSolverInfo & solverInfo) BT_OVERRIDE; + + virtual void predictUnconstraintMotion(btScalar timeStep) BT_OVERRIDE; + + struct UpdaterCreatePredictiveContacts : public btIParallelForBody + { + btScalar timeStep; + btRigidBody** rigidBodies; + btDiscreteDynamicsWorldMt* world; + + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + world->createPredictiveContactsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep); + } + }; + virtual void createPredictiveContacts(btScalar timeStep) BT_OVERRIDE; + + struct UpdaterIntegrateTransforms : public btIParallelForBody + { + btScalar timeStep; + btRigidBody** rigidBodies; + btDiscreteDynamicsWorldMt* world; + + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + world->integrateTransformsInternal(&rigidBodies[iBegin], iEnd - iBegin, timeStep); + } + }; + virtual void integrateTransforms(btScalar timeStep) BT_OVERRIDE; + +public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + + btDiscreteDynamicsWorldMt(btDispatcher * dispatcher, + btBroadphaseInterface * pairCache, + btConstraintSolverPoolMt * solverPool, // Note this should be a solver-pool for multi-threading + btConstraintSolver * constraintSolverMt, // single multi-threaded solver for large islands (or NULL) + btCollisionConfiguration * collisionConfiguration); + virtual ~btDiscreteDynamicsWorldMt(); + + virtual int stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) BT_OVERRIDE; +}; + +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 35dd1400fe7..3c55234a8a9 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -24,144 +24,151 @@ class btActionInterface; class btConstraintSolver; class btDynamicsWorld; - /// Type for the callback for each tick -typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); +typedef void (*btInternalTickCallback)(btDynamicsWorld* world, btScalar timeStep); enum btDynamicsWorldType { - BT_SIMPLE_DYNAMICS_WORLD=1, - BT_DISCRETE_DYNAMICS_WORLD=2, - BT_CONTINUOUS_DYNAMICS_WORLD=3, - BT_SOFT_RIGID_DYNAMICS_WORLD=4, - BT_GPU_DYNAMICS_WORLD=5 + BT_SIMPLE_DYNAMICS_WORLD = 1, + BT_DISCRETE_DYNAMICS_WORLD = 2, + BT_CONTINUOUS_DYNAMICS_WORLD = 3, + BT_SOFT_RIGID_DYNAMICS_WORLD = 4, + BT_GPU_DYNAMICS_WORLD = 5, + BT_SOFT_MULTIBODY_DYNAMICS_WORLD = 6, + BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD = 7 }; ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. class btDynamicsWorld : public btCollisionWorld { - protected: - btInternalTickCallback m_internalTickCallback; - btInternalTickCallback m_internalPreTickCallback; - void* m_worldUserInfo; + btInternalTickCallback m_internalTickCallback; + btInternalTickCallback m_internalPreTickCallback; + void* m_worldUserInfo; - btContactSolverInfo m_solverInfo; + btContactSolverInfo m_solverInfo; public: - + btDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* broadphase, btCollisionConfiguration* collisionConfiguration) + : btCollisionWorld(dispatcher, broadphase, collisionConfiguration), m_internalTickCallback(0), m_internalPreTickCallback(0), m_worldUserInfo(0) + { + } - btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) - :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0),m_internalPreTickCallback(0), m_worldUserInfo(0) - { - } + virtual ~btDynamicsWorld() + { + } - virtual ~btDynamicsWorld() - { - } - - ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. - ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. - ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. - ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; - - virtual void debugDrawWorld() = 0; - - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) - { - (void)constraint; (void)disableCollisionsBetweenLinkedBodies; - } + ///stepSimulation proceeds the simulation over 'timeStep', units in preferably in seconds. + ///By default, Bullet will subdivide the timestep in constant substeps of each 'fixedTimeStep'. + ///in order to keep the simulation real-time, the maximum number of substeps can be clamped to 'maxSubSteps'. + ///You can disable subdividing the timestep/substepping by passing maxSubSteps=0 as second argument to stepSimulation, but in that case you have to keep the timeStep constant. + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)) = 0; - virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;} + virtual void debugDrawWorld() = 0; - virtual void addAction(btActionInterface* action) = 0; + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies = false) + { + (void)constraint; + (void)disableCollisionsBetweenLinkedBodies; + } - virtual void removeAction(btActionInterface* action) = 0; + virtual void removeConstraint(btTypedConstraint* constraint) { (void)constraint; } - //once a rigidbody is added to the dynamics world, it will get this gravity assigned - //existing rigidbodies in the world get gravity assigned too, during this method - virtual void setGravity(const btVector3& gravity) = 0; - virtual btVector3 getGravity () const = 0; + virtual void addAction(btActionInterface* action) = 0; - virtual void synchronizeMotionStates() = 0; + virtual void removeAction(btActionInterface* action) = 0; - virtual void addRigidBody(btRigidBody* body) = 0; + //once a rigidbody is added to the dynamics world, it will get this gravity assigned + //existing rigidbodies in the world get gravity assigned too, during this method + virtual void setGravity(const btVector3& gravity) = 0; + virtual btVector3 getGravity() const = 0; - virtual void addRigidBody(btRigidBody* body, short group, short mask) = 0; + virtual void synchronizeMotionStates() = 0; - virtual void removeRigidBody(btRigidBody* body) = 0; + virtual void addRigidBody(btRigidBody* body) = 0; - virtual void setConstraintSolver(btConstraintSolver* solver) = 0; + virtual void addRigidBody(btRigidBody* body, int group, int mask) = 0; - virtual btConstraintSolver* getConstraintSolver() = 0; - - virtual int getNumConstraints() const { return 0; } - - virtual btTypedConstraint* getConstraint(int index) { (void)index; return 0; } - - virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } + virtual void removeRigidBody(btRigidBody* body) = 0; - virtual btDynamicsWorldType getWorldType() const=0; + virtual void setConstraintSolver(btConstraintSolver* solver) = 0; - virtual void clearForces() = 0; + virtual btConstraintSolver* getConstraintSolver() = 0; - /// Set the callback for when an internal tick (simulation substep) happens, optional user info - void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo=0,bool isPreTick=false) - { - if (isPreTick) - { - m_internalPreTickCallback = cb; - } else - { - m_internalTickCallback = cb; - } - m_worldUserInfo = worldUserInfo; - } + virtual int getNumConstraints() const { return 0; } - void setWorldUserInfo(void* worldUserInfo) - { - m_worldUserInfo = worldUserInfo; - } + virtual btTypedConstraint* getConstraint(int index) + { + (void)index; + return 0; + } + + virtual const btTypedConstraint* getConstraint(int index) const + { + (void)index; + return 0; + } - void* getWorldUserInfo() const + virtual btDynamicsWorldType getWorldType() const = 0; + + virtual void clearForces() = 0; + + /// Set the callback for when an internal tick (simulation substep) happens, optional user info + void setInternalTickCallback(btInternalTickCallback cb, void* worldUserInfo = 0, bool isPreTick = false) + { + if (isPreTick) { - return m_worldUserInfo; + m_internalPreTickCallback = cb; } - - btContactSolverInfo& getSolverInfo() + else { - return m_solverInfo; + m_internalTickCallback = cb; } - - - ///obsolete, use addAction instead. - virtual void addVehicle(btActionInterface* vehicle) {(void)vehicle;} - ///obsolete, use removeAction instead - virtual void removeVehicle(btActionInterface* vehicle) {(void)vehicle;} - ///obsolete, use addAction instead. - virtual void addCharacter(btActionInterface* character) {(void)character;} - ///obsolete, use removeAction instead - virtual void removeCharacter(btActionInterface* character) {(void)character;} - - + m_worldUserInfo = worldUserInfo; + } + + void setWorldUserInfo(void* worldUserInfo) + { + m_worldUserInfo = worldUserInfo; + } + + void* getWorldUserInfo() const + { + return m_worldUserInfo; + } + + btContactSolverInfo& getSolverInfo() + { + return m_solverInfo; + } + + const btContactSolverInfo& getSolverInfo() const + { + return m_solverInfo; + } + + ///obsolete, use addAction instead. + virtual void addVehicle(btActionInterface* vehicle) { (void)vehicle; } + ///obsolete, use removeAction instead + virtual void removeVehicle(btActionInterface* vehicle) { (void)vehicle; } + ///obsolete, use addAction instead. + virtual void addCharacter(btActionInterface* character) { (void)character; } + ///obsolete, use removeAction instead + virtual void removeCharacter(btActionInterface* character) { (void)character; } }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btDynamicsWorldDoubleData { - btContactSolverInfoDoubleData m_solverInfo; - btVector3DoubleData m_gravity; + btContactSolverInfoDoubleData m_solverInfo; + btVector3DoubleData m_gravity; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 struct btDynamicsWorldFloatData { - btContactSolverInfoFloatData m_solverInfo; - btVector3FloatData m_gravity; + btContactSolverInfoFloatData m_solverInfo; + btVector3FloatData m_gravity; }; - -#endif //BT_DYNAMICS_WORLD_H - - +#endif //BT_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index a7882684bf1..27fdead7617 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -22,36 +22,34 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" //'temporarily' global variables -btScalar gDeactivationTime = btScalar(2.); -bool gDisableDeactivation = false; +btScalar gDeactivationTime = btScalar(2.); +bool gDisableDeactivation = false; static int uniqueId = 0; - btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) { setupRigidBody(constructionInfo); } -btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia) +btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia) { - btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); + btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia); setupRigidBody(cinfo); } -void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) +void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) { - - m_internalType=CO_RIGID_BODY; + m_internalType = CO_RIGID_BODY; m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); - m_angularFactor.setValue(1,1,1); - m_linearFactor.setValue(1,1,1); + m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); + m_angularFactor.setValue(1, 1, 1); + m_linearFactor.setValue(1, 1, 1); m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); + setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; @@ -67,46 +65,44 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& if (m_optionalMotionState) { m_optionalMotionState->getWorldTransform(m_worldTransform); - } else + } + else { m_worldTransform = constructionInfo.m_startWorldTransform; } m_interpolationWorldTransform = m_worldTransform; - m_interpolationLinearVelocity.setValue(0,0,0); - m_interpolationAngularVelocity.setValue(0,0,0); - + m_interpolationLinearVelocity.setValue(0, 0, 0); + m_interpolationAngularVelocity.setValue(0, 0, 0); + //moved to btCollisionObject m_friction = constructionInfo.m_friction; m_rollingFriction = constructionInfo.m_rollingFriction; + m_spinningFriction = constructionInfo.m_spinningFriction; + m_restitution = constructionInfo.m_restitution; - setCollisionShape( constructionInfo.m_collisionShape ); + setCollisionShape(constructionInfo.m_collisionShape); m_debugBodyId = uniqueId++; - + setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); updateInertiaTensor(); m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY; - m_deltaLinearVelocity.setZero(); m_deltaAngularVelocity.setZero(); - m_invMass = m_inverseMass*m_linearFactor; + m_invMass = m_inverseMass * m_linearFactor; m_pushVelocity.setZero(); m_turnVelocity.setZero(); - - - } - -void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) +void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) { - btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); + btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); } -void btRigidBody::saveKinematicState(btScalar timeStep) +void btRigidBody::saveKinematicState(btScalar timeStep) { //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities if (timeStep != btScalar(0.)) @@ -114,25 +110,22 @@ void btRigidBody::saveKinematicState(btScalar timeStep) //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform if (getMotionState()) getMotionState()->getWorldTransform(m_worldTransform); - btVector3 linVel,angVel; - - btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity); + btVector3 linVel, angVel; + + btTransformUtil::calculateVelocity(m_interpolationWorldTransform, m_worldTransform, timeStep, m_linearVelocity, m_angularVelocity); m_interpolationLinearVelocity = m_linearVelocity; m_interpolationAngularVelocity = m_angularVelocity; m_interpolationWorldTransform = m_worldTransform; //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); } } - -void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const + +void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const { - getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); + getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax); } - - - -void btRigidBody::setGravity(const btVector3& acceleration) +void btRigidBody::setGravity(const btVector3& acceleration) { if (m_inverseMass != btScalar(0.0)) { @@ -141,33 +134,29 @@ void btRigidBody::setGravity(const btVector3& acceleration) m_gravity_acceleration = acceleration; } - - - - - void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) { - m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); - m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); +#ifdef BT_USE_OLD_DAMPING_METHOD + m_linearDamping = btMax(lin_damping, btScalar(0.0)); + m_angularDamping = btMax(ang_damping, btScalar(0.0)); +#else + m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0)); + m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0)); +#endif } - - - ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping -void btRigidBody::applyDamping(btScalar timeStep) +void btRigidBody::applyDamping(btScalar timeStep) { //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway -//#define USE_OLD_DAMPING_METHOD 1 -#ifdef USE_OLD_DAMPING_METHOD - m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); - m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); +#ifdef BT_USE_OLD_DAMPING_METHOD + m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0)); + m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0)); #else - m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep); - m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep); + m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep); + m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep); #endif if (m_additionalDamping) @@ -180,7 +169,6 @@ void btRigidBody::applyDamping(btScalar timeStep) m_angularVelocity *= m_additionalDampingFactor; m_linearVelocity *= m_additionalDampingFactor; } - btScalar speed = m_linearVelocity.length(); if (speed < m_linearDamping) @@ -189,10 +177,11 @@ void btRigidBody::applyDamping(btScalar timeStep) if (speed > dampVel) { btVector3 dir = m_linearVelocity.normalized(); - m_linearVelocity -= dir * dampVel; - } else + m_linearVelocity -= dir * dampVel; + } + else { - m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } @@ -203,30 +192,36 @@ void btRigidBody::applyDamping(btScalar timeStep) if (angSpeed > angDampVel) { btVector3 dir = m_angularVelocity.normalized(); - m_angularVelocity -= dir * angDampVel; - } else + m_angularVelocity -= dir * angDampVel; + } + else { - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } } } - void btRigidBody::applyGravity() { if (isStaticOrKinematicObject()) return; - - applyCentralForce(m_gravity); + applyCentralForce(m_gravity); +} + +void btRigidBody::clearGravity() +{ + if (isStaticOrKinematicObject()) + return; + + applyCentralForce(-m_gravity); } void btRigidBody::proceedToTransform(const btTransform& newTrans) { - setCenterOfMassTransform( newTrans ); + setCenterOfMassTransform(newTrans); } - void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { @@ -234,7 +229,8 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; m_inverseMass = btScalar(0.); - } else + } + else { m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); m_inverseMass = btScalar(1.0) / mass; @@ -242,50 +238,45 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) //Fg = m * a m_gravity = mass * m_gravity_acceleration; - - m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0), - inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0), - inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0)); - m_invMass = m_linearFactor*m_inverseMass; + m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), + inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), + inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + + m_invMass = m_linearFactor * m_inverseMass; } - -void btRigidBody::updateInertiaTensor() +void btRigidBody::updateInertiaTensor() { m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); } - - btVector3 btRigidBody::getLocalInertia() const { - btVector3 inertiaLocal; const btVector3 inertia = m_invInertiaLocal; inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), - inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), - inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); + inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), + inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); return inertiaLocal; } inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt, - const btMatrix3x3 &I) + const btMatrix3x3& I) { - const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0); + const btVector3 w2 = I * w1 + w1.cross(I * w1) * dt - (T * dt + I * w0); return w2; } inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt, - const btMatrix3x3 &I) + const btMatrix3x3& I) { - btMatrix3x3 w1x, Iw1x; - const btVector3 Iwi = (I*w1); + const btVector3 Iwi = (I * w1); w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]); Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]); - const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt; + const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt; return dfw1; } @@ -293,58 +284,55 @@ btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForc { btVector3 inertiaLocal = getLocalInertia(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); - btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); + btVector3 tmp = inertiaTensorWorld * getAngularVelocity(); btVector3 gf = getAngularVelocity().cross(tmp); btScalar l2 = gf.length2(); - if (l2>maxGyroscopicForce*maxGyroscopicForce) + if (l2 > maxGyroscopicForce * maxGyroscopicForce) { - gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce; + gf *= btScalar(1.) / btSqrt(l2) * maxGyroscopicForce; } return gf; } - btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const -{ +{ btVector3 idl = getLocalInertia(); btVector3 omega1 = getAngularVelocity(); btQuaternion q = getWorldTransform().getRotation(); - + // Convert to body coordinates btVector3 omegab = quatRotate(q.inverse(), omega1); btMatrix3x3 Ib; - Ib.setValue(idl.x(),0,0, - 0,idl.y(),0, - 0,0,idl.z()); - - btVector3 ibo = Ib*omegab; + Ib.setValue(idl.x(), 0, 0, + 0, idl.y(), 0, + 0, 0, idl.z()); + + btVector3 ibo = Ib * omegab; // Residual vector btVector3 f = step * omegab.cross(ibo); - + btMatrix3x3 skew0; omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]); - btVector3 om = Ib*omegab; + btVector3 om = Ib * omegab; btMatrix3x3 skew1; - om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]); - + om.getSkewSymmetricMatrix(&skew1[0], &skew1[1], &skew1[2]); + // Jacobian - btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step; - -// btMatrix3x3 Jinv = J.inverse(); -// btVector3 omega_div = Jinv*f; + btMatrix3x3 J = Ib + (skew0 * Ib - skew1) * step; + + // btMatrix3x3 Jinv = J.inverse(); + // btVector3 omega_div = Jinv*f; btVector3 omega_div = J.solve33(f); - + // Single Newton-Raphson update - omegab = omegab - omega_div;//Solve33(J, f); + omegab = omegab - omega_div; //Solve33(J, f); // Back to world coordinates - btVector3 omega2 = quatRotate(q,omegab); - btVector3 gf = omega2-omega1; + btVector3 omega2 = quatRotate(q, omegab); + btVector3 gf = omega2 - omega1; return gf; } - - btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const { // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior. @@ -359,7 +347,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con m_worldTransform.getBasis().transpose(); // use newtons method to find implicit solution for new angular velocity (w') - // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 + // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 // df/dw' = I + 1xIw'*step + w'xI*step btVector3 w1 = w0; @@ -381,8 +369,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con return gf; } - -void btRigidBody::integrateVelocities(btScalar step) +void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) return; @@ -391,30 +378,31 @@ void btRigidBody::integrateVelocities(btScalar step) m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; #define MAX_ANGVEL SIMD_HALF_PI - /// clamp angular velocity. collision calculations will fail on higher angular velocities + /// clamp angular velocity. collision calculations will fail on higher angular velocities btScalar angvel = m_angularVelocity.length(); - if (angvel*step > MAX_ANGVEL) + if (angvel * step > MAX_ANGVEL) { - m_angularVelocity *= (MAX_ANGVEL/step) /angvel; + m_angularVelocity *= (MAX_ANGVEL / step) / angvel; } - + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif } btQuaternion btRigidBody::getOrientation() const { - btQuaternion orn; - m_worldTransform.getBasis().getRotation(orn); - return orn; + btQuaternion orn; + m_worldTransform.getBasis().getRotation(orn); + return orn; } - - + void btRigidBody::setCenterOfMassTransform(const btTransform& xform) { - if (isKinematicObject()) { m_interpolationWorldTransform = m_worldTransform; - } else + } + else { m_interpolationWorldTransform = xform; } @@ -424,51 +412,59 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform) updateInertiaTensor(); } - -bool btRigidBody::checkCollideWithOverride(const btCollisionObject* co) const -{ - const btRigidBody* otherRb = btRigidBody::upcast(co); - if (!otherRb) - return true; - - for (int i = 0; i < m_constraintRefs.size(); ++i) - { - const btTypedConstraint* c = m_constraintRefs[i]; - if (c->isEnabled()) - if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb) - return false; - } - - return true; -} - - - void btRigidBody::addConstraintRef(btTypedConstraint* c) { + ///disable collision with the 'other' body + int index = m_constraintRefs.findLinearSearch(c); + //don't add constraints that are already referenced + //btAssert(index == m_constraintRefs.size()); if (index == m_constraintRefs.size()) - m_constraintRefs.push_back(c); - - m_checkCollideWith = true; + { + m_constraintRefs.push_back(c); + btCollisionObject* colObjA = &c->getRigidBodyA(); + btCollisionObject* colObjB = &c->getRigidBodyB(); + if (colObjA == this) + { + colObjA->setIgnoreCollisionCheck(colObjB, true); + } + else + { + colObjB->setIgnoreCollisionCheck(colObjA, true); + } + } } void btRigidBody::removeConstraintRef(btTypedConstraint* c) { - m_constraintRefs.remove(c); - m_checkCollideWith = m_constraintRefs.size() > 0; + int index = m_constraintRefs.findLinearSearch(c); + //don't remove constraints that are not referenced + if (index < m_constraintRefs.size()) + { + m_constraintRefs.remove(c); + btCollisionObject* colObjA = &c->getRigidBodyA(); + btCollisionObject* colObjB = &c->getRigidBodyB(); + if (colObjA == this) + { + colObjA->setIgnoreCollisionCheck(colObjB, false); + } + else + { + colObjB->setIgnoreCollisionCheck(colObjA, false); + } + } } -int btRigidBody::calculateSerializeBufferSize() const +int btRigidBody::calculateSerializeBufferSize() const { int sz = sizeof(btRigidBodyData); return sz; } - ///fills the dataBuffer and returns the struct name (and 0 on failure) -const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const +///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const { - btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer; + btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer; btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer); @@ -490,19 +486,20 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr; rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr; rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor; - rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; + rbd->m_linearSleepingThreshold = m_linearSleepingThreshold; rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; + // Fill padding with zeros to appease msan. +#ifdef BT_USE_DOUBLE_PRECISION + memset(rbd->m_padding, 0, sizeof(rbd->m_padding)); +#endif + return btRigidBodyDataName; } - - void btRigidBody::serializeSingleObject(class btSerializer* serializer) const { - btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1); + btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this); } - - diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index c2f8c5d64ae..7442dd1e6a6 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -25,206 +25,197 @@ class btCollisionShape; class btMotionState; class btTypedConstraint; - extern btScalar gDeactivationTime; extern bool gDisableDeactivation; #ifdef BT_USE_DOUBLE_PRECISION -#define btRigidBodyData btRigidBodyDoubleData -#define btRigidBodyDataName "btRigidBodyDoubleData" +#define btRigidBodyData btRigidBodyDoubleData +#define btRigidBodyDataName "btRigidBodyDoubleData" #else -#define btRigidBodyData btRigidBodyFloatData -#define btRigidBodyDataName "btRigidBodyFloatData" -#endif //BT_USE_DOUBLE_PRECISION - +#define btRigidBodyData btRigidBodyFloatData +#define btRigidBodyDataName "btRigidBodyFloatData" +#endif //BT_USE_DOUBLE_PRECISION -enum btRigidBodyFlags +enum btRigidBodyFlags { BT_DISABLE_WORLD_GRAVITY = 1, ///BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards. ///and it BT_ENABLE_GYROPSCOPIC_FORCE becomes equivalent to BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY ///See Demos/GyroscopicDemo and computeGyroscopicImpulseImplicit BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT = 2, - BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD=4, - BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY=8, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD = 4, + BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY = 8, BT_ENABLE_GYROPSCOPIC_FORCE = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY, }; - ///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. ///It is recommended for performance and memory use to share btCollisionShape objects whenever possible. -///There are 3 types of rigid bodies: +///There are 3 types of rigid bodies: ///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics. ///- B) Fixed objects with zero mass. They are not moving (basically collision objects) -///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform. +///- C) Kinematic objects, which are objects without mass, but the user can move them. There is one-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform. ///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time. ///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects) -class btRigidBody : public btCollisionObject +class btRigidBody : public btCollisionObject { - - btMatrix3x3 m_invInertiaTensorWorld; - btVector3 m_linearVelocity; - btVector3 m_angularVelocity; - btScalar m_inverseMass; - btVector3 m_linearFactor; - - btVector3 m_gravity; - btVector3 m_gravity_acceleration; - btVector3 m_invInertiaLocal; - btVector3 m_totalForce; - btVector3 m_totalTorque; - - btScalar m_linearDamping; - btScalar m_angularDamping; - - bool m_additionalDamping; - btScalar m_additionalDampingFactor; - btScalar m_additionalLinearDampingThresholdSqr; - btScalar m_additionalAngularDampingThresholdSqr; - btScalar m_additionalAngularDampingFactor; - - - btScalar m_linearSleepingThreshold; - btScalar m_angularSleepingThreshold; + btMatrix3x3 m_invInertiaTensorWorld; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + btScalar m_inverseMass; + btVector3 m_linearFactor; + + btVector3 m_gravity; + btVector3 m_gravity_acceleration; + btVector3 m_invInertiaLocal; + btVector3 m_totalForce; + btVector3 m_totalTorque; + + btScalar m_linearDamping; + btScalar m_angularDamping; + + bool m_additionalDamping; + btScalar m_additionalDampingFactor; + btScalar m_additionalLinearDampingThresholdSqr; + btScalar m_additionalAngularDampingThresholdSqr; + btScalar m_additionalAngularDampingFactor; + + btScalar m_linearSleepingThreshold; + btScalar m_angularSleepingThreshold; //m_optionalMotionState allows to automatic synchronize the world transform for active objects - btMotionState* m_optionalMotionState; + btMotionState* m_optionalMotionState; //keep track of typed constraints referencing this rigid body, to disable collision between linked bodies btAlignedObjectArray<btTypedConstraint*> m_constraintRefs; - int m_rigidbodyFlags; - - int m_debugBodyId; - - -protected: + int m_rigidbodyFlags; - ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity); - btVector3 m_deltaAngularVelocity; - btVector3 m_angularFactor; - btVector3 m_invMass; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; + int m_debugBodyId; +protected: + ATTRIBUTE_ALIGNED16(btVector3 m_deltaLinearVelocity); + btVector3 m_deltaAngularVelocity; + btVector3 m_angularFactor; + btVector3 m_invMass; + btVector3 m_pushVelocity; + btVector3 m_turnVelocity; public: - - ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument) - ///You can use the motion state to synchronize the world transform between physics and graphics objects. + ///You can use the motion state to synchronize the world transform between physics and graphics objects. ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state, ///m_startWorldTransform is only used when you don't provide a motion state. - struct btRigidBodyConstructionInfo + struct btRigidBodyConstructionInfo { - btScalar m_mass; + btScalar m_mass; ///When a motionState is provided, the rigid body will initialize its world transform from the motion state ///In this case, m_startWorldTransform is ignored. - btMotionState* m_motionState; - btTransform m_startWorldTransform; + btMotionState* m_motionState; + btTransform m_startWorldTransform; - btCollisionShape* m_collisionShape; - btVector3 m_localInertia; - btScalar m_linearDamping; - btScalar m_angularDamping; + btCollisionShape* m_collisionShape; + btVector3 m_localInertia; + btScalar m_linearDamping; + btScalar m_angularDamping; ///best simulation results when friction is non-zero - btScalar m_friction; + btScalar m_friction; ///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever. ///See Bullet/Demos/RollingFrictionDemo for usage - btScalar m_rollingFriction; + btScalar m_rollingFriction; + btScalar m_spinningFriction; //torsional friction around contact normal + ///best simulation results using zero restitution. - btScalar m_restitution; + btScalar m_restitution; - btScalar m_linearSleepingThreshold; - btScalar m_angularSleepingThreshold; + btScalar m_linearSleepingThreshold; + btScalar m_angularSleepingThreshold; //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete - bool m_additionalDamping; - btScalar m_additionalDampingFactor; - btScalar m_additionalLinearDampingThresholdSqr; - btScalar m_additionalAngularDampingThresholdSqr; - btScalar m_additionalAngularDampingFactor; - - btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)): - m_mass(mass), - m_motionState(motionState), - m_collisionShape(collisionShape), - m_localInertia(localInertia), - m_linearDamping(btScalar(0.)), - m_angularDamping(btScalar(0.)), - m_friction(btScalar(0.5)), - m_rollingFriction(btScalar(0)), - m_restitution(btScalar(0.)), - m_linearSleepingThreshold(btScalar(0.8)), - m_angularSleepingThreshold(btScalar(1.f)), - m_additionalDamping(false), - m_additionalDampingFactor(btScalar(0.005)), - m_additionalLinearDampingThresholdSqr(btScalar(0.01)), - m_additionalAngularDampingThresholdSqr(btScalar(0.01)), - m_additionalAngularDampingFactor(btScalar(0.01)) + bool m_additionalDamping; + btScalar m_additionalDampingFactor; + btScalar m_additionalLinearDampingThresholdSqr; + btScalar m_additionalAngularDampingThresholdSqr; + btScalar m_additionalAngularDampingFactor; + + btRigidBodyConstructionInfo(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0)) : m_mass(mass), + m_motionState(motionState), + m_collisionShape(collisionShape), + m_localInertia(localInertia), + m_linearDamping(btScalar(0.)), + m_angularDamping(btScalar(0.)), + m_friction(btScalar(0.5)), + m_rollingFriction(btScalar(0)), + m_spinningFriction(btScalar(0)), + m_restitution(btScalar(0.)), + m_linearSleepingThreshold(btScalar(0.8)), + m_angularSleepingThreshold(btScalar(1.f)), + m_additionalDamping(false), + m_additionalDampingFactor(btScalar(0.005)), + m_additionalLinearDampingThresholdSqr(btScalar(0.01)), + m_additionalAngularDampingThresholdSqr(btScalar(0.01)), + m_additionalAngularDampingFactor(btScalar(0.01)) { m_startWorldTransform.setIdentity(); } }; ///btRigidBody constructor using construction info - btRigidBody( const btRigidBodyConstructionInfo& constructionInfo); + btRigidBody(const btRigidBodyConstructionInfo& constructionInfo); - ///btRigidBody constructor for backwards compatibility. + ///btRigidBody constructor for backwards compatibility. ///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo) - btRigidBody( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)); - + btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia = btVector3(0, 0, 0)); virtual ~btRigidBody() - { - //No constraints should point to this rigidbody - //Remove constraints from the dynamics world before you delete the related rigidbodies. - btAssert(m_constraintRefs.size()==0); - } + { + //No constraints should point to this rigidbody + //Remove constraints from the dynamics world before you delete the related rigidbodies. + btAssert(m_constraintRefs.size() == 0); + } protected: - ///setupRigidBody is only used internally by the constructor - void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); + void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); public: + void proceedToTransform(const btTransform& newTrans); - void proceedToTransform(const btTransform& newTrans); - ///to keep collision detection and dynamics separate we don't store a rigidbody pointer ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast - static const btRigidBody* upcast(const btCollisionObject* colObj) + static const btRigidBody* upcast(const btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) + if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) return (const btRigidBody*)colObj; return 0; } - static btRigidBody* upcast(btCollisionObject* colObj) + static btRigidBody* upcast(btCollisionObject* colObj) { - if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY) + if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY) return (btRigidBody*)colObj; return 0; } - + /// continuous collision detection needs prediction - void predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ; - - void saveKinematicState(btScalar step); - - void applyGravity(); - - void setGravity(const btVector3& acceleration); + void predictIntegratedTransform(btScalar step, btTransform& predictedTransform); + + void saveKinematicState(btScalar step); + + void applyGravity(); + + void clearGravity(); - const btVector3& getGravity() const + void setGravity(const btVector3& acceleration); + + const btVector3& getGravity() const { return m_gravity_acceleration; } - void setDamping(btScalar lin_damping, btScalar ang_damping); + void setDamping(btScalar lin_damping, btScalar ang_damping); btScalar getLinearDamping() const { @@ -246,18 +237,20 @@ public: return m_angularSleepingThreshold; } - void applyDamping(btScalar timeStep); + void applyDamping(btScalar timeStep); - SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const + { return m_collisionShape; } - SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { - return m_collisionShape; + SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() + { + return m_collisionShape; } - - void setMassProps(btScalar mass, const btVector3& inertia); - + + void setMassProps(btScalar mass, const btVector3& inertia); + const btVector3& getLinearFactor() const { return m_linearFactor; @@ -265,20 +258,22 @@ public: void setLinearFactor(const btVector3& linearFactor) { m_linearFactor = linearFactor; - m_invMass = m_linearFactor*m_inverseMass; + m_invMass = m_linearFactor * m_inverseMass; } - btScalar getInvMass() const { return m_inverseMass; } - const btMatrix3x3& getInvInertiaTensorWorld() const { - return m_invInertiaTensorWorld; + btScalar getInvMass() const { return m_inverseMass; } + btScalar getMass() const { return m_inverseMass == btScalar(0.) ? btScalar(0.) : btScalar(1.0) / m_inverseMass; } + const btMatrix3x3& getInvInertiaTensorWorld() const + { + return m_invInertiaTensorWorld; } - - void integrateVelocities(btScalar step); - void setCenterOfMassTransform(const btTransform& xform); + void integrateVelocities(btScalar step); + + void setCenterOfMassTransform(const btTransform& xform); - void applyCentralForce(const btVector3& force) + void applyCentralForce(const btVector3& force) { - m_totalForce += force*m_linearFactor; + m_totalForce += force * m_linearFactor; } const btVector3& getTotalForce() const @@ -290,90 +285,176 @@ public: { return m_totalTorque; }; - + const btVector3& getInvInertiaDiagLocal() const { return m_invInertiaLocal; }; - void setInvInertiaDiagLocal(const btVector3& diagInvInertia) + void setInvInertiaDiagLocal(const btVector3& diagInvInertia) { m_invInertiaLocal = diagInvInertia; } - void setSleepingThresholds(btScalar linear,btScalar angular) + void setSleepingThresholds(btScalar linear, btScalar angular) { m_linearSleepingThreshold = linear; m_angularSleepingThreshold = angular; } - void applyTorque(const btVector3& torque) + void applyTorque(const btVector3& torque) { - m_totalTorque += torque*m_angularFactor; + m_totalTorque += torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_totalTorque); + #endif } - - void applyForce(const btVector3& force, const btVector3& rel_pos) + + void applyForce(const btVector3& force, const btVector3& rel_pos) { applyCentralForce(force); - applyTorque(rel_pos.cross(force*m_linearFactor)); + applyTorque(rel_pos.cross(force * m_linearFactor)); } - + void applyCentralImpulse(const btVector3& impulse) { - m_linearVelocity += impulse *m_linearFactor * m_inverseMass; + m_linearVelocity += impulse * m_linearFactor * m_inverseMass; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_linearVelocity); + #endif } - - void applyTorqueImpulse(const btVector3& torque) + + void applyTorqueImpulse(const btVector3& torque) { - m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif } - - void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) + + void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) { if (m_inverseMass != btScalar(0.)) { applyCentralImpulse(impulse); if (m_angularFactor) { - applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor)); + applyTorqueImpulse(rel_pos.cross(impulse * m_linearFactor)); } } } + + void applyPushImpulse(const btVector3& impulse, const btVector3& rel_pos) + { + if (m_inverseMass != btScalar(0.)) + { + applyCentralPushImpulse(impulse); + if (m_angularFactor) + { + applyTorqueTurnImpulse(rel_pos.cross(impulse * m_linearFactor)); + } + } + } + + btVector3 getPushVelocity() const + { + return m_pushVelocity; + } + + btVector3 getTurnVelocity() const + { + return m_turnVelocity; + } + + void setPushVelocity(const btVector3& v) + { + m_pushVelocity = v; + } + + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + void clampVelocity(btVector3& v) const { + v.setX( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getX())) + ); + v.setY( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getY())) + ); + v.setZ( + fmax(-BT_CLAMP_VELOCITY_TO, + fmin(BT_CLAMP_VELOCITY_TO, v.getZ())) + ); + } + #endif + + void setTurnVelocity(const btVector3& v) + { + m_turnVelocity = v; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_turnVelocity); + #endif + } + + void applyCentralPushImpulse(const btVector3& impulse) + { + m_pushVelocity += impulse * m_linearFactor * m_inverseMass; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_pushVelocity); + #endif + } + + void applyTorqueTurnImpulse(const btVector3& torque) + { + m_turnVelocity += m_invInertiaTensorWorld * torque * m_angularFactor; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_turnVelocity); + #endif + } - void clearForces() + void clearForces() { m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); } - - void updateInertiaTensor(); - - const btVector3& getCenterOfMassPosition() const { - return m_worldTransform.getOrigin(); + + void updateInertiaTensor(); + + const btVector3& getCenterOfMassPosition() const + { + return m_worldTransform.getOrigin(); } btQuaternion getOrientation() const; - - const btTransform& getCenterOfMassTransform() const { - return m_worldTransform; + + const btTransform& getCenterOfMassTransform() const + { + return m_worldTransform; } - const btVector3& getLinearVelocity() const { - return m_linearVelocity; + const btVector3& getLinearVelocity() const + { + return m_linearVelocity; } - const btVector3& getAngularVelocity() const { - return m_angularVelocity; + const btVector3& getAngularVelocity() const + { + return m_angularVelocity; } - inline void setLinearVelocity(const btVector3& lin_vel) - { + { m_updateRevision++; - m_linearVelocity = lin_vel; + m_linearVelocity = lin_vel; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_linearVelocity); + #endif } - inline void setAngularVelocity(const btVector3& ang_vel) - { + inline void setAngularVelocity(const btVector3& ang_vel) + { m_updateRevision++; - m_angularVelocity = ang_vel; + m_angularVelocity = ang_vel; + #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0 + clampVelocity(m_angularVelocity); + #endif } btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const @@ -384,19 +465,20 @@ public: //for kinematic objects, we could also use use: // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; } + + btVector3 getPushVelocityInLocalPoint(const btVector3& rel_pos) const + { + //we also calculate lin/ang velocity for kinematic objects + return m_pushVelocity + m_turnVelocity.cross(rel_pos); + } - void translate(const btVector3& v) + void translate(const btVector3& v) { - m_worldTransform.getOrigin() += v; + m_worldTransform.getOrigin() += v; } - - void getAabb(btVector3& aabbMin,btVector3& aabbMax) const; - - + void getAabb(btVector3& aabbMin, btVector3& aabbMax) const; - - SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const { btVector3 r0 = pos - getCenterOfMassPosition(); @@ -406,7 +488,6 @@ public: btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0); return m_inverseMass + normal.dot(vec); - } SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const @@ -415,26 +496,25 @@ public: return axis.dot(vec); } - SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) + SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) { - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) return; - if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) && - (getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold)) + if ((getLinearVelocity().length2() < m_linearSleepingThreshold * m_linearSleepingThreshold) && + (getAngularVelocity().length2() < m_angularSleepingThreshold * m_angularSleepingThreshold)) { m_deactivationTime += timeStep; - } else + } + else { - m_deactivationTime=btScalar(0.); + m_deactivationTime = btScalar(0.); setActivationState(0); } - } - SIMD_FORCE_INLINE bool wantsSleeping() + SIMD_FORCE_INLINE bool wantsSleeping() { - if (getActivationState() == DISABLE_DEACTIVATION) return false; @@ -442,41 +522,39 @@ public: if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) return false; - if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) return true; - if (m_deactivationTime> gDeactivationTime) + if (m_deactivationTime > gDeactivationTime) { return true; } return false; } - - - const btBroadphaseProxy* getBroadphaseProxy() const + const btBroadphaseProxy* getBroadphaseProxy() const { return m_broadphaseHandle; } - btBroadphaseProxy* getBroadphaseProxy() + btBroadphaseProxy* getBroadphaseProxy() { return m_broadphaseHandle; } - void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) + void setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy) { m_broadphaseHandle = broadphaseProxy; } //btMotionState allows to automatic synchronize the world transform for active objects - btMotionState* getMotionState() + btMotionState* getMotionState() { return m_optionalMotionState; } - const btMotionState* getMotionState() const + const btMotionState* getMotionState() const { return m_optionalMotionState; } - void setMotionState(btMotionState* motionState) + void setMotionState(btMotionState* motionState) { m_optionalMotionState = motionState; if (m_optionalMotionState) @@ -484,33 +562,31 @@ public: } //for experimental overriding of friction/contact solver func - int m_contactSolverType; - int m_frictionSolverType; + int m_contactSolverType; + int m_frictionSolverType; - void setAngularFactor(const btVector3& angFac) + void setAngularFactor(const btVector3& angFac) { m_updateRevision++; m_angularFactor = angFac; } - void setAngularFactor(btScalar angFac) + void setAngularFactor(btScalar angFac) { m_updateRevision++; - m_angularFactor.setValue(angFac,angFac,angFac); + m_angularFactor.setValue(angFac, angFac, angFac); } - const btVector3& getAngularFactor() const + const btVector3& getAngularFactor() const { return m_angularFactor; } //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase? - bool isInWorld() const + bool isInWorld() const { return (getBroadphaseProxy() != 0); } - virtual bool checkCollideWithOverride(const btCollisionObject* co) const; - void addConstraintRef(btTypedConstraint* c); void removeConstraintRef(btTypedConstraint* c); @@ -524,7 +600,7 @@ public: return m_constraintRefs.size(); } - void setFlags(int flags) + void setFlags(int flags) { m_rigidbodyFlags = flags; } @@ -534,12 +610,9 @@ public: return m_rigidbodyFlags; } - - - ///perform implicit force computation in world space btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const; - + ///perform implicit force computation in body space (inertial frame) btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const; @@ -549,70 +622,66 @@ public: /////////////////////////////////////////////// - virtual int calculateSerializeBufferSize() const; + virtual int calculateSerializeBufferSize() const; ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; + virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; virtual void serializeSingleObject(class btSerializer* serializer) const; - }; //@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btRigidBodyFloatData +struct btRigidBodyFloatData { - btCollisionObjectFloatData m_collisionObjectData; - btMatrix3x3FloatData m_invInertiaTensorWorld; - btVector3FloatData m_linearVelocity; - btVector3FloatData m_angularVelocity; - btVector3FloatData m_angularFactor; - btVector3FloatData m_linearFactor; - btVector3FloatData m_gravity; - btVector3FloatData m_gravity_acceleration; - btVector3FloatData m_invInertiaLocal; - btVector3FloatData m_totalForce; - btVector3FloatData m_totalTorque; - float m_inverseMass; - float m_linearDamping; - float m_angularDamping; - float m_additionalDampingFactor; - float m_additionalLinearDampingThresholdSqr; - float m_additionalAngularDampingThresholdSqr; - float m_additionalAngularDampingFactor; - float m_linearSleepingThreshold; - float m_angularSleepingThreshold; - int m_additionalDamping; + btCollisionObjectFloatData m_collisionObjectData; + btMatrix3x3FloatData m_invInertiaTensorWorld; + btVector3FloatData m_linearVelocity; + btVector3FloatData m_angularVelocity; + btVector3FloatData m_angularFactor; + btVector3FloatData m_linearFactor; + btVector3FloatData m_gravity; + btVector3FloatData m_gravity_acceleration; + btVector3FloatData m_invInertiaLocal; + btVector3FloatData m_totalForce; + btVector3FloatData m_totalTorque; + float m_inverseMass; + float m_linearDamping; + float m_angularDamping; + float m_additionalDampingFactor; + float m_additionalLinearDampingThresholdSqr; + float m_additionalAngularDampingThresholdSqr; + float m_additionalAngularDampingFactor; + float m_linearSleepingThreshold; + float m_angularSleepingThreshold; + int m_additionalDamping; }; ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 -struct btRigidBodyDoubleData +struct btRigidBodyDoubleData { - btCollisionObjectDoubleData m_collisionObjectData; - btMatrix3x3DoubleData m_invInertiaTensorWorld; - btVector3DoubleData m_linearVelocity; - btVector3DoubleData m_angularVelocity; - btVector3DoubleData m_angularFactor; - btVector3DoubleData m_linearFactor; - btVector3DoubleData m_gravity; - btVector3DoubleData m_gravity_acceleration; - btVector3DoubleData m_invInertiaLocal; - btVector3DoubleData m_totalForce; - btVector3DoubleData m_totalTorque; - double m_inverseMass; - double m_linearDamping; - double m_angularDamping; - double m_additionalDampingFactor; - double m_additionalLinearDampingThresholdSqr; - double m_additionalAngularDampingThresholdSqr; - double m_additionalAngularDampingFactor; - double m_linearSleepingThreshold; - double m_angularSleepingThreshold; - int m_additionalDamping; - char m_padding[4]; + btCollisionObjectDoubleData m_collisionObjectData; + btMatrix3x3DoubleData m_invInertiaTensorWorld; + btVector3DoubleData m_linearVelocity; + btVector3DoubleData m_angularVelocity; + btVector3DoubleData m_angularFactor; + btVector3DoubleData m_linearFactor; + btVector3DoubleData m_gravity; + btVector3DoubleData m_gravity_acceleration; + btVector3DoubleData m_invInertiaLocal; + btVector3DoubleData m_totalForce; + btVector3DoubleData m_totalTorque; + double m_inverseMass; + double m_linearDamping; + double m_angularDamping; + double m_additionalDampingFactor; + double m_additionalLinearDampingThresholdSqr; + double m_additionalAngularDampingThresholdSqr; + double m_additionalAngularDampingFactor; + double m_linearSleepingThreshold; + double m_angularSleepingThreshold; + int m_additionalDamping; + char m_padding[4]; }; - - -#endif //BT_RIGIDBODY_H - +#endif //BT_RIGIDBODY_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 35dd38840f6..8103390fb1d 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -21,47 +21,40 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" - /* Make sure this dummy function never changes so that it can be used by probes that are checking whether the library is actually installed. */ -extern "C" +extern "C" { - void btBulletDynamicsProbe (); - void btBulletDynamicsProbe () {} + void btBulletDynamicsProbe(); + void btBulletDynamicsProbe() {} } - - - -btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) -:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), -m_constraintSolver(constraintSolver), -m_ownsConstraintSolver(false), -m_gravity(0,0,-10) +btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) + : btDynamicsWorld(dispatcher, pairCache, collisionConfiguration), + m_constraintSolver(constraintSolver), + m_ownsConstraintSolver(false), + m_gravity(0, 0, -10) { - } - btSimpleDynamicsWorld::~btSimpleDynamicsWorld() { if (m_ownsConstraintSolver) - btAlignedFree( m_constraintSolver); + btAlignedFree(m_constraintSolver); } -int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +int btSimpleDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) { (void)fixedTimeStep; (void)maxSubSteps; - ///apply gravity, predict motion predictUnconstraintMotion(timeStep); - btDispatcherInfo& dispatchInfo = getDispatchInfo(); + btDispatcherInfo& dispatchInfo = getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; dispatchInfo.m_debugDraw = getDebugDrawer(); @@ -74,17 +67,17 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b if (numManifolds) { btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer(); - + btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; - m_constraintSolver->prepareSolve(0,numManifolds); - m_constraintSolver->solveGroup(&getCollisionObjectArray()[0],getNumCollisionObjects(),manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_dispatcher1); - m_constraintSolver->allSolved(infoGlobal,m_debugDrawer); + m_constraintSolver->prepareSolve(0, numManifolds); + m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds, 0, 0, infoGlobal, m_debugDrawer, m_dispatcher1); + m_constraintSolver->allSolved(infoGlobal, m_debugDrawer); } ///integrate transforms integrateTransforms(timeStep); - + updateAabbs(); synchronizeMotionStates(); @@ -92,29 +85,27 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b clearForces(); return 1; - } -void btSimpleDynamicsWorld::clearForces() +void btSimpleDynamicsWorld::clearForces() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; - + btRigidBody* body = btRigidBody::upcast(colObj); if (body) { body->clearForces(); } } -} - +} -void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) +void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) { m_gravity = gravity; - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -125,17 +116,17 @@ void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) } } -btVector3 btSimpleDynamicsWorld::getGravity () const +btVector3 btSimpleDynamicsWorld::getGravity() const { return m_gravity; } -void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) +void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) { btCollisionWorld::removeCollisionObject(body); } -void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) { btRigidBody* body = btRigidBody::upcast(collisionObject); if (body) @@ -144,8 +135,7 @@ void btSimpleDynamicsWorld::removeCollisionObject(btCollisionObject* collisionOb btCollisionWorld::removeCollisionObject(collisionObject); } - -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) +void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) { body->setGravity(m_gravity); @@ -155,37 +145,32 @@ void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body) } } -void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask) +void btSimpleDynamicsWorld::addRigidBody(btRigidBody* body, int group, int mask) { body->setGravity(m_gravity); if (body->getCollisionShape()) { - addCollisionObject(body,group,mask); + addCollisionObject(body, group, mask); } } - -void btSimpleDynamicsWorld::debugDrawWorld() +void btSimpleDynamicsWorld::debugDrawWorld() { - } - -void btSimpleDynamicsWorld::addAction(btActionInterface* action) -{ +void btSimpleDynamicsWorld::addAction(btActionInterface* action) +{ } -void btSimpleDynamicsWorld::removeAction(btActionInterface* action) +void btSimpleDynamicsWorld::removeAction(btActionInterface* action) { - } - -void btSimpleDynamicsWorld::updateAabbs() +void btSimpleDynamicsWorld::updateAabbs() { btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -193,19 +178,19 @@ void btSimpleDynamicsWorld::updateAabbs() { if (body->isActive() && (!body->isStaticObject())) { - btVector3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btVector3 minAabb, maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb, maxAabb); btBroadphaseInterface* bp = getBroadphase(); - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); + bp->setAabb(body->getBroadphaseHandle(), minAabb, maxAabb, m_dispatcher1); } } } } -void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) +void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) { btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -214,17 +199,15 @@ void btSimpleDynamicsWorld::integrateTransforms(btScalar timeStep) if (body->isActive() && (!body->isStaticObject())) { body->predictIntegratedTransform(timeStep, predictedTrans); - body->proceedToTransform( predictedTrans); + body->proceedToTransform(predictedTrans); } } } } - - -void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -235,20 +218,19 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) if (body->isActive()) { body->applyGravity(); - body->integrateVelocities( timeStep); + body->integrateVelocities(timeStep); body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); + body->predictIntegratedTransform(timeStep, body->getInterpolationWorldTransform()); } } } } } - -void btSimpleDynamicsWorld::synchronizeMotionStates() +void btSimpleDynamicsWorld::synchronizeMotionStates() { ///@todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) + for (int i = 0; i < m_collisionObjects.size(); i++) { btCollisionObject* colObj = m_collisionObjects[i]; btRigidBody* body = btRigidBody::upcast(colObj); @@ -260,11 +242,9 @@ void btSimpleDynamicsWorld::synchronizeMotionStates() } } } - } - -void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) { diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index d48d2e39c4d..12be231c7f1 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -27,63 +27,58 @@ class btConstraintSolver; class btSimpleDynamicsWorld : public btDynamicsWorld { protected: + btConstraintSolver* m_constraintSolver; - btConstraintSolver* m_constraintSolver; + bool m_ownsConstraintSolver; - bool m_ownsConstraintSolver; - - void predictUnconstraintMotion(btScalar timeStep); - - void integrateTransforms(btScalar timeStep); - - btVector3 m_gravity; - -public: + void predictUnconstraintMotion(btScalar timeStep); + void integrateTransforms(btScalar timeStep); + btVector3 m_gravity; +public: ///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver - btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + btSimpleDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration); virtual ~btSimpleDynamicsWorld(); - + ///maxSubSteps/fixedTimeStep for interpolation is currently ignored for btSimpleDynamicsWorld, use btDiscreteDynamicsWorld instead - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.)); - virtual void setGravity(const btVector3& gravity); + virtual void setGravity(const btVector3& gravity); - virtual btVector3 getGravity () const; + virtual btVector3 getGravity() const; - virtual void addRigidBody(btRigidBody* body); + virtual void addRigidBody(btRigidBody* body); - virtual void addRigidBody(btRigidBody* body, short group, short mask); + virtual void addRigidBody(btRigidBody* body, int group, int mask); - virtual void removeRigidBody(btRigidBody* body); + virtual void removeRigidBody(btRigidBody* body); - virtual void debugDrawWorld(); - - virtual void addAction(btActionInterface* action); + virtual void debugDrawWorld(); - virtual void removeAction(btActionInterface* action); + virtual void addAction(btActionInterface* action); + + virtual void removeAction(btActionInterface* action); ///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btCollisionWorld::removeCollisionObject - virtual void removeCollisionObject(btCollisionObject* collisionObject); - - virtual void updateAabbs(); + virtual void removeCollisionObject(btCollisionObject* collisionObject); + + virtual void updateAabbs(); - virtual void synchronizeMotionStates(); + virtual void synchronizeMotionStates(); - virtual void setConstraintSolver(btConstraintSolver* solver); + virtual void setConstraintSolver(btConstraintSolver* solver); virtual btConstraintSolver* getConstraintSolver(); - virtual btDynamicsWorldType getWorldType() const + virtual btDynamicsWorldType getWorldType() const { return BT_SIMPLE_DYNAMICS_WORLD; } - virtual void clearForces(); - + virtual void clearForces(); }; -#endif //BT_SIMPLE_DYNAMICS_WORLD_H +#endif //BT_SIMPLE_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp new file mode 100644 index 00000000000..772b7742023 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.cpp @@ -0,0 +1,696 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include "LinearMath/btScalar.h" +#include "LinearMath/btThreads.h" +#include "btSimulationIslandManagerMt.h" +#include "BulletCollision/BroadphaseCollision/btDispatcher.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.h" // for s_minimumContactManifoldsForBatching + +//#include <stdio.h> +#include "LinearMath/btQuickprof.h" + +SIMD_FORCE_INLINE int calcBatchCost(int bodies, int manifolds, int constraints) +{ + // rough estimate of the cost of a batch, used for merging + int batchCost = bodies + 8 * manifolds + 4 * constraints; + return batchCost; +} + +SIMD_FORCE_INLINE int calcBatchCost(const btSimulationIslandManagerMt::Island* island) +{ + return calcBatchCost(island->bodyArray.size(), island->manifoldArray.size(), island->constraintArray.size()); +} + +btSimulationIslandManagerMt::btSimulationIslandManagerMt() +{ + m_minimumSolverBatchSize = calcBatchCost(0, 128, 0); + m_batchIslandMinBodyCount = 32; + m_islandDispatch = parallelIslandDispatch; + m_batchIsland = NULL; +} + +btSimulationIslandManagerMt::~btSimulationIslandManagerMt() +{ + for (int i = 0; i < m_allocatedIslands.size(); ++i) + { + delete m_allocatedIslands[i]; + } + m_allocatedIslands.resize(0); + m_activeIslands.resize(0); + m_freeIslands.resize(0); +} + +inline int getIslandId(const btPersistentManifold* lhs) +{ + const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0()); + const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1()); + int islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag(); + return islandId; +} + +SIMD_FORCE_INLINE int btGetConstraintIslandId1(const btTypedConstraint* lhs) +{ + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + int islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); + return islandId; +} + +/// function object that routes calls to operator< +class IslandBatchSizeSortPredicate +{ +public: + bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const + { + int lCost = calcBatchCost(lhs); + int rCost = calcBatchCost(rhs); + return lCost > rCost; + } +}; + +class IslandBodyCapacitySortPredicate +{ +public: + bool operator()(const btSimulationIslandManagerMt::Island* lhs, const btSimulationIslandManagerMt::Island* rhs) const + { + return lhs->bodyArray.capacity() > rhs->bodyArray.capacity(); + } +}; + +void btSimulationIslandManagerMt::Island::append(const Island& other) +{ + // append bodies + for (int i = 0; i < other.bodyArray.size(); ++i) + { + bodyArray.push_back(other.bodyArray[i]); + } + // append manifolds + for (int i = 0; i < other.manifoldArray.size(); ++i) + { + manifoldArray.push_back(other.manifoldArray[i]); + } + // append constraints + for (int i = 0; i < other.constraintArray.size(); ++i) + { + constraintArray.push_back(other.constraintArray[i]); + } +} + +bool btIsBodyInIsland(const btSimulationIslandManagerMt::Island& island, const btCollisionObject* obj) +{ + for (int i = 0; i < island.bodyArray.size(); ++i) + { + if (island.bodyArray[i] == obj) + { + return true; + } + } + return false; +} + +void btSimulationIslandManagerMt::initIslandPools() +{ + // reset island pools + int numElem = getUnionFind().getNumElements(); + m_lookupIslandFromId.resize(numElem); + for (int i = 0; i < m_lookupIslandFromId.size(); ++i) + { + m_lookupIslandFromId[i] = NULL; + } + m_activeIslands.resize(0); + m_freeIslands.resize(0); + // check whether allocated islands are sorted by body capacity (largest to smallest) + int lastCapacity = 0; + bool isSorted = true; + for (int i = 0; i < m_allocatedIslands.size(); ++i) + { + Island* island = m_allocatedIslands[i]; + int cap = island->bodyArray.capacity(); + if (cap > lastCapacity) + { + isSorted = false; + break; + } + lastCapacity = cap; + } + if (!isSorted) + { + m_allocatedIslands.quickSort(IslandBodyCapacitySortPredicate()); + } + + m_batchIsland = NULL; + // mark all islands free (but avoid deallocation) + for (int i = 0; i < m_allocatedIslands.size(); ++i) + { + Island* island = m_allocatedIslands[i]; + island->bodyArray.resize(0); + island->manifoldArray.resize(0); + island->constraintArray.resize(0); + island->id = -1; + island->isSleeping = true; + m_freeIslands.push_back(island); + } +} + +btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id) +{ + btAssert(id >= 0); + btAssert(id < m_lookupIslandFromId.size()); + Island* island = m_lookupIslandFromId[id]; + if (island == NULL) + { + // search for existing island + for (int i = 0; i < m_activeIslands.size(); ++i) + { + if (m_activeIslands[i]->id == id) + { + island = m_activeIslands[i]; + break; + } + } + m_lookupIslandFromId[id] = island; + } + return island; +} + +btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::allocateIsland(int id, int numBodies) +{ + Island* island = NULL; + int allocSize = numBodies; + if (numBodies < m_batchIslandMinBodyCount) + { + if (m_batchIsland) + { + island = m_batchIsland; + m_lookupIslandFromId[id] = island; + // if we've made a large enough batch, + if (island->bodyArray.size() + numBodies >= m_batchIslandMinBodyCount) + { + // next time start a new batch + m_batchIsland = NULL; + } + return island; + } + else + { + // need to allocate a batch island + allocSize = m_batchIslandMinBodyCount * 2; + } + } + btAlignedObjectArray<Island*>& freeIslands = m_freeIslands; + + // search for free island + if (freeIslands.size() > 0) + { + // try to reuse a previously allocated island + int iFound = freeIslands.size(); + // linear search for smallest island that can hold our bodies + for (int i = freeIslands.size() - 1; i >= 0; --i) + { + if (freeIslands[i]->bodyArray.capacity() >= allocSize) + { + iFound = i; + island = freeIslands[i]; + island->id = id; + break; + } + } + // if found, shrink array while maintaining ordering + if (island) + { + int iDest = iFound; + int iSrc = iDest + 1; + while (iSrc < freeIslands.size()) + { + freeIslands[iDest++] = freeIslands[iSrc++]; + } + freeIslands.pop_back(); + } + } + if (island == NULL) + { + // no free island found, allocate + island = new Island(); // TODO: change this to use the pool allocator + island->id = id; + island->bodyArray.reserve(allocSize); + m_allocatedIslands.push_back(island); + } + m_lookupIslandFromId[id] = island; + if (numBodies < m_batchIslandMinBodyCount) + { + m_batchIsland = island; + } + m_activeIslands.push_back(island); + return island; +} + +void btSimulationIslandManagerMt::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld) +{ + BT_PROFILE("buildIslands"); + + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + + //we are going to sort the unionfind array, and store the element id in the size + //afterwards, we clean unionfind, to make sure no-one uses it anymore + + getUnionFind().sortIslands(); + int numElem = getUnionFind().getNumElements(); + + int endIslandIndex = 1; + int startIslandIndex; + + //update the sleeping state for bodies, if all are sleeping + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) + { + int islandId = getUnionFind().getElement(startIslandIndex).m_id; + for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) + { + } + + //int numSleeping = 0; + + bool allSleeping = true; + + int idx; + for (idx = startIslandIndex; idx < endIslandIndex; idx++) + { + int i = getUnionFind().getElement(idx).m_sz; + + btCollisionObject* colObj0 = collisionObjects[i]; + if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { + // printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + if (colObj0->getIslandTag() == islandId) + { + if (colObj0->getActivationState() == ACTIVE_TAG || + colObj0->getActivationState() == DISABLE_DEACTIVATION) + { + allSleeping = false; + break; + } + } + } + + if (allSleeping) + { + int idx; + for (idx = startIslandIndex; idx < endIslandIndex; idx++) + { + int i = getUnionFind().getElement(idx).m_sz; + btCollisionObject* colObj0 = collisionObjects[i]; + if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { + // printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + + if (colObj0->getIslandTag() == islandId) + { + colObj0->setActivationState(ISLAND_SLEEPING); + } + } + } + else + { + int idx; + for (idx = startIslandIndex; idx < endIslandIndex; idx++) + { + int i = getUnionFind().getElement(idx).m_sz; + + btCollisionObject* colObj0 = collisionObjects[i]; + if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1)) + { + // printf("error in island management\n"); + } + + btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1)); + + if (colObj0->getIslandTag() == islandId) + { + if (colObj0->getActivationState() == ISLAND_SLEEPING) + { + colObj0->setActivationState(WANTS_DEACTIVATION); + colObj0->setDeactivationTime(0.f); + } + } + } + } + } +} + +void btSimulationIslandManagerMt::addBodiesToIslands(btCollisionWorld* collisionWorld) +{ + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + int endIslandIndex = 1; + int startIslandIndex; + int numElem = getUnionFind().getNumElements(); + + // create explicit islands and add bodies to each + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) + { + int islandId = getUnionFind().getElement(startIslandIndex).m_id; + + // find end index + for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++) + { + } + // check if island is sleeping + bool islandSleeping = true; + for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++) + { + int i = getUnionFind().getElement(iElem).m_sz; + btCollisionObject* colObj = collisionObjects[i]; + if (colObj->isActive()) + { + islandSleeping = false; + } + } + if (!islandSleeping) + { + // want to count the number of bodies before allocating the island to optimize memory usage of the Island structures + int numBodies = endIslandIndex - startIslandIndex; + Island* island = allocateIsland(islandId, numBodies); + island->isSleeping = false; + + // add bodies to island + for (int iElem = startIslandIndex; iElem < endIslandIndex; iElem++) + { + int i = getUnionFind().getElement(iElem).m_sz; + btCollisionObject* colObj = collisionObjects[i]; + island->bodyArray.push_back(colObj); + } + } + } +} + +void btSimulationIslandManagerMt::addManifoldsToIslands(btDispatcher* dispatcher) +{ + // walk all the manifolds, activating bodies touched by kinematic objects, and add each manifold to its Island + int maxNumManifolds = dispatcher->getNumManifolds(); + for (int i = 0; i < maxNumManifolds; i++) + { + btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i); + + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); + const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); + + ///@todo: check sleeping conditions! + if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || + ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) + { + //kinematic objects don't merge islands, but wake up all connected objects + if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) + { + if (colObj0->hasContactResponse()) + colObj1->activate(); + } + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) + { + if (colObj1->hasContactResponse()) + colObj0->activate(); + } + //filtering for response + if (dispatcher->needsResponse(colObj0, colObj1)) + { + // scatter manifolds into various islands + int islandId = getIslandId(manifold); + // if island not sleeping, + if (Island* island = getIsland(islandId)) + { + island->manifoldArray.push_back(manifold); + } + } + } + } +} + +void btSimulationIslandManagerMt::addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints) +{ + // walk constraints + for (int i = 0; i < constraints.size(); i++) + { + // scatter constraints into various islands + btTypedConstraint* constraint = constraints[i]; + if (constraint->isEnabled()) + { + int islandId = btGetConstraintIslandId1(constraint); + // if island is not sleeping, + if (Island* island = getIsland(islandId)) + { + island->constraintArray.push_back(constraint); + } + } + } +} + +void btSimulationIslandManagerMt::mergeIslands() +{ + // sort islands in order of decreasing batch size + m_activeIslands.quickSort(IslandBatchSizeSortPredicate()); + + // merge small islands to satisfy minimum batch size + // find first small batch island + int destIslandIndex = m_activeIslands.size(); + for (int i = 0; i < m_activeIslands.size(); ++i) + { + Island* island = m_activeIslands[i]; + int batchSize = calcBatchCost(island); + if (batchSize < m_minimumSolverBatchSize) + { + destIslandIndex = i; + break; + } + } + int lastIndex = m_activeIslands.size() - 1; + while (destIslandIndex < lastIndex) + { + // merge islands from the back of the list + Island* island = m_activeIslands[destIslandIndex]; + int numBodies = island->bodyArray.size(); + int numManifolds = island->manifoldArray.size(); + int numConstraints = island->constraintArray.size(); + int firstIndex = lastIndex; + // figure out how many islands we want to merge and find out how many bodies, manifolds and constraints we will have + while (true) + { + Island* src = m_activeIslands[firstIndex]; + numBodies += src->bodyArray.size(); + numManifolds += src->manifoldArray.size(); + numConstraints += src->constraintArray.size(); + int batchCost = calcBatchCost(numBodies, numManifolds, numConstraints); + if (batchCost >= m_minimumSolverBatchSize) + { + break; + } + if (firstIndex - 1 == destIslandIndex) + { + break; + } + firstIndex--; + } + // reserve space for these pointers to minimize reallocation + island->bodyArray.reserve(numBodies); + island->manifoldArray.reserve(numManifolds); + island->constraintArray.reserve(numConstraints); + // merge islands + for (int i = firstIndex; i <= lastIndex; ++i) + { + island->append(*m_activeIslands[i]); + } + // shrink array to exclude the islands that were merged from + m_activeIslands.resize(firstIndex); + lastIndex = firstIndex - 1; + destIslandIndex++; + } +} + +void btSimulationIslandManagerMt::solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams) +{ + btPersistentManifold** manifolds = island.manifoldArray.size() ? &island.manifoldArray[0] : NULL; + btTypedConstraint** constraintsPtr = island.constraintArray.size() ? &island.constraintArray[0] : NULL; + solver->solveGroup(&island.bodyArray[0], + island.bodyArray.size(), + manifolds, + island.manifoldArray.size(), + constraintsPtr, + island.constraintArray.size(), + *solverParams.m_solverInfo, + solverParams.m_debugDrawer, + solverParams.m_dispatcher); +} + +void btSimulationIslandManagerMt::serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams) +{ + BT_PROFILE("serialIslandDispatch"); + // serial dispatch + btAlignedObjectArray<Island*>& islands = *islandsPtr; + btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; + for (int i = 0; i < islands.size(); ++i) + { + solveIsland(solver, *islands[i], solverParams); + } +} + +struct UpdateIslandDispatcher : public btIParallelForBody +{ + btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& m_islandsPtr; + const btSimulationIslandManagerMt::SolverParams& m_solverParams; + + UpdateIslandDispatcher(btAlignedObjectArray<btSimulationIslandManagerMt::Island*>& islandsPtr, const btSimulationIslandManagerMt::SolverParams& solverParams) + : m_islandsPtr(islandsPtr), m_solverParams(solverParams) + { + } + + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + btConstraintSolver* solver = m_solverParams.m_solverPool; + for (int i = iBegin; i < iEnd; ++i) + { + btSimulationIslandManagerMt::Island* island = m_islandsPtr[i]; + btSimulationIslandManagerMt::solveIsland(solver, *island, m_solverParams); + } + } +}; + +void btSimulationIslandManagerMt::parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams) +{ + BT_PROFILE("parallelIslandDispatch"); + // + // if there are islands with many contacts, it may be faster to submit these + // large islands *serially* to a single parallel constraint solver, and then later + // submit the remaining smaller islands in parallel to multiple sequential solvers. + // + // Some task schedulers do not deal well with nested parallelFor loops. One implementation + // of OpenMP was actually slower than doing everything single-threaded. Intel TBB + // on the other hand, seems to do a pretty respectable job with it. + // + // When solving islands in parallel, the worst case performance happens when there + // is one very large island and then perhaps a smattering of very small + // islands -- one worker thread takes the large island and the remaining workers + // tear through the smaller islands and then sit idle waiting for the first worker + // to finish. Solving islands in parallel works best when there are numerous small + // islands, roughly equal in size. + // + // By contrast, the other approach -- the parallel constraint solver -- is only + // able to deliver a worthwhile speedup when the island is large. For smaller islands, + // it is difficult to extract a useful amount of parallelism -- the overhead of grouping + // the constraints into batches and sending the batches to worker threads can nullify + // any gains from parallelism. + // + + UpdateIslandDispatcher dispatcher(*islandsPtr, solverParams); + // We take advantage of the fact the islands are sorted in order of decreasing size + int iBegin = 0; + if (solverParams.m_solverMt) + { + while (iBegin < islandsPtr->size()) + { + btSimulationIslandManagerMt::Island* island = (*islandsPtr)[iBegin]; + if (island->manifoldArray.size() < btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching) + { + // OK to submit the rest of the array in parallel + break; + } + // serial dispatch to parallel solver for large islands (if any) + solveIsland(solverParams.m_solverMt, *island, solverParams); + ++iBegin; + } + } + // parallel dispatch to sequential solvers for rest + btParallelFor(iBegin, islandsPtr->size(), 1, dispatcher); +} + +///@todo: this is random access, it can be walked 'cache friendly'! +void btSimulationIslandManagerMt::buildAndProcessIslands(btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray<btTypedConstraint*>& constraints, + const SolverParams& solverParams) +{ + BT_PROFILE("buildAndProcessIslands"); + btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray(); + + buildIslands(dispatcher, collisionWorld); + + if (!getSplitIslands()) + { + btPersistentManifold** manifolds = dispatcher->getInternalManifoldPointer(); + int maxNumManifolds = dispatcher->getNumManifolds(); + + for (int i = 0; i < maxNumManifolds; i++) + { + btPersistentManifold* manifold = manifolds[i]; + + const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0()); + const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1()); + + ///@todo: check sleeping conditions! + if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) || + ((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING)) + { + //kinematic objects don't merge islands, but wake up all connected objects + if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) + { + if (colObj0->hasContactResponse()) + colObj1->activate(); + } + if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) + { + if (colObj1->hasContactResponse()) + colObj0->activate(); + } + } + } + btTypedConstraint** constraintsPtr = constraints.size() ? &constraints[0] : NULL; + btConstraintSolver* solver = solverParams.m_solverMt ? solverParams.m_solverMt : solverParams.m_solverPool; + solver->solveGroup(&collisionObjects[0], + collisionObjects.size(), + manifolds, + maxNumManifolds, + constraintsPtr, + constraints.size(), + *solverParams.m_solverInfo, + solverParams.m_debugDrawer, + solverParams.m_dispatcher); + } + else + { + initIslandPools(); + + //traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + addBodiesToIslands(collisionWorld); + addManifoldsToIslands(dispatcher); + addConstraintsToIslands(constraints); + + // m_activeIslands array should now contain all non-sleeping Islands, and each Island should + // have all the necessary bodies, manifolds and constraints. + + // if we want to merge islands with small batch counts, + if (m_minimumSolverBatchSize > 1) + { + mergeIslands(); + } + // dispatch islands to solver + m_islandDispatch(&m_activeIslands, solverParams); + } +} diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h new file mode 100644 index 00000000000..ab73a899f14 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h @@ -0,0 +1,112 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_SIMULATION_ISLAND_MANAGER_MT_H +#define BT_SIMULATION_ISLAND_MANAGER_MT_H + +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" + +class btTypedConstraint; +class btConstraintSolver; +struct btContactSolverInfo; +class btIDebugDraw; + +/// +/// SimulationIslandManagerMt -- Multithread capable version of SimulationIslandManager +/// Splits the world up into islands which can be solved in parallel. +/// In order to solve islands in parallel, an IslandDispatch function +/// must be provided which will dispatch calls to multiple threads. +/// The amount of parallelism that can be achieved depends on the number +/// of islands. If only a single island exists, then no parallelism is +/// possible. +/// +class btSimulationIslandManagerMt : public btSimulationIslandManager +{ +public: + struct Island + { + // a simulation island consisting of bodies, manifolds and constraints, + // to be passed into a constraint solver. + btAlignedObjectArray<btCollisionObject*> bodyArray; + btAlignedObjectArray<btPersistentManifold*> manifoldArray; + btAlignedObjectArray<btTypedConstraint*> constraintArray; + int id; // island id + bool isSleeping; + + void append(const Island& other); // add bodies, manifolds, constraints to my own + }; + struct SolverParams + { + btConstraintSolver* m_solverPool; + btConstraintSolver* m_solverMt; + btContactSolverInfo* m_solverInfo; + btIDebugDraw* m_debugDrawer; + btDispatcher* m_dispatcher; + }; + static void solveIsland(btConstraintSolver* solver, Island& island, const SolverParams& solverParams); + + typedef void (*IslandDispatchFunc)(btAlignedObjectArray<Island*>* islands, const SolverParams& solverParams); + static void serialIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams); + static void parallelIslandDispatch(btAlignedObjectArray<Island*>* islandsPtr, const SolverParams& solverParams); + +protected: + btAlignedObjectArray<Island*> m_allocatedIslands; // owner of all Islands + btAlignedObjectArray<Island*> m_activeIslands; // islands actively in use + btAlignedObjectArray<Island*> m_freeIslands; // islands ready to be reused + btAlignedObjectArray<Island*> m_lookupIslandFromId; // big lookup table to map islandId to Island pointer + Island* m_batchIsland; + int m_minimumSolverBatchSize; + int m_batchIslandMinBodyCount; + IslandDispatchFunc m_islandDispatch; + + Island* getIsland(int id); + virtual Island* allocateIsland(int id, int numBodies); + virtual void initIslandPools(); + virtual void addBodiesToIslands(btCollisionWorld* collisionWorld); + virtual void addManifoldsToIslands(btDispatcher* dispatcher); + virtual void addConstraintsToIslands(btAlignedObjectArray<btTypedConstraint*>& constraints); + virtual void mergeIslands(); + +public: + btSimulationIslandManagerMt(); + virtual ~btSimulationIslandManagerMt(); + + virtual void buildAndProcessIslands(btDispatcher* dispatcher, + btCollisionWorld* collisionWorld, + btAlignedObjectArray<btTypedConstraint*>& constraints, + const SolverParams& solverParams); + + virtual void buildIslands(btDispatcher* dispatcher, btCollisionWorld* colWorld); + + int getMinimumSolverBatchSize() const + { + return m_minimumSolverBatchSize; + } + void setMinimumSolverBatchSize(int sz) + { + m_minimumSolverBatchSize = sz; + } + IslandDispatchFunc getIslandDispatchFunction() const + { + return m_islandDispatch; + } + // allow users to set their own dispatch function for multithreaded dispatch + void setIslandDispatchFunction(IslandDispatchFunc func) + { + m_islandDispatch = func; + } +}; + +#endif //BT_SIMULATION_ISLAND_MANAGER_H |