diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 3293c8de9..1025da42f 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -52,6 +52,7 @@ struct btContactSolverInfoData int m_solverMode; int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; }; @@ -79,6 +80,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_warmstartingFactor=btScalar(0.85); m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution + m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit } }; diff --git a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index 54f16c8bb..2beb3f50f 100644 --- a/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -548,7 +548,6 @@ class btSortConstraintOnIslandPredicate - void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); @@ -563,6 +562,11 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) btIDebugDraw* m_debugDrawer; btStackAlloc* m_stackAlloc; btDispatcher* m_dispatcher; + + btAlignedObjectArray m_bodies; + btAlignedObjectArray m_manifolds; + btAlignedObjectArray m_constraints; + InplaceSolverIslandCallback( btContactSolverInfo& solverInfo, @@ -583,6 +587,7 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } + InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) { btAssert(0); @@ -623,17 +628,48 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) } } - ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive - if (numManifolds + numCurConstraints) + if (m_solverInfo.m_minimumSolverBatchSize<=1) { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive + if (numManifolds + numCurConstraints) + { + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + } else + { + + for (i=0;im_solverInfo.m_minimumSolverBatchSize) + { + processConstraints(); + } else + { + //printf("deferred\n"); + } } - } } + void processConstraints() + { + if (m_manifolds.size() + m_constraints.size()>0) + { + m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + + } }; + + //sorted version of all btTypedConstraint, based on islandId btAlignedObjectArray sortedConstraints; sortedConstraints.resize( m_constraints.size()); @@ -658,6 +694,8 @@ void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) /// solve all the constraints for this island m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); + solverCallback.processConstraints(); + m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); }