diff --git a/Bullet/BroadphaseCollision/Dispatcher.h b/Bullet/BroadphaseCollision/Dispatcher.h index 47a006f69..06af72e30 100644 --- a/Bullet/BroadphaseCollision/Dispatcher.h +++ b/Bullet/BroadphaseCollision/Dispatcher.h @@ -87,7 +87,7 @@ public: virtual void ReleaseManifoldResult(ManifoldResult*)=0; - virtual void DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo)=0; + virtual void DispatchAllCollisionPairs(class BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo)=0; virtual int GetNumManifolds() const = 0; diff --git a/Bullet/CollisionDispatch/CollisionDispatcher.cpp b/Bullet/CollisionDispatch/CollisionDispatcher.cpp index 3fe8f4760..59a01bf5c 100644 --- a/Bullet/CollisionDispatch/CollisionDispatcher.cpp +++ b/Bullet/CollisionDispatch/CollisionDispatcher.cpp @@ -179,7 +179,7 @@ void CollisionDispatcher::ReleaseManifoldResult(ManifoldResult*) } -void CollisionDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo) +void CollisionDispatcher::DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo) { //m_blockedForChanges = true; @@ -187,12 +187,12 @@ void CollisionDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* pairCa int dispatcherId = GetUniqueId(); - pairCache->RefreshOverlappingPairs(); + - for (i=0;iGetNumOverlappingPairs();i++) + for (i=0;iGetOverlappingPair(i); + BroadphasePair& pair = pairs[i]; if (dispatcherId>= 0) { diff --git a/Bullet/CollisionDispatch/CollisionDispatcher.h b/Bullet/CollisionDispatch/CollisionDispatcher.h index b99130297..773978b92 100644 --- a/Bullet/CollisionDispatch/CollisionDispatcher.h +++ b/Bullet/CollisionDispatch/CollisionDispatcher.h @@ -103,7 +103,7 @@ public: virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;} - virtual void DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo); + virtual void DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo); diff --git a/Bullet/CollisionDispatch/CollisionWorld.cpp b/Bullet/CollisionDispatch/CollisionWorld.cpp index d1eb4122e..011f05066 100644 --- a/Bullet/CollisionDispatch/CollisionWorld.cpp +++ b/Bullet/CollisionDispatch/CollisionWorld.cpp @@ -104,7 +104,7 @@ void CollisionWorld::PerformDiscreteCollisionDetection() Dispatcher* dispatcher = GetDispatcher(); if (dispatcher) - dispatcher->DispatchAllCollisionPairs(m_pairCache,dispatchInfo); + dispatcher->DispatchAllCollisionPairs(&m_pairCache->GetOverlappingPair(0),m_pairCache->GetNumOverlappingPairs(),dispatchInfo); } diff --git a/Bullet/CollisionDispatch/CollisionWorld.h b/Bullet/CollisionDispatch/CollisionWorld.h index 29d641602..8414ba4e4 100644 --- a/Bullet/CollisionDispatch/CollisionWorld.h +++ b/Bullet/CollisionDispatch/CollisionWorld.h @@ -84,14 +84,14 @@ class CollisionWorld std::vector m_collisionObjects; - CollisionDispatcher* m_dispatcher1; + Dispatcher* m_dispatcher1; OverlappingPairCache* m_pairCache; public: - CollisionWorld(CollisionDispatcher* dispatcher,OverlappingPairCache* pairCache) + CollisionWorld(Dispatcher* dispatcher,OverlappingPairCache* pairCache) :m_dispatcher1(dispatcher), m_pairCache(pairCache) { diff --git a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp index 9312445f8..0392960c8 100644 --- a/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp +++ b/Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp @@ -145,8 +145,8 @@ int main(int argc,char** argv) SimdVector3 worldAabbMin(-30000,-30000,-30000); SimdVector3 worldAabbMax(30000,30000,30000); - //BroadphaseInterface* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies,maxOverlap); - OverlappingPairCache* broadphase = new SimpleBroadphase(maxProxies,maxOverlap); + OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax,maxProxies,maxOverlap); + //OverlappingPairCache* broadphase = new SimpleBroadphase(maxProxies,maxOverlap); #ifdef USE_PARALLEL_DISPATCHER physicsEnvironmentPtr = new ParallelPhysicsEnvironment(dispatcher2,broadphase); diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp index 750385828..3170d34af 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.cpp @@ -321,7 +321,7 @@ static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdV -CcdPhysicsEnvironment::CcdPhysicsEnvironment(CollisionDispatcher* dispatcher,OverlappingPairCache* pairCache) +CcdPhysicsEnvironment::CcdPhysicsEnvironment(Dispatcher* dispatcher,OverlappingPairCache* pairCache) :m_scalingPropagated(false), m_numIterations(4), m_numTimeSubSteps(1), @@ -676,7 +676,8 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) dispatchInfo.m_stepCount = 0; dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection; - GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo); + scene->RefreshOverlappingPairs(); + GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo); #ifdef USE_QUICKPROF @@ -844,7 +845,8 @@ bool CcdPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) dispatchInfo.m_stepCount = 0; dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS; - GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo); + //pairCache->RefreshOverlappingPairs();//?? + GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(&scene->GetOverlappingPair(0),scene->GetNumOverlappingPairs(),dispatchInfo); toi = dispatchInfo.m_timeOfImpact; diff --git a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h index f546248ce..ccb4da1f9 100644 --- a/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h +++ b/Extras/PhysicsInterface/CcdPhysics/CcdPhysicsEnvironment.h @@ -54,6 +54,8 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment //timestep subdivisions int m_numTimeSubSteps; +protected: + int m_ccdMode; int m_solverType; int m_profileTimings; @@ -64,7 +66,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment SimulationIslandManager* m_islandManager; public: - CcdPhysicsEnvironment(CollisionDispatcher* dispatcher=0, OverlappingPairCache* pairCache=0); + CcdPhysicsEnvironment(Dispatcher* dispatcher=0, OverlappingPairCache* pairCache=0); virtual ~CcdPhysicsEnvironment(); @@ -200,7 +202,17 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment return m_collisionWorld; } - private: + SimulationIslandManager* GetSimulationIslandManager() + { + return m_islandManager; + } + + const SimulationIslandManager* GetSimulationIslandManager() const + { + return m_islandManager; + } + + protected: @@ -220,6 +232,7 @@ class CcdPhysicsEnvironment : public PHY_IPhysicsEnvironment bool m_scalingPropagated; + }; diff --git a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp b/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp index 7d9d93e2d..b53ee273c 100644 --- a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.cpp @@ -267,8 +267,69 @@ void ParallelIslandDispatcher::ReleaseManifoldResult(ManifoldResult*) } -void ParallelIslandDispatcher::DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo) + +void ParallelIslandDispatcher::DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo) { + //m_blockedForChanges = true; + + int i; + + int dispatcherId = GetUniqueId(); + + + + for (i=0;i= 0) + { + //dispatcher will keep algorithms persistent in the collision pair + if (!pair.m_algorithms[dispatcherId]) + { + pair.m_algorithms[dispatcherId] = FindAlgorithm( + *pair.m_pProxy0, + *pair.m_pProxy1); + } + + if (pair.m_algorithms[dispatcherId]) + { + if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) + { + pair.m_algorithms[dispatcherId]->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + } else + { + float toi = pair.m_algorithms[dispatcherId]->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + + } + } + } else + { + //non-persistent algorithm dispatcher + CollisionAlgorithm* algo = FindAlgorithm( + *pair.m_pProxy0, + *pair.m_pProxy1); + + if (algo) + { + if (dispatchInfo.m_dispatchFunc == DispatcherInfo::DISPATCH_DISCRETE) + { + algo->ProcessCollision(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + } else + { + float toi = algo->CalculateTimeOfImpact(pair.m_pProxy0,pair.m_pProxy1,dispatchInfo); + if (dispatchInfo.m_timeOfImpact > toi) + dispatchInfo.m_timeOfImpact = toi; + } + } + } + + } + + //m_blockedForChanges = false; } diff --git a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h b/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h index 01e1fe947..bdee39cdc 100644 --- a/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h +++ b/Extras/PhysicsInterface/CcdPhysics/ParallelIslandDispatcher.h @@ -118,8 +118,7 @@ public: virtual int GetUniqueId() { return RIGIDBODY_DISPATCHER;} - virtual void DispatchAllCollisionPairs(OverlappingPairCache* pairCache,DispatcherInfo& dispatchInfo); - + virtual void DispatchAllCollisionPairs(BroadphasePair* pairs,int numPairs,DispatcherInfo& dispatchInfo); diff --git a/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp b/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp index 5a0f0c03f..0ab99760a 100644 --- a/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/ParallelPhysicsEnvironment.cpp @@ -20,13 +20,14 @@ subject to the following restrictions: #include "ParallelIslandDispatcher.h" #include "CollisionDispatch/CollisionWorld.h" #include "ConstraintSolver/TypedConstraint.h" - +#include "CollisionDispatch/SimulationIslandManager.h" +#include "SimulationIsland.h" ParallelPhysicsEnvironment::ParallelPhysicsEnvironment(ParallelIslandDispatcher* dispatcher, OverlappingPairCache* pairCache): -CcdPhysicsEnvironment(0,pairCache) +CcdPhysicsEnvironment(dispatcher,pairCache) { - + } ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment() @@ -39,13 +40,15 @@ ParallelPhysicsEnvironment::~ParallelPhysicsEnvironment() /// Perform an integration step of duration 'timeStep'. bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) { + OverlappingPairCache* scene = m_collisionWorld->GetPairCache(); + scene->RefreshOverlappingPairs(); + #ifdef USE_QUICKPROF - Profiler::beginBlock("CalcSimulationIslands"); + Profiler::beginBlock("IslandUnionFind"); #endif //USE_QUICKPROF - /* - GetCollisionWorld()->UpdateActivationState(); + GetSimulationIslandManager()->UpdateActivationState(GetCollisionWorld(),GetCollisionWorld()->GetDispatcher()); { int i; @@ -56,367 +59,100 @@ bool ParallelPhysicsEnvironment::proceedDeltaTimeOneStep(float timeStep) const RigidBody* colObj0 = &constraint->GetRigidBodyA(); const RigidBody* colObj1 = &constraint->GetRigidBodyB(); - + if (((colObj0) && ((colObj0)->mergesSimulationIslands())) && - ((colObj1) && ((colObj1)->mergesSimulationIslands()))) + ((colObj1) && ((colObj1)->mergesSimulationIslands()))) { if (colObj0->IsActive() || colObj1->IsActive()) { - GetDispatcher()->GetUnionFind().unite((colObj0)->m_islandTag1, + + GetSimulationIslandManager()->GetUnionFind().unite((colObj0)->m_islandTag1, (colObj1)->m_islandTag1); } } } } - GetCollisionWorld()->StoreIslandActivationState(); -*/ + GetSimulationIslandManager()->StoreIslandActivationState(GetCollisionWorld()); #ifdef USE_QUICKPROF - Profiler::endBlock("CalcSimulationIslands"); + Profiler::endBlock("IslandUnionFind"); #endif //USE_QUICKPROF - - -/* - //printf("CcdPhysicsEnvironment::proceedDeltaTime\n"); - - if (SimdFuzzyZero(timeStep)) - return true; - - if (m_debugDrawer) - { - gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation); - } - - + + ///calculate simulation islands + #ifdef USE_QUICKPROF - Profiler::beginBlock("SyncMotionStates"); + Profiler::beginBlock("BuildIslands"); #endif //USE_QUICKPROF + std::vector simulationIslands; + simulationIslands.resize(GetNumControllers()); - //this is needed because scaling is not known in advance, and scaling has to propagate to the shape - if (!m_scalingPropagated) + int k; + for (k=0;k::iterator i; - - - - int k; - for (k=0;kGetRigidBody(); - if (body->IsActive()) + int tag = ctrl->GetRigidBody()->m_islandTag1; + if (tag>=0) { - if (!body->IsStatic()) - { - body->applyForces( timeStep); - body->integrateVelocities( timeStep); - body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); - } + simulationIslands[tag].m_controllers.push_back(ctrl); } + } + Dispatcher* dispatcher = GetCollisionWorld()->GetDispatcher(); + + + + int i; + for (int i=0;i< scene->GetNumOverlappingPairs();i++) + { + BroadphasePair* pair = &scene->GetOverlappingPair(i); + + CollisionObject* col0 = static_cast(pair->m_pProxy0->m_clientObject); + CollisionObject* col1 = static_cast(pair->m_pProxy1->m_clientObject); + + if (col0->m_islandTag1 > col1->m_islandTag1) + { + simulationIslands[col0->m_islandTag1].m_overlappingPairs.push_back(*pair); + } else + { + simulationIslands[col1->m_islandTag1].m_overlappingPairs.push_back(*pair); } } -#ifdef USE_QUICKPROF - Profiler::endBlock("predictIntegratedTransform"); -#endif //USE_QUICKPROF + //add all overlapping pairs for each island - BroadphaseInterface* scene = GetBroadphase(); + for (i=0;iGetNumManifolds();i++) + { + PersistentManifold* manifold = dispatcher->GetManifoldByIndexInternal(i); + + //filtering for response + CollisionObject* colObj0 = static_cast(manifold->GetBody0()); + CollisionObject* colObj1 = static_cast(manifold->GetBody1()); + { + int islandTag = colObj0->m_islandTag1; + if (colObj1->m_islandTag1 > islandTag) + islandTag = colObj1->m_islandTag1; - // - // collision detection (?) - // - - -#ifdef USE_QUICKPROF - Profiler::beginBlock("DispatchAllCollisionPairs"); -#endif //USE_QUICKPROF - - - int numsubstep = m_numIterations; - - - DispatcherInfo dispatchInfo; - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_enableSatConvex = m_enableSatCollisionDetection; - - scene->DispatchAllCollisionPairs(*GetDispatcher(),dispatchInfo);///numsubstep,g); - - -#ifdef USE_QUICKPROF - Profiler::endBlock("DispatchAllCollisionPairs"); -#endif //USE_QUICKPROF - - - int numRigidBodies = m_controllers.size(); - + if (dispatcher->NeedsResponse(*colObj0,*colObj1)) + simulationIslands[islandTag].m_manifolds.push_back(manifold); + + } + } - //contacts -#ifdef USE_QUICKPROF - Profiler::beginBlock("SolveConstraint"); -#endif //USE_QUICKPROF - - - //solve the regular constraints (point 2 point, hinge, etc) - - for (int g=0;gBuildJacobian(); - constraint->SolveConstraint( timeStep ); - + simulationIslands[k].Simulate(dispatcher,GetBroadphase(),m_solver,timeStep); } - - } - -#ifdef USE_QUICKPROF - Profiler::endBlock("SolveConstraint"); -#endif //USE_QUICKPROF - - //solve the vehicles - -#ifdef NEW_BULLET_VEHICLE_SUPPORT - //vehicles - int numVehicles = m_wrapperVehicles.size(); - for (int i=0;iGetVehicle(); - vehicle->UpdateVehicle( timeStep); - } -#endif //NEW_BULLET_VEHICLE_SUPPORT - - - struct InplaceSolverIslandCallback : public ParallelIslandDispatcher::IslandCallback - { - - ContactSolverInfo& m_solverInfo; - ConstraintSolver* m_solver; - IDebugDraw* m_debugDrawer; - - InplaceSolverIslandCallback( - ContactSolverInfo& solverInfo, - ConstraintSolver* solver, - IDebugDraw* debugDrawer) - :m_solverInfo(solverInfo), - m_solver(solver), - m_debugDrawer(debugDrawer) - { - - } - - virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) - { - m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer); - } - - }; - - - m_solverInfo.m_friction = 0.9f; - m_solverInfo.m_numIterations = m_numIterations; - m_solverInfo.m_timeStep = timeStep; - m_solverInfo.m_restitution = 0.f;//m_restitution; - - InplaceSolverIslandCallback solverCallback( - m_solverInfo, - m_solver, - m_debugDrawer); - -#ifdef USE_QUICKPROF - Profiler::beginBlock("BuildAndProcessIslands"); -#endif //USE_QUICKPROF - - /// solve all the contact points and contact friction - GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback); - -#ifdef USE_QUICKPROF - Profiler::endBlock("BuildAndProcessIslands"); - - Profiler::beginBlock("CallbackTriggers"); -#endif //USE_QUICKPROF - - CallbackTriggers(); - -#ifdef USE_QUICKPROF - Profiler::endBlock("CallbackTriggers"); - - - Profiler::beginBlock("proceedToTransform"); - -#endif //USE_QUICKPROF - { - - - - { - - - - UpdateAabbs(timeStep); - - - float toi = 1.f; - - - - if (m_ccdMode == 3) - { - DispatcherInfo dispatchInfo; - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS; - - scene->DispatchAllCollisionPairs( *GetDispatcher(),dispatchInfo);///numsubstep,g); - toi = dispatchInfo.m_timeOfImpact; - - } - - - - // - // integrating solution - // - - { - - std::vector::iterator i; - - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - - CcdPhysicsController* ctrl = *i; - - SimdTransform predictedTrans; - RigidBody* body = ctrl->GetRigidBody(); - - if (body->IsActive()) - { - - if (!body->IsStatic()) - { - body->predictIntegratedTransform(timeStep* toi, predictedTrans); - body->proceedToTransform( predictedTrans); - } - - } - } - - } - - - - - - // - // disable sleeping physics objects - // - - std::vector m_sleepingControllers; - - std::vector::iterator i; - - for (i=m_controllers.begin(); - !(i==m_controllers.end()); i++) - { - CcdPhysicsController* ctrl = (*i); - RigidBody* body = ctrl->GetRigidBody(); - - ctrl->UpdateDeactivation(timeStep); - - - if (ctrl->wantsSleeping()) - { - if (body->GetActivationState() == ACTIVE_TAG) - body->SetActivationState( WANTS_DEACTIVATION ); - } else - { - if (body->GetActivationState() != DISABLE_DEACTIVATION) - body->SetActivationState( ACTIVE_TAG ); - } - - if (useIslands) - { - if (body->GetActivationState() == ISLAND_SLEEPING) - { - m_sleepingControllers.push_back(ctrl); - } - } else - { - if (ctrl->wantsSleeping()) - { - m_sleepingControllers.push_back(ctrl); - } - } - } - - - - - } - - -#ifdef USE_QUICKPROF - Profiler::endBlock("proceedToTransform"); - - Profiler::beginBlock("SyncMotionStates"); -#endif //USE_QUICKPROF - - SyncMotionStates(timeStep); - -#ifdef USE_QUICKPROF - Profiler::endBlock("SyncMotionStates"); - - Profiler::endProfilingCycle(); -#endif //USE_QUICKPROF - - -#ifdef NEW_BULLET_VEHICLE_SUPPORT - //sync wheels for vehicles - int numVehicles = m_wrapperVehicles.size(); - for (int i=0;iSyncWheels(); - } -#endif //NEW_BULLET_VEHICLE_SUPPORT - } -*/ - return true; -} + +} \ No newline at end of file diff --git a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp index 3d78a2df6..94d180584 100644 --- a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp +++ b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.cpp @@ -14,4 +14,513 @@ subject to the following restrictions: */ #include "SimulationIsland.h" +#include "SimdTransform.h" +#include "CcdPhysicsController.h" +#include "BroadphaseCollision/OverlappingPairCache.h" +#include "CollisionShapes/CollisionShape.h" +#include "BroadphaseCollision/Dispatcher.h" +#include "ConstraintSolver/ContactSolverInfo.h" +#include "ConstraintSolver/ConstraintSolver.h" +extern float gContactBreakingTreshold; + +bool SimulationIsland::Simulate(Dispatcher* dispatcher,BroadphaseInterface* broadphase,class ConstraintSolver* solver,float timeStep) +{ + //then execute all stuff below for each simulation island + + +#ifdef USE_QUICKPROF + Profiler::endBlock("BuildIslands"); +#endif //USE_QUICKPROF + + + + + ///build simulation islands, and add them to a job queue, which can be processed in parallel + ///or on the GPU + + + //printf("CcdPhysicsEnvironment::proceedDeltaTime\n"); + + if (SimdFuzzyZero(timeStep)) + return true; + + // if (m_debugDrawer) + // { + // gDisableDeactivation = (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_NoDeactivation); + // } + + +#ifdef USE_QUICKPROF + Profiler::beginBlock("SyncMotionStates"); +#endif //USE_QUICKPROF + + + //this is needed because scaling is not known in advance, and scaling has to propagate to the shape + //if (!m_scalingPropagated) + //{ + // SyncMotionStates(timeStep); + // m_scalingPropagated = true; + //} + + +#ifdef USE_QUICKPROF + Profiler::endBlock("SyncMotionStates"); + + Profiler::beginBlock("predictIntegratedTransform"); +#endif //USE_QUICKPROF + + { + // std::vector::iterator i; + + + + int k; + for (k=0;kGetRigidBody(); + if (body->IsActive()) + { + if (!body->IsStatic()) + { + body->applyForces( timeStep); + body->integrateVelocities( timeStep); + body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform); + } + } + + } + } + +#ifdef USE_QUICKPROF + Profiler::endBlock("predictIntegratedTransform"); +#endif //USE_QUICKPROF + + //BroadphaseInterface* scene = GetBroadphase(); + + + // + // collision detection (?) + // + + + #ifdef USE_QUICKPROF + Profiler::beginBlock("DispatchAllCollisionPairs"); + #endif //USE_QUICKPROF + + +// int numsubstep = m_numIterations; + + + DispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_enableSatConvex = false;//m_enableSatCollisionDetection; + + //pairCache->RefreshOverlappingPairs(); + if (m_overlappingPairs.size()) + { + dispatcher->DispatchAllCollisionPairs(&m_overlappingPairs[0],m_overlappingPairs.size(),dispatchInfo);///numsubstep,g); + } + + + #ifdef USE_QUICKPROF + Profiler::endBlock("DispatchAllCollisionPairs"); + #endif //USE_QUICKPROF + +/* + + + int numRigidBodies = m_controllers.size(); + + + + + //contacts + #ifdef USE_QUICKPROF + Profiler::beginBlock("SolveConstraint"); + #endif //USE_QUICKPROF + + + //solve the regular constraints (point 2 point, hinge, etc) + + for (int g=0;gBuildJacobian(); + constraint->SolveConstraint( timeStep ); + + } + + + } + + #ifdef USE_QUICKPROF + Profiler::endBlock("SolveConstraint"); + #endif //USE_QUICKPROF + + //solve the vehicles + + #ifdef NEW_BULLET_VEHICLE_SUPPORT + //vehicles + int numVehicles = m_wrapperVehicles.size(); + for (int i=0;iGetVehicle(); + vehicle->UpdateVehicle( timeStep); + } + #endif //NEW_BULLET_VEHICLE_SUPPORT + + + struct InplaceSolverIslandCallback : public ParallelIslandDispatcher::IslandCallback + { + + ContactSolverInfo& m_solverInfo; + ConstraintSolver* m_solver; + IDebugDraw* m_debugDrawer; + + InplaceSolverIslandCallback( + ContactSolverInfo& solverInfo, + ConstraintSolver* solver, + IDebugDraw* debugDrawer) + :m_solverInfo(solverInfo), + m_solver(solver), + m_debugDrawer(debugDrawer) + { + + } + + virtual void ProcessIsland(PersistentManifold** manifolds,int numManifolds) + { + m_solver->SolveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer); + } + + }; + + + m_solverInfo.m_friction = 0.9f; + m_solverInfo.m_numIterations = m_numIterations; + m_solverInfo.m_timeStep = timeStep; + m_solverInfo.m_restitution = 0.f;//m_restitution; + + InplaceSolverIslandCallback solverCallback( + m_solverInfo, + m_solver, + m_debugDrawer); + + #ifdef USE_QUICKPROF + Profiler::beginBlock("BuildAndProcessIslands"); + #endif //USE_QUICKPROF + + /// solve all the contact points and contact friction + GetDispatcher()->BuildAndProcessIslands(m_collisionWorld->GetCollisionObjectArray(),&solverCallback); + + #ifdef USE_QUICKPROF + Profiler::endBlock("BuildAndProcessIslands"); + + Profiler::beginBlock("CallbackTriggers"); + #endif //USE_QUICKPROF + + CallbackTriggers(); + + #ifdef USE_QUICKPROF + Profiler::endBlock("CallbackTriggers"); + + } + */ + + //OverlappingPairCache* scene = GetCollisionWorld()->GetPairCache(); + + ContactSolverInfo solverInfo; + + solverInfo.m_friction = 0.9f; + solverInfo.m_numIterations = 10;//m_numIterations; + solverInfo.m_timeStep = timeStep; + solverInfo.m_restitution = 0.f;//m_restitution; + + + if (m_manifolds.size()) + { + solver->SolveGroup( &m_manifolds[0],m_manifolds.size(),solverInfo,0); + } + + +#ifdef USE_QUICKPROF + Profiler::beginBlock("proceedToTransform"); +#endif //USE_QUICKPROF + { + + + + { + + + UpdateAabbs(broadphase,timeStep); + + + float toi = 1.f; + + + + /* if (m_ccdMode == 3) + { + DispatcherInfo dispatchInfo; + dispatchInfo.m_timeStep = timeStep; + dispatchInfo.m_stepCount = 0; + dispatchInfo.m_dispatchFunc = DispatcherInfo::DISPATCH_CONTINUOUS; + + // GetCollisionWorld()->GetDispatcher()->DispatchAllCollisionPairs(scene,dispatchInfo); + toi = dispatchInfo.m_timeOfImpact; + + } + */ + + + + // + // integrating solution + // + + { + + std::vector::iterator i; + + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + + CcdPhysicsController* ctrl = *i; + + SimdTransform predictedTrans; + RigidBody* body = ctrl->GetRigidBody(); + + if (body->IsActive()) + { + + if (!body->IsStatic()) + { + body->predictIntegratedTransform(timeStep* toi, predictedTrans); + body->proceedToTransform( predictedTrans); + } + + } + } + + } + + + + + + // + // disable sleeping physics objects + // + + std::vector m_sleepingControllers; + + std::vector::iterator i; + + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + RigidBody* body = ctrl->GetRigidBody(); + + ctrl->UpdateDeactivation(timeStep); + + + if (ctrl->wantsSleeping()) + { + if (body->GetActivationState() == ACTIVE_TAG) + body->SetActivationState( WANTS_DEACTIVATION ); + } else + { + if (body->GetActivationState() != DISABLE_DEACTIVATION) + body->SetActivationState( ACTIVE_TAG ); + } + + if (true) + { + if (body->GetActivationState() == ISLAND_SLEEPING) + { + m_sleepingControllers.push_back(ctrl); + } + } else + { + if (ctrl->wantsSleeping()) + { + m_sleepingControllers.push_back(ctrl); + } + } + } + + + + + } + + +#ifdef USE_QUICKPROF + Profiler::endBlock("proceedToTransform"); + + Profiler::beginBlock("SyncMotionStates"); +#endif //USE_QUICKPROF + + SyncMotionStates(timeStep); + +#ifdef USE_QUICKPROF + Profiler::endBlock("SyncMotionStates"); + + Profiler::endProfilingCycle(); +#endif //USE_QUICKPROF + + +#ifdef NEW_BULLET_VEHICLE_SUPPORT + //sync wheels for vehicles + int numVehicles = m_wrapperVehicles.size(); + for (int i=0;iSyncWheels(); + } +#endif //NEW_BULLET_VEHICLE_SUPPORT + + return true; + } +} + + + +void SimulationIsland::SyncMotionStates(float timeStep) +{ + std::vector::iterator i; + + // + // synchronize the physics and graphics transformations + // + + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + ctrl->SynchronizeMotionStates(timeStep); + + } + +} + + + +void SimulationIsland::UpdateAabbs(BroadphaseInterface* scene,float timeStep) +{ + std::vector::iterator i; + + + // + // update aabbs, only for moving objects (!) + // + for (i=m_controllers.begin(); + !(i==m_controllers.end()); i++) + { + CcdPhysicsController* ctrl = (*i); + RigidBody* body = ctrl->GetRigidBody(); + + + SimdPoint3 minAabb,maxAabb; + CollisionShape* shapeinterface = ctrl->GetCollisionShape(); + + + + shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(), + body->getLinearVelocity(), + //body->getAngularVelocity(), + SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(), + timeStep,minAabb,maxAabb); + + + SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold); + minAabb -= manifoldExtraExtents; + maxAabb += manifoldExtraExtents; + + BroadphaseProxy* bp = body->m_broadphaseHandle; + if (bp) + { + + SimdVector3 color (1,1,0); + + class IDebugDraw* m_debugDrawer = 0; +/* + if (m_debugDrawer) + { + //draw aabb + switch (body->GetActivationState()) + { + case ISLAND_SLEEPING: + { + color.setValue(1,1,1); + break; + } + case WANTS_DEACTIVATION: + { + color.setValue(0,0,1); + break; + } + case ACTIVE_TAG: + { + break; + } + case DISABLE_DEACTIVATION: + { + color.setValue(1,0,1); + }; + + }; + + if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb) + { + DrawAabb(m_debugDrawer,minAabb,maxAabb,color); + } + } +*/ + + + if ( (maxAabb-minAabb).length2() < 1e12f) + { + scene->SetAabb(bp,minAabb,maxAabb); + } else + { + //something went wrong, investigate + //removeCcdPhysicsController(ctrl); + body->SetActivationState(DISABLE_SIMULATION); + + static bool reportMe = true; + if (reportMe) + { + reportMe = false; + printf("Overflow in AABB, object removed from simulation \n"); + printf("If you can reproduce this, please email bugs@continuousphysics.com\n"); + printf("Please include above information, your Platform, version of OS.\n"); + printf("Thanks.\n"); + } + + } + + } + } +} \ No newline at end of file diff --git a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h index a59737fbf..8e79e2ca7 100644 --- a/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h +++ b/Extras/PhysicsInterface/CcdPhysics/SimulationIsland.h @@ -16,14 +16,35 @@ subject to the following restrictions: #ifndef SIMULATION_ISLAND_H #define SIMULATION_ISLAND_H +#include +class BroadphaseInterface; +class Dispatcher; + ///SimulationIsland groups all computations and data (for collision detection and dynamics) that can execute in parallel with other SimulationIsland's ///The ParallelPhysicsEnvironment and ParallelIslandDispatcher will dispatch SimulationIsland's ///At the start of the simulation timestep the simulation islands are re-calculated ///During one timestep there is no merging or splitting of Simulation Islands class SimulationIsland { + public: - + std::vector m_controllers; + std::vector m_manifolds; + std::vector m_overlappingPairs; + + bool Simulate(Dispatcher* dispatcher,BroadphaseInterface* broadphase, class ConstraintSolver* solver, float timeStep); + + + int GetNumControllers() + { + return m_controllers.size(); + } + + + + + void SyncMotionStates(float timeStep); + void UpdateAabbs(BroadphaseInterface* broadphase,float timeStep); }; #endif //SIMULATION_ISLAND_H \ No newline at end of file