diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp | 2555 |
1 files changed, 1244 insertions, 1311 deletions
diff --git a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp index 4306c37e49..2718da4a50 100644 --- a/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp +++ b/thirdparty/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp @@ -13,7 +13,6 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ - #include "btSequentialImpulseConstraintSolverMt.h" #include "LinearMath/btQuickprof.h" @@ -23,8 +22,6 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" #include "BulletDynamics/Dynamics/btRigidBody.h" - - bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false; // some task schedulers don't like nested loops int btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = 250; int btSequentialImpulseConstraintSolverMt::s_minBatchSize = 50; @@ -32,613 +29,594 @@ int btSequentialImpulseConstraintSolverMt::s_maxBatchSize = 100; btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D; btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_jointBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D; - btSequentialImpulseConstraintSolverMt::btSequentialImpulseConstraintSolverMt() { - m_numFrictionDirections = 1; - m_useBatching = false; - m_useObsoleteJointConstraints = false; + m_numFrictionDirections = 1; + m_useBatching = false; + m_useObsoleteJointConstraints = false; } - btSequentialImpulseConstraintSolverMt::~btSequentialImpulseConstraintSolverMt() { } - void btSequentialImpulseConstraintSolverMt::setupBatchedContactConstraints() { - BT_PROFILE("setupBatchedContactConstraints"); - m_batchedContactConstraints.setup( &m_tmpSolverContactConstraintPool, - m_tmpSolverBodyPool, - s_contactBatchingMethod, - s_minBatchSize, - s_maxBatchSize, - &m_scratchMemory - ); + BT_PROFILE("setupBatchedContactConstraints"); + m_batchedContactConstraints.setup(&m_tmpSolverContactConstraintPool, + m_tmpSolverBodyPool, + s_contactBatchingMethod, + s_minBatchSize, + s_maxBatchSize, + &m_scratchMemory); } - void btSequentialImpulseConstraintSolverMt::setupBatchedJointConstraints() { - BT_PROFILE("setupBatchedJointConstraints"); - m_batchedJointConstraints.setup( &m_tmpSolverNonContactConstraintPool, - m_tmpSolverBodyPool, - s_jointBatchingMethod, - s_minBatchSize, - s_maxBatchSize, - &m_scratchMemory - ); + BT_PROFILE("setupBatchedJointConstraints"); + m_batchedJointConstraints.setup(&m_tmpSolverNonContactConstraintPool, + m_tmpSolverBodyPool, + s_jointBatchingMethod, + s_minBatchSize, + s_maxBatchSize, + &m_scratchMemory); } - void btSequentialImpulseConstraintSolverMt::internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal) { - btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint]; - - btVector3 rel_pos1; - btVector3 rel_pos2; - btScalar relaxation; - - int solverBodyIdA = contactConstraint.m_solverBodyIdA; - int solverBodyIdB = contactConstraint.m_solverBodyIdB; - - btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ]; - btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ]; - - btRigidBody* colObj0 = solverBodyA->m_originalBody; - btRigidBody* colObj1 = solverBodyB->m_originalBody; - - btManifoldPoint& cp = *static_cast<btManifoldPoint*>( contactConstraint.m_originalContactPoint ); - - const btVector3& pos1 = cp.getPositionWorldOnA(); - const btVector3& pos2 = cp.getPositionWorldOnB(); - - rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin(); - - btVector3 vel1; - btVector3 vel2; - - solverBodyA->getVelocityInLocalPointNoDelta( rel_pos1, vel1 ); - solverBodyB->getVelocityInLocalPointNoDelta( rel_pos2, vel2 ); - - btVector3 vel = vel1 - vel2; - btScalar rel_vel = cp.m_normalWorldOnB.dot( vel ); - - setupContactConstraint( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2 ); - - // setup rolling friction constraints - int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint]; - if (rollingFrictionIndex >= 0) - { - btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ]; - btAssert( spinningFrictionConstraint.m_frictionIndex == iContactConstraint ); - setupTorsionalFrictionConstraint( spinningFrictionConstraint, - cp.m_normalWorldOnB, - solverBodyIdA, - solverBodyIdB, - cp, - cp.m_combinedSpinningFriction, - rel_pos1, - rel_pos2, - colObj0, - colObj1, - relaxation, - 0.0f, - 0.0f - ); - btVector3 axis[2]; - btPlaneSpace1( cp.m_normalWorldOnB, axis[0], axis[1] ); - axis[0].normalize(); - axis[1].normalize(); - - applyAnisotropicFriction( colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); - applyAnisotropicFriction( colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); - applyAnisotropicFriction( colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); - applyAnisotropicFriction( colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION ); - // put the largest axis first - if (axis[1].length2() > axis[0].length2()) - { - btSwap(axis[0], axis[1]); - } - const btScalar kRollingFrictionThreshold = 0.001f; - for (int i = 0; i < 2; ++i) - { - int iRollingFric = rollingFrictionIndex + 1 + i; - btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ]; - btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint); - btVector3 dir = axis[i]; - if ( dir.length() > kRollingFrictionThreshold ) - { - setupTorsionalFrictionConstraint( rollingFrictionConstraint, - dir, - solverBodyIdA, - solverBodyIdB, - cp, - cp.m_combinedRollingFriction, - rel_pos1, - rel_pos2, - colObj0, - colObj1, - relaxation, - 0.0f, - 0.0f - ); - } - else - { - rollingFrictionConstraint.m_frictionIndex = -1; // disable constraint - } - } - } - - // setup friction constraints - // setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); - { - ///Bullet has several options to set the friction directions - ///By default, each contact has only a single friction direction that is recomputed automatically very frame - ///based on the relative linear velocity. - ///If the relative velocity it zero, it will automatically compute a friction direction. - - ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. - ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. - /// - ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. - /// - ///The user can manually override the friction directions for certain contacts using a contact callback, - ///and set the cp.m_lateralFrictionInitialized to true - ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) - ///this will give a conveyor belt effect - /// - btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex]; - btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint); - - btSolverConstraint* frictionConstraint2 = NULL; - if ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) - { - frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1]; - btAssert( frictionConstraint2->m_frictionIndex == iContactConstraint ); - } - - if ( !( infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING ) || !( cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED ) ) - { - cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; - btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); - if ( !( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) && lat_rel_vel > SIMD_EPSILON ) - { - cp.m_lateralFrictionDir1 *= 1.f / btSqrt( lat_rel_vel ); - applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); - applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); - setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); - - if ( frictionConstraint2 ) - { - cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross( cp.m_normalWorldOnB ); - cp.m_lateralFrictionDir2.normalize();//?? - applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); - applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); - setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); - } - } - else - { - btPlaneSpace1( cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2 ); - - applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); - applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION ); - setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); - - if ( frictionConstraint2 ) - { - applyAnisotropicFriction( colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); - applyAnisotropicFriction( colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION ); - setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal ); - } - - if ( ( infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS ) && ( infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION ) ) - { - cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; - } - } - } - else - { - setupFrictionConstraint( *frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM ); - if ( frictionConstraint2 ) - { - setupFrictionConstraint( *frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM ); - } - } - } - - setFrictionConstraintImpulse( contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal ); -} + btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint]; + + btVector3 rel_pos1; + btVector3 rel_pos2; + btScalar relaxation; + + int solverBodyIdA = contactConstraint.m_solverBodyIdA; + int solverBodyIdB = contactConstraint.m_solverBodyIdB; + + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + + btRigidBody* colObj0 = solverBodyA->m_originalBody; + btRigidBody* colObj1 = solverBodyB->m_originalBody; + + btManifoldPoint& cp = *static_cast<btManifoldPoint*>(contactConstraint.m_originalContactPoint); + + const btVector3& pos1 = cp.getPositionWorldOnA(); + const btVector3& pos2 = cp.getPositionWorldOnB(); + + rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin(); + + btVector3 vel1; + btVector3 vel2; + + solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1); + solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2); + + btVector3 vel = vel1 - vel2; + btScalar rel_vel = cp.m_normalWorldOnB.dot(vel); + + setupContactConstraint(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2); + + // setup rolling friction constraints + int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint]; + if (rollingFrictionIndex >= 0) + { + btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex]; + btAssert(spinningFrictionConstraint.m_frictionIndex == iContactConstraint); + setupTorsionalFrictionConstraint(spinningFrictionConstraint, + cp.m_normalWorldOnB, + solverBodyIdA, + solverBodyIdB, + cp, + cp.m_combinedSpinningFriction, + rel_pos1, + rel_pos2, + colObj0, + colObj1, + relaxation, + 0.0f, + 0.0f); + btVector3 axis[2]; + btPlaneSpace1(cp.m_normalWorldOnB, axis[0], axis[1]); + axis[0].normalize(); + axis[1].normalize(); + + applyAnisotropicFriction(colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + // put the largest axis first + if (axis[1].length2() > axis[0].length2()) + { + btSwap(axis[0], axis[1]); + } + const btScalar kRollingFrictionThreshold = 0.001f; + for (int i = 0; i < 2; ++i) + { + int iRollingFric = rollingFrictionIndex + 1 + i; + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric]; + btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint); + btVector3 dir = axis[i]; + if (dir.length() > kRollingFrictionThreshold) + { + setupTorsionalFrictionConstraint(rollingFrictionConstraint, + dir, + solverBodyIdA, + solverBodyIdB, + cp, + cp.m_combinedRollingFriction, + rel_pos1, + rel_pos2, + colObj0, + colObj1, + relaxation, + 0.0f, + 0.0f); + } + else + { + rollingFrictionConstraint.m_frictionIndex = -1; // disable constraint + } + } + } + + // setup friction constraints + // setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip); + { + ///Bullet has several options to set the friction directions + ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///based on the relative linear velocity. + ///If the relative velocity it zero, it will automatically compute a friction direction. + + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. + ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. + /// + ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. + /// + ///The user can manually override the friction directions for certain contacts using a contact callback, + ///and set the cp.m_lateralFrictionInitialized to true + ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) + ///this will give a conveyor belt effect + /// + btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex]; + btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint); + + btSolverConstraint* frictionConstraint2 = NULL; + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) + { + frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1]; + btAssert(frictionConstraint2->m_frictionIndex == iContactConstraint); + } + + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) + { + cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; + btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); + if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) + { + cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel); + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + + if (frictionConstraint2) + { + cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); + cp.m_lateralFrictionDir2.normalize(); //?? + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + } + } + else + { + btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2); + + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION); + setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + + if (frictionConstraint2) + { + applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION); + setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal); + } + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) + { + cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; + } + } + } + else + { + setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM); + if (frictionConstraint2) + { + setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM); + } + } + } + setFrictionConstraintImpulse(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); +} struct SetupContactConstraintsLoop : public btIParallelForBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btBatchedConstraints* m_bc; - const btContactSolverInfo* m_infoGlobal; - - SetupContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal ) - { - m_solver = solver; - m_bc = bc; - m_infoGlobal = &infoGlobal; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - BT_PROFILE( "SetupContactConstraintsLoop" ); - for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) - { - const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; - for (int i = batch.begin; i < batch.end; ++i) - { - int iContact = m_bc->m_constraintIndices[i]; - m_solver->internalSetupContactConstraints( iContact, *m_infoGlobal ); - } - } - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + const btContactSolverInfo* m_infoGlobal; + SetupContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal) + { + m_solver = solver; + m_bc = bc; + m_infoGlobal = &infoGlobal; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + BT_PROFILE("SetupContactConstraintsLoop"); + for (int iBatch = iBegin; iBatch < iEnd; ++iBatch) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch]; + for (int i = batch.begin; i < batch.end; ++i) + { + int iContact = m_bc->m_constraintIndices[i]; + m_solver->internalSetupContactConstraints(iContact, *m_infoGlobal); + } + } + } +}; void btSequentialImpulseConstraintSolverMt::setupAllContactConstraints(const btContactSolverInfo& infoGlobal) { - BT_PROFILE( "setupAllContactConstraints" ); - if ( m_useBatching ) - { - const btBatchedConstraints& batchedCons = m_batchedContactConstraints; - SetupContactConstraintsLoop loop( this, &batchedCons, infoGlobal ); - for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) - { - int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; - const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; - int grainSize = 1; - btParallelFor( phase.begin, phase.end, grainSize, loop ); - } - } - else - { - for ( int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i ) - { - internalSetupContactConstraints( i, infoGlobal ); - } - } + BT_PROFILE("setupAllContactConstraints"); + if (m_useBatching) + { + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + SetupContactConstraintsLoop loop(this, &batchedCons, infoGlobal); + for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase) + { + int iPhase = batchedCons.m_phaseOrder[iiPhase]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase]; + int grainSize = 1; + btParallelFor(phase.begin, phase.end, grainSize, loop); + } + } + else + { + for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i) + { + internalSetupContactConstraints(i, infoGlobal); + } + } } - -int btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body,btScalar timeStep) +int btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep) { - // - // getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers) - // - // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism) - // - int solverBodyId = -1; - bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL; - if ( isRigidBodyType && !body.isStaticOrKinematicObject() ) - { - // dynamic body - // Dynamic bodies can only be in one island, so it's safe to write to the companionId - solverBodyId = body.getCompanionId(); - if ( solverBodyId < 0 ) - { - m_bodySolverArrayMutex.lock(); - // now that we have the lock, check again - solverBodyId = body.getCompanionId(); - if ( solverBodyId < 0 ) - { - solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &solverBody, &body, timeStep ); - body.setCompanionId( solverBodyId ); - } - m_bodySolverArrayMutex.unlock(); - } - } - else if (isRigidBodyType && body.isKinematicObject()) - { - // - // NOTE: must test for kinematic before static because some kinematic objects also - // identify as "static" - // - // Kinematic bodies can be in multiple islands at once, so it is a - // race condition to write to them, so we use an alternate method - // to record the solverBodyId - int uniqueId = body.getWorldArrayIndex(); - const int INVALID_SOLVER_BODY_ID = -1; - if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId ) - { - m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock(); - // now that we have the lock, check again - if ( m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId ) - { - m_kinematicBodyUniqueIdToSolverBodyTable.resize( uniqueId + 1, INVALID_SOLVER_BODY_ID ); - } - m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock(); - } - solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ]; - // if no table entry yet, - if ( INVALID_SOLVER_BODY_ID == solverBodyId ) - { - // need to acquire both locks - m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock(); - m_bodySolverArrayMutex.lock(); - // now that we have the lock, check again - solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ]; - if ( INVALID_SOLVER_BODY_ID == solverBodyId ) - { - // create a table entry for this body - solverBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &solverBody, &body, timeStep ); - m_kinematicBodyUniqueIdToSolverBodyTable[ uniqueId ] = solverBodyId; - } - m_bodySolverArrayMutex.unlock(); - m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock(); - } - } - else - { - // all fixed bodies (inf mass) get mapped to a single solver id - if ( m_fixedBodyId < 0 ) - { - m_bodySolverArrayMutex.lock(); - // now that we have the lock, check again - if ( m_fixedBodyId < 0 ) - { - m_fixedBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &fixedBody, 0, timeStep ); - } - m_bodySolverArrayMutex.unlock(); - } - solverBodyId = m_fixedBodyId; - } - btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() ); + // + // getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers) + // + // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism) + // + int solverBodyId = -1; + bool isRigidBodyType = btRigidBody::upcast(&body) != NULL; + if (isRigidBodyType && !body.isStaticOrKinematicObject()) + { + // dynamic body + // Dynamic bodies can only be in one island, so it's safe to write to the companionId + solverBodyId = body.getCompanionId(); + if (solverBodyId < 0) + { + m_bodySolverArrayMutex.lock(); + // now that we have the lock, check again + solverBodyId = body.getCompanionId(); + if (solverBodyId < 0) + { + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + body.setCompanionId(solverBodyId); + } + m_bodySolverArrayMutex.unlock(); + } + } + else if (isRigidBodyType && body.isKinematicObject()) + { + // + // NOTE: must test for kinematic before static because some kinematic objects also + // identify as "static" + // + // Kinematic bodies can be in multiple islands at once, so it is a + // race condition to write to them, so we use an alternate method + // to record the solverBodyId + int uniqueId = body.getWorldArrayIndex(); + const int INVALID_SOLVER_BODY_ID = -1; + if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId) + { + m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock(); + // now that we have the lock, check again + if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId) + { + m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID); + } + m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock(); + } + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId]; + // if no table entry yet, + if (INVALID_SOLVER_BODY_ID == solverBodyId) + { + // need to acquire both locks + m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock(); + m_bodySolverArrayMutex.lock(); + // now that we have the lock, check again + solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId]; + if (INVALID_SOLVER_BODY_ID == solverBodyId) + { + // create a table entry for this body + solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody, &body, timeStep); + m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId; + } + m_bodySolverArrayMutex.unlock(); + m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock(); + } + } + else + { + // all fixed bodies (inf mass) get mapped to a single solver id + if (m_fixedBodyId < 0) + { + m_bodySolverArrayMutex.lock(); + // now that we have the lock, check again + if (m_fixedBodyId < 0) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody, 0, timeStep); + } + m_bodySolverArrayMutex.unlock(); + } + solverBodyId = m_fixedBodyId; + } + btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size()); return solverBodyId; } - void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { - BT_PROFILE("internalCollectContactManifoldCachedInfo"); - for (int i = 0; i < numManifolds; ++i) - { - btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i]; - btPersistentManifold* manifold = manifoldPtr[i]; - btCollisionObject* colObj0 = (btCollisionObject*) manifold->getBody0(); - btCollisionObject* colObj1 = (btCollisionObject*) manifold->getBody1(); - - int solverBodyIdA = getOrInitSolverBodyThreadsafe( *colObj0, infoGlobal.m_timeStep ); - int solverBodyIdB = getOrInitSolverBodyThreadsafe( *colObj1, infoGlobal.m_timeStep ); - - cachedInfo->solverBodyIds[ 0 ] = solverBodyIdA; - cachedInfo->solverBodyIds[ 1 ] = solverBodyIdB; - cachedInfo->numTouchingContacts = 0; - - btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ]; - btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ]; - - // A contact manifold between 2 static object should not exist! - // check the collision flags of your objects if this assert fires. - // Incorrectly set collision object flags can degrade performance in various ways. - btAssert( !m_tmpSolverBodyPool[ solverBodyIdA ].m_invMass.isZero() || !m_tmpSolverBodyPool[ solverBodyIdB ].m_invMass.isZero() ); - - int iContact = 0; - for ( int j = 0; j < manifold->getNumContacts(); j++ ) - { - btManifoldPoint& cp = manifold->getContactPoint( j ); - - if ( cp.getDistance() <= manifold->getContactProcessingThreshold() ) - { - cachedInfo->contactPoints[ iContact ] = &cp; - cachedInfo->contactHasRollingFriction[ iContact ] = ( cp.m_combinedRollingFriction > 0.f ); - iContact++; - } - } - cachedInfo->numTouchingContacts = iContact; - } -} + BT_PROFILE("internalCollectContactManifoldCachedInfo"); + for (int i = 0; i < numManifolds; ++i) + { + btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i]; + btPersistentManifold* manifold = manifoldPtr[i]; + btCollisionObject* colObj0 = (btCollisionObject*)manifold->getBody0(); + btCollisionObject* colObj1 = (btCollisionObject*)manifold->getBody1(); + + int solverBodyIdA = getOrInitSolverBodyThreadsafe(*colObj0, infoGlobal.m_timeStep); + int solverBodyIdB = getOrInitSolverBodyThreadsafe(*colObj1, infoGlobal.m_timeStep); + + cachedInfo->solverBodyIds[0] = solverBodyIdA; + cachedInfo->solverBodyIds[1] = solverBodyIdB; + cachedInfo->numTouchingContacts = 0; + + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + // A contact manifold between 2 static object should not exist! + // check the collision flags of your objects if this assert fires. + // Incorrectly set collision object flags can degrade performance in various ways. + btAssert(!m_tmpSolverBodyPool[solverBodyIdA].m_invMass.isZero() || !m_tmpSolverBodyPool[solverBodyIdB].m_invMass.isZero()); + + int iContact = 0; + for (int j = 0; j < manifold->getNumContacts(); j++) + { + btManifoldPoint& cp = manifold->getContactPoint(j); + + if (cp.getDistance() <= manifold->getContactProcessingThreshold()) + { + cachedInfo->contactPoints[iContact] = &cp; + cachedInfo->contactHasRollingFriction[iContact] = (cp.m_combinedRollingFriction > 0.f); + iContact++; + } + } + cachedInfo->numTouchingContacts = iContact; + } +} struct CollectContactManifoldCachedInfoLoop : public btIParallelForBody { - btSequentialImpulseConstraintSolverMt* m_solver; - btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray; - btPersistentManifold** m_manifoldPtr; - const btContactSolverInfo* m_infoGlobal; - - CollectContactManifoldCachedInfoLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal ) - { - m_solver = solver; - m_cachedInfoArray = cachedInfoArray; - m_manifoldPtr = manifoldPtr; - m_infoGlobal = &infoGlobal; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - m_solver->internalCollectContactManifoldCachedInfo( m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal ); - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray; + btPersistentManifold** m_manifoldPtr; + const btContactSolverInfo* m_infoGlobal; + CollectContactManifoldCachedInfoLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal) + { + m_solver = solver; + m_cachedInfoArray = cachedInfoArray; + m_manifoldPtr = manifoldPtr; + m_infoGlobal = &infoGlobal; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + m_solver->internalCollectContactManifoldCachedInfo(m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal); + } +}; void btSequentialImpulseConstraintSolverMt::internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds) { - BT_PROFILE("internalAllocContactConstraints"); - // possibly parallel part - for ( int iManifold = 0; iManifold < numManifolds; ++iManifold ) - { - const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ]; - int contactIndex = cachedInfo.contactIndex; - int frictionIndex = contactIndex * m_numFrictionDirections; - int rollingFrictionIndex = cachedInfo.rollingFrictionIndex; - for ( int i = 0; i < cachedInfo.numTouchingContacts; i++ ) - { - btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex]; - contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[ 0 ]; - contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[ 1 ]; - contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[ i ]; - - // allocate the friction constraints - contactConstraint.m_frictionIndex = frictionIndex; - for ( int iDir = 0; iDir < m_numFrictionDirections; ++iDir ) - { - btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex]; - frictionConstraint.m_frictionIndex = contactIndex; - frictionIndex++; - } - - // allocate rolling friction constraints - if ( cachedInfo.contactHasRollingFriction[ i ] ) - { - m_rollingFrictionIndexTable[ contactIndex ] = rollingFrictionIndex; - // allocate 3 (although we may use only 2 sometimes) - for ( int i = 0; i < 3; i++ ) - { - m_tmpSolverContactRollingFrictionConstraintPool[ rollingFrictionIndex ].m_frictionIndex = contactIndex; - rollingFrictionIndex++; - } - } - else - { - // indicate there is no rolling friction for this contact point - m_rollingFrictionIndexTable[ contactIndex ] = -1; - } - contactIndex++; - } - } -} + BT_PROFILE("internalAllocContactConstraints"); + // possibly parallel part + for (int iManifold = 0; iManifold < numManifolds; ++iManifold) + { + const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold]; + int contactIndex = cachedInfo.contactIndex; + int frictionIndex = contactIndex * m_numFrictionDirections; + int rollingFrictionIndex = cachedInfo.rollingFrictionIndex; + for (int i = 0; i < cachedInfo.numTouchingContacts; i++) + { + btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex]; + contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[0]; + contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[1]; + contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[i]; + + // allocate the friction constraints + contactConstraint.m_frictionIndex = frictionIndex; + for (int iDir = 0; iDir < m_numFrictionDirections; ++iDir) + { + btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex]; + frictionConstraint.m_frictionIndex = contactIndex; + frictionIndex++; + } + // allocate rolling friction constraints + if (cachedInfo.contactHasRollingFriction[i]) + { + m_rollingFrictionIndexTable[contactIndex] = rollingFrictionIndex; + // allocate 3 (although we may use only 2 sometimes) + for (int i = 0; i < 3; i++) + { + m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex].m_frictionIndex = contactIndex; + rollingFrictionIndex++; + } + } + else + { + // indicate there is no rolling friction for this contact point + m_rollingFrictionIndexTable[contactIndex] = -1; + } + contactIndex++; + } + } +} struct AllocContactConstraintsLoop : public btIParallelForBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray; - - AllocContactConstraintsLoop( btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray ) - { - m_solver = solver; - m_cachedInfoArray = cachedInfoArray; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - m_solver->internalAllocContactConstraints( m_cachedInfoArray + iBegin, iEnd - iBegin ); - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray; + AllocContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray) + { + m_solver = solver; + m_cachedInfoArray = cachedInfoArray; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + m_solver->internalAllocContactConstraints(m_cachedInfoArray + iBegin, iEnd - iBegin); + } +}; void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { - BT_PROFILE( "allocAllContactConstraints" ); - btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray; // = m_manifoldCachedInfoArray; - cachedInfoArray.resizeNoInitialize( numManifolds ); - if (/* DISABLES CODE */ (false)) - { - // sequential - internalCollectContactManifoldCachedInfo(&cachedInfoArray[ 0 ], manifoldPtr, numManifolds, infoGlobal); - } - else - { - // may alter ordering of bodies which affects determinism - CollectContactManifoldCachedInfoLoop loop( this, &cachedInfoArray[ 0 ], manifoldPtr, infoGlobal ); - int grainSize = 200; - btParallelFor( 0, numManifolds, grainSize, loop ); - } - - { - // serial part - int numContacts = 0; - int numRollingFrictionConstraints = 0; - for ( int iManifold = 0; iManifold < numManifolds; ++iManifold ) - { - btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[ iManifold ]; - cachedInfo.contactIndex = numContacts; - cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints; - numContacts += cachedInfo.numTouchingContacts; - for (int i = 0; i < cachedInfo.numTouchingContacts; ++i) - { - if (cachedInfo.contactHasRollingFriction[i]) - { - numRollingFrictionConstraints += 3; - } - } - } - { - BT_PROFILE( "allocPools" ); - if ( m_tmpSolverContactConstraintPool.capacity() < numContacts ) - { - // if we need to reallocate, reserve some extra so we don't have to reallocate again next frame - int extraReserve = numContacts / 16; - m_tmpSolverContactConstraintPool.reserve( numContacts + extraReserve ); - m_rollingFrictionIndexTable.reserve( numContacts + extraReserve ); - m_tmpSolverContactFrictionConstraintPool.reserve( ( numContacts + extraReserve )*m_numFrictionDirections ); - m_tmpSolverContactRollingFrictionConstraintPool.reserve( numRollingFrictionConstraints + extraReserve ); - } - m_tmpSolverContactConstraintPool.resizeNoInitialize( numContacts ); - m_rollingFrictionIndexTable.resizeNoInitialize( numContacts ); - m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize( numContacts*m_numFrictionDirections ); - m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize( numRollingFrictionConstraints ); - } - } - { - AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]); - int grainSize = 200; - btParallelFor( 0, numManifolds, grainSize, loop ); - } -} + BT_PROFILE("allocAllContactConstraints"); + btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray; // = m_manifoldCachedInfoArray; + cachedInfoArray.resizeNoInitialize(numManifolds); + if (/* DISABLES CODE */ (false)) + { + // sequential + internalCollectContactManifoldCachedInfo(&cachedInfoArray[0], manifoldPtr, numManifolds, infoGlobal); + } + else + { + // may alter ordering of bodies which affects determinism + CollectContactManifoldCachedInfoLoop loop(this, &cachedInfoArray[0], manifoldPtr, infoGlobal); + int grainSize = 200; + btParallelFor(0, numManifolds, grainSize, loop); + } + { + // serial part + int numContacts = 0; + int numRollingFrictionConstraints = 0; + for (int iManifold = 0; iManifold < numManifolds; ++iManifold) + { + btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold]; + cachedInfo.contactIndex = numContacts; + cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints; + numContacts += cachedInfo.numTouchingContacts; + for (int i = 0; i < cachedInfo.numTouchingContacts; ++i) + { + if (cachedInfo.contactHasRollingFriction[i]) + { + numRollingFrictionConstraints += 3; + } + } + } + { + BT_PROFILE("allocPools"); + if (m_tmpSolverContactConstraintPool.capacity() < numContacts) + { + // if we need to reallocate, reserve some extra so we don't have to reallocate again next frame + int extraReserve = numContacts / 16; + m_tmpSolverContactConstraintPool.reserve(numContacts + extraReserve); + m_rollingFrictionIndexTable.reserve(numContacts + extraReserve); + m_tmpSolverContactFrictionConstraintPool.reserve((numContacts + extraReserve) * m_numFrictionDirections); + m_tmpSolverContactRollingFrictionConstraintPool.reserve(numRollingFrictionConstraints + extraReserve); + } + m_tmpSolverContactConstraintPool.resizeNoInitialize(numContacts); + m_rollingFrictionIndexTable.resizeNoInitialize(numContacts); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(numContacts * m_numFrictionDirections); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(numRollingFrictionConstraints); + } + } + { + AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]); + int grainSize = 200; + btParallelFor(0, numManifolds, grainSize, loop); + } +} void btSequentialImpulseConstraintSolverMt::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal) { - if (!m_useBatching) - { - btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal); - return; - } - BT_PROFILE( "convertContacts" ); - if (numManifolds > 0) - { - if ( m_fixedBodyId < 0 ) - { - m_fixedBodyId = m_tmpSolverBodyPool.size(); - btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); - initSolverBody( &fixedBody, 0, infoGlobal.m_timeStep ); - } - allocAllContactConstraints( manifoldPtr, numManifolds, infoGlobal ); - if ( m_useBatching ) - { - setupBatchedContactConstraints(); - } - setupAllContactConstraints( infoGlobal ); - } + if (!m_useBatching) + { + btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal); + return; + } + BT_PROFILE("convertContacts"); + if (numManifolds > 0) + { + if (m_fixedBodyId < 0) + { + m_fixedBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody, 0, infoGlobal.m_timeStep); + } + allocAllContactConstraints(manifoldPtr, numManifolds, infoGlobal); + if (m_useBatching) + { + setupBatchedContactConstraints(); + } + setupAllContactConstraints(infoGlobal); + } } - -void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints( btTypedConstraint** constraints, int iBegin, int iEnd ) +void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd) { - BT_PROFILE("internalInitMultipleJoints"); - for ( int i = iBegin; i < iEnd; i++ ) + BT_PROFILE("internalInitMultipleJoints"); + for (int i = iBegin; i < iEnd; i++) { btTypedConstraint* constraint = constraints[i]; btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; if (constraint->isEnabled()) - { - constraint->buildJacobian(); - constraint->internalSetAppliedImpulse( 0.0f ); - btJointFeedback* fb = constraint->getJointFeedback(); - if ( fb ) - { - fb->m_appliedForceBodyA.setZero(); - fb->m_appliedTorqueBodyA.setZero(); - fb->m_appliedForceBodyB.setZero(); - fb->m_appliedTorqueBodyB.setZero(); - } - constraint->getInfo1( &info1 ); - } - else + { + constraint->buildJacobian(); + constraint->internalSetAppliedImpulse(0.0f); + btJointFeedback* fb = constraint->getJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA.setZero(); + fb->m_appliedTorqueBodyA.setZero(); + fb->m_appliedForceBodyB.setZero(); + fb->m_appliedTorqueBodyB.setZero(); + } + constraint->getInfo1(&info1); + } + else { info1.m_numConstraintRows = 0; info1.nub = 0; @@ -646,158 +624,151 @@ void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints( btTypedC } } - struct InitJointsLoop : public btIParallelForBody { - btSequentialImpulseConstraintSolverMt* m_solver; - btTypedConstraint** m_constraints; - - InitJointsLoop( btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints ) - { - m_solver = solver; - m_constraints = constraints; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - m_solver->internalInitMultipleJoints( m_constraints, iBegin, iEnd ); - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + btTypedConstraint** m_constraints; + InitJointsLoop(btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints) + { + m_solver = solver; + m_constraints = constraints; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + m_solver->internalInitMultipleJoints(m_constraints, iBegin, iEnd); + } +}; -void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints( const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - BT_PROFILE("internalConvertMultipleJoints"); - for ( int i = iBegin; i < iEnd; ++i ) - { - const JointParams& jointParams = jointParamsArray[ i ]; - int currentRow = jointParams.m_solverConstraint; - if ( currentRow != -1 ) - { - const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[ i ]; - btAssert( currentRow < m_tmpSolverNonContactConstraintPool.size() ); - btAssert( info1.m_numConstraintRows > 0 ); - - btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[ currentRow ]; - btTypedConstraint* constraint = constraints[ i ]; - - convertJoint( currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal ); - } - } -} + BT_PROFILE("internalConvertMultipleJoints"); + for (int i = iBegin; i < iEnd; ++i) + { + const JointParams& jointParams = jointParamsArray[i]; + int currentRow = jointParams.m_solverConstraint; + if (currentRow != -1) + { + const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; + btAssert(currentRow < m_tmpSolverNonContactConstraintPool.size()); + btAssert(info1.m_numConstraintRows > 0); + + btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; + btTypedConstraint* constraint = constraints[i]; + convertJoint(currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal); + } + } +} struct ConvertJointsLoop : public btIParallelForBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray; - btTypedConstraint** m_srcConstraints; - const btContactSolverInfo& m_infoGlobal; - - ConvertJointsLoop( btSequentialImpulseConstraintSolverMt* solver, - const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray, - btTypedConstraint** srcConstraints, - const btContactSolverInfo& infoGlobal - ) : - m_jointParamsArray(jointParamsArray), - m_infoGlobal(infoGlobal) - { - m_solver = solver; - m_srcConstraints = srcConstraints; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - m_solver->internalConvertMultipleJoints( m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal ); - } + btSequentialImpulseConstraintSolverMt* m_solver; + const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray; + btTypedConstraint** m_srcConstraints; + const btContactSolverInfo& m_infoGlobal; + + ConvertJointsLoop(btSequentialImpulseConstraintSolverMt* solver, + const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray, + btTypedConstraint** srcConstraints, + const btContactSolverInfo& infoGlobal) : m_jointParamsArray(jointParamsArray), + m_infoGlobal(infoGlobal) + { + m_solver = solver; + m_srcConstraints = srcConstraints; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + m_solver->internalConvertMultipleJoints(m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal); + } }; - void btSequentialImpulseConstraintSolverMt::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal) { - if ( !m_useBatching ) - { - btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal); - return; - } - BT_PROFILE("convertJoints"); - bool parallelJointSetup = true; + if (!m_useBatching) + { + btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal); + return; + } + BT_PROFILE("convertJoints"); + bool parallelJointSetup = true; m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); - if (parallelJointSetup) - { - InitJointsLoop loop(this, constraints); - int grainSize = 40; - btParallelFor(0, numConstraints, grainSize, loop); - } - else - { - internalInitMultipleJoints( constraints, 0, numConstraints ); - } + if (parallelJointSetup) + { + InitJointsLoop loop(this, constraints); + int grainSize = 40; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + internalInitMultipleJoints(constraints, 0, numConstraints); + } int totalNumRows = 0; - btAlignedObjectArray<JointParams> jointParamsArray; - jointParamsArray.resizeNoInitialize(numConstraints); + btAlignedObjectArray<JointParams> jointParamsArray; + jointParamsArray.resizeNoInitialize(numConstraints); //calculate the total number of contraint rows - for (int i=0;i<numConstraints;i++) + for (int i = 0; i < numConstraints; i++) { - btTypedConstraint* constraint = constraints[ i ]; + btTypedConstraint* constraint = constraints[i]; - JointParams& params = jointParamsArray[ i ]; + JointParams& params = jointParamsArray[i]; const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; if (info1.m_numConstraintRows) { - params.m_solverConstraint = totalNumRows; - params.m_solverBodyA = getOrInitSolverBody( constraint->getRigidBodyA(), infoGlobal.m_timeStep ); - params.m_solverBodyB = getOrInitSolverBody( constraint->getRigidBodyB(), infoGlobal.m_timeStep ); + params.m_solverConstraint = totalNumRows; + params.m_solverBodyA = getOrInitSolverBody(constraint->getRigidBodyA(), infoGlobal.m_timeStep); + params.m_solverBodyB = getOrInitSolverBody(constraint->getRigidBodyB(), infoGlobal.m_timeStep); } - else + else { - params.m_solverConstraint = -1; + params.m_solverConstraint = -1; } totalNumRows += info1.m_numConstraintRows; } m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); ///setup the btSolverConstraints - if ( parallelJointSetup ) - { - ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal); - int grainSize = 20; - btParallelFor(0, numConstraints, grainSize, loop); - } - else - { - internalConvertMultipleJoints( jointParamsArray, constraints, 0, numConstraints, infoGlobal ); - } - setupBatchedJointConstraints(); + if (parallelJointSetup) + { + ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal); + int grainSize = 20; + btParallelFor(0, numConstraints, grainSize, loop); + } + else + { + internalConvertMultipleJoints(jointParamsArray, constraints, 0, numConstraints, infoGlobal); + } + setupBatchedJointConstraints(); } - void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - BT_PROFILE("internalConvertBodies"); - for (int i=iBegin; i < iEnd; i++) + BT_PROFILE("internalConvertBodies"); + for (int i = iBegin; i < iEnd; i++) { - btCollisionObject* obj = bodies[i]; + btCollisionObject* obj = bodies[i]; obj->setCompanionId(i); btSolverBody& solverBody = m_tmpSolverBodyPool[i]; - initSolverBody(&solverBody, obj, infoGlobal.m_timeStep); + initSolverBody(&solverBody, obj, infoGlobal.m_timeStep); btRigidBody* body = btRigidBody::upcast(obj); if (body && body->getInvMass()) { - btVector3 gyroForce (0,0,0); - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) + btVector3 gyroForce(0, 0, 0); + if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT) { gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce); - solverBody.m_externalTorqueImpulse -= gyroForce*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; + solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep; } - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) + if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD) { gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep); solverBody.m_externalTorqueImpulse += gyroForce; } - if (body->getFlags()&BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) + if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY) { gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep); solverBody.m_externalTorqueImpulse += gyroForce; @@ -806,809 +777,772 @@ void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObj } } - struct ConvertBodiesLoop : public btIParallelForBody { - btSequentialImpulseConstraintSolverMt* m_solver; - btCollisionObject** m_bodies; - int m_numBodies; - const btContactSolverInfo& m_infoGlobal; - - ConvertBodiesLoop( btSequentialImpulseConstraintSolverMt* solver, - btCollisionObject** bodies, - int numBodies, - const btContactSolverInfo& infoGlobal - ) : - m_infoGlobal(infoGlobal) - { - m_solver = solver; - m_bodies = bodies; - m_numBodies = numBodies; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - m_solver->internalConvertBodies( m_bodies, iBegin, iEnd, m_infoGlobal ); - } + btSequentialImpulseConstraintSolverMt* m_solver; + btCollisionObject** m_bodies; + int m_numBodies; + const btContactSolverInfo& m_infoGlobal; + + ConvertBodiesLoop(btSequentialImpulseConstraintSolverMt* solver, + btCollisionObject** bodies, + int numBodies, + const btContactSolverInfo& infoGlobal) : m_infoGlobal(infoGlobal) + { + m_solver = solver; + m_bodies = bodies; + m_numBodies = numBodies; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + m_solver->internalConvertBodies(m_bodies, iBegin, iEnd, m_infoGlobal); + } }; - void btSequentialImpulseConstraintSolverMt::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { - BT_PROFILE("convertBodies"); - m_kinematicBodyUniqueIdToSolverBodyTable.resize( 0 ); - - m_tmpSolverBodyPool.resizeNoInitialize(numBodies+1); - - m_fixedBodyId = numBodies; - { - btSolverBody& fixedBody = m_tmpSolverBodyPool[ m_fixedBodyId ]; - initSolverBody( &fixedBody, NULL, infoGlobal.m_timeStep ); - } - - bool parallelBodySetup = true; - if (parallelBodySetup) - { - ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal); - int grainSize = 40; - btParallelFor(0, numBodies, grainSize, loop); - } - else - { - internalConvertBodies( bodies, 0, numBodies, infoGlobal ); - } -} + BT_PROFILE("convertBodies"); + m_kinematicBodyUniqueIdToSolverBodyTable.resize(0); + m_tmpSolverBodyPool.resizeNoInitialize(numBodies + 1); + + m_fixedBodyId = numBodies; + { + btSolverBody& fixedBody = m_tmpSolverBodyPool[m_fixedBodyId]; + initSolverBody(&fixedBody, NULL, infoGlobal.m_timeStep); + } + + bool parallelBodySetup = true; + if (parallelBodySetup) + { + ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal); + int grainSize = 40; + btParallelFor(0, numBodies, grainSize, loop); + } + else + { + internalConvertBodies(bodies, 0, numBodies, infoGlobal); + } +} btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySetup( - btCollisionObject** bodies, - int numBodies, - btPersistentManifold** manifoldPtr, - int numManifolds, - btTypedConstraint** constraints, - int numConstraints, - const btContactSolverInfo& infoGlobal, - btIDebugDraw* debugDrawer - ) + btCollisionObject** bodies, + int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& infoGlobal, + btIDebugDraw* debugDrawer) { - m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; - m_useBatching = false; - if ( numManifolds >= s_minimumContactManifoldsForBatching && - (s_allowNestedParallelForLoops || !btThreadsAreRunning()) - ) - { - m_useBatching = true; - m_batchedContactConstraints.m_debugDrawer = debugDrawer; - m_batchedJointConstraints.m_debugDrawer = debugDrawer; - } - btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies, - numBodies, - manifoldPtr, - numManifolds, - constraints, - numConstraints, - infoGlobal, - debugDrawer - ); - return 0.0f; + m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1; + m_useBatching = false; + if (numManifolds >= s_minimumContactManifoldsForBatching && + (s_allowNestedParallelForLoops || !btThreadsAreRunning())) + { + m_useBatching = true; + m_batchedContactConstraints.m_debugDrawer = debugDrawer; + m_batchedJointConstraints.m_debugDrawer = debugDrawer; + } + btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, + numBodies, + manifoldPtr, + numManifolds, + constraints, + numConstraints, + infoGlobal, + debugDrawer); + return 0.0f; } - -btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ) +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd) { - btScalar leastSquaresResidual = 0.f; - for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) - { - int iCons = consIndices[ iiCons ]; - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ]; - btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; - btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; - btScalar residual = resolveSplitPenetrationImpulse( bodyA, bodyB, solveManifold ); - leastSquaresResidual += residual*residual; - } - return leastSquaresResidual; + btScalar leastSquaresResidual = 0.f; + for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons) + { + int iCons = consIndices[iiCons]; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons]; + btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA]; + btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB]; + btScalar residual = resolveSplitPenetrationImpulse(bodyA, bodyB, solveManifold); + leastSquaresResidual += residual * residual; + } + return leastSquaresResidual; } - struct ContactSplitPenetrationImpulseSolverLoop : public btIParallelSumBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btBatchedConstraints* m_bc; - - ContactSplitPenetrationImpulseSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) - { - m_solver = solver; - m_bc = bc; - } - btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - BT_PROFILE( "ContactSplitPenetrationImpulseSolverLoop" ); - btScalar sum = 0; - for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) - { - const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; - sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); - } - return sum; - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + ContactSplitPenetrationImpulseSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + BT_PROFILE("ContactSplitPenetrationImpulseSolverLoop"); + btScalar sum = 0; + for (int iBatch = iBegin; iBatch < iEnd; ++iBatch) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch]; + sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints(m_bc->m_constraintIndices, batch.begin, batch.end); + } + return sum; + } +}; -void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations"); if (infoGlobal.m_splitImpulse) { - for ( int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++ ) - { - btScalar leastSquaresResidual = 0.f; - if (m_useBatching) - { - const btBatchedConstraints& batchedCons = m_batchedContactConstraints; - ContactSplitPenetrationImpulseSolverLoop loop( this, &batchedCons ); - btScalar leastSquaresResidual = 0.f; - for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) - { - int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; - const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; - int grainSize = batchedCons.m_phaseGrainSize[iPhase]; - leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); - } - } - else - { - // non-batched - leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size()); - } - if ( leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= ( infoGlobal.m_numIterations - 1 ) ) - { + for (int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) + { + btScalar leastSquaresResidual = 0.f; + if (m_useBatching) + { + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactSplitPenetrationImpulseSolverLoop loop(this, &batchedCons); + btScalar leastSquaresResidual = 0.f; + for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase) + { + int iPhase = batchedCons.m_phaseOrder[iiPhase]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase]; + int grainSize = batchedCons.m_phaseGrainSize[iPhase]; + leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop); + } + } + else + { + // non-batched + leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size()); + } + if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) + { #ifdef VERBOSE_RESIDUAL_PRINTF - printf( "residual = %f at iteration #%d\n", leastSquaresResidual, iteration ); + printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration); #endif - break; - } - } + break; + } + } } } - -btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) +btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer) { - if ( !m_useBatching ) - { - return btSequentialImpulseConstraintSolver::solveSingleIteration( iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer ); - } - BT_PROFILE( "solveSingleIterationMt" ); - btScalar leastSquaresResidual = 0.f; + if (!m_useBatching) + { + return btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); + } + BT_PROFILE("solveSingleIterationMt"); + btScalar leastSquaresResidual = 0.f; if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { - if (1) // uncomment this for a bit less random ((iteration & 7) == 0) + if (1) // uncomment this for a bit less random ((iteration & 7) == 0) { - randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations); + randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations); } } { ///solve all joint constraints - leastSquaresResidual += resolveAllJointConstraints(iteration); + leastSquaresResidual += resolveAllJointConstraints(iteration); - if (iteration< infoGlobal.m_numIterations) + if (iteration < infoGlobal.m_numIterations) { - // this loop is only used for cone-twist constraints, - // it would be nice to skip this loop if none of the constraints need it - if ( m_useObsoleteJointConstraints ) - { - for ( int j = 0; j<numConstraints; j++ ) - { - if ( constraints[ j ]->isEnabled() ) - { - int bodyAid = getOrInitSolverBody( constraints[ j ]->getRigidBodyA(), infoGlobal.m_timeStep ); - int bodyBid = getOrInitSolverBody( constraints[ j ]->getRigidBodyB(), infoGlobal.m_timeStep ); - btSolverBody& bodyA = m_tmpSolverBodyPool[ bodyAid ]; - btSolverBody& bodyB = m_tmpSolverBodyPool[ bodyBid ]; - constraints[ j ]->solveConstraintObsolete( bodyA, bodyB, infoGlobal.m_timeStep ); - } - } - } + // this loop is only used for cone-twist constraints, + // it would be nice to skip this loop if none of the constraints need it + if (m_useObsoleteJointConstraints) + { + for (int j = 0; j < numConstraints; j++) + { + if (constraints[j]->isEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep); + } + } + } if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) { - // solve all contact, contact-friction, and rolling friction constraints interleaved - leastSquaresResidual += resolveAllContactConstraintsInterleaved(); + // solve all contact, contact-friction, and rolling friction constraints interleaved + leastSquaresResidual += resolveAllContactConstraintsInterleaved(); } - else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS + else //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS { - // don't interleave them + // don't interleave them // solve all contact constraints - leastSquaresResidual += resolveAllContactConstraints(); + leastSquaresResidual += resolveAllContactConstraints(); // solve all contact friction constraints - leastSquaresResidual += resolveAllContactFrictionConstraints(); + leastSquaresResidual += resolveAllContactFrictionConstraints(); - // solve all rolling friction constraints - leastSquaresResidual += resolveAllRollingFrictionConstraints(); + // solve all rolling friction constraints + leastSquaresResidual += resolveAllRollingFrictionConstraints(); } } } - return leastSquaresResidual; + return leastSquaresResidual; } - -btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration ) +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration) { - btScalar leastSquaresResidual = 0.f; - for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) - { - int iCons = consIndices[ iiCons ]; - const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[ iCons ]; - if ( iteration < constraint.m_overrideNumSolverIterations ) - { - btSolverBody& bodyA = m_tmpSolverBodyPool[ constraint.m_solverBodyIdA ]; - btSolverBody& bodyB = m_tmpSolverBodyPool[ constraint.m_solverBodyIdB ]; - btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, constraint ); - leastSquaresResidual += residual*residual; - } - } - return leastSquaresResidual; + btScalar leastSquaresResidual = 0.f; + for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons) + { + int iCons = consIndices[iiCons]; + const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[iCons]; + if (iteration < constraint.m_overrideNumSolverIterations) + { + btSolverBody& bodyA = m_tmpSolverBodyPool[constraint.m_solverBodyIdA]; + btSolverBody& bodyB = m_tmpSolverBodyPool[constraint.m_solverBodyIdB]; + btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, constraint); + leastSquaresResidual += residual * residual; + } + } + return leastSquaresResidual; } - -btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ) +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd) { - btScalar leastSquaresResidual = 0.f; - for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) - { - int iCons = consIndices[ iiCons ]; - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iCons ]; - btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; - btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; - btScalar residual = resolveSingleConstraintRowLowerLimit( bodyA, bodyB, solveManifold ); - leastSquaresResidual += residual*residual; - } - return leastSquaresResidual; + btScalar leastSquaresResidual = 0.f; + for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons) + { + int iCons = consIndices[iiCons]; + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons]; + btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA]; + btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB]; + btScalar residual = resolveSingleConstraintRowLowerLimit(bodyA, bodyB, solveManifold); + leastSquaresResidual += residual * residual; + } + return leastSquaresResidual; } - -btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ) +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd) { - btScalar leastSquaresResidual = 0.f; - for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) - { - int iContact = consIndices[ iiCons ]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse; - - // apply sliding friction - if ( totalImpulse > 0.0f ) - { - int iBegin = iContact * m_numFrictionDirections; - int iEnd = iBegin + m_numFrictionDirections; - for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction ) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction++ ]; - btAssert( solveManifold.m_frictionIndex == iContact ); - - solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse ); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; - btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; - btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold ); - leastSquaresResidual += residual*residual; - } - } - } - return leastSquaresResidual; -} + btScalar leastSquaresResidual = 0.f; + for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons) + { + int iContact = consIndices[iiCons]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse; + // apply sliding friction + if (totalImpulse > 0.0f) + { + int iBegin = iContact * m_numFrictionDirections; + int iEnd = iBegin + m_numFrictionDirections; + for (int iFriction = iBegin; iFriction < iEnd; ++iFriction) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction++]; + btAssert(solveManifold.m_frictionIndex == iContact); -btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints( const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd ) -{ - btScalar leastSquaresResidual = 0.f; - for ( int iiCons = batchBegin; iiCons < batchEnd; ++iiCons ) - { - int iContact = consIndices[ iiCons ]; - int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ]; - if ( iFirstRollingFriction >= 0 ) - { - btScalar totalImpulse = m_tmpSolverContactConstraintPool[ iContact ].m_appliedImpulse; - // apply rolling friction - if ( totalImpulse > 0.0f ) - { - int iBegin = iFirstRollingFriction; - int iEnd = iBegin + 3; - for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric ) - { - btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ]; - if ( rollingFrictionConstraint.m_frictionIndex != iContact ) - { - break; - } - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) - { - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - } - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint ); - leastSquaresResidual += residual*residual; - } - } - } - } - return leastSquaresResidual; -} + solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; + btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA]; + btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB]; + btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold); + leastSquaresResidual += residual * residual; + } + } + } + return leastSquaresResidual; +} -btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved( const btAlignedObjectArray<int>& contactIndices, - int batchBegin, - int batchEnd - ) +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd) { - btScalar leastSquaresResidual = 0.f; - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - - for ( int iiCons = batchBegin; iiCons < batchEnd; iiCons++ ) - { - btScalar totalImpulse = 0; - int iContact = contactIndices[ iiCons ]; - // apply penetration constraint - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[ iContact ]; - btScalar residual = resolveSingleConstraintRowLowerLimit( m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ], m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ], solveManifold ); - leastSquaresResidual += residual*residual; - totalImpulse = solveManifold.m_appliedImpulse; - } - - // apply sliding friction - if ( totalImpulse > 0.0f ) - { - int iBegin = iContact * m_numFrictionDirections; - int iEnd = iBegin + m_numFrictionDirections; - for ( int iFriction = iBegin; iFriction < iEnd; ++iFriction ) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[ iFriction ]; - btAssert( solveManifold.m_frictionIndex == iContact ); - - solveManifold.m_lowerLimit = -( solveManifold.m_friction*totalImpulse ); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - - btSolverBody& bodyA = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdA ]; - btSolverBody& bodyB = m_tmpSolverBodyPool[ solveManifold.m_solverBodyIdB ]; - btScalar residual = resolveSingleConstraintRowGeneric( bodyA, bodyB, solveManifold ); - leastSquaresResidual += residual*residual; - } - } - - // apply rolling friction - int iFirstRollingFriction = m_rollingFrictionIndexTable[ iContact ]; - if ( totalImpulse > 0.0f && iFirstRollingFriction >= 0) - { - int iBegin = iFirstRollingFriction; - int iEnd = iBegin + 3; - for ( int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric ) - { - btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ iRollingFric ]; - if ( rollingFrictionConstraint.m_frictionIndex != iContact ) - { - break; - } - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) - { - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - } - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint ); - leastSquaresResidual += residual*residual; - } - } - } - return leastSquaresResidual; + btScalar leastSquaresResidual = 0.f; + for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons) + { + int iContact = consIndices[iiCons]; + int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact]; + if (iFirstRollingFriction >= 0) + { + btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse; + // apply rolling friction + if (totalImpulse > 0.0f) + { + int iBegin = iFirstRollingFriction; + int iEnd = iBegin + 3; + for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric]; + if (rollingFrictionConstraint.m_frictionIndex != iContact) + { + break; + } + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; + if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction) + { + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + } + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); + leastSquaresResidual += residual * residual; + } + } + } + } + return leastSquaresResidual; } - -void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering( btBatchedConstraints* batchedConstraints ) +btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices, + int batchBegin, + int batchEnd) { - btBatchedConstraints& bc = *batchedConstraints; - // randomize ordering of phases - for ( int ii = 1; ii < bc.m_phaseOrder.size(); ++ii ) - { - int iSwap = btRandInt2( ii + 1 ); - bc.m_phaseOrder.swap( ii, iSwap ); - } - - // for each batch, - for ( int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch ) - { - // randomize ordering of constraints within the batch - const btBatchedConstraints::Range& batch = bc.m_batches[ iBatch ]; - for ( int iiCons = batch.begin; iiCons < batch.end; ++iiCons ) - { - int iSwap = batch.begin + btRandInt2( iiCons - batch.begin + 1 ); - btAssert(iSwap >= batch.begin && iSwap < batch.end); - bc.m_constraintIndices.swap( iiCons, iSwap ); - } - } + btScalar leastSquaresResidual = 0.f; + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + + for (int iiCons = batchBegin; iiCons < batchEnd; iiCons++) + { + btScalar totalImpulse = 0; + int iContact = contactIndices[iiCons]; + // apply penetration constraint + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iContact]; + btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold); + leastSquaresResidual += residual * residual; + totalImpulse = solveManifold.m_appliedImpulse; + } + + // apply sliding friction + if (totalImpulse > 0.0f) + { + int iBegin = iContact * m_numFrictionDirections; + int iEnd = iBegin + m_numFrictionDirections; + for (int iFriction = iBegin; iFriction < iEnd; ++iFriction) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction]; + btAssert(solveManifold.m_frictionIndex == iContact); + + solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse; + + btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA]; + btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB]; + btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold); + leastSquaresResidual += residual * residual; + } + } + + // apply rolling friction + int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact]; + if (totalImpulse > 0.0f && iFirstRollingFriction >= 0) + { + int iBegin = iFirstRollingFriction; + int iEnd = iBegin + 3; + for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric]; + if (rollingFrictionConstraint.m_frictionIndex != iContact) + { + break; + } + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; + if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction) + { + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + } + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); + leastSquaresResidual += residual * residual; + } + } + } + return leastSquaresResidual; } +void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering(btBatchedConstraints* batchedConstraints) +{ + btBatchedConstraints& bc = *batchedConstraints; + // randomize ordering of phases + for (int ii = 1; ii < bc.m_phaseOrder.size(); ++ii) + { + int iSwap = btRandInt2(ii + 1); + bc.m_phaseOrder.swap(ii, iSwap); + } + + // for each batch, + for (int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch) + { + // randomize ordering of constraints within the batch + const btBatchedConstraints::Range& batch = bc.m_batches[iBatch]; + for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons) + { + int iSwap = batch.begin + btRandInt2(iiCons - batch.begin + 1); + btAssert(iSwap >= batch.begin && iSwap < batch.end); + bc.m_constraintIndices.swap(iiCons, iSwap); + } + } +} void btSequentialImpulseConstraintSolverMt::randomizeConstraintOrdering(int iteration, int numIterations) { - // randomize ordering of joint constraints - randomizeBatchedConstraintOrdering( &m_batchedJointConstraints ); - - //contact/friction constraints are not solved more than numIterations - if ( iteration < numIterations ) - { - randomizeBatchedConstraintOrdering( &m_batchedContactConstraints ); - } -} + // randomize ordering of joint constraints + randomizeBatchedConstraintOrdering(&m_batchedJointConstraints); + //contact/friction constraints are not solved more than numIterations + if (iteration < numIterations) + { + randomizeBatchedConstraintOrdering(&m_batchedContactConstraints); + } +} struct JointSolverLoop : public btIParallelSumBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btBatchedConstraints* m_bc; - int m_iteration; - - JointSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration ) - { - m_solver = solver; - m_bc = bc; - m_iteration = iteration; - } - btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - BT_PROFILE( "JointSolverLoop" ); - btScalar sum = 0; - for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) - { - const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; - sum += m_solver->resolveMultipleJointConstraints( m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration ); - } - return sum; - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + int m_iteration; + JointSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration) + { + m_solver = solver; + m_bc = bc; + m_iteration = iteration; + } + btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + BT_PROFILE("JointSolverLoop"); + btScalar sum = 0; + for (int iBatch = iBegin; iBatch < iEnd; ++iBatch) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch]; + sum += m_solver->resolveMultipleJointConstraints(m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration); + } + return sum; + } +}; btScalar btSequentialImpulseConstraintSolverMt::resolveAllJointConstraints(int iteration) { - BT_PROFILE( "resolveAllJointConstraints" ); - const btBatchedConstraints& batchedCons = m_batchedJointConstraints; - JointSolverLoop loop( this, &batchedCons, iteration ); - btScalar leastSquaresResidual = 0.f; - for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) - { - int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; - const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; - int grainSize = 1; - leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); - } - return leastSquaresResidual; + BT_PROFILE("resolveAllJointConstraints"); + const btBatchedConstraints& batchedCons = m_batchedJointConstraints; + JointSolverLoop loop(this, &batchedCons, iteration); + btScalar leastSquaresResidual = 0.f; + for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase) + { + int iPhase = batchedCons.m_phaseOrder[iiPhase]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase]; + int grainSize = 1; + leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop); + } + return leastSquaresResidual; } - struct ContactSolverLoop : public btIParallelSumBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btBatchedConstraints* m_bc; - - ContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) - { - m_solver = solver; - m_bc = bc; - } - btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - BT_PROFILE( "ContactSolverLoop" ); - btScalar sum = 0; - for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) - { - const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; - sum += m_solver->resolveMultipleContactConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); - } - return sum; - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + ContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + BT_PROFILE("ContactSolverLoop"); + btScalar sum = 0; + for (int iBatch = iBegin; iBatch < iEnd; ++iBatch) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch]; + sum += m_solver->resolveMultipleContactConstraints(m_bc->m_constraintIndices, batch.begin, batch.end); + } + return sum; + } +}; btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraints() { - BT_PROFILE( "resolveAllContactConstraints" ); - const btBatchedConstraints& batchedCons = m_batchedContactConstraints; - ContactSolverLoop loop( this, &batchedCons ); - btScalar leastSquaresResidual = 0.f; - for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) - { - int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; - const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; - int grainSize = batchedCons.m_phaseGrainSize[iPhase]; - leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); - } - return leastSquaresResidual; + BT_PROFILE("resolveAllContactConstraints"); + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactSolverLoop loop(this, &batchedCons); + btScalar leastSquaresResidual = 0.f; + for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase) + { + int iPhase = batchedCons.m_phaseOrder[iiPhase]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase]; + int grainSize = batchedCons.m_phaseGrainSize[iPhase]; + leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop); + } + return leastSquaresResidual; } - struct ContactFrictionSolverLoop : public btIParallelSumBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btBatchedConstraints* m_bc; - - ContactFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) - { - m_solver = solver; - m_bc = bc; - } - btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - BT_PROFILE( "ContactFrictionSolverLoop" ); - btScalar sum = 0; - for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) - { - const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; - sum += m_solver->resolveMultipleContactFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); - } - return sum; - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + ContactFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + BT_PROFILE("ContactFrictionSolverLoop"); + btScalar sum = 0; + for (int iBatch = iBegin; iBatch < iEnd; ++iBatch) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch]; + sum += m_solver->resolveMultipleContactFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end); + } + return sum; + } +}; btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactFrictionConstraints() { - BT_PROFILE( "resolveAllContactFrictionConstraints" ); - const btBatchedConstraints& batchedCons = m_batchedContactConstraints; - ContactFrictionSolverLoop loop( this, &batchedCons ); - btScalar leastSquaresResidual = 0.f; - for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) - { - int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; - const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; - int grainSize = batchedCons.m_phaseGrainSize[iPhase]; - leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); - } - return leastSquaresResidual; + BT_PROFILE("resolveAllContactFrictionConstraints"); + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactFrictionSolverLoop loop(this, &batchedCons); + btScalar leastSquaresResidual = 0.f; + for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase) + { + int iPhase = batchedCons.m_phaseOrder[iiPhase]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase]; + int grainSize = batchedCons.m_phaseGrainSize[iPhase]; + leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop); + } + return leastSquaresResidual; } - struct InterleavedContactSolverLoop : public btIParallelSumBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btBatchedConstraints* m_bc; - - InterleavedContactSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) - { - m_solver = solver; - m_bc = bc; - } - btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - BT_PROFILE( "InterleavedContactSolverLoop" ); - btScalar sum = 0; - for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) - { - const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; - sum += m_solver->resolveMultipleContactConstraintsInterleaved( m_bc->m_constraintIndices, batch.begin, batch.end ); - } - return sum; - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + InterleavedContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + BT_PROFILE("InterleavedContactSolverLoop"); + btScalar sum = 0; + for (int iBatch = iBegin; iBatch < iEnd; ++iBatch) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch]; + sum += m_solver->resolveMultipleContactConstraintsInterleaved(m_bc->m_constraintIndices, batch.begin, batch.end); + } + return sum; + } +}; btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraintsInterleaved() { - BT_PROFILE( "resolveAllContactConstraintsInterleaved" ); - const btBatchedConstraints& batchedCons = m_batchedContactConstraints; - InterleavedContactSolverLoop loop( this, &batchedCons ); - btScalar leastSquaresResidual = 0.f; - for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) - { - int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; - const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; - int grainSize = 1; - leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); - } - return leastSquaresResidual; + BT_PROFILE("resolveAllContactConstraintsInterleaved"); + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + InterleavedContactSolverLoop loop(this, &batchedCons); + btScalar leastSquaresResidual = 0.f; + for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase) + { + int iPhase = batchedCons.m_phaseOrder[iiPhase]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase]; + int grainSize = 1; + leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop); + } + return leastSquaresResidual; } - struct ContactRollingFrictionSolverLoop : public btIParallelSumBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btBatchedConstraints* m_bc; - - ContactRollingFrictionSolverLoop( btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc ) - { - m_solver = solver; - m_bc = bc; - } - btScalar sumLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - BT_PROFILE( "ContactFrictionSolverLoop" ); - btScalar sum = 0; - for ( int iBatch = iBegin; iBatch < iEnd; ++iBatch ) - { - const btBatchedConstraints::Range& batch = m_bc->m_batches[ iBatch ]; - sum += m_solver->resolveMultipleContactRollingFrictionConstraints( m_bc->m_constraintIndices, batch.begin, batch.end ); - } - return sum; - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btBatchedConstraints* m_bc; + ContactRollingFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc) + { + m_solver = solver; + m_bc = bc; + } + btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + BT_PROFILE("ContactFrictionSolverLoop"); + btScalar sum = 0; + for (int iBatch = iBegin; iBatch < iEnd; ++iBatch) + { + const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch]; + sum += m_solver->resolveMultipleContactRollingFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end); + } + return sum; + } +}; btScalar btSequentialImpulseConstraintSolverMt::resolveAllRollingFrictionConstraints() { - BT_PROFILE( "resolveAllRollingFrictionConstraints" ); - btScalar leastSquaresResidual = 0.f; - // - // We do not generate batches for rolling friction constraints. We assume that - // one of two cases is true: - // - // 1. either most bodies in the simulation have rolling friction, in which case we can use the - // batches for contacts and use a lookup table to translate contact indices to rolling friction - // (ignoring any contact indices that don't map to a rolling friction constraint). As long as - // most contacts have a corresponding rolling friction constraint, this should parallelize well. - // - // -OR- - // - // 2. few bodies in the simulation have rolling friction, so it is not worth trying to use the - // batches from contacts as most of the contacts won't have corresponding rolling friction - // constraints and most threads would end up doing very little work. Most of the time would - // go to threading overhead, so we don't bother with threading. - // - int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); - if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size()) - { - // use batching if there are many rolling friction constraints - const btBatchedConstraints& batchedCons = m_batchedContactConstraints; - ContactRollingFrictionSolverLoop loop( this, &batchedCons ); - btScalar leastSquaresResidual = 0.f; - for ( int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase ) - { - int iPhase = batchedCons.m_phaseOrder[ iiPhase ]; - const btBatchedConstraints::Range& phase = batchedCons.m_phases[ iPhase ]; - int grainSize = 1; - leastSquaresResidual += btParallelSum( phase.begin, phase.end, grainSize, loop ); - } - } - else - { - // no batching, also ignores SOLVER_RANDMIZE_ORDER - for ( int j = 0; j < numRollingFrictionPoolConstraints; j++ ) - { - btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[ j ]; - if ( rollingFrictionConstraint.m_frictionIndex >= 0 ) - { - btScalar totalImpulse = m_tmpSolverContactConstraintPool[ rollingFrictionConstraint.m_frictionIndex ].m_appliedImpulse; - if ( totalImpulse > 0.0f ) - { - btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; - if ( rollingFrictionMagnitude > rollingFrictionConstraint.m_friction ) - rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; - - rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; - rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; - - btScalar residual = resolveSingleConstraintRowGeneric( m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdA ], m_tmpSolverBodyPool[ rollingFrictionConstraint.m_solverBodyIdB ], rollingFrictionConstraint ); - leastSquaresResidual += residual*residual; - } - } - } - } - return leastSquaresResidual; + BT_PROFILE("resolveAllRollingFrictionConstraints"); + btScalar leastSquaresResidual = 0.f; + // + // We do not generate batches for rolling friction constraints. We assume that + // one of two cases is true: + // + // 1. either most bodies in the simulation have rolling friction, in which case we can use the + // batches for contacts and use a lookup table to translate contact indices to rolling friction + // (ignoring any contact indices that don't map to a rolling friction constraint). As long as + // most contacts have a corresponding rolling friction constraint, this should parallelize well. + // + // -OR- + // + // 2. few bodies in the simulation have rolling friction, so it is not worth trying to use the + // batches from contacts as most of the contacts won't have corresponding rolling friction + // constraints and most threads would end up doing very little work. Most of the time would + // go to threading overhead, so we don't bother with threading. + // + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size()) + { + // use batching if there are many rolling friction constraints + const btBatchedConstraints& batchedCons = m_batchedContactConstraints; + ContactRollingFrictionSolverLoop loop(this, &batchedCons); + btScalar leastSquaresResidual = 0.f; + for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase) + { + int iPhase = batchedCons.m_phaseOrder[iiPhase]; + const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase]; + int grainSize = 1; + leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop); + } + } + else + { + // no batching, also ignores SOLVER_RANDMIZE_ORDER + for (int j = 0; j < numRollingFrictionPoolConstraints; j++) + { + btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j]; + if (rollingFrictionConstraint.m_frictionIndex >= 0) + { + btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse; + if (totalImpulse > 0.0f) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse; + if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint); + leastSquaresResidual += residual * residual; + } + } + } + } + return leastSquaresResidual; } - -void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { - BT_PROFILE("internalWriteBackContacts"); - writeBackContacts(iBegin, iEnd, infoGlobal); - //for ( int iContact = iBegin; iContact < iEnd; ++iContact) - //{ - // const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ]; - // btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint; - // btAssert( pt ); - // pt->m_appliedImpulse = contactConstraint.m_appliedImpulse; - // pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse; - // if ( m_numFrictionDirections == 2 ) - // { - // pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse; - // } - //} + BT_PROFILE("internalWriteBackContacts"); + writeBackContacts(iBegin, iEnd, infoGlobal); + //for ( int iContact = iBegin; iContact < iEnd; ++iContact) + //{ + // const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ]; + // btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint; + // btAssert( pt ); + // pt->m_appliedImpulse = contactConstraint.m_appliedImpulse; + // pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse; + // if ( m_numFrictionDirections == 2 ) + // { + // pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse; + // } + //} } - -void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { BT_PROFILE("internalWriteBackJoints"); - writeBackJoints(iBegin, iEnd, infoGlobal); + writeBackJoints(iBegin, iEnd, infoGlobal); } - -void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies( int iBegin, int iEnd, const btContactSolverInfo& infoGlobal ) +void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal) { BT_PROFILE("internalWriteBackBodies"); - writeBackBodies( iBegin, iEnd, infoGlobal ); + writeBackBodies(iBegin, iEnd, infoGlobal); } - struct WriteContactPointsLoop : public btIParallelForBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btContactSolverInfo* m_infoGlobal; - - WriteContactPointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal ) - { - m_solver = solver; - m_infoGlobal = &infoGlobal; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - m_solver->internalWriteBackContacts( iBegin, iEnd, *m_infoGlobal ); - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btContactSolverInfo* m_infoGlobal; + WriteContactPointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal) + { + m_solver = solver; + m_infoGlobal = &infoGlobal; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + m_solver->internalWriteBackContacts(iBegin, iEnd, *m_infoGlobal); + } +}; struct WriteJointsLoop : public btIParallelForBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btContactSolverInfo* m_infoGlobal; - - WriteJointsLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal ) - { - m_solver = solver; - m_infoGlobal = &infoGlobal; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - m_solver->internalWriteBackJoints( iBegin, iEnd, *m_infoGlobal ); - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btContactSolverInfo* m_infoGlobal; + WriteJointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal) + { + m_solver = solver; + m_infoGlobal = &infoGlobal; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + m_solver->internalWriteBackJoints(iBegin, iEnd, *m_infoGlobal); + } +}; struct WriteBodiesLoop : public btIParallelForBody { - btSequentialImpulseConstraintSolverMt* m_solver; - const btContactSolverInfo* m_infoGlobal; - - WriteBodiesLoop( btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal ) - { - m_solver = solver; - m_infoGlobal = &infoGlobal; - } - void forLoop( int iBegin, int iEnd ) const BT_OVERRIDE - { - m_solver->internalWriteBackBodies( iBegin, iEnd, *m_infoGlobal ); - } -}; + btSequentialImpulseConstraintSolverMt* m_solver; + const btContactSolverInfo* m_infoGlobal; + WriteBodiesLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal) + { + m_solver = solver; + m_infoGlobal = &infoGlobal; + } + void forLoop(int iBegin, int iEnd) const BT_OVERRIDE + { + m_solver->internalWriteBackBodies(iBegin, iEnd, *m_infoGlobal); + } +}; btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveGroupCacheFriendlyFinish"); if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - WriteContactPointsLoop loop( this, infoGlobal ); - int grainSize = 500; - btParallelFor( 0, m_tmpSolverContactConstraintPool.size(), grainSize, loop ); - } - - { - WriteJointsLoop loop( this, infoGlobal ); - int grainSize = 400; - btParallelFor( 0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop ); - } - { - WriteBodiesLoop loop( this, infoGlobal ); - int grainSize = 100; - btParallelFor( 0, m_tmpSolverBodyPool.size(), grainSize, loop ); - } + { + WriteContactPointsLoop loop(this, infoGlobal); + int grainSize = 500; + btParallelFor(0, m_tmpSolverContactConstraintPool.size(), grainSize, loop); + } + + { + WriteJointsLoop loop(this, infoGlobal); + int grainSize = 400; + btParallelFor(0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop); + } + { + WriteBodiesLoop loop(this, infoGlobal); + int grainSize = 100; + btParallelFor(0, m_tmpSolverBodyPool.size(), grainSize, loop); + } m_tmpSolverContactConstraintPool.resizeNoInitialize(0); m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); @@ -1618,4 +1552,3 @@ btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(bt m_tmpSolverBodyPool.resizeNoInitialize(0); return 0.f; } - |