diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Dynamics')
10 files changed, 1188 insertions, 489 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp index 248c582dcd8..b28a46e299e 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp @@ -24,97 +24,287 @@ subject to the following restrictions: #include "btBulletDynamicsCommon.h" #include "LinearMath/btAlignedAllocator.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) + { + + } -#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/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/btGjkEpa.h" -#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" -#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" -#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" - -#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" -#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "LinearMath/btStackAlloc.h" - -extern "C" -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]); - btTriangleShape 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]); - btTriangleShape 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; - - btStackAlloc gStackAlloc(1024*1024*2); - - input.m_stackAlloc = &gStackAlloc; - - btTransform tr; - tr.setIdentity(); - - input.m_transformA = tr; - input.m_transformB = tr; - - convexConvex.getClosestPoints(input, gjkOutput, 0); - - if (gjkOutput.m_hasResult) +}; + +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); + assert(dynamicsWorld); + dynamicsWorld->stepSimulation(timeStep); +} + +void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) +{ + btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); + assert(dynamicsWorld); + btRigidBody* body = reinterpret_cast< btRigidBody* >(object); + assert(body); + + dynamicsWorld->addRigidBody(body); +} + +void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) +{ + btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); + assert(dynamicsWorld); + btRigidBody* body = reinterpret_cast< btRigidBody* >(object); + assert(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); + assert(shape); + if (mass) { - - 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; + shape->calculateLocalInertia(mass,localInertia); } - return -1.0f; + 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); + assert(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 + btVector3 inertiaHalfExtents(radius,height,radius); + 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(inertiaHalfExtents,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(btPoint3(x,y,z)); + +} + +void plDeleteShape(plCollisionShapeHandle cshape) +{ + btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); + assert(shape); + btAlignedFree(shape); +} +void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling) +{ + btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); + assert(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 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); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp new file mode 100644 index 00000000000..19443adc723 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp @@ -0,0 +1,193 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 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 "btContinuousDynamicsWorld.h" +#include "LinearMath/btQuickprof.h" + +//collision detection +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" + +//rigidbody & constraints +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + + + +#include <stdio.h> + +btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) +:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) +{ +} + +btContinuousDynamicsWorld::~btContinuousDynamicsWorld() +{ +} + + +void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) +{ + + startProfiling(timeStep); + + + ///update aabbs information + updateAabbs(); + //static int frame=0; +// printf("frame %d\n",frame++); + + ///apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_debugDraw = getDebugDrawer(); + + ///perform collision detection + performDiscreteCollisionDetection(); + + calculateSimulationIslands(); + + + getSolverInfo().m_timeStep = timeStep; + + + + ///solve contact and other joint constraints + solveConstraints(getSolverInfo()); + + ///CallbackTriggers(); + calculateTimeOfImpacts(timeStep); + + btScalar toi = dispatchInfo.m_timeOfImpact; +// if (toi < 1.f) +// printf("toi = %f\n",toi); + if (toi < 0.f) + printf("toi = %f\n",toi); + + + ///integrate transforms + integrateTransforms(timeStep * toi); + + ///update vehicle simulation + updateVehicles(timeStep); + + + updateActivationState( timeStep ); + + if(0 != m_internalTickCallback) { + (*m_internalTickCallback)(this, timeStep); + } +} + +void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep) +{ + ///these should be 'temporal' aabbs! + updateTemporalAabbs(timeStep); + + ///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually. + ///so we handle the case moving versus static properly, and we cheat for moving versus moving + btScalar toi = 1.f; + + + btDispatcherInfo& dispatchInfo = getDispatchInfo(); + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_timeOfImpact = 1.f; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS; + + ///calculate time of impact for overlapping pairs + + + btDispatcher* dispatcher = getDispatcher(); + if (dispatcher) + dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); + + toi = dispatchInfo.m_timeOfImpact; + + dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE; + +} + +void btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep) +{ + + btVector3 temporalAabbMin,temporalAabbMax; + + for ( int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + body->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax); + const btVector3& linvel = body->getLinearVelocity(); + + //make the AABB temporal + btScalar temporalAabbMaxx = temporalAabbMax.getX(); + btScalar temporalAabbMaxy = temporalAabbMax.getY(); + btScalar temporalAabbMaxz = temporalAabbMax.getZ(); + btScalar temporalAabbMinx = temporalAabbMin.getX(); + btScalar temporalAabbMiny = temporalAabbMin.getY(); + btScalar temporalAabbMinz = temporalAabbMin.getZ(); + + // add linear motion + btVector3 linMotion = linvel*timeStep; + + if (linMotion.x() > 0.f) + temporalAabbMaxx += linMotion.x(); + else + temporalAabbMinx += linMotion.x(); + if (linMotion.y() > 0.f) + temporalAabbMaxy += linMotion.y(); + else + temporalAabbMiny += linMotion.y(); + if (linMotion.z() > 0.f) + temporalAabbMaxz += linMotion.z(); + else + temporalAabbMinz += linMotion.z(); + + //add conservative angular motion + btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep; + btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); + temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); + temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); + + temporalAabbMin -= angularMotion3d; + temporalAabbMax += angularMotion3d; + + m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1); + } + } + + //update aabb (of all moved objects) + + m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); + + + +} + + + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h new file mode 100644 index 00000000000..61c8dea03eb --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h @@ -0,0 +1,46 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2007 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_CONTINUOUS_DYNAMICS_WORLD_H +#define BT_CONTINUOUS_DYNAMICS_WORLD_H + +#include "btDiscreteDynamicsWorld.h" + +///btContinuousDynamicsWorld adds optional (per object) continuous collision detection for fast moving objects to the btDiscreteDynamicsWorld. +///This copes with fast moving objects that otherwise would tunnel/miss collisions. +///Under construction, don't use yet! Please use btDiscreteDynamicsWorld instead. +class btContinuousDynamicsWorld : public btDiscreteDynamicsWorld +{ + + void updateTemporalAabbs(btScalar timeStep); + + public: + + btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); + virtual ~btContinuousDynamicsWorld(); + + ///time stepping with calculation of time of impact for selected fast moving objects + virtual void internalSingleStepSimulation( btScalar timeStep); + + virtual void calculateTimeOfImpacts(btScalar timeStep); + + virtual btDynamicsWorldType getWorldType() const + { + return BT_CONTINUOUS_DYNAMICS_WORLD; + } + +}; + +#endif //BT_CONTINUOUS_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 1017c8af6ea..c7b1af245e9 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -21,7 +21,8 @@ subject to the following restrictions: #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include <LinearMath/btTransformUtil.h> +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btQuickprof.h" //rigidbody & constraints #include "BulletDynamics/Dynamics/btRigidBody.h" @@ -41,6 +42,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btSphereShape.h" #include "BulletCollision/CollisionShapes/btTriangleCallback.h" #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" #include "LinearMath/btIDebugDraw.h" @@ -57,17 +59,29 @@ subject to the following restrictions: -btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) -:btDynamicsWorld(dispatcher,pairCache), -m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver), -m_debugDrawer(0), +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) +:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), +m_constraintSolver(constraintSolver), m_gravity(0,-10,0), m_localTime(btScalar(1.)/btScalar(60.)), m_profileTimings(0) { - m_islandManager = new btSimulationIslandManager(); + if (!m_constraintSolver) + { + void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16); + m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver; + m_ownsConstraintSolver = true; + } else + { + m_ownsConstraintSolver = false; + } + + { + void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16); + m_islandManager = new (mem) btSimulationIslandManager(); + } + m_ownsIslandManager = true; - m_ownsConstraintSolver = (constraintSolver==0); } @@ -75,9 +89,16 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() { //only delete it when we created it if (m_ownsIslandManager) - delete m_islandManager; + { + m_islandManager->~btSimulationIslandManager(); + btAlignedFree( m_islandManager); + } if (m_ownsConstraintSolver) - delete m_constraintSolver; + { + + m_constraintSolver->~btConstraintSolver(); + btAlignedFree(m_constraintSolver); + } } void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) @@ -102,14 +123,35 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) } } -void btDiscreteDynamicsWorld::synchronizeMotionStates() +void btDiscreteDynamicsWorld::debugDrawWorld() { - //debug vehicle wheels - - + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) + { + int numManifolds = getDispatcher()->getNumManifolds(); + btVector3 color(0,0,0); + for (int i=0;i<numManifolds;i++) + { + btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i); + //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0()); + //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1()); + + int numContacts = contactManifold->getNumContacts(); + for (int j=0;j<numContacts;j++) + { + btManifoldPoint& cp = contactManifold->getContactPoint(j); + getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color); + } + } + } + + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) { + int i; + //todo: iterate over awake simulation islands! - for ( int i=0;i<m_collisionObjects.size();i++) + for ( i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) @@ -135,26 +177,17 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); } - btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - //we need to call the update at least once, even for sleeping objects - //otherwise the 'graphics' transform never updates properly - //so todo: add 'dirty' flag - //if (body->getActivationState() != ISLAND_SLEEPING) - { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); - } + btPoint3 minAabb,maxAabb; + btVector3 colorvec(1,0,0); + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); } - } - } - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) - { - for ( int i=0;i<this->m_vehicles.size();i++) + } + + for ( i=0;i<this->m_vehicles.size();i++) { for (int v=0;v<m_vehicles[i]->getNumWheels();v++) { @@ -166,10 +199,7 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() { wheelColor.setValue(255,0,255); } - - //synchronize the wheels with the (interpolated) chassis worldtransform - m_vehicles[i]->updateWheelTransform(v,true); - + btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin(); btVector3 axle = btVector3( @@ -186,12 +216,87 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() } } } +} + +void btDiscreteDynamicsWorld::clearForces() +{ + //todo: iterate over awake simulation islands! + for ( int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + + btRigidBody* body = btRigidBody::upcast(colObj); + if (body) + { + body->clearForces(); + } + } +} + +///apply gravity, call this once per timestep +void btDiscreteDynamicsWorld::applyGravity() +{ + //todo: iterate over awake simulation islands! + for ( int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + + btRigidBody* body = btRigidBody::upcast(colObj); + if (body && body->isActive()) + { + body->applyGravity(); + } + } +} + + + +void btDiscreteDynamicsWorld::synchronizeMotionStates() +{ + { + //todo: iterate over awake simulation islands! + for ( int i=0;i<m_collisionObjects.size();i++) + { + btCollisionObject* colObj = m_collisionObjects[i]; + + btRigidBody* body = btRigidBody::upcast(colObj); + if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) + { + //we need to call the update at least once, even for sleeping objects + //otherwise the 'graphics' transform never updates properly + //so todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } + } + } + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + for ( int i=0;i<this->m_vehicles.size();i++) + { + for (int v=0;v<m_vehicles[i]->getNumWheels();v++) + { + //synchronize the wheels with the (interpolated) chassis worldtransform + m_vehicles[i]->updateWheelTransform(v,true); + } + } + } } int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) { + startProfiling(timeStep); + + BT_PROFILE("stepSimulation"); + int numSimulationSubSteps = 0; if (maxSubSteps) @@ -229,6 +334,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, saveKinematicState(fixedTimeStep); + applyGravity(); + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; @@ -242,16 +349,19 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, synchronizeMotionStates(); + clearForces(); + +#ifndef BT_NO_PROFILE + CProfileManager::Increment_Frame_Counter(); +#endif //BT_NO_PROFILE + return numSimulationSubSteps; } void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { - startProfiling(timeStep); - - ///update aabbs information - updateAabbs(); + BT_PROFILE("internalSingleStepSimulation"); ///apply gravity, predict motion predictUnconstraintMotion(timeStep); @@ -286,8 +396,9 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) updateActivationState( timeStep ); - - + if(0 != m_internalTickCallback) { + (*m_internalTickCallback)(this, timeStep); + } } void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) @@ -304,6 +415,11 @@ void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) } } +btVector3 btDiscreteDynamicsWorld::getGravity () const +{ + return m_gravity; +} + void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { @@ -343,19 +459,18 @@ void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) { - BEGIN_PROFILE("updateVehicles"); - + BT_PROFILE("updateVehicles"); + for ( int i=0;i<m_vehicles.size();i++) { btRaycastVehicle* vehicle = m_vehicles[i]; vehicle->updateVehicle( timeStep); } - END_PROFILE("updateVehicles"); } void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { - BEGIN_PROFILE("updateActivationState"); + BT_PROFILE("updateActivationState"); for ( int i=0;i<m_collisionObjects.size();i++) { @@ -374,6 +489,12 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { if (body->getActivationState() == ACTIVE_TAG) body->setActivationState( WANTS_DEACTIVATION ); + if (body->getActivationState() == ISLAND_SLEEPING) + { + body->setAngularVelocity(btVector3(0,0,0)); + body->setLinearVelocity(btVector3(0,0,0)); + } + } } else { @@ -382,7 +503,6 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) } } } - END_PROFILE("updateActivationState"); } void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) @@ -412,7 +532,7 @@ void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) m_vehicles.remove(vehicle); } -inline int btGetConstraintIslandId(const btTypedConstraint* lhs) +SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { int islandId; @@ -442,6 +562,7 @@ class btSortConstraintOnIslandPredicate void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { + BT_PROFILE("solveConstraints"); struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { @@ -452,7 +573,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) int m_numConstraints; btIDebugDraw* m_debugDrawer; btStackAlloc* m_stackAlloc; - + btDispatcher* m_dispatcher; InplaceSolverIslandCallback( btContactSolverInfo& solverInfo, @@ -460,13 +581,15 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer, - btStackAlloc* stackAlloc) + btStackAlloc* stackAlloc, + btDispatcher* dispatcher) :m_solverInfo(solverInfo), m_solver(solver), m_sortedConstraints(sortedConstraints), m_numConstraints(numConstraints), m_debugDrawer(debugDrawer), - m_stackAlloc(stackAlloc) + m_stackAlloc(stackAlloc), + m_dispatcher(dispatcher) { } @@ -479,30 +602,42 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - int numCurConstraints = 0; - int i; - - //find the first constraint for this island - for (i=0;i<m_numConstraints;i++) + 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_stackAlloc,m_dispatcher); + } else { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + int numCurConstraints = 0; + int i; + + //find the first constraint for this island + for (i=0;i<m_numConstraints;i++) { - startConstraint = &m_sortedConstraints[i]; - break; + if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + { + startConstraint = &m_sortedConstraints[i]; + break; + } } - } - //count the number of constraints in this island - for (;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + //count the number of constraints in this island + for (;i<m_numConstraints;i++) { - numCurConstraints++; + if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + { + numCurConstraints++; + } } - } - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc); + ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive + if (numManifolds + numCurConstraints) + { + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + + } } }; @@ -520,18 +655,18 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) - sortedConstraints.heapSort(btSortConstraintOnIslandPredicate()); + sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0; - InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc); - + InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1); + m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); - + m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); } @@ -539,7 +674,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) void btDiscreteDynamicsWorld::calculateSimulationIslands() { - BEGIN_PROFILE("calculateSimulationIslands"); + BT_PROFILE("calculateSimulationIslands"); getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); @@ -569,66 +704,14 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - END_PROFILE("calculateSimulationIslands"); - -} - - -void btDiscreteDynamicsWorld::updateAabbs() -{ - BEGIN_PROFILE("updateAabbs"); - btVector3 colorvec(1,0,0); - btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - // if (body->IsActive() && (!body->IsStatic())) - { - btPoint3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; - - //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) - { - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb); - } else - { - //something went wrong, investigate - //this assert is unwanted in 3D modelers (danger of loosing work) - body->setActivationState(DISABLE_SIMULATION); - - static bool reportMe = true; - if (reportMe && m_debugDrawer) - { - reportMe = false; - m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); - m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); - m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); - m_debugDrawer->reportErrorWarning("Thanks.\n"); - } +} - } - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); - } - } - } - } - - END_PROFILE("updateAabbs"); -} void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { - BEGIN_PROFILE("integrateTransforms"); + BT_PROFILE("integrateTransforms"); btTransform predictedTrans; for ( int i=0;i<m_collisionObjects.size();i++) { @@ -643,14 +726,13 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) } } } - END_PROFILE("integrateTransforms"); } void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { - BEGIN_PROFILE("predictUnconstraintMotion"); + BT_PROFILE("predictUnconstraintMotion"); for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -661,51 +743,26 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { if (body->isActive()) { - body->applyForces( timeStep); body->integrateVelocities( timeStep); + //damping + body->applyDamping(timeStep); + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } } } - END_PROFILE("predictUnconstraintMotion"); } void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep) { (void)timeStep; - #ifdef USE_QUICKPROF +#ifndef BT_NO_PROFILE + CProfileManager::Reset(); +#endif //BT_NO_PROFILE - //toggle btProfiler - if ( m_debugDrawer && m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_ProfileTimings) - { - if (!m_profileTimings) - { - m_profileTimings = 1; - // To disable profiling, simply comment out the following line. - static int counter = 0; - - char filename[128]; - sprintf(filename,"quickprof_bullet_timings%i.csv",counter++); - btProfiler::init(filename, btProfiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS - } else - { - btProfiler::endProfilingCycle(); - } - - } else - { - if (m_profileTimings) - { - btProfiler::endProfilingCycle(); - - m_profileTimings = 0; - btProfiler::destroy(); - } - } -#endif //USE_QUICKPROF } @@ -827,27 +884,52 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, btScalar radius = capsuleShape->getRadius(); btScalar halfHeight = capsuleShape->getHalfHeight(); + + int upAxis = capsuleShape->getUpAxis(); + + + btVector3 capStart(0.f,0.f,0.f); + capStart[upAxis] = -halfHeight; + + btVector3 capEnd(0.f,0.f,0.f); + capEnd[upAxis] = halfHeight; // Draw the ends { + btTransform childTransform = worldTransform; - childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0); + childTransform.getOrigin() = worldTransform * capStart; debugDrawSphere(radius, childTransform, color); } { btTransform childTransform = worldTransform; - childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0); + childTransform.getOrigin() = worldTransform * capEnd; debugDrawSphere(radius, childTransform, color); } // Draw some additional lines btVector3 start = worldTransform.getOrigin(); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color); + + capStart[(upAxis+1)%3] = radius; + capEnd[(upAxis+1)%3] = radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); + capStart[(upAxis+1)%3] = -radius; + capEnd[(upAxis+1)%3] = -radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); + + capStart[(upAxis+1)%3] = 0.f; + capEnd[(upAxis+1)%3] = 0.f; + + capStart[(upAxis+2)%3] = radius; + capEnd[(upAxis+2)%3] = radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); + capStart[(upAxis+2)%3] = -radius; + capEnd[(upAxis+2)%3] = -radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color); + + break; } case CONE_SHAPE_PROXYTYPE: @@ -856,9 +938,10 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); btScalar height = coneShape->getHeight();//+coneShape->getMargin(); btVector3 start = worldTransform.getOrigin(); - // insert here Bullet 2.69 that fixes representation of cone + int upAxis= coneShape->getConeUpIndex(); + btVector3 offsetHeight(0,0,0); offsetHeight[upAxis] = height * btScalar(0.5); btVector3 offsetRadius(0,0,0); @@ -871,11 +954,8 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color); getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color); - // buggy code that does not take into account the direction of the cone - //getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(radius,btScalar(0.),btScalar(-0.5)*height),color); - //getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(-radius,btScalar(0.),btScalar(-0.5)*height),color); - //getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),radius,btScalar(-0.5)*height),color); - //getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),-radius,btScalar(-0.5)*height),color); + + break; } @@ -884,7 +964,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); int upAxis = cylinder->getUpAxis(); btScalar radius = cylinder->getRadius(); - btScalar halfHeight = cylinder->getHalfExtents()[upAxis]; + btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; btVector3 start = worldTransform.getOrigin(); btVector3 offsetHeight(0,0,0); offsetHeight[upAxis] = halfHeight; @@ -894,6 +974,25 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); break; } + + case STATIC_PLANE_PROXYTYPE: + { + const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + btVector3 planeOrigin = planeNormal * planeConst; + btVector3 vec0,vec1; + btPlaneSpace1(planeNormal,vec0,vec1); + btScalar vecLen = 100.f; + btVector3 pt0 = planeOrigin + vec0*vecLen; + btVector3 pt1 = planeOrigin - vec0*vecLen; + btVector3 pt2 = planeOrigin + vec1*vecLen; + btVector3 pt3 = planeOrigin - vec1*vecLen; + getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color); + getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color); + break; + + } default: { @@ -918,7 +1017,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); //DebugDrawcallback drawCallback; DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); } @@ -950,12 +1049,18 @@ void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) { - delete m_constraintSolver; + btAlignedFree( m_constraintSolver); } m_ownsConstraintSolver = false; m_constraintSolver = solver; } +btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() +{ + return m_constraintSolver; +} + + int btDiscreteDynamicsWorld::getNumConstraints() const { return int(m_constraints.size()); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 83b90bfeebc..d206a604960 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -23,11 +23,11 @@ class btOverlappingPairCache; class btConstraintSolver; class btSimulationIslandManager; class btTypedConstraint; -#include "../ConstraintSolver/btContactSolverInfo.h" + class btRaycastVehicle; class btIDebugDraw; -#include "../../LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btAlignedObjectArray.h" ///btDiscreteDynamicsWorld provides discrete rigid body simulation @@ -42,7 +42,6 @@ protected: btAlignedObjectArray<btTypedConstraint*> m_constraints; - btIDebugDraw* m_debugDrawer; btVector3 m_gravity; @@ -53,14 +52,12 @@ protected: bool m_ownsIslandManager; bool m_ownsConstraintSolver; - btContactSolverInfo m_solverInfo; - - + btAlignedObjectArray<btRaycastVehicle*> m_vehicles; int m_profileTimings; - void predictUnconstraintMotion(btScalar timeStep); + virtual void predictUnconstraintMotion(btScalar timeStep); void integrateTransforms(btScalar timeStep); @@ -86,14 +83,13 @@ public: ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those - btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver); + 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 void updateAabbs(); void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); @@ -118,17 +114,9 @@ public: return this; } - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) - { - m_debugDrawer = debugDrawer; - } - - virtual btIDebugDraw* getDebugDrawer() - { - return m_debugDrawer; - } virtual void setGravity(const btVector3& gravity); + virtual btVector3 getGravity () const; virtual void addRigidBody(btRigidBody* body); @@ -138,7 +126,11 @@ public: void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + virtual void debugDrawWorld(); + virtual void setConstraintSolver(btConstraintSolver* solver); + + virtual btConstraintSolver* getConstraintSolver(); virtual int getNumConstraints() const; @@ -146,11 +138,21 @@ public: virtual const btTypedConstraint* getConstraint(int index) const; - btContactSolverInfo& getSolverInfo() + + virtual btDynamicsWorldType getWorldType() const { - return m_solverInfo; + return BT_DISCRETE_DYNAMICS_WORLD; } + + ///the forces on each rigidbody is accumulating together with gravity. clear this after each timestep. + virtual void clearForces(); + + ///apply gravity, call this once per timestep + virtual void applyGravity(); + virtual void setNumTasks(int numTasks) + { + } }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 65b63fad4b5..dd9dfa71b7f 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -16,20 +16,39 @@ subject to the following restrictions: #ifndef BT_DYNAMICS_WORLD_H #define BT_DYNAMICS_WORLD_H -#include "../../BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletCollision/CollisionDispatch/btCollisionWorld.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" + class btTypedConstraint; class btRaycastVehicle; class btConstraintSolver; +class btDynamicsWorld; + +/// Type for the callback for each tick +typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); +enum btDynamicsWorldType +{ + BT_SIMPLE_DYNAMICS_WORLD=1, + BT_DISCRETE_DYNAMICS_WORLD=2, + BT_CONTINUOUS_DYNAMICS_WORLD=3 +}; -///btDynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous +///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. class btDynamicsWorld : public btCollisionWorld { - public: + +protected: + btInternalTickCallback m_internalTickCallback; + void* m_worldUserInfo; + + btContactSolverInfo m_solverInfo; + +public: - btDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache) - :btCollisionWorld(dispatcher,pairCache) + btDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* broadphase,btCollisionConfiguration* collisionConfiguration) + :btCollisionWorld(dispatcher,broadphase,collisionConfiguration), m_internalTickCallback(0), m_worldUserInfo(0) { } @@ -37,34 +56,37 @@ class btDynamicsWorld : public btCollisionWorld { } - ///stepSimulation proceeds the simulation over timeStep units - ///if maxSubSteps > 0, it will interpolate time steps - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; + ///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=10, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.))=0; - virtual void updateAabbs() = 0; + virtual void debugDrawWorld() = 0; - virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) { (void)constraint;}; - - virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;}; - - virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; - - virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;}; + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false) + { + (void)constraint; (void)disableCollisionsBetweenLinkedBodies; + } + virtual void removeConstraint(btTypedConstraint* constraint) {(void)constraint;} - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) = 0; + virtual void addVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} - virtual btIDebugDraw* getDebugDrawer() = 0; + virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} //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) = 0; virtual void removeRigidBody(btRigidBody* body) = 0; virtual void setConstraintSolver(btConstraintSolver* solver) = 0; + + virtual btConstraintSolver* getConstraintSolver() = 0; virtual int getNumConstraints() const { return 0; } @@ -72,7 +94,35 @@ class btDynamicsWorld : public btCollisionWorld virtual const btTypedConstraint* getConstraint(int index) const { (void)index; return 0; } + 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) + { + m_internalTickCallback = cb; + m_worldUserInfo = worldUserInfo; + } + + void setWorldUserInfo(void* worldUserInfo) + { + m_worldUserInfo = worldUserInfo; + } + + void* getWorldUserInfo() const + { + return m_worldUserInfo; + } + + btContactSolverInfo& getSolverInfo() + { + return m_solverInfo; + } + + }; #endif //BT_DYNAMICS_WORLD_H + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 9ed3579d89c..e2afb687ac6 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -20,36 +20,53 @@ subject to the following restrictions: #include "LinearMath/btMotionState.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" -btScalar gLinearAirDamping = btScalar(1.); //'temporarily' global variables btScalar gDeactivationTime = btScalar(2.); bool gDisableDeactivation = false; - -btScalar gLinearSleepingThreshold = btScalar(0.8); -btScalar gAngularSleepingThreshold = btScalar(1.0); static int uniqueId = 0; -btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) -: - m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)), - m_angularFactor(btScalar(1.)), - m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_linearDamping(btScalar(0.)), - m_angularDamping(btScalar(0.5)), - m_optionalMotionState(motionState), - m_contactSolverType(0), - m_frictionSolverType(0) + +btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) +{ + setupRigidBody(constructionInfo); +} + +btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia) +{ + btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); + setupRigidBody(cinfo); +} + +void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) { - if (motionState) + 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 = btScalar(1.); + m_gravity.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)), + m_linearDamping = btScalar(0.); + m_angularDamping = btScalar(0.5); + m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; + m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; + m_optionalMotionState = constructionInfo.m_motionState; + m_contactSolverType = 0; + m_frictionSolverType = 0; + m_additionalDamping = constructionInfo.m_additionalDamping; + m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor; + m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr; + m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr; + m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor; + + if (m_optionalMotionState) { - motionState->getWorldTransform(m_worldTransform); + m_optionalMotionState->getWorldTransform(m_worldTransform); } else { - m_worldTransform = btTransform::getIdentity(); + m_worldTransform = constructionInfo.m_startWorldTransform; } m_interpolationWorldTransform = m_worldTransform; @@ -57,90 +74,21 @@ btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionS m_interpolationAngularVelocity.setValue(0,0,0); //moved to btCollisionObject - m_friction = friction; - m_restitution = restitution; - - m_collisionShape = collisionShape; - m_debugBodyId = uniqueId++; - - //m_internalOwner is to allow upcasting from collision object to rigid body - m_internalOwner = this; - - setMassProps(mass, localInertia); - setDamping(linearDamping, angularDamping); - updateInertiaTensor(); - -} - -#ifdef OBSOLETE_MOTIONSTATE_LESS -btRigidBody::btRigidBody( btScalar mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution) -: - m_gravity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_totalForce(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_totalTorque(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_linearVelocity(btScalar(0.0), btScalar(0.0), btScalar(0.0)), - m_angularVelocity(btScalar(0.),btScalar(0.),btScalar(0.)), - m_linearDamping(btScalar(0.)), - m_angularDamping(btScalar(0.5)), - m_optionalMotionState(0), - m_contactSolverType(0), - m_frictionSolverType(0) -{ - - m_worldTransform = worldTransform; - m_interpolationWorldTransform = m_worldTransform; - m_interpolationLinearVelocity.setValue(0,0,0); - m_interpolationAngularVelocity.setValue(0,0,0); - - //moved to btCollisionObject - m_friction = friction; - m_restitution = restitution; + m_friction = constructionInfo.m_friction; + m_restitution = constructionInfo.m_restitution; - m_collisionShape = collisionShape; + setCollisionShape( constructionInfo.m_collisionShape ); m_debugBodyId = uniqueId++; - //m_internalOwner is to allow upcasting from collision object to rigid body - m_internalOwner = this; - - setMassProps(mass, localInertia); - setDamping(linearDamping, angularDamping); + setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); + setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); updateInertiaTensor(); } -#endif //OBSOLETE_MOTIONSTATE_LESS - - - - -//#define EXPERIMENTAL_JITTER_REMOVAL 1 -#ifdef EXPERIMENTAL_JITTER_REMOVAL -//Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate -//doesn't work very well yet (value 0 disabled this damping) -//note there this influences deactivation thresholds! -btScalar gClippedAngvelThresholdSqr = btScalar(0.01); -btScalar gClippedLinearThresholdSqr = btScalar(0.01); -#endif //EXPERIMENTAL_JITTER_REMOVAL - -btScalar gJitterVelocityDampingFactor = btScalar(0.7); void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) { - -#ifdef EXPERIMENTAL_JITTER_REMOVAL - //if (wantsSleeping()) - { - //clip to avoid jitter - if ((m_angularVelocity.length2() < gClippedAngvelThresholdSqr) && - (m_linearVelocity.length2() < gClippedLinearThresholdSqr)) - { - m_angularVelocity *= gJitterVelocityDampingFactor; - m_linearVelocity *= gJitterVelocityDampingFactor; - } - } - -#endif //EXPERIMENTAL_JITTER_REMOVAL - btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); } @@ -191,50 +139,63 @@ void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) -#include <stdio.h> - -void btRigidBody::applyForces(btScalar step) +///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping +void btRigidBody::applyDamping(btScalar timeStep) { - if (isStaticOrKinematicObject()) - return; - - applyCentralForce(m_gravity); - - m_linearVelocity *= GEN_clamped((btScalar(1.) - step * gLinearAirDamping * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); - m_angularVelocity *= GEN_clamped((btScalar(1.) - step * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + 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)); -#define FORCE_VELOCITY_DAMPING 1 -#ifdef FORCE_VELOCITY_DAMPING - btScalar speed = m_linearVelocity.length(); - if (speed < m_linearDamping) + if (m_additionalDamping) { - btScalar dampVel = btScalar(0.005); - if (speed > dampVel) + //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 + if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) && + (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr)) { - btVector3 dir = m_linearVelocity.normalized(); - m_linearVelocity -= dir * dampVel; - } else - { - m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_angularVelocity *= m_additionalDampingFactor; + m_linearVelocity *= m_additionalDampingFactor; } - } + - btScalar angSpeed = m_angularVelocity.length(); - if (angSpeed < m_angularDamping) - { - btScalar angDampVel = btScalar(0.005); - if (angSpeed > angDampVel) + btScalar speed = m_linearVelocity.length(); + if (speed < m_linearDamping) { - btVector3 dir = m_angularVelocity.normalized(); - m_angularVelocity -= dir * angDampVel; - } else + btScalar dampVel = btScalar(0.005); + if (speed > dampVel) + { + btVector3 dir = m_linearVelocity.normalized(); + m_linearVelocity -= dir * dampVel; + } else + { + m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + } + } + + btScalar angSpeed = m_angularVelocity.length(); + if (angSpeed < m_angularDamping) { - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + btScalar angDampVel = btScalar(0.005); + if (angSpeed > angDampVel) + { + btVector3 dir = m_angularVelocity.normalized(); + m_angularVelocity -= dir * angDampVel; + } else + { + m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + } } } -#endif //FORCE_VELOCITY_DAMPING +} + + +void btRigidBody::applyGravity() +{ + if (isStaticOrKinematicObject()) + return; + applyCentralForce(m_gravity); + } void btRigidBody::proceedToTransform(const btTransform& newTrans) @@ -285,7 +246,6 @@ void btRigidBody::integrateVelocities(btScalar step) m_angularVelocity *= (MAX_ANGVEL/step) /angvel; } - clearForces(); } btQuaternion btRigidBody::getOrientation() const diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index 0707595d48e..4596f90a00f 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -16,27 +16,29 @@ subject to the following restrictions: #ifndef RIGIDBODY_H #define RIGIDBODY_H -#include "../../LinearMath/btAlignedObjectArray.h" -#include "../../LinearMath/btPoint3.h" -#include "../../LinearMath/btTransform.h" -#include "../../BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" -#include "../../BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btPoint3.h" +#include "LinearMath/btTransform.h" +#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" class btCollisionShape; class btMotionState; class btTypedConstraint; -extern btScalar gLinearAirDamping; - extern btScalar gDeactivationTime; extern bool gDisableDeactivation; -extern btScalar gLinearSleepingThreshold; -extern btScalar gAngularSleepingThreshold; -/// btRigidBody class for btRigidBody Dynamics -/// +///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: +///- 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. +///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 { @@ -53,7 +55,16 @@ class btRigidBody : public btCollisionObject 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; @@ -63,12 +74,85 @@ class btRigidBody : public btCollisionObject public: -#ifdef OBSOLETE_MOTIONSTATE_LESS - //not supported, please use btMotionState - btRigidBody(btScalar mass, const btTransform& worldTransform, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.)); -#endif //OBSOLETE_MOTIONSTATE_LESS - btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0),btScalar linearDamping=btScalar(0.),btScalar angularDamping=btScalar(0.),btScalar friction=btScalar(0.5),btScalar restitution=btScalar(0.)); + ///btRigidBodyConstructionInfo 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. + ///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 + { + 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; + + btCollisionShape* m_collisionShape; + btVector3 m_localInertia; + btScalar m_linearDamping; + btScalar m_angularDamping; + + ///best simulation results when friction is non-zero + btScalar m_friction; + ///best simulation results using zero restitution. + btScalar m_restitution; + + 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_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 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)); + + + 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); + } + +protected: + + ///setupRigidBody is only used internally by the constructor + void setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo); + +public: void proceedToTransform(const btTransform& newTrans); @@ -76,11 +160,15 @@ public: ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast static const btRigidBody* upcast(const btCollisionObject* colObj) { - return (const btRigidBody*)colObj->getInternalOwner(); + if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY) + return (const btRigidBody*)colObj; + return 0; } static btRigidBody* upcast(btCollisionObject* colObj) { - return (btRigidBody*)colObj->getInternalOwner(); + if (colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY) + return (btRigidBody*)colObj; + return 0; } /// continuous collision detection needs prediction @@ -88,8 +176,7 @@ public: void saveKinematicState(btScalar step); - - void applyForces(btScalar step); + void applyGravity(); void setGravity(const btVector3& acceleration); @@ -99,12 +186,34 @@ public: } void setDamping(btScalar lin_damping, btScalar ang_damping); - - inline const btCollisionShape* getCollisionShape() const { + + btScalar getLinearDamping() const + { + return m_linearDamping; + } + + btScalar getAngularDamping() const + { + return m_angularDamping; + } + + btScalar getLinearSleepingThreshold() const + { + return m_linearSleepingThreshold; + } + + btScalar getAngularSleepingThreshold() const + { + return m_angularSleepingThreshold; + } + + void applyDamping(btScalar timeStep); + + SIMD_FORCE_INLINE const btCollisionShape* getCollisionShape() const { return m_collisionShape; } - inline btCollisionShape* getCollisionShape() { + SIMD_FORCE_INLINE btCollisionShape* getCollisionShape() { return m_collisionShape; } @@ -134,6 +243,12 @@ public: m_invInertiaLocal = diagInvInertia; } + void setSleepingThresholds(btScalar linear,btScalar angular) + { + m_linearSleepingThreshold = linear; + m_angularSleepingThreshold = angular; + } + void applyTorque(const btVector3& torque) { m_totalTorque += torque; @@ -142,7 +257,7 @@ public: void applyForce(const btVector3& force, const btVector3& rel_pos) { applyCentralForce(force); - applyTorque(rel_pos.cross(force)); + applyTorque(rel_pos.cross(force)*m_angularFactor); } void applyCentralImpulse(const btVector3& impulse) @@ -168,7 +283,7 @@ public: } //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position - inline void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) { if (m_inverseMass != btScalar(0.)) { @@ -238,7 +353,7 @@ public: - inline btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const + SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const { btVector3 r0 = pos - getCenterOfMassPosition(); @@ -250,19 +365,19 @@ public: } - inline btScalar computeAngularImpulseDenominator(const btVector3& axis) const + SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const { btVector3 vec = axis * getInvInertiaTensorWorld(); return axis.dot(vec); } - inline void updateDeactivation(btScalar timeStep) + SIMD_FORCE_INLINE void updateDeactivation(btScalar timeStep) { if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) return; - if ((getLinearVelocity().length2() < gLinearSleepingThreshold*gLinearSleepingThreshold) && - (getAngularVelocity().length2() < gAngularSleepingThreshold*gAngularSleepingThreshold)) + if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) && + (getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold)) { m_deactivationTime += timeStep; } else @@ -273,7 +388,7 @@ public: } - inline bool wantsSleeping() + SIMD_FORCE_INLINE bool wantsSleeping() { if (getActivationState() == DISABLE_DEACTIVATION) @@ -348,6 +463,16 @@ public: void addConstraintRef(btTypedConstraint* c); void removeConstraintRef(btTypedConstraint* c); + btTypedConstraint* getConstraintRef(int index) + { + return m_constraintRefs[index]; + } + + int getNumConstraintRefs() + { + return m_constraintRefs.size(); + } + int m_debugBodyId; }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 4ebcb8e7517..3be04d1a4ad 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -27,16 +27,19 @@ subject to the following restrictions: can be used by probes that are checking whether the library is actually installed. */ -extern "C" void btBulletDynamicsProbe () {} +extern "C" +{ + void btBulletDynamicsProbe (); + void btBulletDynamicsProbe () {} +} -btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) -:btDynamicsWorld(dispatcher,pairCache), +btSimpleDynamicsWorld::btSimpleDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) +:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration), m_constraintSolver(constraintSolver), m_ownsConstraintSolver(false), -m_debugDrawer(0), m_gravity(0,0,-10) { @@ -46,7 +49,7 @@ m_gravity(0,0,-10) btSimpleDynamicsWorld::~btSimpleDynamicsWorld() { if (m_ownsConstraintSolver) - delete m_constraintSolver; + btAlignedFree( m_constraintSolver); } int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) @@ -74,8 +77,9 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; - - m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc); + m_constraintSolver->prepareSolve(0,numManifolds); + m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc,m_dispatcher1); + m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc); } ///integrate transforms @@ -85,10 +89,27 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b synchronizeMotionStates(); + clearForces(); + return 1; } +void btSimpleDynamicsWorld::clearForces() +{ + //todo: iterate over awake simulation islands! + 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) { @@ -104,6 +125,11 @@ void btSimpleDynamicsWorld::setGravity(const btVector3& gravity) } } +btVector3 btSimpleDynamicsWorld::getGravity () const +{ + return m_gravity; +} + void btSimpleDynamicsWorld::removeRigidBody(btRigidBody* body) { removeCollisionObject(body); @@ -133,7 +159,7 @@ void btSimpleDynamicsWorld::updateAabbs() btPoint3 minAabb,maxAabb; colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); btBroadphaseInterface* bp = getBroadphase(); - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb); + bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); } } } @@ -171,8 +197,9 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { if (body->isActive()) { - body->applyForces( timeStep); + body->applyGravity(); body->integrateVelocities( timeStep); + body->applyDamping(timeStep); body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } @@ -204,8 +231,13 @@ void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) { - delete m_constraintSolver; + btAlignedFree(m_constraintSolver); } m_ownsConstraintSolver = false; m_constraintSolver = solver; } + +btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver() +{ + return m_constraintSolver; +} diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index 25f4ccd8e68..5c56fdf1327 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -22,11 +22,8 @@ class btDispatcher; class btOverlappingPairCache; class btConstraintSolver; -///btSimpleDynamicsWorld demonstrates very basic usage of Bullet rigid body dynamics -///It can be used for basic simulations, and as a starting point for porting Bullet -///btSimpleDynamicsWorld lacks object deactivation, island management and other concepts. -///For more complicated simulations, btDiscreteDynamicsWorld and btContinuousDynamicsWorld are recommended -///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController +///btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds. +///Please use btDiscreteDynamicsWorld instead (or btContinuousDynamicsWorld once it is finished). class btSimpleDynamicsWorld : public btDynamicsWorld { protected: @@ -35,8 +32,6 @@ protected: bool m_ownsConstraintSolver; - btIDebugDraw* m_debugDrawer; - void predictUnconstraintMotion(btScalar timeStep); void integrateTransforms(btScalar timeStep); @@ -48,25 +43,17 @@ public: ///this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver - btSimpleDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver); + 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 void setDebugDrawer(btIDebugDraw* debugDrawer) - { - m_debugDrawer = debugDrawer; - }; - - virtual btIDebugDraw* getDebugDrawer() - { - return m_debugDrawer; - } - virtual void setGravity(const btVector3& gravity); + virtual btVector3 getGravity () const; + virtual void addRigidBody(btRigidBody* body); virtual void removeRigidBody(btRigidBody* body); @@ -77,6 +64,15 @@ public: virtual void setConstraintSolver(btConstraintSolver* solver); + virtual btConstraintSolver* getConstraintSolver(); + + virtual btDynamicsWorldType getWorldType() const + { + return BT_SIMPLE_DYNAMICS_WORLD; + } + + virtual void clearForces(); + }; #endif //BT_SIMPLE_DYNAMICS_WORLD_H |