diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Dynamics')
8 files changed, 396 insertions, 96 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp index c2fd71d67fe..47addbac45b 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/Bullet-C-API.cpp @@ -38,7 +38,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" -#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h" +#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h" #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" #include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h" @@ -110,16 +110,16 @@ void plDeleteDynamicsWorld(plDynamicsWorldHandle world) void plStepSimulation(plDynamicsWorldHandle world, plReal timeStep) { btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - assert(dynamicsWorld); + btAssert(dynamicsWorld); dynamicsWorld->stepSimulation(timeStep); } void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) { btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - assert(dynamicsWorld); + btAssert(dynamicsWorld); btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - assert(body); + btAssert(body); dynamicsWorld->addRigidBody(body); } @@ -127,9 +127,9 @@ void plAddRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) void plRemoveRigidBody(plDynamicsWorldHandle world, plRigidBodyHandle object) { btDynamicsWorld* dynamicsWorld = reinterpret_cast< btDynamicsWorld* >(world); - assert(dynamicsWorld); + btAssert(dynamicsWorld); btRigidBody* body = reinterpret_cast< btRigidBody* >(object); - assert(body); + btAssert(body); dynamicsWorld->removeRigidBody(body); } @@ -142,7 +142,7 @@ plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionSh trans.setIdentity(); btVector3 localInertia(0,0,0); btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - assert(shape); + btAssert(shape); if (mass) { shape->calculateLocalInertia(mass,localInertia); @@ -158,7 +158,7 @@ plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionSh void plDeleteRigidBody(plRigidBodyHandle cbody) { btRigidBody* body = reinterpret_cast< btRigidBody* >(cbody); - assert(body); + btAssert(body); btAlignedFree( body); } @@ -255,20 +255,20 @@ void plAddVertex(plCollisionShapeHandle cshape, plReal x,plReal y,plReal z) (void)colShape; btAssert(colShape->getShapeType()==CONVEX_HULL_SHAPE_PROXYTYPE); btConvexHullShape* convexHullShape = reinterpret_cast<btConvexHullShape*>( cshape); - convexHullShape->addPoint(btPoint3(x,y,z)); + convexHullShape->addPoint(btVector3(x,y,z)); } void plDeleteShape(plCollisionShapeHandle cshape) { btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - assert(shape); + btAssert(shape); btAlignedFree(shape); } void plSetScaling(plCollisionShapeHandle cshape, plVector3 cscaling) { btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape); - assert(shape); + btAssert(shape); btVector3 scaling(cscaling[0],cscaling[1],cscaling[2]); shape->setLocalScaling(scaling); } diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 745694e2f70..ba48f404929 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -29,6 +29,11 @@ subject to the following restrictions: #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/btSliderConstraint.h" //for debug rendering #include "BulletCollision/CollisionShapes/btBoxShape.h" @@ -51,6 +56,9 @@ subject to the following restrictions: #include "BulletDynamics/Vehicle/btRaycastVehicle.h" #include "BulletDynamics/Vehicle/btVehicleRaycaster.h" #include "BulletDynamics/Vehicle/btWheelInfo.h" +//character +#include "BulletDynamics/Character/btCharacterControllerInterface.h" + #include "LinearMath/btIDebugDraw.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btMotionState.h" @@ -110,7 +118,6 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) btRigidBody* body = btRigidBody::upcast(colObj); if (body) { - btTransform predictedTrans; if (body->getActivationState() != ISLAND_SLEEPING) { if (body->isKinematicObject()) @@ -125,6 +132,7 @@ void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) void btDiscreteDynamicsWorld::debugDrawWorld() { + BT_PROFILE("debugDrawWorld"); if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { @@ -144,13 +152,30 @@ void btDiscreteDynamicsWorld::debugDrawWorld() } } } + bool drawConstraints = false; + if (getDebugDrawer()) + { + int mode = getDebugDrawer()->getDebugMode(); + if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) + { + drawConstraints = true; + } + } + if(drawConstraints) + { + for(int i = getNumConstraints()-1; i>=0 ;i--) + { + btTypedConstraint* constraint = getConstraint(i); + debugDrawConstraint(constraint); + } + } + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) { int i; - //todo: iterate over awake simulation islands! for ( i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -179,7 +204,7 @@ void btDiscreteDynamicsWorld::debugDrawWorld() } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) { - btPoint3 minAabb,maxAabb; + btVector3 minAabb,maxAabb; btVector3 colorvec(1,0,0); colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); @@ -220,7 +245,7 @@ void btDiscreteDynamicsWorld::debugDrawWorld() void btDiscreteDynamicsWorld::clearForces() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -236,7 +261,7 @@ void btDiscreteDynamicsWorld::clearForces() ///apply gravity, call this once per timestep void btDiscreteDynamicsWorld::applyGravity() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -250,9 +275,29 @@ void btDiscreteDynamicsWorld::applyGravity() } +void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body) +{ + btAssert(body); + + if (body->getMotionState() && !body->isStaticOrKinematicObject()) + { + //we need to call the update at least once, even for sleeping objects + //otherwise the 'graphics' transform never updates properly + ///@todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } +} + void btDiscreteDynamicsWorld::synchronizeMotionStates() { + BT_PROFILE("synchronizeMotionStates"); { //todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) @@ -260,19 +305,8 @@ void btDiscreteDynamicsWorld::synchronizeMotionStates() 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*body->getHitFraction(),interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); - } - } + if (body) + synchronizeSingleMotionState(body); } } @@ -327,7 +361,8 @@ int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, //process some debugging flags if (getDebugDrawer()) { - gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + btIDebugDraw* debugDrawer = getDebugDrawer (); + gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; } if (numSimulationSubSteps) { @@ -392,7 +427,8 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) ///update vehicle simulation updateVehicles(timeStep); - + + updateCharacters(timeStep); updateActivationState( timeStep ); @@ -423,13 +459,6 @@ btVector3 btDiscreteDynamicsWorld::getGravity () const void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { - //remove all constraints too - while (body->getNumConstraintRefs()) - { - btTypedConstraint* constraint = body->getConstraintRef(0); - removeConstraint(constraint); - } - removeCollisionObject(body); } @@ -475,6 +504,20 @@ void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) } } +void btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep) +{ + BT_PROFILE("updateCharacters"); + + for ( int i=0;i<m_characters.size();i++) + { + btCharacterControllerInterface* character = m_characters[i]; + character->preStep (this); + character->playerStep (this,timeStep); + } +} + + + void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) { BT_PROFILE("updateActivationState"); @@ -539,6 +582,17 @@ void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) m_vehicles.remove(vehicle); } +void btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character) +{ + m_characters.push_back(character); +} + +void btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character) +{ + m_characters.remove(character); +} + + SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) { int islandId; @@ -611,8 +665,11 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { 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); + if (numManifolds + m_numConstraints) + { + ///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 { //also add all non-contact constraints/joints for this island @@ -658,7 +715,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) sortedConstraints[i] = m_constraints[i]; } -// assert(0); +// btAssert(0); @@ -671,7 +728,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); } @@ -722,21 +779,27 @@ class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConve btCollisionObject* m_me; btScalar m_allowedPenetration; btOverlappingPairCache* m_pairCache; + btDispatcher* m_dispatcher; public: - btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : + btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA,toA), m_allowedPenetration(0.0f), m_me(me), - m_pairCache(pairCache) + m_pairCache(pairCache), + m_dispatcher(dispatcher) { } virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) { if (convexResult.m_hitCollisionObject == m_me) - return 1.0; + return 1.0f; + + //ignore result if there is no contact response + if(!convexResult.m_hitCollisionObject->hasContactResponse()) + return 1.0f; btVector3 linVelA,linVelB; linVelA = m_convexToWorld-m_convexFromWorld; @@ -757,23 +820,29 @@ public: return false; ///don't do CCD when the collision filters are not matching - if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0)) + if (!ClosestConvexResultCallback::needsCollision(proxy0)) return false; - ///don't do CCD when there are already contact points (touching contact/penetration) - btAlignedObjectArray<btPersistentManifold*> manifoldArray; - btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); - if (collisionPair) + btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + + //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 + if (m_dispatcher->needsResponse(m_me,otherObj)) { - if (collisionPair->m_algorithm) + ///don't do CCD when there are already contact points (touching contact/penetration) + btAlignedObjectArray<btPersistentManifold*> manifoldArray; + btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); + if (collisionPair) { - manifoldArray.resize(0); - collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); - for (int j=0;j<manifoldArray.size();j++) + if (collisionPair->m_algorithm) { - btPersistentManifold* manifold = manifoldArray[j]; - if (manifold->getNumContacts()>0) - return false; + manifoldArray.resize(0); + collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); + for (int j=0;j<manifoldArray.size();j++) + { + btPersistentManifold* manifold = manifoldArray[j]; + if (manifold->getNumContacts()>0) + return false; + } } } } @@ -811,9 +880,13 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { gNumClampedCcdMotions++; - btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache()); + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + + sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; + sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { @@ -833,6 +906,8 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) + + void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BT_PROFILE("predictUnconstraintMotion"); @@ -844,14 +919,12 @@ void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { if (!body->isStaticOrKinematicObject()) { - if (body->isActive()) - { - body->integrateVelocities( timeStep); - //damping - body->applyDamping(timeStep); + + body->integrateVelocities( timeStep); + //damping + body->applyDamping(timeStep); - body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); - } + body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform()); } } } @@ -1103,7 +1176,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, { btConcaveShape* concaveMesh = (btConcaveShape*) shape; - //todo pass camera, for some culling + ///@todo pass camera, for some culling? no -> we are not a graphics lib btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); @@ -1132,7 +1205,7 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, int i; for (i=0;i<polyshape->getNumEdges();i++) { - btPoint3 a,b; + btVector3 a,b; polyshape->getEdge(i,a,b); btVector3 wa = worldTransform * a; btVector3 wb = worldTransform * b; @@ -1148,6 +1221,189 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, } +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)) + { + return; + } + + 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) + { + break; + } + bool drawSect = true; + if(minAng > maxAng) + { + 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); + } + } + 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) + { + //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)); + + 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; + 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) + { + 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)); + } + } + 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) + { + btTransform tr = pSlider->getCalculatedTransformA(); + 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; + } + return; +} // btDiscreteDynamicsWorld::debugDrawConstraint() + + + + + void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) { if (m_ownsConstraintSolver) @@ -1176,3 +1432,5 @@ const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const { return m_constraints[index]; } + + diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index d9e2652aaf6..34d4e6b353f 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -26,6 +26,7 @@ class btTypedConstraint; class btRaycastVehicle; +class btCharacterControllerInterface; class btIDebugDraw; #include "LinearMath/btAlignedObjectArray.h" @@ -42,7 +43,6 @@ protected: btAlignedObjectArray<btTypedConstraint*> m_constraints; - btVector3 m_gravity; //for variable timesteps @@ -54,6 +54,9 @@ protected: btAlignedObjectArray<btRaycastVehicle*> m_vehicles; + + btAlignedObjectArray<btCharacterControllerInterface*> m_characters; + int m_profileTimings; @@ -61,24 +64,26 @@ protected: virtual void integrateTransforms(btScalar timeStep); - void calculateSimulationIslands(); + virtual void calculateSimulationIslands(); - void solveConstraints(btContactSolverInfo& solverInfo); + virtual void solveConstraints(btContactSolverInfo& solverInfo); void updateActivationState(btScalar timeStep); void updateVehicles(btScalar timeStep); + void updateCharacters(btScalar timeStep); + void startProfiling(btScalar timeStep); virtual void internalSingleStepSimulation( btScalar timeStep); - void synchronizeMotionStates(); - void saveKinematicState(btScalar timeStep); + virtual void saveKinematicState(btScalar timeStep); void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); + public: @@ -91,13 +96,23 @@ public: virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + virtual void synchronizeMotionStates(); + + ///this can be useful to synchronize a single rigid body -> graphics object + void synchronizeSingleMotionState(btRigidBody* body); - void removeConstraint(btTypedConstraint* constraint); + virtual void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); - void addVehicle(btRaycastVehicle* vehicle); + virtual void removeConstraint(btTypedConstraint* constraint); - void removeVehicle(btRaycastVehicle* vehicle); + virtual void addVehicle(btRaycastVehicle* vehicle); + + virtual void removeVehicle(btRaycastVehicle* vehicle); + + virtual void addCharacter(btCharacterControllerInterface* character); + + virtual void removeCharacter(btCharacterControllerInterface* character); + btSimulationIslandManager* getSimulationIslandManager() { @@ -114,7 +129,6 @@ public: return this; } - virtual void setGravity(const btVector3& gravity); virtual btVector3 getGravity () const; @@ -126,6 +140,8 @@ public: void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + void debugDrawConstraint(btTypedConstraint* constraint); + virtual void debugDrawWorld(); virtual void setConstraintSolver(btConstraintSolver* solver); @@ -152,6 +168,7 @@ public: virtual void setNumTasks(int numTasks) { + (void) numTasks; } }; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 929e24d337c..2d90e212f7a 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -23,6 +23,7 @@ class btTypedConstraint; class btRaycastVehicle; class btConstraintSolver; class btDynamicsWorld; +class btCharacterControllerInterface; /// Type for the callback for each tick typedef void (*btInternalTickCallback)(btDynamicsWorld *world, btScalar timeStep); @@ -75,11 +76,18 @@ public: virtual void removeVehicle(btRaycastVehicle* vehicle) {(void)vehicle;} + virtual void addCharacter(btCharacterControllerInterface* character) {(void)character;} + + virtual void removeCharacter(btCharacterControllerInterface* character) {(void)character;} + + //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 synchronizeMotionStates() = 0; + virtual void addRigidBody(btRigidBody* body) = 0; virtual void removeRigidBody(btRigidBody* body) = 0; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index 93d70de39d8..a2b8a7dade4 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -45,8 +45,8 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& 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_anisotropicFriction.setValue(1.f,1.f,1.f); 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)), m_linearDamping = btScalar(0.); @@ -125,6 +125,7 @@ void btRigidBody::setGravity(const btVector3& acceleration) { m_gravity = acceleration * (btScalar(1.0) / m_inverseMass); } + m_gravity_acceleration = acceleration; } @@ -144,8 +145,17 @@ void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping 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)); +#else + m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep); + m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep); +#endif if (m_additionalDamping) { diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index 4596f90a00f..ec570cab875 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -17,7 +17,6 @@ subject to the following restrictions: #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" @@ -31,7 +30,7 @@ extern btScalar gDeactivationTime; extern bool gDisableDeactivation; -///btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. +///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: ///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics. @@ -49,6 +48,7 @@ class btRigidBody : public btCollisionObject btScalar m_angularFactor; btVector3 m_gravity; + btVector3 m_gravity_acceleration; btVector3 m_invInertiaLocal; btVector3 m_totalForce; btVector3 m_totalTorque; @@ -75,7 +75,7 @@ class btRigidBody : public btCollisionObject public: - ///btRigidBodyConstructionInfo provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body. + ///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. ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state, @@ -182,7 +182,7 @@ public: const btVector3& getGravity() const { - return m_gravity; + return m_gravity_acceleration; } void setDamping(btScalar lin_damping, btScalar ang_damping); @@ -232,8 +232,18 @@ public: { m_totalForce += force; } + + const btVector3& getTotalForce() + { + return m_totalForce; + }; + + const btVector3& getTotalTorque() + { + return m_totalTorque; + }; - const btVector3& getInvInertiaDiagLocal() + const btVector3& getInvInertiaDiagLocal() const { return m_invInertiaLocal; }; @@ -303,7 +313,7 @@ public: void updateInertiaTensor(); - const btPoint3& getCenterOfMassPosition() const { + const btVector3& getCenterOfMassPosition() const { return m_worldTransform.getOrigin(); } btQuaternion getOrientation() const; @@ -321,15 +331,12 @@ public: inline void setLinearVelocity(const btVector3& lin_vel) { - assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT); m_linearVelocity = lin_vel; } - inline void setAngularVelocity(const btVector3& ang_vel) { - assert (m_collisionFlags != btCollisionObject::CF_STATIC_OBJECT); - { - m_angularVelocity = ang_vel; - } + inline void setAngularVelocity(const btVector3& ang_vel) + { + m_angularVelocity = ang_vel; } btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const @@ -353,7 +360,7 @@ public: - SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btPoint3& pos, const btVector3& normal) const + SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const { btVector3 r0 = pos - getCenterOfMassPosition(); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index 3be04d1a4ad..3f6141463c3 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -97,7 +97,7 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b void btSimpleDynamicsWorld::clearForces() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; @@ -156,7 +156,7 @@ void btSimpleDynamicsWorld::updateAabbs() { if (body->isActive() && (!body->isStaticObject())) { - btPoint3 minAabb,maxAabb; + btVector3 minAabb,maxAabb; colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); btBroadphaseInterface* bp = getBroadphase(); bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1); @@ -210,7 +210,7 @@ void btSimpleDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) void btSimpleDynamicsWorld::synchronizeMotionStates() { - //todo: iterate over awake simulation islands! + ///@todo: iterate over awake simulation islands! for ( int i=0;i<m_collisionObjects.size();i++) { btCollisionObject* colObj = m_collisionObjects[i]; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index 5c56fdf1327..ea1a2a1ccc7 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -22,7 +22,7 @@ class btDispatcher; class btOverlappingPairCache; class btConstraintSolver; -///btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds. +///The 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 { @@ -60,7 +60,7 @@ public: virtual void updateAabbs(); - void synchronizeMotionStates(); + virtual void synchronizeMotionStates(); virtual void setConstraintSolver(btConstraintSolver* solver); |