diff options
author | Brecht Van Lommel <brechtvanlommel@pandora.be> | 2008-11-13 00:16:53 +0300 |
---|---|---|
committer | Brecht Van Lommel <brechtvanlommel@pandora.be> | 2008-11-13 00:16:53 +0300 |
commit | bdfe7d89e2f1292644577972c716931b4ce3c6c3 (patch) | |
tree | d00eb50b749cb001e2b08272c91791e66740b05d /extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp | |
parent | 78a1c27c4a6abe0ed31ca93ad21910f3df04da56 (diff) | |
parent | 7e4db234cee71ead34ee81a12e27da4bd548eb4b (diff) |
Merge of trunk into blender 2.5:
svn merge https://svn.blender.org/svnroot/bf-blender/trunk/blender -r12987:17416
Issues:
* GHOST/X11 had conflicting changes. Some code was added in 2.5, which was
later added in trunk also, but reverted partially, specifically revision
16683. I have left out this reversion in the 2.5 branch since I think it is
needed there.
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=16683
* Scons had various conflicting changes, I decided to go with trunk version
for everything except priorities and some library renaming.
* In creator.c, there were various fixes and fixes for fixes related to the -w
-W and -p options. In 2.5 -w and -W is not coded yet, and -p is done
differently. Since this is changed so much, and I don't think those fixes
would be needed in 2.5, I've left them out.
* Also in creator.c: there was code for a python bugfix where the screen was not
initialized when running with -P. The code that initializes the screen there
I had to disable, that can't work in 2.5 anymore but left it commented as a
reminder.
Further I had to disable some new function calls. using src/ and python/, as
was done already in this branch, disabled function calls:
* bpath.c: error reporting
* BME_conversions.c: editmesh conversion functions.
* SHD_dynamic: disabled almost completely, there is no python/.
* KX_PythonInit.cpp and Ketsji/ build files: Mathutils is not there, disabled.
* text.c: clipboard copy call.
* object.c: OB_SUPPORT_MATERIAL.
* DerivedMesh.c and subsurf_ccg, stipple_quarttone.
Still to be done:
* Go over files and functions that were moved to a different location but could
still use changes that were done in trunk.
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp')
-rw-r--r-- | extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp | 554 |
1 files changed, 389 insertions, 165 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 29719ec9a3e..745694e2f70 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,9 +415,21 @@ void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity) } } +btVector3 btDiscreteDynamicsWorld::getGravity () const +{ + return m_gravity; +} + void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) { + //remove all constraints too + while (body->getNumConstraintRefs()) + { + btTypedConstraint* constraint = body->getConstraintRef(0); + removeConstraint(constraint); + } + removeCollisionObject(body); } @@ -343,19 +466,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 +496,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 +510,6 @@ void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) } } } - END_PROFILE("updateActivationState"); } void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) @@ -412,7 +539,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 +569,7 @@ class btSortConstraintOnIslandPredicate void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { + BT_PROFILE("solveConstraints"); struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback { @@ -452,7 +580,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) int m_numConstraints; btIDebugDraw* m_debugDrawer; btStackAlloc* m_stackAlloc; - + btDispatcher* m_dispatcher; InplaceSolverIslandCallback( btContactSolverInfo& solverInfo, @@ -460,13 +588,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 +609,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 +662,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 +681,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) void btDiscreteDynamicsWorld::calculateSimulationIslands() { - BEGIN_PROFILE("calculateSimulationIslands"); + BT_PROFILE("calculateSimulationIslands"); getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); @@ -553,8 +695,8 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() const btRigidBody* colObj0 = &constraint->getRigidBodyA(); const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { if (colObj0->isActive() || colObj1->isActive()) { @@ -569,66 +711,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 +797,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 +846,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 +987,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,10 +1041,24 @@ void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); btScalar height = coneShape->getHeight();//+coneShape->getMargin(); btVector3 start = worldTransform.getOrigin(); - 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); + + int upAxis= coneShape->getConeUpIndex(); + + + btVector3 offsetHeight(0,0,0); + offsetHeight[upAxis] = height * btScalar(0.5); + btVector3 offsetRadius(0,0,0); + offsetRadius[(upAxis+1)%3] = radius; + btVector3 offset2Radius(0,0,0); + offset2Radius[(upAxis+2)%3] = radius; + + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color); + + + break; } @@ -868,7 +1067,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; @@ -878,6 +1077,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: { @@ -902,7 +1120,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); } @@ -934,12 +1152,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()); |