diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp | 531 |
1 files changed, 366 insertions, 165 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 1017c8af6ea..e46c4e6136b 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*body->getHitFraction(),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) { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + ///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 + 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++) + { + if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + { + numCurConstraints++; + } + } + + ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive + if (numManifolds + numCurConstraints) { - numCurConstraints++; + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); } + } - - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc); } }; @@ -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,85 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() //Store the island id in each body getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - END_PROFILE("calculateSimulationIslands"); - + } -void btDiscreteDynamicsWorld::updateAabbs() +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" + +class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { - BEGIN_PROFILE("updateAabbs"); - - btVector3 colorvec(1,0,0); - btTransform predictedTrans; - for ( int i=0;i<m_collisionObjects.size();i++) + btCollisionObject* m_me; + btScalar m_allowedPenetration; + btOverlappingPairCache* m_pairCache; + + +public: + btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : + btCollisionWorld::ClosestConvexResultCallback(fromA,toA), + m_allowedPenetration(0.0f), + m_me(me), + m_pairCache(pairCache) { - 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"); - } + virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace) + { + if (convexResult.m_hitCollisionObject == m_me) + return 1.0; + btVector3 linVelA,linVelB; + linVelA = m_convexToWorld-m_convexFromWorld; + linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin(); - } - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + btVector3 relativeVelocity = (linVelA-linVelB); + //don't report time of impact for motion away from the contact normal (or causes minor penetration) + if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration) + return 1.f; + + return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace); + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + //don't collide with itself + if (proxy0->m_clientObject == m_me) + return false; + + ///don't do CCD when the collision filters are not matching + if (!btCollisionWorld::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) + { + if (collisionPair->m_algorithm) + { + manifoldArray.resize(0); + collisionPair->m_algorithm->getAllContactManifolds(manifoldArray); + for (int j=0;j<manifoldArray.size();j++) { - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); + btPersistentManifold* manifold = manifoldArray[j]; + if (manifold->getNumContacts()>0) + return false; } } } + return true; } - - END_PROFILE("updateAabbs"); -} + +}; + +///internal debugging variable. this value shouldn't be too high +int gNumClampedCcdMotions=0; + +//#include "stdio.h" void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { - BEGIN_PROFILE("integrateTransforms"); + BT_PROFILE("integrateTransforms"); btTransform predictedTrans; for ( int i=0;i<m_collisionObjects.size();i++) { @@ -636,21 +790,45 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) btRigidBody* body = btRigidBody::upcast(colObj); if (body) { + body->setHitFraction(1.f); + if (body->isActive() && (!body->isStaticOrKinematicObject())) { body->predictIntegratedTransform(timeStep, predictedTrans); + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + + if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) + { + BT_PROFILE("CCD motion clamping"); + if (body->getCollisionShape()->isConvex()) + { + gNumClampedCcdMotions++; + + btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache()); + btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); + if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) + { + body->setHitFraction(sweepResults.m_closestHitFraction); + body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); + body->setHitFraction(0.f); +// printf("clamped integration to hit fraction = %f\n",fraction); + } + } + } + body->proceedToTransform( predictedTrans); } } } - 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 +839,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 - - - //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(); +#ifndef BT_NO_PROFILE + CProfileManager::Reset(); +#endif //BT_NO_PROFILE - m_profileTimings = 0; - btProfiler::destroy(); - } - } -#endif //USE_QUICKPROF } @@ -827,27 +980,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 +1034,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 +1050,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 +1060,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 +1070,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 +1113,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 +1145,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()); |