diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index 4c874bd40..03de70952 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -26,6 +26,9 @@ subject to the following restrictions: //#define USE_CUSTOM_NEAR_CALLBACK 1 //#define CENTER_OF_MASS_SHIFT 1 +//#define USE_PARALLEL_SOLVER 1 //experimental parallel solver +//#define USE_PARALLEL_DISPATCHER 1 + //following define allows to compare/replace Bullet's constraint solver with ODE quickstep //this define requires to either add the libquickstep library (win32, see msvc/8/libquickstep.vcproj) or manually add the files from Extras/quickstep //#define COMPARE_WITH_QUICKSTEP 1 @@ -38,6 +41,15 @@ subject to the following restrictions: #endif //REGISTER_BOX_BOX #include "BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h" +#ifdef USE_PARALLEL_DISPATCHER +#include "../../Extras/BulletMultiThreaded/SpuGatheringCollisionDispatcher.h" +#include "../../Extras/BulletMultiThreaded/Win32ThreadSupport.h" +#include "../../Extras/BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h" +#include "../../Extras/BulletMultiThreaded/SpuParallelSolver.h" +#include "../../Extras/BulletMultiThreaded/SpuSolverTask/SpuParallellSolverTask.h" +#endif//USE_PARALLEL_DISPATCHER + + #ifdef COMPARE_WITH_QUICKSTEP #include "../Extras/quickstep/OdeConstraintSolver.h" #endif //COMPARE_WITH_QUICKSTEP @@ -351,7 +363,33 @@ void CcdPhysicsDemo::initPhysics() m_azi = 90.f; #endif //DO_BENCHMARK_PYRAMIDS - btCollisionDispatcher* dispatcher = new btCollisionDispatcher(); + btCollisionDispatcher* dispatcher=0; + + +#ifdef USE_PARALLEL_DISPATCHER + +#ifdef USE_WIN32_THREADING + +int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks + Win32ThreadSupport* threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo( + "collision", + processCollisionTask, + createCollisionLocalStoreMemory, + maxNumOutstandingTasks)); +#else +///todo other platform threading +///Playstation 3 SPU (SPURS) version is available through PS3 Devnet +///Libspe2 SPU support will be available soon +///pthreads version +///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface +#endif + + + dispatcher = new SpuGatheringCollisionDispatcher(threadSupportCollision,maxNumOutstandingTasks); +// dispatcher = new btCollisionDispatcher(); +#else + dispatcher = new btCollisionDispatcher(); +#endif //USE_PARALLEL_DISPATCHER #ifdef USE_CUSTOM_NEAR_CALLBACK //this is optional @@ -379,11 +417,24 @@ void CcdPhysicsDemo::initPhysics() #ifdef COMPARE_WITH_QUICKSTEP btConstraintSolver* solver = new OdeConstraintSolver(); #else - //default constraint solver + + +#ifdef USE_PARALLEL_SOLVER + + Win32ThreadSupport* threadSupportSolver = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo( + "solver", + processSolverTask, + createSolverLocalStoreMemory, + maxNumOutstandingTasks)); + + btConstraintSolver* solver = new btParallelSequentialImpulseSolver(threadSupportSolver,maxNumOutstandingTasks); +#else btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver; //default solverMode is SOLVER_RANDMIZE_ORDER. Warmstarting seems not to improve convergence, see //solver->setSolverMode(btSequentialImpulseConstraintSolver::SOLVER_USE_WARMSTARTING | btSequentialImpulseConstraintSolver::SOLVER_RANDMIZE_ORDER); +#endif //USE_PARALLEL_SOLVER + #endif #ifdef USER_DEFINED_FRICTION_MODEL @@ -392,6 +443,7 @@ void CcdPhysicsDemo::initPhysics() #endif //USER_DEFINED_FRICTION_MODEL m_dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver); + m_dynamicsWorld->getDispatchInfo().m_enableSPU = true; m_dynamicsWorld->setGravity(btVector3(0,-10,0)); m_dynamicsWorld->setDebugDrawer(&debugDrawer); diff --git a/Extras/BulletMultiThreaded/SpuParallelSolver.cpp b/Extras/BulletMultiThreaded/SpuParallelSolver.cpp new file mode 100644 index 000000000..429848a49 --- /dev/null +++ b/Extras/BulletMultiThreaded/SpuParallelSolver.cpp @@ -0,0 +1,568 @@ +/* +Bullet Continuous Collision Detection and Physics Library - Parallel solver +Copyright (c) 2007 Starbreeze Studios + +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. + +Written by: Marten Svanfeldt +*/ + +#include "SpuParallelSolver.h" + +//#include "SpuFakeDma.h" +#include "SpuSync.h" + +#include "LinearMath/btVector3.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "LinearMath/btMinMax.h" +#include "BulletCollision/CollisionShapes/btCollisionShape.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" + +#include "SpuSolverTask/SpuParallellSolverTask.h" + +#include + +enum +{ + PARALLEL_SOLVER_BODIES_PER_TASK = 64, + PARALLEL_SOLVER_CELLS_PER_TASK = SPU_HASH_NUMCELLS >> 3 +}; + + +//-- Hash handling +static void recordDependency(SpuSolverHash* hash, unsigned int i, unsigned int j) +{ + hash->m_dependencyMatrix[i][j >> 5] |= (1 << (j & 31)); + hash->m_dependencyMatrix[j][i >> 5] |= (1 << (i & 31)); +} + + +// Clear the given hash +static void clearHash (SpuSolverHash* hash) +{ + size_t hashSize = sizeof(SpuSolverHash); + memset(hash, 0, hashSize); + + // Setup basic dependency + for (int i = 0; i < SPU_HASH_NUMCELLS; ++i) + { + hash->m_dependencyMatrix[i][i >> 5] |= (1 << (i & 31)); + } + + // Set some ones to "unused cells" + for (int i = SPU_HASH_WORDWIDTH-SPU_HASH_NUMUNUSEDBITS; i < SPU_HASH_WORDWIDTH; ++i) + { + hash->m_currentMask[0][SPU_HASH_NUMCELLDWORDS-1] |= (1 << i); + } +} + +static bool getDependency(SpuSolverHash* hash, unsigned int i, unsigned int j) +{ + return (hash->m_dependencyMatrix[i][j >> 5] & (1 << (j & 31))) != 0; +} + +static unsigned int getObjectIndex (btCollisionObject* object) +{ + btVector3 center = object->getWorldTransform().getOrigin(); + int cx = (int)floorf(center.x() / SPU_HASH_PHYSSIZE); + int cy = (int)floorf(center.y() / SPU_HASH_PHYSSIZE); + int cz = (int)floorf(center.z() / SPU_HASH_PHYSSIZE); + + return spuGetHashCellIndex(cx, cy, cz); +}; + + + + + +btParallelSequentialImpulseSolver::btParallelSequentialImpulseSolver (btThreadSupportInterface* threadIf, int maxOutstandingTasks) +: m_numberOfContacts(0), m_taskScheduler (threadIf, maxOutstandingTasks) +{ + m_solverHash = new SpuSolverHash; + clearHash(m_solverHash); +} + +btParallelSequentialImpulseSolver::~btParallelSequentialImpulseSolver () +{ + delete m_solverHash; +} + + +void btParallelSequentialImpulseSolver::prepareSolve(int numBodies, int numManifolds) +{ + m_sortedManifolds.reserve(numManifolds); + m_allObjects.reserve(numBodies); +} + +btScalar btParallelSequentialImpulseSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) +{ + if (!numManifolds && !numConstraints) + return 0; + + for (int i = 0; i < numManifolds; ++i) + { + btPersistentManifold* currManifold = manifold[i]; + btRigidBody* rb0 = (btRigidBody*)currManifold->getBody0(); + btRigidBody* rb1 = (btRigidBody*)currManifold->getBody1(); + + currManifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); + } + + // Record and mark the manifolds to the cells + for (int i = 0; i < numManifolds; ++i) + { + // Compute a hash cell for this manifold + btPersistentManifold* currManifold = manifold[i]; + + btCollisionObject *ownerObject, *otherObject; + + btRigidBody* rb0 = (btRigidBody*)currManifold->getBody0(); + btRigidBody* rb1 = (btRigidBody*)currManifold->getBody1(); + + if (rb0->getIslandTag() >= 0) + { + ownerObject = rb0; + otherObject = rb1; + } + else + { + ownerObject = rb1; + otherObject = rb0; + } + + // Save the cell + unsigned int ownerCellIdx = getObjectIndex(ownerObject); + ManifoldCellHolder holder = {ownerCellIdx, currManifold}; + m_sortedManifolds.push_back(holder); + m_solverHash->m_Hash[ownerCellIdx].m_numManifolds++; + + // Record dependency + if (rb0->getIslandTag() >= 0 && rb1->getIslandTag() >= 0) + { + unsigned int otherCellIdx = getObjectIndex(otherObject); + recordDependency(m_solverHash, ownerCellIdx, otherCellIdx); + } + + // Save statistics + int numContacts = currManifold->getNumContacts(); + m_solverHash->m_Hash[ownerCellIdx].m_numContacts += numContacts; + m_numberOfContacts += numContacts; + } + + // Record and mark constraints to the cells + for (int i = 0; i < numConstraints; ++i) + { + // Compute a hash cell for this manifold + btTypedConstraint* currConstraint = constraints[i]; + + if (!constraintTypeSupported(currConstraint->getConstraintType())) + continue; + + btCollisionObject *ownerObject, *otherObject; + + btRigidBody* rb0 = &currConstraint->getRigidBodyA(); + btRigidBody* rb1 = &currConstraint->getRigidBodyB(); + + if (rb0->getIslandTag() >= 0) + { + ownerObject = rb0; + otherObject = rb1; + } + else + { + ownerObject = rb1; + otherObject = rb0; + } + + // Save the cell + unsigned int ownerCellIdx = getObjectIndex(ownerObject); + ConstraintCellHolder holder = {ownerCellIdx, currConstraint->getConstraintType(), currConstraint}; + m_sortedConstraints.push_back(holder); + m_solverHash->m_Hash[ownerCellIdx].m_numConstraints++; + + // Record dependency + if (rb0 && rb1 && rb0->getIslandTag() >= 0 && rb1->getIslandTag() >= 0) + { + unsigned int otherCellIdx = getObjectIndex(otherObject); + recordDependency(m_solverHash, ownerCellIdx, otherCellIdx); + } + } + + // Save all RBs + for (int i = 0; i < numBodies; ++i) + { + btCollisionObject* obj = bodies[i]; + //unsigned int cellIdx = getObjectIndex(obj); + + btRigidBody* rb = btRigidBody::upcast(obj); + m_allObjects.push_back(rb); + } + + return 0; +} + +template +class CellHolderPredicate +{ +public: + SIMD_FORCE_INLINE bool operator() ( const T& lhs, const T& rhs ) + { + return lhs.m_hashCellIndex < rhs.m_hashCellIndex; + } +}; + + +static void printDependencyMatrix(SpuSolverHash* hash) +{ + for (int r = 0; r < SPU_HASH_NUMCELLS; ++r) + { + for (int c = 0; c < SPU_HASH_NUMCELLS; ++c) + { + if (getDependency(hash, r, c)) + { + printf("1"); + } + else + { + printf("0"); + } + } + + printf("\n"); + } + printf("\n"); + fflush(stdout); +} + +// Solver caches +btAlignedObjectArray solverBodyPool_persist; +btAlignedObjectArray solverBodyOffsetList_persist; +btAlignedObjectArray solverInternalConstraintPool_persist; +btAlignedObjectArray solverConstraintPool_persist; + + +void btParallelSequentialImpulseSolver::allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) +{ + if (!m_numberOfContacts && !m_sortedConstraints.size()) + { + m_sortedManifolds.clear(); + m_sortedConstraints.clear(); + m_allObjects.clear(); + clearHash(m_solverHash); + return; + } + + + //printDependencyMatrix(m_solverHash); + + // Sort the manifolds list + int numManifolds = m_sortedManifolds.size(); + m_sortedManifolds.heapSort(CellHolderPredicate()); + + // Sort the constraint list + int numConstraints = m_sortedConstraints.size(); + m_sortedConstraints.heapSort(CellHolderPredicate()); + + + // Sort the body list + int numBodies = m_allObjects.size(); + + // Reassign the hash offset + uint32_t emptyCellMask[SPU_HASH_NUMCELLDWORDS] = {0}; + int numBodyOffsets = 0; + { + int manifoldRunner = 0; + int bodyOffsetRunner = 0; + int internalConstraintRunner = 0; + int constraintRunner = 0; + + for (int i = 0; i < SPU_HASH_NUMCELLS; ++i) + { + bool empty = true; + + SpuSolverHashCell& hashCell = m_solverHash->m_Hash[i]; + hashCell.m_solverBodyOffsetListOffset = bodyOffsetRunner; + + if (hashCell.m_numManifolds) + { + hashCell.m_manifoldListOffset = manifoldRunner; + manifoldRunner += hashCell.m_numManifolds; + + bodyOffsetRunner += hashCell.m_numManifolds*2; + } + if (hashCell.m_numContacts) + { + hashCell.m_internalConstraintListOffset = internalConstraintRunner*3; + internalConstraintRunner += hashCell.m_numContacts; + empty = false; + } + + if (hashCell.m_numConstraints) + { + hashCell.m_constraintListOffset = constraintRunner; + constraintRunner += hashCell.m_numConstraints; + + bodyOffsetRunner += hashCell.m_numConstraints*2; + + empty = false; + } + + + emptyCellMask[i >> 5] |= (empty ? (1 << (i&31)) : 0); + // Align the bodyOffsetRunner to a whole number of 4 for right alignment in the list + bodyOffsetRunner = (bodyOffsetRunner+3)&~0x3; + } + + numBodyOffsets = bodyOffsetRunner; + } + + // Setup rigid bodies + // Allocate temporary data + solverBodyPool_persist.resize(numBodies + numManifolds + numConstraints); + SpuSolverBody* solverBodyPool = &solverBodyPool_persist[0]; + + solverBodyOffsetList_persist.resize(numBodyOffsets); + uint32_t* solverBodyOffsetList = &solverBodyOffsetList_persist[0]; + + solverInternalConstraintPool_persist.resize(m_numberOfContacts*3); + SpuSolverInternalConstraint* solverInternalConstraintPool = &solverInternalConstraintPool_persist[0]; + + solverConstraintPool_persist.resize(numConstraints); + SpuSolverConstraint* solverConstraintPool = &solverConstraintPool_persist[0]; + + // Setup all the moving rigid bodies + { + int bodiesPerTask = PARALLEL_SOLVER_BODIES_PER_TASK; + int bodiesToSchedule = numBodies; + int startBody = 0; + + while (bodiesToSchedule > 0) + { + // Schedule a bunch of hash cells + int numBodiesInTask = bodiesToSchedule > bodiesPerTask ? bodiesPerTask : bodiesToSchedule; + + SpuSolverTaskDesc* desc = m_taskScheduler.getTask(); + + desc->m_solverCommand = CMD_SOLVER_SETUP_BODIES; + desc->m_solverData.m_solverHash = m_solverHash; + desc->m_solverData.m_solverBodyList = solverBodyPool; + + desc->m_commandData.m_bodySetup.m_startBody = startBody; + desc->m_commandData.m_bodySetup.m_numBodies = numBodiesInTask; + desc->m_commandData.m_bodySetup.m_rbList = &m_allObjects[0]; + + m_taskScheduler.issueTask(); + bodiesToSchedule -= numBodiesInTask; + startBody += numBodiesInTask; + } + + m_taskScheduler.flushTasks(); + } + + // Manifold setup + { + int cellsPerTask = PARALLEL_SOLVER_CELLS_PER_TASK; + int cellsToSchedule = SPU_HASH_NUMCELLS; + int startCell = 0; + + while (cellsToSchedule > 0) + { + int numCellsInTask = cellsToSchedule > cellsPerTask ? cellsPerTask : cellsToSchedule; + + SpuSolverTaskDesc* desc = m_taskScheduler.getTask(); + + desc->m_solverCommand = CMD_SOLVER_MANIFOLD_SETUP; + desc->m_solverData.m_solverHash = m_solverHash; + desc->m_solverData.m_solverBodyList = solverBodyPool; + desc->m_solverData.m_solverBodyOffsetList = solverBodyOffsetList; + desc->m_solverData.m_solverInternalConstraintList = solverInternalConstraintPool; + desc->m_solverData.m_solverConstraintList = solverConstraintPool; + + desc->m_commandData.m_manifoldSetup.m_startCell = startCell; + desc->m_commandData.m_manifoldSetup.m_numCells = numCellsInTask; + desc->m_commandData.m_manifoldSetup.m_numBodies = numBodies; + desc->m_commandData.m_manifoldSetup.m_numManifolds = numManifolds; + desc->m_commandData.m_manifoldSetup.m_manifoldHolders = &m_sortedManifolds[0]; + desc->m_commandData.m_manifoldSetup.m_constraintHolders = &m_sortedConstraints[0]; + desc->m_commandData.m_manifoldSetup.m_solverInfo = info; + + m_taskScheduler.issueTask(); + cellsToSchedule -= numCellsInTask; + startCell += numCellsInTask; + } + m_taskScheduler.flushTasks(); + } + + btSpinlock::SpinVariable* spinVar = (btSpinlock::SpinVariable*)btAlignedAlloc(sizeof(btSpinlock::SpinVariable), 128); + for (int iter = 0; iter < info.m_numIterations; ++iter) + { + btSpinlock lock (spinVar); + lock.Init(); + + // Clear the "processed cells" part of the hash + memcpy(m_solverHash->m_currentMask[0], emptyCellMask, sizeof(uint32_t)*SPU_HASH_NUMCELLDWORDS); + + for (int task = 0; task < m_taskScheduler.getMaxOutstandingTasks(); ++task) + { + SpuSolverTaskDesc* desc = m_taskScheduler.getTask(); + desc->m_solverCommand = CMD_SOLVER_SOLVE_ITERATE; + + desc->m_solverData.m_solverHash = m_solverHash; + desc->m_solverData.m_solverBodyList = solverBodyPool; + desc->m_solverData.m_solverBodyOffsetList = solverBodyOffsetList; + desc->m_solverData.m_solverInternalConstraintList = solverInternalConstraintPool; + desc->m_solverData.m_solverConstraintList = solverConstraintPool; + + desc->m_commandData.m_iterate.m_spinLockVar = spinVar; + + m_taskScheduler.issueTask(); + } + m_taskScheduler.flushTasks(); + } + btAlignedFree((void*)spinVar); + + // Write back velocity + { + int bodiesPerTask = PARALLEL_SOLVER_BODIES_PER_TASK; + int bodiesToSchedule = numBodies; + int startBody = 0; + + while (bodiesToSchedule > 0) + { + // Schedule a bunch of hash cells + int numBodiesInTask = bodiesToSchedule > bodiesPerTask ? bodiesPerTask : bodiesToSchedule; + + SpuSolverTaskDesc* desc = m_taskScheduler.getTask(); + + desc->m_solverCommand = CMD_SOLVER_COPYBACK_BODIES; + desc->m_solverData.m_solverHash = m_solverHash; + desc->m_solverData.m_solverBodyList = solverBodyPool; + + desc->m_commandData.m_bodyCopyback.m_startBody = startBody; + desc->m_commandData.m_bodyCopyback.m_numBodies = numBodiesInTask; + desc->m_commandData.m_bodyCopyback.m_rbList = &m_allObjects[0]; + + m_taskScheduler.issueTask(); + bodiesToSchedule -= numBodiesInTask; + startBody += numBodiesInTask; + } + + m_taskScheduler.flushTasks(); + } + + + // Clean up + m_sortedManifolds.resize(0); + m_sortedConstraints.resize(0); + m_allObjects.resize(0); + clearHash(m_solverHash); + + + m_numberOfContacts = 0; +} + +void btParallelSequentialImpulseSolver::reset() +{ + m_sortedManifolds.clear(); + m_allObjects.clear(); + m_numberOfContacts = 0; + clearHash(m_solverHash); + + solverBodyPool_persist.clear(); + solverBodyOffsetList_persist.clear(); + solverConstraintPool_persist.clear(); + solverInternalConstraintPool_persist.clear(); +} + + +SolverTaskScheduler::SolverTaskScheduler(btThreadSupportInterface* threadIf, int maxOutstandingTasks) +: m_threadInterface (threadIf), m_maxNumOutstandingTasks (maxOutstandingTasks > SPU_MAX_SPUS ? SPU_MAX_SPUS : maxOutstandingTasks), +m_currentTask (0), m_numBusyTasks (0) +{ + m_taskDescriptors.resize(m_maxNumOutstandingTasks); + m_taskBusy.resize(m_maxNumOutstandingTasks); + + m_threadInterface->startSPU(); +} + + +SolverTaskScheduler::~SolverTaskScheduler() +{ + m_threadInterface->stopSPU(); +} + +SpuSolverTaskDesc* SolverTaskScheduler::getTask() +{ + int taskIdx = -1; + + if (m_taskBusy[m_currentTask]) + { + //try to find a new one + for (unsigned int i = 0; i < m_maxNumOutstandingTasks; ++i) + { + if (!m_taskBusy[i]) + { + taskIdx = i; + break; + } + } + + if (taskIdx < 0) + { + // Have to wait + unsigned int taskId; + unsigned int outputSize; + + m_threadInterface->waitForResponse(&taskId, &outputSize); + + m_taskBusy[taskId] = false; + m_numBusyTasks--; + + taskIdx = taskId; + } + + m_currentTask = taskIdx; + } + + + SpuSolverTaskDesc* result = &m_taskDescriptors[m_currentTask]; + memset(result, 0, sizeof(SpuSolverTaskDesc)); + result->m_taskId = m_currentTask; + + return result; +} + +void SolverTaskScheduler::issueTask() +{ + m_taskBusy[m_currentTask] = true; + m_numBusyTasks++; + + SpuSolverTaskDesc& desc = m_taskDescriptors[m_currentTask]; + + m_threadInterface->sendRequest(1, (uint32_t)&desc, m_currentTask); +} + +void SolverTaskScheduler::flushTasks() +{ + while (m_numBusyTasks > 0) + { + unsigned int taskId; + unsigned int outputSize; + + m_threadInterface->waitForResponse(&taskId, &outputSize); + + m_taskBusy[taskId] = false; + m_numBusyTasks--; + } +} diff --git a/Extras/BulletMultiThreaded/SpuParallelSolver.h b/Extras/BulletMultiThreaded/SpuParallelSolver.h new file mode 100644 index 000000000..f71b530ce --- /dev/null +++ b/Extras/BulletMultiThreaded/SpuParallelSolver.h @@ -0,0 +1,75 @@ +/* +Bullet Continuous Collision Detection and Physics Library - Parallel solver +Copyright (c) 2007 Starbreeze Studios + +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. + +Written by: Marten Svanfeldt +*/ + +#ifndef SPU_PARALLELSOLVER_H +#define SPU_PARALLELSOLVER_H + +#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" +#include "btThreadSupportInterface.h" +#include "LinearMath/btAlignedObjectArray.h" + +class SolverTaskScheduler +{ +protected: + class btThreadSupportInterface* m_threadInterface; + unsigned int m_maxNumOutstandingTasks; + + unsigned int m_currentTask; + unsigned int m_numBusyTasks; + + btAlignedObjectArray m_taskDescriptors; + btAlignedObjectArray m_taskBusy; + +public: + SolverTaskScheduler (btThreadSupportInterface* threadIf, int maxOutstandingTasks); + ~SolverTaskScheduler (); + + struct SpuSolverTaskDesc* getTask (); + + void issueTask(); + void flushTasks(); + + unsigned int getMaxOutstandingTasks() + { + return m_maxNumOutstandingTasks; + } +}; + +class btParallelSequentialImpulseSolver : public btConstraintSolver +{ +protected: + + struct SpuSolverHash* m_solverHash; + btAlignedObjectArray m_sortedManifolds; + btAlignedObjectArray m_sortedConstraints; + btAlignedObjectArray m_allObjects; + + int m_numberOfContacts; + + SolverTaskScheduler m_taskScheduler; + +public: + btParallelSequentialImpulseSolver (btThreadSupportInterface* threadIf, int maxOutstandingTasks); + virtual ~btParallelSequentialImpulseSolver(); + + virtual void prepareSolve (int numBodies, int numManifolds); + virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); + virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc); + virtual void reset (); +}; + +#endif diff --git a/Extras/BulletMultiThreaded/SpuSync.h b/Extras/BulletMultiThreaded/SpuSync.h new file mode 100644 index 000000000..74dc9c42a --- /dev/null +++ b/Extras/BulletMultiThreaded/SpuSync.h @@ -0,0 +1,112 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2007 Starbreeze Studios + +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. + +Written by: Marten Svanfeldt +*/ + +#ifndef SPU_SYNC_H +#define SPU_SYNC_H + + +#include "PlatformDefinitions.h" + + +#if defined(WIN32) + +#define WIN32_LEAN_AND_MEAN +#ifdef _XBOX +#include +#else +#include +#endif + +class btSpinlock +{ +public: + //typedef volatile LONG SpinVariable; + typedef CRITICAL_SECTION SpinVariable; + + btSpinlock (SpinVariable* var) + : spinVariable (var) + {} + + void Init () + { + //*spinVariable = 0; + InitializeCriticalSection(spinVariable); + } + + void Lock () + { + EnterCriticalSection(spinVariable); + } + + void Unlock () + { + LeaveCriticalSection(spinVariable); + } + +private: + SpinVariable* spinVariable; +}; + + +#elif defined (__CELLOS_LV2__) + +//#include +#include + +class btSpinlock +{ +public: + typedef CellSyncMutex SpinVariable; + + btSpinlock (SpinVariable* var) + : spinVariable (var) + {} +#ifndef SPU + void Init () + { + //*spinVariable = 1; + cellSyncMutexInitialize(spinVariable); + } +#endif + +#ifdef SPU + void Lock () + { + // lock semaphore + /*while (cellAtomicTestAndDecr32(atomic_buf, (uint64_t)spinVariable) == 0) + { + + };*/ + cellSyncMutexLock((uint64_t)spinVariable); + } + + void Unlock () + { + //cellAtomicIncr32(atomic_buf, (uint64_t)spinVariable); + cellSyncMutexUnlock((uint64_t)spinVariable); + } +#endif + +private: + SpinVariable* spinVariable; + uint32_t atomic_buf[32] __attribute__((aligned(128))); +}; + +#endif + + +#endif diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 12a33d785..a841fef73 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -23,13 +23,14 @@ Written by: Marcus Hennix #include btConeTwistConstraint::btConeTwistConstraint() +:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE) { } btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame,const btTransform& rbBFrame) - :btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), + :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false) { // flip axis for correct angles @@ -49,7 +50,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, } btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) - :btTypedConstraint(rbA),m_rbAFrame(rbAFrame), + :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), m_angularOnly(false) { m_rbBFrame = m_rbAFrame; diff --git a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 874669c80..f121919c8 100644 --- a/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -30,6 +30,9 @@ class btRigidBody; ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc) class btConeTwistConstraint : public btTypedConstraint { +#ifdef IN_PARALLELL_SOLVER +public: +#endif btJacobianEntry m_jac[3]; //3 orthogonal linear constraints btTransform m_rbAFrame; diff --git a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 3a8e3c729..6ad58073d 100644 --- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -35,12 +35,15 @@ public: virtual ~btConstraintSolver() {} + virtual void prepareSolve (int numBodies, int numManifolds) {;} + ///solve a group of constraints virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) = 0; + virtual void allSolved (const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc) {;} + ///clear internal cached data and reset random seed virtual void reset() = 0; - }; diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index c3c73e300..ad2c40e21 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -16,8 +16,21 @@ subject to the following restrictions: #ifndef CONTACT_SOLVER_INFO #define CONTACT_SOLVER_INFO +struct btContactSolverInfoData +{ + btScalar m_tau; + btScalar m_damping; + btScalar m_friction; + btScalar m_timeStep; + btScalar m_restitution; + int m_numIterations; + btScalar m_maxErrorReduction; + btScalar m_sor; + btScalar m_erp; -struct btContactSolverInfo +}; + +struct btContactSolverInfo : public btContactSolverInfoData { inline btContactSolverInfo() @@ -32,16 +45,7 @@ struct btContactSolverInfo m_sor = btScalar(1.3); } - btScalar m_tau; - btScalar m_damping; - btScalar m_friction; - btScalar m_timeStep; - btScalar m_restitution; - int m_numIterations; - btScalar m_maxErrorReduction; - btScalar m_sor; - btScalar m_erp; - + }; #endif //CONTACT_SOLVER_INFO diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 747d10d1f..6aa6d70ff 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -25,11 +25,12 @@ static const int kAxisB[] = { 2, 2, 1 }; #define GENERIC_D6_DISABLE_WARMSTARTING 1 btGeneric6DofConstraint::btGeneric6DofConstraint() +:btTypedConstraint(D6_CONSTRAINT_TYPE) { } btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB) -: btTypedConstraint(rbA, rbB) +: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB) , m_frameInA(frameInA) , m_frameInB(frameInB) { diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index acd3b9513..0120f79ab 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -21,14 +21,15 @@ subject to the following restrictions: #include -btHingeConstraint::btHingeConstraint(): +btHingeConstraint::btHingeConstraint() +: btTypedConstraint (HINGE_CONSTRAINT_TYPE), m_enableAngularMotor(false) { } btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB) - :btTypedConstraint(rbA,rbB), + :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), m_angularOnly(false), m_enableAngularMotor(false) { @@ -70,7 +71,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA) -:btTypedConstraint(rbA), m_angularOnly(false), m_enableAngularMotor(false) +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false) { // since no frame is given, assume this to be zero angle and just pick rb transform axis @@ -113,7 +114,7 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA, btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame) -:btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), m_angularOnly(false), m_enableAngularMotor(false) { @@ -134,7 +135,7 @@ m_enableAngularMotor(false) btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame) -:btTypedConstraint(rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), m_angularOnly(false), m_enableAngularMotor(false) { diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 4b3976842..8d8adfe92 100644 --- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -28,6 +28,9 @@ class btRigidBody; /// axis defines the orientation of the hinge axis class btHingeConstraint : public btTypedConstraint { +#ifdef IN_PARALLELL_SOLVER +public: +#endif btJacobianEntry m_jac[3]; //3 orthogonal linear constraints btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index aacb0a3ea..ff918ea56 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -21,18 +21,19 @@ subject to the following restrictions: btPoint2PointConstraint::btPoint2PointConstraint() +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB) -:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB) { } btPoint2PointConstraint::btPoint2PointConstraint(btRigidBody& rbA,const btVector3& pivotInA) -:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) +:btTypedConstraint(POINT2POINT_CONSTRAINT_TYPE,rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)) { } diff --git a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index 538fab878..0fcec43c6 100644 --- a/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -36,6 +36,9 @@ struct btConstraintSetting /// point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space class btPoint2PointConstraint : public btTypedConstraint { +#ifdef IN_PARALLELL_SOLVER +public: +#endif btJacobianEntry m_jac[3]; //3 orthogonal linear constraints btVector3 m_pivotInA; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 5ee92e698..1d7221640 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -105,7 +105,9 @@ public: }; - +#ifndef BT_PREFER_SIMD +typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered; +#endif #endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index a15b3e026..15a18851d 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -19,8 +19,9 @@ subject to the following restrictions: static btRigidBody s_fixed(0, 0,0); -btTypedConstraint::btTypedConstraint() -: m_userConstraintType(-1), +btTypedConstraint::btTypedConstraint(btTypedConstraintType type) +: m_constraintType (type), +m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(s_fixed), m_rbB(s_fixed), @@ -28,8 +29,9 @@ m_appliedImpulse(btScalar(0.)) { s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); } -btTypedConstraint::btTypedConstraint(btRigidBody& rbA) -: m_userConstraintType(-1), +btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) +: m_constraintType (type), +m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(rbA), m_rbB(s_fixed), @@ -40,8 +42,9 @@ m_appliedImpulse(btScalar(0.)) } -btTypedConstraint::btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB) -: m_userConstraintType(-1), +btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB) +: m_constraintType (type), +m_userConstraintType(-1), m_userConstraintId(-1), m_rbA(rbA), m_rbB(rbB), diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 10a475f39..745d0afde 100644 --- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -19,12 +19,23 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btScalar.h" +enum btTypedConstraintType +{ + POINT2POINT_CONSTRAINT_TYPE, + HINGE_CONSTRAINT_TYPE, + CONETWIST_CONSTRAINT_TYPE, + D6_CONSTRAINT_TYPE, + VEHICLE_CONSTRAINT_TYPE +}; + ///TypedConstraint is the baseclass for Bullet constraints and vehicles class btTypedConstraint { int m_userConstraintType; int m_userConstraintId; + btTypedConstraintType m_constraintType; + btTypedConstraint& operator=(btTypedConstraint& other) { btAssert(0); @@ -40,11 +51,11 @@ protected: public: - btTypedConstraint(); + btTypedConstraint(btTypedConstraintType type); virtual ~btTypedConstraint() {}; - btTypedConstraint(btRigidBody& rbA); + btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); - btTypedConstraint(btRigidBody& rbA,btRigidBody& rbB); + btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); virtual void buildJacobian() = 0; @@ -59,7 +70,7 @@ public: return m_rbB; } - btRigidBody& getRigidBodyA() + btRigidBody& getRigidBodyA() { return m_rbA; } @@ -83,14 +94,19 @@ public: m_userConstraintId = uid; } - int getUserConstraintId() + int getUserConstraintId() const { return m_userConstraintId; } - btScalar getAppliedImpulse() + btScalar getAppliedImpulse() const { return m_appliedImpulse; } + + btTypedConstraintType getConstraintType () const + { + return m_constraintType; + } }; #endif //TYPED_CONSTRAINT_H diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index e4cd20764..c3ec67120 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -1,960 +1,960 @@ -/* -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,btBroadphaseInterface* 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::addRigidBody(btRigidBody* body, short group, short mask) -{ - if (!body->isStaticOrKinematicObject()) - { - body->setGravity(m_gravity); - } - - if (body->getCollisionShape()) - { - addCollisionObject(body,group,mask); - } -} - - -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; -} - -btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} - - -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,btBroadphaseInterface* 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::addRigidBody(btRigidBody* body, short group, short mask) +{ + if (!body->isStaticOrKinematicObject()) + { + body->setGravity(m_gravity); + } + + if (body->getCollisionShape()) + { + addCollisionObject(body,group,mask); + } +} + + +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;iprepareSolve(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); +} + + + + +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; +} + +btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver() +{ + return m_constraintSolver; +} + + +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/btSimpleDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp index dc970bba4..29d0b58c3 100644 --- a/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp @@ -74,8 +74,9 @@ int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, b btContactSolverInfo infoGlobal; infoGlobal.m_timeStep = timeStep; - + m_constraintSolver->prepareSolve(0,numManifolds); m_constraintSolver->solveGroup(0,0,manifoldPtr, numManifolds,0,0,infoGlobal,m_debugDrawer, m_stackAlloc); + m_constraintSolver->allSolved(infoGlobal,m_debugDrawer, m_stackAlloc); } ///integrate transforms @@ -210,7 +211,7 @@ void btSimpleDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) m_constraintSolver = solver; } -btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver() -{ - return m_constraintSolver; -} +btConstraintSolver* btSimpleDynamicsWorld::getConstraintSolver() +{ + return m_constraintSolver; +} diff --git a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp index 347305335..ec008ab0a 100644 --- a/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp +++ b/src/BulletDynamics/Vehicle/btRaycastVehicle.cpp @@ -28,7 +28,8 @@ static btRigidBody s_fixedObject( 0,0,0); btRaycastVehicle::btRaycastVehicle(const btVehicleTuning& tuning,btRigidBody* chassis, btVehicleRaycaster* raycaster ) -:m_vehicleRaycaster(raycaster), +: btTypedConstraint(VEHICLE_CONSTRAINT_TYPE), +m_vehicleRaycaster(raycaster), m_pitchControl(btScalar(0.)) { m_chassisBody = chassis; diff --git a/src/LinearMath/btQuadWord.h b/src/LinearMath/btQuadWord.h index 30edb9bd1..24751b1be 100644 --- a/src/LinearMath/btQuadWord.h +++ b/src/LinearMath/btQuadWord.h @@ -19,16 +19,19 @@ subject to the following restrictions: #include "btScalar.h" #include "btSimdMinMax.h" +class btQuadWordStorage +{ +protected: + btScalar m_x; + btScalar m_y; + btScalar m_z; + btScalar m_unusedW; +}; + ///btQuadWord is base-class for vectors, points -class btQuadWord +class btQuadWord : public btQuadWordStorage { - protected: - btScalar m_x; - btScalar m_y; - btScalar m_z; - btScalar m_unusedW; - public: // SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_x)[i]; } @@ -88,16 +91,19 @@ class btQuadWord { } - SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) - :m_x(x),m_y(y),m_z(z) - //todo, remove this in release/simd ? - ,m_unusedW(btScalar(0.)) + SIMD_FORCE_INLINE btQuadWord(const btQuadWordStorage& q) { + *((btQuadWordStorage*)this) = q; + } + + SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z) + { + m_x = x, m_y = y, m_z = z, m_unusedW = 0.0f; } SIMD_FORCE_INLINE btQuadWord(const btScalar& x, const btScalar& y, const btScalar& z,const btScalar& w) - :m_x(x),m_y(y),m_z(z),m_unusedW(w) { + m_x = x, m_y = y, m_z = z, m_unusedW = w; } diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 50334970b..785adb43f 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -285,7 +285,7 @@ slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) } SIMD_FORCE_INLINE btVector3 -quatRotate(btQuaternion& rotation, btVector3& v) +quatRotate(const btQuaternion& rotation, const btVector3& v) { btQuaternion q = rotation * v; q *= rotation.inverse(); @@ -293,7 +293,7 @@ quatRotate(btQuaternion& rotation, btVector3& v) } SIMD_FORCE_INLINE btQuaternion -shortestArcQuat(btVector3& v0,btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized +shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized { btVector3 c = v0.cross(v1); btScalar d = v0.dot(v1); @@ -308,7 +308,7 @@ shortestArcQuat(btVector3& v0,btVector3& v1) // Game Programming Gems 2.10. make } SIMD_FORCE_INLINE btQuaternion -shortestArcQuatNormalize(btVector3& v0,btVector3& v1) +shortestArcQuatNormalize(btVector3 v0,btVector3 v1) { v0.normalize(); v1.normalize(); diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index 74d41ad2a..2153dcea5 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -27,6 +27,10 @@ class btVector3 : public btQuadWord { public: SIMD_FORCE_INLINE btVector3() {} + SIMD_FORCE_INLINE btVector3(const btQuadWordStorage& q) + : btQuadWord(q) + { + } SIMD_FORCE_INLINE btVector3(const btScalar& x, const btScalar& y, const btScalar& z)