diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 198276d76..b535fac65 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -273,6 +273,8 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO if ((!body0->isActive()) && (!body1->isActive())) needsCollision = false; + else if (!body0->checkCollideWith(body1)) + needsCollision = false; return needsCollision ; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp index 7b6f06627..b12b2ec6f 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -24,7 +24,8 @@ btCollisionObject::btCollisionObject() m_userObjectPointer(0), m_hitFraction(btScalar(1.)), m_ccdSweptSphereRadius(btScalar(0.)), - m_ccdSquareMotionThreshold(btScalar(0.)) + m_ccdSquareMotionThreshold(btScalar(0.)), + m_checkCollideWith(false) { } diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 93990235a..9d29f43b6 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -69,15 +69,23 @@ protected: void* m_internalOwner; ///time of impact calculation - btScalar m_hitFraction; + btScalar m_hitFraction; ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: - btScalar m_ccdSweptSphereRadius; + btScalar m_ccdSweptSphereRadius; /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold - btScalar m_ccdSquareMotionThreshold; + btScalar m_ccdSquareMotionThreshold; - char m_pad[8]; + /// If some object should have elaborate collision filtering by sub-classes + bool m_checkCollideWith; + + char m_pad[7]; + + virtual bool checkCollideWithOverride(btCollisionObject* co) + { + return true; + } public: @@ -159,7 +167,7 @@ public: return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); } - void setRestitution(btScalar rest) + void setRestitution(btScalar rest) { m_restitution = rest; } @@ -322,6 +330,15 @@ public: m_userObjectPointer = userPointer; } + inline bool checkCollideWith(btCollisionObject* co) + { + if (m_checkCollideWith) + return checkCollideWithOverride(co); + + return true; + } + + } ; diff --git a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index db92702ee..184234648 100644 --- a/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -290,16 +290,32 @@ void btHeightfieldTerrainShape::processAllTriangles(btTriangleCallback* callback for(int x=startX; xprocessTriangle(vertices,x,j); - //second triangle - getVertex(x+1,j,vertices[0]); - getVertex(x,j+1,vertices[1]); - getVertex(x+1,j+1,vertices[2]); - callback->processTriangle(vertices,x,j); + if (!m_flipQuadEdges) + { + //first triangle + getVertex(x,j,vertices[0]); + getVertex(x,j+1,vertices[1]); + getVertex(x+1,j,vertices[2]); + callback->processTriangle(vertices,x,j); + //second triangle + getVertex(x+1,j,vertices[0]); + getVertex(x,j+1,vertices[1]); + getVertex(x+1,j+1,vertices[2]); + callback->processTriangle(vertices,x,j); + } else + { + //first triangle + getVertex(x,j,vertices[0]); + getVertex(x+1,j,vertices[1]); + getVertex(x+1,j+1,vertices[2]); + callback->processTriangle(vertices,x,j); + //second triangle + getVertex(x,j,vertices[0]); + getVertex(x+1,j+1,vertices[1]); + getVertex(x,j+1,vertices[2]); + callback->processTriangle(vertices,x,j); + + } } } diff --git a/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index be044dbc1..1897b4740 100644 --- a/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -46,6 +46,20 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + int getSphereCount() const + { + return m_numSpheres; + } + + const btVector3& getSpherePosition(int index) const + { + return m_localPositions[index]; + } + + btScalar getSphereRadius(int index) const + { + return m_radi[index]; + } virtual int getShapeType() const { return MULTI_SPHERE_SHAPE_PROXYTYPE; } diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 102b1f00e..0e7c2c4eb 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -1,864 +1,941 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btDiscreteDynamicsWorld.h" - -//collision detection -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" -#include - -//rigidbody & constraints -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" - -//for debug rendering -#include "BulletCollision/CollisionShapes/btCompoundShape.h" -#include "BulletCollision/CollisionShapes/btSphereShape.h" -#include "BulletCollision/CollisionShapes/btBoxShape.h" -#include "BulletCollision/CollisionShapes/btCylinderShape.h" -#include "BulletCollision/CollisionShapes/btConeShape.h" -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" -#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" -#include "BulletCollision/CollisionShapes/btTriangleCallback.h" -#include "LinearMath/btIDebugDraw.h" - - - -//vehicle -#include "BulletDynamics/Vehicle/btRaycastVehicle.h" -#include "BulletDynamics/Vehicle/btVehicleRaycaster.h" -#include "BulletDynamics/Vehicle/btWheelInfo.h" -#include "LinearMath/btIDebugDraw.h" -#include "LinearMath/btQuickprof.h" -#include "LinearMath/btMotionState.h" - - - - - -btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) -:btDynamicsWorld(dispatcher,pairCache), -m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver), -m_debugDrawer(0), -m_gravity(0,-10,0), -m_localTime(btScalar(1.)/btScalar(60.)), -m_profileTimings(0) -{ - m_islandManager = new btSimulationIslandManager(); - m_ownsIslandManager = true; - m_ownsConstraintSolver = (constraintSolver==0); -} - - -btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() -{ - //only delete it when we created it - if (m_ownsIslandManager) - delete m_islandManager; - if (m_ownsConstraintSolver) - delete m_constraintSolver; -} - -void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) -{ - - for (int i=0;igetActivationState() != ISLAND_SLEEPING) - { - if (body->isKinematicObject()) - { - //to calculate velocities next frame - body->saveKinematicState(timeStep); - } - } - } - } -} - -void btDiscreteDynamicsWorld::synchronizeMotionStates() -{ - //debug vehicle wheels - - - { - //todo: iterate over awake simulation islands! - for ( int i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) - { - btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.)); - switch(colObj->getActivationState()) - { - case ACTIVE_TAG: - color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break; - case ISLAND_SLEEPING: - color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break; - case WANTS_DEACTIVATION: - color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break; - case DISABLE_DEACTIVATION: - color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break; - case DISABLE_SIMULATION: - color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break; - default: - { - color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.)); - } - }; - - debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); - } - btRigidBody* body = btRigidBody::upcast(colObj); - if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) - { - //we need to call the update at least once, even for sleeping objects - //otherwise the 'graphics' transform never updates properly - //so todo: add 'dirty' flag - //if (body->getActivationState() != ISLAND_SLEEPING) - { - btTransform interpolatedTransform; - btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), - body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform); - body->getMotionState()->setWorldTransform(interpolatedTransform); - } - } - } - } - - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) - { - for ( int i=0;im_vehicles.size();i++) - { - for (int v=0;vgetNumWheels();v++) - { - btVector3 wheelColor(0,255,255); - if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) - { - wheelColor.setValue(0,0,255); - } else - { - 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( - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], - m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); - - - //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS - //debug wheels (cylinders) - m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); - m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); - - } - } - } - -} - - -int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) -{ - int numSimulationSubSteps = 0; - - if (maxSubSteps) - { - //fixed timestep with interpolation - m_localTime += timeStep; - if (m_localTime >= fixedTimeStep) - { - numSimulationSubSteps = int( m_localTime / fixedTimeStep); - m_localTime -= numSimulationSubSteps * fixedTimeStep; - } - } else - { - //variable timestep - fixedTimeStep = timeStep; - m_localTime = timeStep; - if (btFuzzyZero(timeStep)) - { - numSimulationSubSteps = 0; - maxSubSteps = 0; - } else - { - numSimulationSubSteps = 1; - maxSubSteps = 1; - } - } - - //process some debugging flags - if (getDebugDrawer()) - { - gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; - } - if (numSimulationSubSteps) - { - - saveKinematicState(fixedTimeStep); - - //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt - int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; - - for (int i=0;isetGravity(gravity); - } - } -} - - -void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) -{ - removeCollisionObject(body); -} - -void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) -{ - if (!body->isStaticOrKinematicObject()) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); - short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); - short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - - addCollisionObject(body,collisionFilterGroup,collisionFilterMask); - } -} - - -void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) -{ - BEGIN_PROFILE("updateVehicles"); - - for ( int i=0;iupdateVehicle( timeStep); - } - END_PROFILE("updateVehicles"); -} - -void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) -{ - BEGIN_PROFILE("updateActivationState"); - - for ( int i=0;iupdateDeactivation(timeStep); - - if (body->wantsSleeping()) - { - if (body->isStaticOrKinematicObject()) - { - body->setActivationState(ISLAND_SLEEPING); - } else - { - if (body->getActivationState() == ACTIVE_TAG) - body->setActivationState( WANTS_DEACTIVATION ); - } - } else - { - if (body->getActivationState() != DISABLE_DEACTIVATION) - body->setActivationState( ACTIVE_TAG ); - } - } - } - END_PROFILE("updateActivationState"); -} - -void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint) -{ - m_constraints.push_back(constraint); -} - -void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) -{ - m_constraints.remove(constraint); -} - -void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) -{ - m_vehicles.push_back(vehicle); -} - -void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) -{ - m_vehicles.remove(vehicle); -} - -inline int btGetConstraintIslandId(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); - return islandId; - -} - - -class btSortConstraintOnIslandPredicate -{ - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId(rhs); - lIslandId0 = btGetConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; - - - - -void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) -{ - - struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback - { - - btContactSolverInfo& m_solverInfo; - btConstraintSolver* m_solver; - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btStackAlloc* m_stackAlloc; - - - InplaceSolverIslandCallback( - btContactSolverInfo& solverInfo, - btConstraintSolver* solver, - btTypedConstraint** sortedConstraints, - int numConstraints, - btIDebugDraw* debugDrawer, - btStackAlloc* stackAlloc) - :m_solverInfo(solverInfo), - m_solver(solver), - m_sortedConstraints(sortedConstraints), - m_numConstraints(numConstraints), - m_debugDrawer(debugDrawer), - m_stackAlloc(stackAlloc) - { - - } - - InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - 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;isolveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc); - } - - }; - - //sorted version of all btTypedConstraint, based on islandId - btAlignedObjectArray sortedConstraints; - sortedConstraints.resize( m_constraints.size()); - int i; - for (i=0;ibuildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); - - -} - - - - -void btDiscreteDynamicsWorld::calculateSimulationIslands() -{ - BEGIN_PROFILE("calculateSimulationIslands"); - - getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); - - { - int i; - int numConstraints = int(m_constraints.size()); - for (i=0;i< numConstraints ; i++ ) - { - btTypedConstraint* constraint = m_constraints[i]; - - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) - { - if (colObj0->isActive() || colObj1->isActive()) - { - - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); - } - } - } - } - - //Store the island id in each body - getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); - - END_PROFILE("calculateSimulationIslands"); - -} - - -void btDiscreteDynamicsWorld::updateAabbs() -{ - BEGIN_PROFILE("updateAabbs"); - - btVector3 colorvec(1,0,0); - btTransform predictedTrans; - for ( int i=0;iIsActive() && (!body->IsStatic())) - { - btPoint3 minAabb,maxAabb; - colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); - btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; - - //moving objects should be moderately sized, probably something wrong if not - if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) - { - bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb); - } else - { - //something went wrong, investigate - //this assert is unwanted in 3D modelers (danger of loosing work) - body->setActivationState(DISABLE_SIMULATION); - - static bool reportMe = true; - if (reportMe && m_debugDrawer) - { - reportMe = false; - m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); - m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); - m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); - m_debugDrawer->reportErrorWarning("Thanks.\n"); - } - - - } - if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) - { - m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); - } - } - } - } - - END_PROFILE("updateAabbs"); -} - -void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) -{ - BEGIN_PROFILE("integrateTransforms"); - btTransform predictedTrans; - for ( int i=0;iisActive() && (!body->isStaticOrKinematicObject())) - { - body->predictIntegratedTransform(timeStep, predictedTrans); - body->proceedToTransform( predictedTrans); - } - } - } - END_PROFILE("integrateTransforms"); -} - - - -void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) -{ - BEGIN_PROFILE("predictUnconstraintMotion"); - for ( int i=0;iisStaticOrKinematicObject()) - { - if (body->isActive()) - { - body->applyForces( timeStep); - body->integrateVelocities( 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(); - - m_profileTimings = 0; - btProfiler::destroy(); - } - } -#endif //USE_QUICKPROF -} - - - - - - -class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback -{ - btIDebugDraw* m_debugDrawer; - btVector3 m_color; - btTransform m_worldTrans; - -public: - - DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : - m_debugDrawer(debugDrawer), - m_color(color), - m_worldTrans(worldTrans) - { - } - - virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) - { - processTriangle(triangle,partId,triangleIndex); - } - - virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) - { - (void)partId; - (void)triangleIndex; - - btVector3 wv0,wv1,wv2; - wv0 = m_worldTrans*triangle[0]; - wv1 = m_worldTrans*triangle[1]; - wv2 = m_worldTrans*triangle[2]; - m_debugDrawer->drawLine(wv0,wv1,m_color); - m_debugDrawer->drawLine(wv1,wv2,m_color); - m_debugDrawer->drawLine(wv2,wv0,m_color); - } -}; - - - -void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) -{ - - if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) - { - const btCompoundShape* compoundShape = static_cast(shape); - for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) - { - btTransform childTrans = compoundShape->getChildTransform(i); - const btCollisionShape* colShape = compoundShape->getChildShape(i); - debugDrawObject(worldTransform*childTrans,colShape,color); - } - - } else - { - switch (shape->getShapeType()) - { - - case SPHERE_SHAPE_PROXYTYPE: - { - const btSphereShape* sphereShape = static_cast(shape); - btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin - btVector3 start = worldTransform.getOrigin(); - getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(radius,0,0),color); - getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,radius,0),color); - getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,0,radius),color); - //drawSphere - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: - case CONE_SHAPE_PROXYTYPE: - { - const btConeShape* coneShape = static_cast(shape); - 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); - break; - - } - case CYLINDER_SHAPE_PROXYTYPE: - { - const btCylinderShape* cylinder = static_cast(shape); - int upAxis = cylinder->getUpAxis(); - btScalar radius = cylinder->getRadius(); - btScalar halfHeight = cylinder->getHalfExtents()[upAxis]; - btVector3 start = worldTransform.getOrigin(); - btVector3 offsetHeight(0,0,0); - offsetHeight[upAxis] = halfHeight; - btVector3 offsetRadius(0,0,0); - offsetRadius[(upAxis+1)%3] = radius; - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); - getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); - break; - } - default: - { - - if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) - { - btTriangleMeshShape* concaveMesh = (btTriangleMeshShape*) shape; - //btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - //btVector3 aabbMax(100,100,100);//btScalar(1e30),btScalar(1e30),btScalar(1e30)); - - //todo pass camera, for some culling - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); - - } - - if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) - { - btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; - //todo: pass camera for some culling - btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); - btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); - //DebugDrawcallback drawCallback; - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); - } - - - /// for polyhedral shapes - if (shape->isPolyhedral()) - { - btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - - int i; - for (i=0;igetNumEdges();i++) - { - btPoint3 a,b; - polyshape->getEdge(i,a,b); - btVector3 wa = worldTransform * a; - btVector3 wb = worldTransform * b; - getDebugDrawer()->drawLine(wa,wb,color); - - } - - - } - } - } - } -} - - -void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) -{ - if (m_ownsConstraintSolver) - { - delete m_constraintSolver; - } - m_ownsConstraintSolver = false; - m_constraintSolver = solver; -} - -int btDiscreteDynamicsWorld::getNumConstraints() const -{ - return int(m_constraints.size()); -} -btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) -{ - return m_constraints[index]; -} -const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const -{ - return m_constraints[index]; -} +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#include "btDiscreteDynamicsWorld.h" + +//collision detection +#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" +#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" +#include + +//rigidbody & constraints +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + +//for debug rendering +#include "BulletCollision/CollisionShapes/btBoxShape.h" +#include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletCollision/CollisionShapes/btCompoundShape.h" +#include "BulletCollision/CollisionShapes/btConeShape.h" +#include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btCylinderShape.h" +#include "BulletCollision/CollisionShapes/btMultiSphereShape.h" +#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "BulletCollision/CollisionShapes/btSphereShape.h" +#include "BulletCollision/CollisionShapes/btTriangleCallback.h" +#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" +#include "LinearMath/btIDebugDraw.h" + + + +//vehicle +#include "BulletDynamics/Vehicle/btRaycastVehicle.h" +#include "BulletDynamics/Vehicle/btVehicleRaycaster.h" +#include "BulletDynamics/Vehicle/btWheelInfo.h" +#include "LinearMath/btIDebugDraw.h" +#include "LinearMath/btQuickprof.h" +#include "LinearMath/btMotionState.h" + + + + + +btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver) +:btDynamicsWorld(dispatcher,pairCache), +m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver), +m_debugDrawer(0), +m_gravity(0,-10,0), +m_localTime(btScalar(1.)/btScalar(60.)), +m_profileTimings(0) +{ + m_islandManager = new btSimulationIslandManager(); + m_ownsIslandManager = true; + m_ownsConstraintSolver = (constraintSolver==0); +} + + +btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() +{ + //only delete it when we created it + if (m_ownsIslandManager) + delete m_islandManager; + if (m_ownsConstraintSolver) + delete m_constraintSolver; +} + +void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep) +{ + + for (int i=0;igetActivationState() != ISLAND_SLEEPING) + { + if (body->isKinematicObject()) + { + //to calculate velocities next frame + body->saveKinematicState(timeStep); + } + } + } + } +} + +void btDiscreteDynamicsWorld::synchronizeMotionStates() +{ + //debug vehicle wheels + + + { + //todo: iterate over awake simulation islands! + for ( int i=0;igetDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.)); + switch(colObj->getActivationState()) + { + case ACTIVE_TAG: + color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break; + case ISLAND_SLEEPING: + color = btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break; + case WANTS_DEACTIVATION: + color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break; + case DISABLE_DEACTIVATION: + color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break; + case DISABLE_SIMULATION: + color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break; + default: + { + color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.)); + } + }; + + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); + } + btRigidBody* body = btRigidBody::upcast(colObj); + if (body && body->getMotionState() && !body->isStaticOrKinematicObject()) + { + //we need to call the update at least once, even for sleeping objects + //otherwise the 'graphics' transform never updates properly + //so todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) + { + btTransform interpolatedTransform; + btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(), + body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform); + body->getMotionState()->setWorldTransform(interpolatedTransform); + } + } + } + } + + if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + { + for ( int i=0;im_vehicles.size();i++) + { + for (int v=0;vgetNumWheels();v++) + { + btVector3 wheelColor(0,255,255); + if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact) + { + wheelColor.setValue(0,0,255); + } else + { + 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( + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()], + m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]); + + + //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS + //debug wheels (cylinders) + m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor); + m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor); + + } + } + } + +} + + +int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep) +{ + int numSimulationSubSteps = 0; + + if (maxSubSteps) + { + //fixed timestep with interpolation + m_localTime += timeStep; + if (m_localTime >= fixedTimeStep) + { + numSimulationSubSteps = int( m_localTime / fixedTimeStep); + m_localTime -= numSimulationSubSteps * fixedTimeStep; + } + } else + { + //variable timestep + fixedTimeStep = timeStep; + m_localTime = timeStep; + if (btFuzzyZero(timeStep)) + { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } else + { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + //process some debugging flags + if (getDebugDrawer()) + { + gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0; + } + if (numSimulationSubSteps) + { + + saveKinematicState(fixedTimeStep); + + //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps; + + for (int i=0;isetGravity(gravity); + } + } +} + + +void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body) +{ + removeCollisionObject(body); +} + +void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body) +{ + if (!body->isStaticOrKinematicObject()) + { + body->setGravity(m_gravity); + } + + if (body->getCollisionShape()) + { + bool isDynamic = !(body->isStaticObject() || body->isKinematicObject()); + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + addCollisionObject(body,collisionFilterGroup,collisionFilterMask); + } +} + + +void btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep) +{ + BEGIN_PROFILE("updateVehicles"); + + for ( int i=0;iupdateVehicle( timeStep); + } + END_PROFILE("updateVehicles"); +} + +void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep) +{ + BEGIN_PROFILE("updateActivationState"); + + for ( int i=0;iupdateDeactivation(timeStep); + + if (body->wantsSleeping()) + { + if (body->isStaticOrKinematicObject()) + { + body->setActivationState(ISLAND_SLEEPING); + } else + { + if (body->getActivationState() == ACTIVE_TAG) + body->setActivationState( WANTS_DEACTIVATION ); + } + } else + { + if (body->getActivationState() != DISABLE_DEACTIVATION) + body->setActivationState( ACTIVE_TAG ); + } + } + } + END_PROFILE("updateActivationState"); +} + +void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies) +{ + m_constraints.push_back(constraint); + if (disableCollisionsBetweenLinkedBodies) + { + constraint->getRigidBodyA().addConstraintRef(constraint); + constraint->getRigidBodyB().addConstraintRef(constraint); + } +} + +void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint) +{ + m_constraints.remove(constraint); + constraint->getRigidBodyA().removeConstraintRef(constraint); + constraint->getRigidBodyB().removeConstraintRef(constraint); +} + +void btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle) +{ + m_vehicles.push_back(vehicle); +} + +void btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle) +{ + m_vehicles.remove(vehicle); +} + +inline int btGetConstraintIslandId(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + return islandId; + +} + + +class btSortConstraintOnIslandPredicate +{ + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + + + + +void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) +{ + + struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback + { + + btContactSolverInfo& m_solverInfo; + btConstraintSolver* m_solver; + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btStackAlloc* m_stackAlloc; + + + InplaceSolverIslandCallback( + btContactSolverInfo& solverInfo, + btConstraintSolver* solver, + btTypedConstraint** sortedConstraints, + int numConstraints, + btIDebugDraw* debugDrawer, + btStackAlloc* stackAlloc) + :m_solverInfo(solverInfo), + m_solver(solver), + m_sortedConstraints(sortedConstraints), + m_numConstraints(numConstraints), + m_debugDrawer(debugDrawer), + m_stackAlloc(stackAlloc) + { + + } + + InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + 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;isolveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc); + } + + }; + + //sorted version of all btTypedConstraint, based on islandId + btAlignedObjectArray sortedConstraints; + sortedConstraints.resize( m_constraints.size()); + int i; + for (i=0;ibuildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback); + + +} + + + + +void btDiscreteDynamicsWorld::calculateSimulationIslands() +{ + BEGIN_PROFILE("calculateSimulationIslands"); + + getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); + + { + int i; + int numConstraints = int(m_constraints.size()); + for (i=0;i< numConstraints ; i++ ) + { + btTypedConstraint* constraint = m_constraints[i]; + + const btRigidBody* colObj0 = &constraint->getRigidBodyA(); + const btRigidBody* colObj1 = &constraint->getRigidBodyB(); + + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + { + if (colObj0->isActive() || colObj1->isActive()) + { + + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); + } + } + } + } + + //Store the island id in each body + getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); + + END_PROFILE("calculateSimulationIslands"); + +} + + +void btDiscreteDynamicsWorld::updateAabbs() +{ + BEGIN_PROFILE("updateAabbs"); + + btVector3 colorvec(1,0,0); + btTransform predictedTrans; + for ( int i=0;iIsActive() && (!body->IsStatic())) + { + btPoint3 minAabb,maxAabb; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb); + btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache; + + //moving objects should be moderately sized, probably something wrong if not + if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12))) + { + bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb); + } else + { + //something went wrong, investigate + //this assert is unwanted in 3D modelers (danger of loosing work) + body->setActivationState(DISABLE_SIMULATION); + + static bool reportMe = true; + if (reportMe && m_debugDrawer) + { + reportMe = false; + m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation"); + m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n"); + m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n"); + m_debugDrawer->reportErrorWarning("Thanks.\n"); + } + + + } + if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) + { + m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); + } + } + } + } + + END_PROFILE("updateAabbs"); +} + +void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) +{ + BEGIN_PROFILE("integrateTransforms"); + btTransform predictedTrans; + for ( int i=0;iisActive() && (!body->isStaticOrKinematicObject())) + { + body->predictIntegratedTransform(timeStep, predictedTrans); + body->proceedToTransform( predictedTrans); + } + } + } + END_PROFILE("integrateTransforms"); +} + + + +void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) +{ + BEGIN_PROFILE("predictUnconstraintMotion"); + for ( int i=0;iisStaticOrKinematicObject()) + { + if (body->isActive()) + { + body->applyForces( timeStep); + body->integrateVelocities( 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(); + + m_profileTimings = 0; + btProfiler::destroy(); + } + } +#endif //USE_QUICKPROF +} + + + + + + +class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback +{ + btIDebugDraw* m_debugDrawer; + btVector3 m_color; + btTransform m_worldTrans; + +public: + + DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) : + m_debugDrawer(debugDrawer), + m_color(color), + m_worldTrans(worldTrans) + { + } + + virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int triangleIndex) + { + processTriangle(triangle,partId,triangleIndex); + } + + virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex) + { + (void)partId; + (void)triangleIndex; + + btVector3 wv0,wv1,wv2; + wv0 = m_worldTrans*triangle[0]; + wv1 = m_worldTrans*triangle[1]; + wv2 = m_worldTrans*triangle[2]; + m_debugDrawer->drawLine(wv0,wv1,m_color); + m_debugDrawer->drawLine(wv1,wv2,m_color); + m_debugDrawer->drawLine(wv2,wv0,m_color); + } +}; + +void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color) +{ + btVector3 start = transform.getOrigin(); + + const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0); + const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0); + const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius); + + // XY + getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color); + getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color); + getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color); + getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color); + + // XZ + getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color); + getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color); + getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color); + getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color); + + // YZ + getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color); + getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color); + getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color); + getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color); +} + +void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color) +{ + // Draw a small simplex at the center of the object + { + btVector3 start = worldTransform.getOrigin(); + getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0)); + getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0)); + getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1)); + } + + if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) + { + const btCompoundShape* compoundShape = static_cast(shape); + for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) + { + btTransform childTrans = compoundShape->getChildTransform(i); + const btCollisionShape* colShape = compoundShape->getChildShape(i); + debugDrawObject(worldTransform*childTrans,colShape,color); + } + + } else + { + switch (shape->getShapeType()) + { + + case SPHERE_SHAPE_PROXYTYPE: + { + const btSphereShape* sphereShape = static_cast(shape); + btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + + debugDrawSphere(radius, worldTransform, color); + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + const btMultiSphereShape* multiSphereShape = static_cast(shape); + + for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) + { + btTransform childTransform = worldTransform; + childTransform.getOrigin() += multiSphereShape->getSpherePosition(i); + debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color); + } + + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + const btCapsuleShape* capsuleShape = static_cast(shape); + + btScalar radius = capsuleShape->getRadius(); + btScalar halfHeight = capsuleShape->getHalfHeight(); + + // Draw the ends + { + btTransform childTransform = worldTransform; + childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0); + debugDrawSphere(radius, childTransform, color); + } + + { + btTransform childTransform = worldTransform; + childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0); + 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); + + break; + } + case CONE_SHAPE_PROXYTYPE: + { + const btConeShape* coneShape = static_cast(shape); + 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); + break; + + } + case CYLINDER_SHAPE_PROXYTYPE: + { + const btCylinderShape* cylinder = static_cast(shape); + int upAxis = cylinder->getUpAxis(); + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtents()[upAxis]; + btVector3 start = worldTransform.getOrigin(); + btVector3 offsetHeight(0,0,0); + offsetHeight[upAxis] = halfHeight; + btVector3 offsetRadius(0,0,0); + offsetRadius[(upAxis+1)%3] = radius; + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); + getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); + break; + } + default: + { + + if (shape->isConcave()) + { + btConcaveShape* concaveMesh = (btConcaveShape*) shape; + + //todo pass camera, for some culling + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); + + } + + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; + //todo: pass camera for some culling + btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + } + + + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; + + int i; + for (i=0;igetNumEdges();i++) + { + btPoint3 a,b; + polyshape->getEdge(i,a,b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa,wb,color); + + } + + + } + } + } + } +} + + +void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +{ + if (m_ownsConstraintSolver) + { + delete m_constraintSolver; + } + m_ownsConstraintSolver = false; + m_constraintSolver = solver; +} + +int btDiscreteDynamicsWorld::getNumConstraints() const +{ + return int(m_constraints.size()); +} +btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) +{ + return m_constraints[index]; +} +const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const +{ + return m_constraints[index]; +} diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index 2c05c5f86..e07894cdd 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -1,154 +1,155 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_DISCRETE_DYNAMICS_WORLD_H -#define BT_DISCRETE_DYNAMICS_WORLD_H - -#include "btDynamicsWorld.h" - -class btDispatcher; -class btOverlappingPairCache; -class btConstraintSolver; -class btSimulationIslandManager; -class btTypedConstraint; -#include "../ConstraintSolver/btContactSolverInfo.h" - -class btRaycastVehicle; -class btIDebugDraw; -#include "../../LinearMath/btAlignedObjectArray.h" - - -///btDiscreteDynamicsWorld provides discrete rigid body simulation -///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController -class btDiscreteDynamicsWorld : public btDynamicsWorld -{ -protected: - - btConstraintSolver* m_constraintSolver; - - btSimulationIslandManager* m_islandManager; - - btAlignedObjectArray m_constraints; - - btIDebugDraw* m_debugDrawer; - - btVector3 m_gravity; - - //for variable timesteps - btScalar m_localTime; - //for variable timesteps - - bool m_ownsIslandManager; - bool m_ownsConstraintSolver; - - btContactSolverInfo m_solverInfo; - - - btAlignedObjectArray m_vehicles; - - int m_profileTimings; - - void predictUnconstraintMotion(btScalar timeStep); - - void integrateTransforms(btScalar timeStep); - - void calculateSimulationIslands(); - - void solveConstraints(btContactSolverInfo& solverInfo); - - void updateActivationState(btScalar timeStep); - - void updateVehicles(btScalar timeStep); - - void startProfiling(btScalar timeStep); - - virtual void internalSingleStepSimulation( btScalar timeStep); - - void synchronizeMotionStates(); - - void saveKinematicState(btScalar timeStep); - - -public: - - - ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those - btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver); - - virtual ~btDiscreteDynamicsWorld(); - - ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's - virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); - - virtual void updateAabbs(); - - void addConstraint(btTypedConstraint* constraint); - - void removeConstraint(btTypedConstraint* constraint); - - void addVehicle(btRaycastVehicle* vehicle); - - void removeVehicle(btRaycastVehicle* vehicle); - - btSimulationIslandManager* getSimulationIslandManager() - { - return m_islandManager; - } - - const btSimulationIslandManager* getSimulationIslandManager() const - { - return m_islandManager; - } - - btCollisionWorld* getCollisionWorld() - { - return this; - } - - virtual void setDebugDrawer(btIDebugDraw* debugDrawer) - { - m_debugDrawer = debugDrawer; - } - - virtual btIDebugDraw* getDebugDrawer() - { - return m_debugDrawer; - } - - virtual void setGravity(const btVector3& gravity); - - virtual void addRigidBody(btRigidBody* body); - - virtual void removeRigidBody(btRigidBody* body); - - void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); - - virtual void setConstraintSolver(btConstraintSolver* solver); - - virtual int getNumConstraints() const; - - virtual btTypedConstraint* getConstraint(int index) ; - - virtual const btTypedConstraint* getConstraint(int index) const; - - btContactSolverInfo& getSolverInfo() - { - return m_solverInfo; - } - - -}; - -#endif //BT_DISCRETE_DYNAMICS_WORLD_H +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_DISCRETE_DYNAMICS_WORLD_H +#define BT_DISCRETE_DYNAMICS_WORLD_H + +#include "btDynamicsWorld.h" + +class btDispatcher; +class btOverlappingPairCache; +class btConstraintSolver; +class btSimulationIslandManager; +class btTypedConstraint; +#include "../ConstraintSolver/btContactSolverInfo.h" + +class btRaycastVehicle; +class btIDebugDraw; +#include "../../LinearMath/btAlignedObjectArray.h" + + +///btDiscreteDynamicsWorld provides discrete rigid body simulation +///those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController +class btDiscreteDynamicsWorld : public btDynamicsWorld +{ +protected: + + btConstraintSolver* m_constraintSolver; + + btSimulationIslandManager* m_islandManager; + + btAlignedObjectArray m_constraints; + + btIDebugDraw* m_debugDrawer; + + btVector3 m_gravity; + + //for variable timesteps + btScalar m_localTime; + //for variable timesteps + + bool m_ownsIslandManager; + bool m_ownsConstraintSolver; + + btContactSolverInfo m_solverInfo; + + + btAlignedObjectArray m_vehicles; + + int m_profileTimings; + + void predictUnconstraintMotion(btScalar timeStep); + + void integrateTransforms(btScalar timeStep); + + void calculateSimulationIslands(); + + void solveConstraints(btContactSolverInfo& solverInfo); + + void updateActivationState(btScalar timeStep); + + void updateVehicles(btScalar timeStep); + + void startProfiling(btScalar timeStep); + + virtual void internalSingleStepSimulation( btScalar timeStep); + + void synchronizeMotionStates(); + + void saveKinematicState(btScalar timeStep); + + void debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color); + +public: + + + ///this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those + btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver); + + virtual ~btDiscreteDynamicsWorld(); + + ///if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's + virtual int stepSimulation( btScalar timeStep,int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.)); + + virtual void updateAabbs(); + + void addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies=false); + + void removeConstraint(btTypedConstraint* constraint); + + void addVehicle(btRaycastVehicle* vehicle); + + void removeVehicle(btRaycastVehicle* vehicle); + + btSimulationIslandManager* getSimulationIslandManager() + { + return m_islandManager; + } + + const btSimulationIslandManager* getSimulationIslandManager() const + { + return m_islandManager; + } + + btCollisionWorld* getCollisionWorld() + { + return this; + } + + virtual void setDebugDrawer(btIDebugDraw* debugDrawer) + { + m_debugDrawer = debugDrawer; + } + + virtual btIDebugDraw* getDebugDrawer() + { + return m_debugDrawer; + } + + virtual void setGravity(const btVector3& gravity); + + virtual void addRigidBody(btRigidBody* body); + + virtual void removeRigidBody(btRigidBody* body); + + void debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color); + + virtual void setConstraintSolver(btConstraintSolver* solver); + + virtual int getNumConstraints() const; + + virtual btTypedConstraint* getConstraint(int index) ; + + virtual const btTypedConstraint* getConstraint(int index) const; + + btContactSolverInfo& getSolverInfo() + { + return m_solverInfo; + } + + +}; + +#endif //BT_DISCRETE_DYNAMICS_WORLD_H diff --git a/src/BulletDynamics/Dynamics/btRigidBody.cpp b/src/BulletDynamics/Dynamics/btRigidBody.cpp index dbf73b85b..3dae930a4 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -16,8 +16,9 @@ subject to the following restrictions: #include "btRigidBody.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "LinearMath/btMinMax.h" -#include -#include +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btMotionState.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" btScalar gLinearAirDamping = btScalar(1.); //'temporarily' global variables @@ -305,4 +306,33 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform) } +bool btRigidBody::checkCollideWithOverride(btCollisionObject* co) +{ + btRigidBody* otherRb = btRigidBody::upcast(co); + if (!otherRb) + return true; + for (int i = 0; i < m_constraintRefs.size(); ++i) + { + btTypedConstraint* c = m_constraintRefs[i]; + if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb) + return false; + } + + return true; +} + +void btRigidBody::addConstraintRef(btTypedConstraint* c) +{ + int index = m_constraintRefs.findLinearSearch(c); + if (index == m_constraintRefs.size()) + m_constraintRefs.push_back(c); + + m_checkCollideWith = true; +} + +void btRigidBody::removeConstraintRef(btTypedConstraint* c) +{ + m_constraintRefs.remove(c); + m_checkCollideWith = m_constraintRefs.size() > 0; +} \ No newline at end of file diff --git a/src/BulletDynamics/Dynamics/btRigidBody.h b/src/BulletDynamics/Dynamics/btRigidBody.h index 9da545adb..0707595d4 100644 --- a/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/src/BulletDynamics/Dynamics/btRigidBody.h @@ -16,6 +16,7 @@ subject to the following restrictions: #ifndef RIGIDBODY_H #define RIGIDBODY_H +#include "../../LinearMath/btAlignedObjectArray.h" #include "../../LinearMath/btPoint3.h" #include "../../LinearMath/btTransform.h" #include "../../BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -23,7 +24,7 @@ subject to the following restrictions: class btCollisionShape; class btMotionState; - +class btTypedConstraint; extern btScalar gLinearAirDamping; @@ -57,6 +58,9 @@ class btRigidBody : public btCollisionObject //m_optionalMotionState allows to automatic synchronize the world transform for active objects btMotionState* m_optionalMotionState; + //keep track of typed constraints referencing this rigid body + btAlignedObjectArray m_constraintRefs; + public: #ifdef OBSOLETE_MOTIONSTATE_LESS @@ -339,6 +343,11 @@ public: return (getBroadphaseProxy() != 0); } + virtual bool checkCollideWithOverride(btCollisionObject* co); + + void addConstraintRef(btTypedConstraint* c); + void removeConstraintRef(btTypedConstraint* c); + int m_debugBodyId; };