dali-physics/third-party/bullet3/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btSequentialImpulseConstraintSolverMt.h"
  17
  18 #include "LinearMath/btQuickprof.h"
  19
  20 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
  21
  22 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
  23 #include "BulletDynamics/Dynamics/btRigidBody.h"
  24
  25 bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false;  // some task schedulers don't like nested loops
  26 int btSequentialImpulseConstraintSolverMt::s_minimumContactManifoldsForBatching = 250;
  27 int btSequentialImpulseConstraintSolverMt::s_minBatchSize = 50;
  28 int btSequentialImpulseConstraintSolverMt::s_maxBatchSize = 100;
  29 btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_contactBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
  30 btBatchedConstraints::BatchingMethod btSequentialImpulseConstraintSolverMt::s_jointBatchingMethod = btBatchedConstraints::BATCHING_METHOD_SPATIAL_GRID_2D;
  31
  32 btSequentialImpulseConstraintSolverMt::btSequentialImpulseConstraintSolverMt()
  33 {
  34         m_numFrictionDirections = 1;
  35         m_useBatching = false;
  36         m_useObsoleteJointConstraints = false;
  37 }
  38
  39 btSequentialImpulseConstraintSolverMt::~btSequentialImpulseConstraintSolverMt()
  40 {
  41 }
  42
  43 void btSequentialImpulseConstraintSolverMt::setupBatchedContactConstraints()
  44 {
  45         BT_PROFILE("setupBatchedContactConstraints");
  46         m_batchedContactConstraints.setup(&m_tmpSolverContactConstraintPool,
  47                                                                           m_tmpSolverBodyPool,
  48                                                                           s_contactBatchingMethod,
  49                                                                           s_minBatchSize,
  50                                                                           s_maxBatchSize,
  51                                                                           &m_scratchMemory);
  52 }
  53
  54 void btSequentialImpulseConstraintSolverMt::setupBatchedJointConstraints()
  55 {
  56         BT_PROFILE("setupBatchedJointConstraints");
  57         m_batchedJointConstraints.setup(&m_tmpSolverNonContactConstraintPool,
  58                                                                         m_tmpSolverBodyPool,
  59                                                                         s_jointBatchingMethod,
  60                                                                         s_minBatchSize,
  61                                                                         s_maxBatchSize,
  62                                                                         &m_scratchMemory);
  63 }
  64
  65 void btSequentialImpulseConstraintSolverMt::internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo& infoGlobal)
  66 {
  67         btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[iContactConstraint];
  68
  69         btVector3 rel_pos1;
  70         btVector3 rel_pos2;
  71         btScalar relaxation;
  72
  73         int solverBodyIdA = contactConstraint.m_solverBodyIdA;
  74         int solverBodyIdB = contactConstraint.m_solverBodyIdB;
  75
  76         btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
  77         btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
  78
  79         btRigidBody* colObj0 = solverBodyA->m_originalBody;
  80         btRigidBody* colObj1 = solverBodyB->m_originalBody;
  81
  82         btManifoldPoint& cp = *static_cast<btManifoldPoint*>(contactConstraint.m_originalContactPoint);
  83
  84         const btVector3& pos1 = cp.getPositionWorldOnA();
  85         const btVector3& pos2 = cp.getPositionWorldOnB();
  86
  87         rel_pos1 = pos1 - solverBodyA->getWorldTransform().getOrigin();
  88         rel_pos2 = pos2 - solverBodyB->getWorldTransform().getOrigin();
  89
  90         btVector3 vel1;
  91         btVector3 vel2;
  92
  93         solverBodyA->getVelocityInLocalPointNoDelta(rel_pos1, vel1);
  94         solverBodyB->getVelocityInLocalPointNoDelta(rel_pos2, vel2);
  95
  96         btVector3 vel = vel1 - vel2;
  97         btScalar rel_vel = cp.m_normalWorldOnB.dot(vel);
  98
  99         setupContactConstraint(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, relaxation, rel_pos1, rel_pos2);
 100
 101         // setup rolling friction constraints
 102         int rollingFrictionIndex = m_rollingFrictionIndexTable[iContactConstraint];
 103         if (rollingFrictionIndex >= 0)
 104         {
 105                 btSolverConstraint& spinningFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex];
 106                 btAssert(spinningFrictionConstraint.m_frictionIndex == iContactConstraint);
 107                 setupTorsionalFrictionConstraint(spinningFrictionConstraint,
 108                                                                                  cp.m_normalWorldOnB,
 109                                                                                  solverBodyIdA,
 110                                                                                  solverBodyIdB,
 111                                                                                  cp,
 112                                                                                  cp.m_combinedSpinningFriction,
 113                                                                                  rel_pos1,
 114                                                                                  rel_pos2,
 115                                                                                  colObj0,
 116                                                                                  colObj1,
 117                                                                                  relaxation,
 118                                                                                  0.0f,
 119                                                                                  0.0f);
 120                 btVector3 axis[2];
 121                 btPlaneSpace1(cp.m_normalWorldOnB, axis[0], axis[1]);
 122                 axis[0].normalize();
 123                 axis[1].normalize();
 124
 125                 applyAnisotropicFriction(colObj0, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 126                 applyAnisotropicFriction(colObj1, axis[0], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 127                 applyAnisotropicFriction(colObj0, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 128                 applyAnisotropicFriction(colObj1, axis[1], btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 129                 // put the largest axis first
 130                 if (axis[1].length2() > axis[0].length2())
 131                 {
 132                         btSwap(axis[0], axis[1]);
 133                 }
 134                 const btScalar kRollingFrictionThreshold = 0.001f;
 135                 for (int i = 0; i < 2; ++i)
 136                 {
 137                         int iRollingFric = rollingFrictionIndex + 1 + i;
 138                         btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
 139                         btAssert(rollingFrictionConstraint.m_frictionIndex == iContactConstraint);
 140                         btVector3 dir = axis[i];
 141                         if (dir.length() > kRollingFrictionThreshold)
 142                         {
 143                                 setupTorsionalFrictionConstraint(rollingFrictionConstraint,
 144                                                                                                  dir,
 145                                                                                                  solverBodyIdA,
 146                                                                                                  solverBodyIdB,
 147                                                                                                  cp,
 148                                                                                                  cp.m_combinedRollingFriction,
 149                                                                                                  rel_pos1,
 150                                                                                                  rel_pos2,
 151                                                                                                  colObj0,
 152                                                                                                  colObj1,
 153                                                                                                  relaxation,
 154                                                                                                  0.0f,
 155                                                                                                  0.0f);
 156                         }
 157                         else
 158                         {
 159                                 rollingFrictionConstraint.m_frictionIndex = -1;  // disable constraint
 160                         }
 161                 }
 162         }
 163
 164         // setup friction constraints
 165         //      setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
 166         {
 167                 ///Bullet has several options to set the friction directions
 168                 ///By default, each contact has only a single friction direction that is recomputed automatically very frame
 169                 ///based on the relative linear velocity.
 170                 ///If the relative velocity it zero, it will automatically compute a friction direction.
 171
 172                 ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
 173                 ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
 174                 ///
 175                 ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
 176                 ///
 177                 ///The user can manually override the friction directions for certain contacts using a contact callback,
 178                 ///and set the cp.m_lateralFrictionInitialized to true
 179                 ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 180                 ///this will give a conveyor belt effect
 181                 ///
 182                 btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex];
 183                 btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint);
 184
 185                 btSolverConstraint* frictionConstraint2 = NULL;
 186                 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
 187                 {
 188                         frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1];
 189                         btAssert(frictionConstraint2->m_frictionIndex == iContactConstraint);
 190                 }
 191
 192                 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
 193                 {
 194                         cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 195                         btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 196                         if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
 197                         {
 198                                 cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
 199                                 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 200                                 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 201                                 setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 202
 203                                 if (frictionConstraint2)
 204                                 {
 205                                         cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
 206                                         cp.m_lateralFrictionDir2.normalize();  //??
 207                                         applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 208                                         applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 209                                         setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 210                                 }
 211                         }
 212                         else
 213                         {
 214                                 btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
 215
 216                                 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 217                                 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 218                                 setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 219
 220                                 if (frictionConstraint2)
 221                                 {
 222                                         applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 223                                         applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 224                                         setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
 225                                 }
 226
 227                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 228                                 {
 229                                         cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
 230                                 }
 231                         }
 232                 }
 233                 else
 234                 {
 235                         setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
 236                         if (frictionConstraint2)
 237                         {
 238                                 setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
 239                         }
 240                 }
 241         }
 242
 243         setFrictionConstraintImpulse(contactConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 244 }
 245
 246 struct SetupContactConstraintsLoop : public btIParallelForBody
 247 {
 248         btSequentialImpulseConstraintSolverMt* m_solver;
 249         const btBatchedConstraints* m_bc;
 250         const btContactSolverInfo* m_infoGlobal;
 251
 252         SetupContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, const btContactSolverInfo& infoGlobal)
 253         {
 254                 m_solver = solver;
 255                 m_bc = bc;
 256                 m_infoGlobal = &infoGlobal;
 257         }
 258         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
 259         {
 260                 BT_PROFILE("SetupContactConstraintsLoop");
 261                 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
 262                 {
 263                         const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
 264                         for (int i = batch.begin; i < batch.end; ++i)
 265                         {
 266                                 int iContact = m_bc->m_constraintIndices[i];
 267                                 m_solver->internalSetupContactConstraints(iContact, *m_infoGlobal);
 268                         }
 269                 }
 270         }
 271 };
 272
 273 void btSequentialImpulseConstraintSolverMt::setupAllContactConstraints(const btContactSolverInfo& infoGlobal)
 274 {
 275         BT_PROFILE("setupAllContactConstraints");
 276         if (m_useBatching)
 277         {
 278                 const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
 279                 SetupContactConstraintsLoop loop(this, &batchedCons, infoGlobal);
 280                 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
 281                 {
 282                         int iPhase = batchedCons.m_phaseOrder[iiPhase];
 283                         const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
 284                         int grainSize = 1;
 285                         btParallelFor(phase.begin, phase.end, grainSize, loop);
 286                 }
 287         }
 288         else
 289         {
 290                 for (int i = 0; i < m_tmpSolverContactConstraintPool.size(); ++i)
 291                 {
 292                         internalSetupContactConstraints(i, infoGlobal);
 293                 }
 294         }
 295 }
 296
 297 int btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btCollisionObject& body, btScalar timeStep)
 298 {
 299         //
 300         // getOrInitSolverBody is threadsafe only for a single thread per solver (with potentially multiple solvers)
 301         //
 302         // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism)
 303         //
 304         int solverBodyId = -1;
 305         bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
 306         if (isRigidBodyType && !body.isStaticOrKinematicObject())
 307         {
 308                 // dynamic body
 309                 // Dynamic bodies can only be in one island, so it's safe to write to the companionId
 310                 solverBodyId = body.getCompanionId();
 311                 if (solverBodyId < 0)
 312                 {
 313                         m_bodySolverArrayMutex.lock();
 314                         // now that we have the lock, check again
 315                         solverBodyId = body.getCompanionId();
 316                         if (solverBodyId < 0)
 317                         {
 318                                 solverBodyId = m_tmpSolverBodyPool.size();
 319                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 320                                 initSolverBody(&solverBody, &body, timeStep);
 321                                 body.setCompanionId(solverBodyId);
 322                         }
 323                         m_bodySolverArrayMutex.unlock();
 324                 }
 325         }
 326         else if (isRigidBodyType && body.isKinematicObject())
 327         {
 328                 //
 329                 // NOTE: must test for kinematic before static because some kinematic objects also
 330                 //   identify as "static"
 331                 //
 332                 // Kinematic bodies can be in multiple islands at once, so it is a
 333                 // race condition to write to them, so we use an alternate method
 334                 // to record the solverBodyId
 335                 int uniqueId = body.getWorldArrayIndex();
 336                 const int INVALID_SOLVER_BODY_ID = -1;
 337                 if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
 338                 {
 339                         m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
 340                         // now that we have the lock, check again
 341                         if (m_kinematicBodyUniqueIdToSolverBodyTable.size() <= uniqueId)
 342                         {
 343                                 m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
 344                         }
 345                         m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
 346                 }
 347                 solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
 348                 // if no table entry yet,
 349                 if (INVALID_SOLVER_BODY_ID == solverBodyId)
 350                 {
 351                         // need to acquire both locks
 352                         m_kinematicBodyUniqueIdToSolverBodyTableMutex.lock();
 353                         m_bodySolverArrayMutex.lock();
 354                         // now that we have the lock, check again
 355                         solverBodyId = m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId];
 356                         if (INVALID_SOLVER_BODY_ID == solverBodyId)
 357                         {
 358                                 // create a table entry for this body
 359                                 solverBodyId = m_tmpSolverBodyPool.size();
 360                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 361                                 initSolverBody(&solverBody, &body, timeStep);
 362                                 m_kinematicBodyUniqueIdToSolverBodyTable[uniqueId] = solverBodyId;
 363                         }
 364                         m_bodySolverArrayMutex.unlock();
 365                         m_kinematicBodyUniqueIdToSolverBodyTableMutex.unlock();
 366                 }
 367         }
 368         else
 369         {
 370                 // all fixed bodies (inf mass) get mapped to a single solver id
 371                 if (m_fixedBodyId < 0)
 372                 {
 373                         m_bodySolverArrayMutex.lock();
 374                         // now that we have the lock, check again
 375                         if (m_fixedBodyId < 0)
 376                         {
 377                                 m_fixedBodyId = m_tmpSolverBodyPool.size();
 378                                 btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
 379                                 initSolverBody(&fixedBody, 0, timeStep);
 380                         }
 381                         m_bodySolverArrayMutex.unlock();
 382                 }
 383                 solverBodyId = m_fixedBodyId;
 384         }
 385         btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
 386         return solverBodyId;
 387 }
 388
 389 void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 390 {
 391         BT_PROFILE("internalCollectContactManifoldCachedInfo");
 392         for (int i = 0; i < numManifolds; ++i)
 393         {
 394                 btContactManifoldCachedInfo* cachedInfo = &cachedInfoArray[i];
 395                 btPersistentManifold* manifold = manifoldPtr[i];
 396                 btCollisionObject* colObj0 = (btCollisionObject*)manifold->getBody0();
 397                 btCollisionObject* colObj1 = (btCollisionObject*)manifold->getBody1();
 398
 399                 int solverBodyIdA = getOrInitSolverBodyThreadsafe(*colObj0, infoGlobal.m_timeStep);
 400                 int solverBodyIdB = getOrInitSolverBodyThreadsafe(*colObj1, infoGlobal.m_timeStep);
 401
 402                 cachedInfo->solverBodyIds[0] = solverBodyIdA;
 403                 cachedInfo->solverBodyIds[1] = solverBodyIdB;
 404                 cachedInfo->numTouchingContacts = 0;
 405
 406                 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 407                 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 408
 409                 // A contact manifold between 2 static object should not exist!
 410                 // check the collision flags of your objects if this assert fires.
 411                 // Incorrectly set collision object flags can degrade performance in various ways.
 412                 btAssert(!m_tmpSolverBodyPool[solverBodyIdA].m_invMass.isZero() || !m_tmpSolverBodyPool[solverBodyIdB].m_invMass.isZero());
 413
 414                 int iContact = 0;
 415                 for (int j = 0; j < manifold->getNumContacts(); j++)
 416                 {
 417                         btManifoldPoint& cp = manifold->getContactPoint(j);
 418
 419                         if (cp.getDistance() <= manifold->getContactProcessingThreshold())
 420                         {
 421                                 cachedInfo->contactPoints[iContact] = &cp;
 422                                 cachedInfo->contactHasRollingFriction[iContact] = (cp.m_combinedRollingFriction > 0.f);
 423                                 iContact++;
 424                         }
 425                 }
 426                 cachedInfo->numTouchingContacts = iContact;
 427         }
 428 }
 429
 430 struct CollectContactManifoldCachedInfoLoop : public btIParallelForBody
 431 {
 432         btSequentialImpulseConstraintSolverMt* m_solver;
 433         btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
 434         btPersistentManifold** m_manifoldPtr;
 435         const btContactSolverInfo* m_infoGlobal;
 436
 437         CollectContactManifoldCachedInfoLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal)
 438         {
 439                 m_solver = solver;
 440                 m_cachedInfoArray = cachedInfoArray;
 441                 m_manifoldPtr = manifoldPtr;
 442                 m_infoGlobal = &infoGlobal;
 443         }
 444         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
 445         {
 446                 m_solver->internalCollectContactManifoldCachedInfo(m_cachedInfoArray + iBegin, m_manifoldPtr + iBegin, iEnd - iBegin, *m_infoGlobal);
 447         }
 448 };
 449
 450 void btSequentialImpulseConstraintSolverMt::internalAllocContactConstraints(const btContactManifoldCachedInfo* cachedInfoArray, int numManifolds)
 451 {
 452         BT_PROFILE("internalAllocContactConstraints");
 453         // possibly parallel part
 454         for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
 455         {
 456                 const btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
 457                 int contactIndex = cachedInfo.contactIndex;
 458                 int frictionIndex = contactIndex * m_numFrictionDirections;
 459                 int rollingFrictionIndex = cachedInfo.rollingFrictionIndex;
 460                 for (int i = 0; i < cachedInfo.numTouchingContacts; i++)
 461                 {
 462                         btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[contactIndex];
 463                         contactConstraint.m_solverBodyIdA = cachedInfo.solverBodyIds[0];
 464                         contactConstraint.m_solverBodyIdB = cachedInfo.solverBodyIds[1];
 465                         contactConstraint.m_originalContactPoint = cachedInfo.contactPoints[i];
 466
 467                         // allocate the friction constraints
 468                         contactConstraint.m_frictionIndex = frictionIndex;
 469                         for (int iDir = 0; iDir < m_numFrictionDirections; ++iDir)
 470                         {
 471                                 btSolverConstraint& frictionConstraint = m_tmpSolverContactFrictionConstraintPool[frictionIndex];
 472                                 frictionConstraint.m_frictionIndex = contactIndex;
 473                                 frictionIndex++;
 474                         }
 475
 476                         // allocate rolling friction constraints
 477                         if (cachedInfo.contactHasRollingFriction[i])
 478                         {
 479                                 m_rollingFrictionIndexTable[contactIndex] = rollingFrictionIndex;
 480                                 // allocate 3 (although we may use only 2 sometimes)
 481                                 for (int i = 0; i < 3; i++)
 482                                 {
 483                                         m_tmpSolverContactRollingFrictionConstraintPool[rollingFrictionIndex].m_frictionIndex = contactIndex;
 484                                         rollingFrictionIndex++;
 485                                 }
 486                         }
 487                         else
 488                         {
 489                                 // indicate there is no rolling friction for this contact point
 490                                 m_rollingFrictionIndexTable[contactIndex] = -1;
 491                         }
 492                         contactIndex++;
 493                 }
 494         }
 495 }
 496
 497 struct AllocContactConstraintsLoop : public btIParallelForBody
 498 {
 499         btSequentialImpulseConstraintSolverMt* m_solver;
 500         const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
 501
 502         AllocContactConstraintsLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray)
 503         {
 504                 m_solver = solver;
 505                 m_cachedInfoArray = cachedInfoArray;
 506         }
 507         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
 508         {
 509                 m_solver->internalAllocContactConstraints(m_cachedInfoArray + iBegin, iEnd - iBegin);
 510         }
 511 };
 512
 513 void btSequentialImpulseConstraintSolverMt::allocAllContactConstraints(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 514 {
 515         BT_PROFILE("allocAllContactConstraints");
 516         btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray;  // = m_manifoldCachedInfoArray;
 517         cachedInfoArray.resizeNoInitialize(numManifolds);
 518         if (/* DISABLES CODE */ (false))
 519         {
 520                 // sequential
 521                 internalCollectContactManifoldCachedInfo(&cachedInfoArray[0], manifoldPtr, numManifolds, infoGlobal);
 522         }
 523         else
 524         {
 525                 // may alter ordering of bodies which affects determinism
 526                 CollectContactManifoldCachedInfoLoop loop(this, &cachedInfoArray[0], manifoldPtr, infoGlobal);
 527                 int grainSize = 200;
 528                 btParallelFor(0, numManifolds, grainSize, loop);
 529         }
 530
 531         {
 532                 // serial part
 533                 int numContacts = 0;
 534                 int numRollingFrictionConstraints = 0;
 535                 for (int iManifold = 0; iManifold < numManifolds; ++iManifold)
 536                 {
 537                         btContactManifoldCachedInfo& cachedInfo = cachedInfoArray[iManifold];
 538                         cachedInfo.contactIndex = numContacts;
 539                         cachedInfo.rollingFrictionIndex = numRollingFrictionConstraints;
 540                         numContacts += cachedInfo.numTouchingContacts;
 541                         for (int i = 0; i < cachedInfo.numTouchingContacts; ++i)
 542                         {
 543                                 if (cachedInfo.contactHasRollingFriction[i])
 544                                 {
 545                                         numRollingFrictionConstraints += 3;
 546                                 }
 547                         }
 548                 }
 549                 {
 550                         BT_PROFILE("allocPools");
 551                         if (m_tmpSolverContactConstraintPool.capacity() < numContacts)
 552                         {
 553                                 // if we need to reallocate, reserve some extra so we don't have to reallocate again next frame
 554                                 int extraReserve = numContacts / 16;
 555                                 m_tmpSolverContactConstraintPool.reserve(numContacts + extraReserve);
 556                                 m_rollingFrictionIndexTable.reserve(numContacts + extraReserve);
 557                                 m_tmpSolverContactFrictionConstraintPool.reserve((numContacts + extraReserve) * m_numFrictionDirections);
 558                                 m_tmpSolverContactRollingFrictionConstraintPool.reserve(numRollingFrictionConstraints + extraReserve);
 559                         }
 560                         m_tmpSolverContactConstraintPool.resizeNoInitialize(numContacts);
 561                         m_rollingFrictionIndexTable.resizeNoInitialize(numContacts);
 562                         m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(numContacts * m_numFrictionDirections);
 563                         m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(numRollingFrictionConstraints);
 564                 }
 565         }
 566         {
 567                 AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]);
 568                 int grainSize = 200;
 569                 btParallelFor(0, numManifolds, grainSize, loop);
 570         }
 571 }
 572
 573 void btSequentialImpulseConstraintSolverMt::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 574 {
 575         if (!m_useBatching)
 576         {
 577                 btSequentialImpulseConstraintSolver::convertContacts(manifoldPtr, numManifolds, infoGlobal);
 578                 return;
 579         }
 580         BT_PROFILE("convertContacts");
 581         if (numManifolds > 0)
 582         {
 583                 if (m_fixedBodyId < 0)
 584                 {
 585                         m_fixedBodyId = m_tmpSolverBodyPool.size();
 586                         btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
 587                         initSolverBody(&fixedBody, 0, infoGlobal.m_timeStep);
 588                 }
 589                 allocAllContactConstraints(manifoldPtr, numManifolds, infoGlobal);
 590                 if (m_useBatching)
 591                 {
 592                         setupBatchedContactConstraints();
 593                 }
 594                 setupAllContactConstraints(infoGlobal);
 595         }
 596 }
 597
 598 void btSequentialImpulseConstraintSolverMt::internalInitMultipleJoints(btTypedConstraint** constraints, int iBegin, int iEnd)
 599 {
 600         BT_PROFILE("internalInitMultipleJoints");
 601         for (int i = iBegin; i < iEnd; i++)
 602         {
 603                 btTypedConstraint* constraint = constraints[i];
 604                 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
 605                 if (constraint->isEnabled())
 606                 {
 607                         constraint->buildJacobian();
 608                         constraint->internalSetAppliedImpulse(0.0f);
 609                         btJointFeedback* fb = constraint->getJointFeedback();
 610                         if (fb)
 611                         {
 612                                 fb->m_appliedForceBodyA.setZero();
 613                                 fb->m_appliedTorqueBodyA.setZero();
 614                                 fb->m_appliedForceBodyB.setZero();
 615                                 fb->m_appliedTorqueBodyB.setZero();
 616                         }
 617                         constraint->getInfo1(&info1);
 618                 }
 619                 else
 620                 {
 621                         info1.m_numConstraintRows = 0;
 622                         info1.nub = 0;
 623                 }
 624         }
 625 }
 626
 627 struct InitJointsLoop : public btIParallelForBody
 628 {
 629         btSequentialImpulseConstraintSolverMt* m_solver;
 630         btTypedConstraint** m_constraints;
 631
 632         InitJointsLoop(btSequentialImpulseConstraintSolverMt* solver, btTypedConstraint** constraints)
 633         {
 634                 m_solver = solver;
 635                 m_constraints = constraints;
 636         }
 637         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
 638         {
 639                 m_solver->internalInitMultipleJoints(m_constraints, iBegin, iEnd);
 640         }
 641 };
 642
 643 void btSequentialImpulseConstraintSolverMt::internalConvertMultipleJoints(const btAlignedObjectArray<JointParams>& jointParamsArray, btTypedConstraint** constraints, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 644 {
 645         BT_PROFILE("internalConvertMultipleJoints");
 646         for (int i = iBegin; i < iEnd; ++i)
 647         {
 648                 const JointParams& jointParams = jointParamsArray[i];
 649                 int currentRow = jointParams.m_solverConstraint;
 650                 if (currentRow != -1)
 651                 {
 652                         const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
 653                         btAssert(currentRow < m_tmpSolverNonContactConstraintPool.size());
 654                         btAssert(info1.m_numConstraintRows > 0);
 655
 656                         btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
 657                         btTypedConstraint* constraint = constraints[i];
 658
 659                         convertJoint(currentConstraintRow, constraint, info1, jointParams.m_solverBodyA, jointParams.m_solverBodyB, infoGlobal);
 660                 }
 661         }
 662 }
 663
 664 struct ConvertJointsLoop : public btIParallelForBody
 665 {
 666         btSequentialImpulseConstraintSolverMt* m_solver;
 667         const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& m_jointParamsArray;
 668         btTypedConstraint** m_srcConstraints;
 669         const btContactSolverInfo& m_infoGlobal;
 670
 671         ConvertJointsLoop(btSequentialImpulseConstraintSolverMt* solver,
 672                                           const btAlignedObjectArray<btSequentialImpulseConstraintSolverMt::JointParams>& jointParamsArray,
 673                                           btTypedConstraint** srcConstraints,
 674                                           const btContactSolverInfo& infoGlobal) : m_jointParamsArray(jointParamsArray),
 675                                                                                                                            m_infoGlobal(infoGlobal)
 676         {
 677                 m_solver = solver;
 678                 m_srcConstraints = srcConstraints;
 679         }
 680         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
 681         {
 682                 m_solver->internalConvertMultipleJoints(m_jointParamsArray, m_srcConstraints, iBegin, iEnd, m_infoGlobal);
 683         }
 684 };
 685
 686 void btSequentialImpulseConstraintSolverMt::convertJoints(btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal)
 687 {
 688         if (!m_useBatching)
 689         {
 690                 btSequentialImpulseConstraintSolver::convertJoints(constraints, numConstraints, infoGlobal);
 691                 return;
 692         }
 693         BT_PROFILE("convertJoints");
 694         bool parallelJointSetup = true;
 695         m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
 696         if (parallelJointSetup)
 697         {
 698                 InitJointsLoop loop(this, constraints);
 699                 int grainSize = 40;
 700                 btParallelFor(0, numConstraints, grainSize, loop);
 701         }
 702         else
 703         {
 704                 internalInitMultipleJoints(constraints, 0, numConstraints);
 705         }
 706
 707         int totalNumRows = 0;
 708         btAlignedObjectArray<JointParams> jointParamsArray;
 709         jointParamsArray.resizeNoInitialize(numConstraints);
 710
 711         //calculate the total number of contraint rows
 712         for (int i = 0; i < numConstraints; i++)
 713         {
 714                 btTypedConstraint* constraint = constraints[i];
 715
 716                 JointParams& params = jointParamsArray[i];
 717                 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
 718
 719                 if (info1.m_numConstraintRows)
 720                 {
 721                         params.m_solverConstraint = totalNumRows;
 722                         params.m_solverBodyA = getOrInitSolverBody(constraint->getRigidBodyA(), infoGlobal.m_timeStep);
 723                         params.m_solverBodyB = getOrInitSolverBody(constraint->getRigidBodyB(), infoGlobal.m_timeStep);
 724                 }
 725                 else
 726                 {
 727                         params.m_solverConstraint = -1;
 728                 }
 729                 totalNumRows += info1.m_numConstraintRows;
 730         }
 731         m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
 732
 733         ///setup the btSolverConstraints
 734         if (parallelJointSetup)
 735         {
 736                 ConvertJointsLoop loop(this, jointParamsArray, constraints, infoGlobal);
 737                 int grainSize = 20;
 738                 btParallelFor(0, numConstraints, grainSize, loop);
 739         }
 740         else
 741         {
 742                 internalConvertMultipleJoints(jointParamsArray, constraints, 0, numConstraints, infoGlobal);
 743         }
 744         setupBatchedJointConstraints();
 745 }
 746
 747 void btSequentialImpulseConstraintSolverMt::internalConvertBodies(btCollisionObject** bodies, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
 748 {
 749         BT_PROFILE("internalConvertBodies");
 750         for (int i = iBegin; i < iEnd; i++)
 751         {
 752                 btCollisionObject* obj = bodies[i];
 753                 obj->setCompanionId(i);
 754                 btSolverBody& solverBody = m_tmpSolverBodyPool[i];
 755                 initSolverBody(&solverBody, obj, infoGlobal.m_timeStep);
 756
 757                 btRigidBody* body = btRigidBody::upcast(obj);
 758                 if (body && body->getInvMass())
 759                 {
 760                         btVector3 gyroForce(0, 0, 0);
 761                         if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT)
 762                         {
 763                                 gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
 764                                 solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
 765                         }
 766                         if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD)
 767                         {
 768                                 gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
 769                                 solverBody.m_externalTorqueImpulse += gyroForce;
 770                         }
 771                         if (body->getFlags() & BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY)
 772                         {
 773                                 gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
 774                                 solverBody.m_externalTorqueImpulse += gyroForce;
 775                         }
 776                 }
 777         }
 778 }
 779
 780 struct ConvertBodiesLoop : public btIParallelForBody
 781 {
 782         btSequentialImpulseConstraintSolverMt* m_solver;
 783         btCollisionObject** m_bodies;
 784         int m_numBodies;
 785         const btContactSolverInfo& m_infoGlobal;
 786
 787         ConvertBodiesLoop(btSequentialImpulseConstraintSolverMt* solver,
 788                                           btCollisionObject** bodies,
 789                                           int numBodies,
 790                                           const btContactSolverInfo& infoGlobal) : m_infoGlobal(infoGlobal)
 791         {
 792                 m_solver = solver;
 793                 m_bodies = bodies;
 794                 m_numBodies = numBodies;
 795         }
 796         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
 797         {
 798                 m_solver->internalConvertBodies(m_bodies, iBegin, iEnd, m_infoGlobal);
 799         }
 800 };
 801
 802 void btSequentialImpulseConstraintSolverMt::convertBodies(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
 803 {
 804         BT_PROFILE("convertBodies");
 805         m_kinematicBodyUniqueIdToSolverBodyTable.resize(0);
 806
 807         m_tmpSolverBodyPool.resizeNoInitialize(numBodies + 1);
 808
 809         m_fixedBodyId = numBodies;
 810         {
 811                 btSolverBody& fixedBody = m_tmpSolverBodyPool[m_fixedBodyId];
 812                 initSolverBody(&fixedBody, NULL, infoGlobal.m_timeStep);
 813         }
 814
 815         bool parallelBodySetup = true;
 816         if (parallelBodySetup)
 817         {
 818                 ConvertBodiesLoop loop(this, bodies, numBodies, infoGlobal);
 819                 int grainSize = 40;
 820                 btParallelFor(0, numBodies, grainSize, loop);
 821         }
 822         else
 823         {
 824                 internalConvertBodies(bodies, 0, numBodies, infoGlobal);
 825         }
 826 }
 827
 828 btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySetup(
 829         btCollisionObject** bodies,
 830         int numBodies,
 831         btPersistentManifold** manifoldPtr,
 832         int numManifolds,
 833         btTypedConstraint** constraints,
 834         int numConstraints,
 835         const btContactSolverInfo& infoGlobal,
 836         btIDebugDraw* debugDrawer)
 837 {
 838         m_numFrictionDirections = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) ? 2 : 1;
 839         m_useBatching = false;
 840         if (numManifolds >= s_minimumContactManifoldsForBatching &&
 841                 (s_allowNestedParallelForLoops || !btThreadsAreRunning()))
 842         {
 843                 m_useBatching = true;
 844                 m_batchedContactConstraints.m_debugDrawer = debugDrawer;
 845                 m_batchedJointConstraints.m_debugDrawer = debugDrawer;
 846         }
 847         btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies,
 848                                                                                                                                           numBodies,
 849                                                                                                                                           manifoldPtr,
 850                                                                                                                                           numManifolds,
 851                                                                                                                                           constraints,
 852                                                                                                                                           numConstraints,
 853                                                                                                                                           infoGlobal,
 854                                                                                                                                           debugDrawer);
 855         return 0.0f;
 856 }
 857
 858 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
 859 {
 860         btScalar leastSquaresResidual = 0.f;
 861         for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
 862         {
 863                 int iCons = consIndices[iiCons];
 864                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
 865                 btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
 866                 btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
 867                 btScalar residual = resolveSplitPenetrationImpulse(bodyA, bodyB, solveManifold);
 868                 leastSquaresResidual += residual * residual;
 869         }
 870         return leastSquaresResidual;
 871 }
 872
 873 struct ContactSplitPenetrationImpulseSolverLoop : public btIParallelSumBody
 874 {
 875         btSequentialImpulseConstraintSolverMt* m_solver;
 876         const btBatchedConstraints* m_bc;
 877
 878         ContactSplitPenetrationImpulseSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
 879         {
 880                 m_solver = solver;
 881                 m_bc = bc;
 882         }
 883         btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
 884         {
 885                 BT_PROFILE("ContactSplitPenetrationImpulseSolverLoop");
 886                 btScalar sum = 0;
 887                 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
 888                 {
 889                         const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
 890                         sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
 891                 }
 892                 return sum;
 893         }
 894 };
 895
 896 void btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 897 {
 898         BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
 899         if (infoGlobal.m_splitImpulse)
 900         {
 901                 for (int iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
 902                 {
 903                         btScalar leastSquaresResidual = 0.f;
 904                         if (m_useBatching)
 905                         {
 906                                 const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
 907                                 ContactSplitPenetrationImpulseSolverLoop loop(this, &batchedCons);
 908                                 btScalar leastSquaresResidual = 0.f;
 909                                 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
 910                                 {
 911                                         int iPhase = batchedCons.m_phaseOrder[iiPhase];
 912                                         const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
 913                                         int grainSize = batchedCons.m_phaseGrainSize[iPhase];
 914                                         leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
 915                                 }
 916                         }
 917                         else
 918                         {
 919                                 // non-batched
 920                                 leastSquaresResidual = resolveMultipleContactSplitPenetrationImpulseConstraints(m_orderTmpConstraintPool, 0, m_tmpSolverContactConstraintPool.size());
 921                         }
 922                         if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
 923                         {
 924 #ifdef VERBOSE_RESIDUAL_PRINTF
 925                                 printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration);
 926 #endif
 927                                 break;
 928                         }
 929                 }
 930         }
 931 }
 932
 933 btScalar btSequentialImpulseConstraintSolverMt::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 934 {
 935         if (!m_useBatching)
 936         {
 937                 return btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
 938         }
 939         BT_PROFILE("solveSingleIterationMt");
 940         btScalar leastSquaresResidual = 0.f;
 941
 942         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 943         {
 944                 if (1)  // uncomment this for a bit less random ((iteration & 7) == 0)
 945                 {
 946                         randomizeConstraintOrdering(iteration, infoGlobal.m_numIterations);
 947                 }
 948         }
 949
 950         {
 951                 ///solve all joint constraints
 952                 leastSquaresResidual += resolveAllJointConstraints(iteration);
 953
 954                 if (iteration < infoGlobal.m_numIterations)
 955                 {
 956                         // this loop is only used for cone-twist constraints,
 957                         // it would be nice to skip this loop if none of the constraints need it
 958                         if (m_useObsoleteJointConstraints)
 959                         {
 960                                 for (int j = 0; j < numConstraints; j++)
 961                                 {
 962                                         if (constraints[j]->isEnabled())
 963                                         {
 964                                                 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(), infoGlobal.m_timeStep);
 965                                                 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(), infoGlobal.m_timeStep);
 966                                                 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
 967                                                 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
 968                                                 constraints[j]->solveConstraintObsolete(bodyA, bodyB, infoGlobal.m_timeStep);
 969                                         }
 970                                 }
 971                         }
 972
 973                         if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
 974                         {
 975                                 // solve all contact, contact-friction, and rolling friction constraints interleaved
 976                                 leastSquaresResidual += resolveAllContactConstraintsInterleaved();
 977                         }
 978                         else  //SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
 979                         {
 980                                 // don't interleave them
 981                                 // solve all contact constraints
 982                                 leastSquaresResidual += resolveAllContactConstraints();
 983
 984                                 // solve all contact friction constraints
 985                                 leastSquaresResidual += resolveAllContactFrictionConstraints();
 986
 987                                 // solve all rolling friction constraints
 988                                 leastSquaresResidual += resolveAllRollingFrictionConstraints();
 989                         }
 990                 }
 991         }
 992         return leastSquaresResidual;
 993 }
 994
 995 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleJointConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd, int iteration)
 996 {
 997         btScalar leastSquaresResidual = 0.f;
 998         for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
 999         {
1000                 int iCons = consIndices[iiCons];
1001                 const btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[iCons];
1002                 if (iteration < constraint.m_overrideNumSolverIterations)
1003                 {
1004                         btSolverBody& bodyA = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
1005                         btSolverBody& bodyB = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
1006                         btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, constraint);
1007                         leastSquaresResidual += residual * residual;
1008                 }
1009         }
1010         return leastSquaresResidual;
1011 }
1012
1013 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
1014 {
1015         btScalar leastSquaresResidual = 0.f;
1016         for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
1017         {
1018                 int iCons = consIndices[iiCons];
1019                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iCons];
1020                 btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
1021                 btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
1022                 btScalar residual = resolveSingleConstraintRowLowerLimit(bodyA, bodyB, solveManifold);
1023                 leastSquaresResidual += residual * residual;
1024         }
1025         return leastSquaresResidual;
1026 }
1027
1028 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
1029 {
1030         btScalar leastSquaresResidual = 0.f;
1031         for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
1032         {
1033                 int iContact = consIndices[iiCons];
1034                 btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
1035
1036                 // apply sliding friction
1037                 if (totalImpulse > 0.0f)
1038                 {
1039                         int iBegin = iContact * m_numFrictionDirections;
1040                         int iEnd = iBegin + m_numFrictionDirections;
1041                         for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
1042                         {
1043                                 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction++];
1044                                 btAssert(solveManifold.m_frictionIndex == iContact);
1045
1046                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1047                                 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1048
1049                                 btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
1050                                 btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
1051                                 btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
1052                                 leastSquaresResidual += residual * residual;
1053                         }
1054                 }
1055         }
1056         return leastSquaresResidual;
1057 }
1058
1059 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray<int>& consIndices, int batchBegin, int batchEnd)
1060 {
1061         btScalar leastSquaresResidual = 0.f;
1062         for (int iiCons = batchBegin; iiCons < batchEnd; ++iiCons)
1063         {
1064                 int iContact = consIndices[iiCons];
1065                 int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
1066                 if (iFirstRollingFriction >= 0)
1067                 {
1068                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[iContact].m_appliedImpulse;
1069                         // apply rolling friction
1070                         if (totalImpulse > 0.0f)
1071                         {
1072                                 int iBegin = iFirstRollingFriction;
1073                                 int iEnd = iBegin + 3;
1074                                 for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
1075                                 {
1076                                         btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
1077                                         if (rollingFrictionConstraint.m_frictionIndex != iContact)
1078                                         {
1079                                                 break;
1080                                         }
1081                                         btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1082                                         if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1083                                         {
1084                                                 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1085                                         }
1086
1087                                         rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1088                                         rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1089
1090                                         btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1091                                         leastSquaresResidual += residual * residual;
1092                                 }
1093                         }
1094                 }
1095         }
1096         return leastSquaresResidual;
1097 }
1098
1099 btScalar btSequentialImpulseConstraintSolverMt::resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray<int>& contactIndices,
1100                                                                                                                                                                                          int batchBegin,
1101                                                                                                                                                                                          int batchEnd)
1102 {
1103         btScalar leastSquaresResidual = 0.f;
1104         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1105
1106         for (int iiCons = batchBegin; iiCons < batchEnd; iiCons++)
1107         {
1108                 btScalar totalImpulse = 0;
1109                 int iContact = contactIndices[iiCons];
1110                 // apply penetration constraint
1111                 {
1112                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[iContact];
1113                         btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA], m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB], solveManifold);
1114                         leastSquaresResidual += residual * residual;
1115                         totalImpulse = solveManifold.m_appliedImpulse;
1116                 }
1117
1118                 // apply sliding friction
1119                 if (totalImpulse > 0.0f)
1120                 {
1121                         int iBegin = iContact * m_numFrictionDirections;
1122                         int iEnd = iBegin + m_numFrictionDirections;
1123                         for (int iFriction = iBegin; iFriction < iEnd; ++iFriction)
1124                         {
1125                                 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[iFriction];
1126                                 btAssert(solveManifold.m_frictionIndex == iContact);
1127
1128                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction * totalImpulse);
1129                                 solveManifold.m_upperLimit = solveManifold.m_friction * totalImpulse;
1130
1131                                 btSolverBody& bodyA = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA];
1132                                 btSolverBody& bodyB = m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB];
1133                                 btScalar residual = resolveSingleConstraintRowGeneric(bodyA, bodyB, solveManifold);
1134                                 leastSquaresResidual += residual * residual;
1135                         }
1136                 }
1137
1138                 // apply rolling friction
1139                 int iFirstRollingFriction = m_rollingFrictionIndexTable[iContact];
1140                 if (totalImpulse > 0.0f && iFirstRollingFriction >= 0)
1141                 {
1142                         int iBegin = iFirstRollingFriction;
1143                         int iEnd = iBegin + 3;
1144                         for (int iRollingFric = iBegin; iRollingFric < iEnd; ++iRollingFric)
1145                         {
1146                                 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[iRollingFric];
1147                                 if (rollingFrictionConstraint.m_frictionIndex != iContact)
1148                                 {
1149                                         break;
1150                                 }
1151                                 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1152                                 if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1153                                 {
1154                                         rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1155                                 }
1156
1157                                 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1158                                 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1159
1160                                 btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1161                                 leastSquaresResidual += residual * residual;
1162                         }
1163                 }
1164         }
1165         return leastSquaresResidual;
1166 }
1167
1168 void btSequentialImpulseConstraintSolverMt::randomizeBatchedConstraintOrdering(btBatchedConstraints* batchedConstraints)
1169 {
1170         btBatchedConstraints& bc = *batchedConstraints;
1171         // randomize ordering of phases
1172         for (int ii = 1; ii < bc.m_phaseOrder.size(); ++ii)
1173         {
1174                 int iSwap = btRandInt2(ii + 1);
1175                 bc.m_phaseOrder.swap(ii, iSwap);
1176         }
1177
1178         // for each batch,
1179         for (int iBatch = 0; iBatch < bc.m_batches.size(); ++iBatch)
1180         {
1181                 // randomize ordering of constraints within the batch
1182                 const btBatchedConstraints::Range& batch = bc.m_batches[iBatch];
1183                 for (int iiCons = batch.begin; iiCons < batch.end; ++iiCons)
1184                 {
1185                         int iSwap = batch.begin + btRandInt2(iiCons - batch.begin + 1);
1186                         btAssert(iSwap >= batch.begin && iSwap < batch.end);
1187                         bc.m_constraintIndices.swap(iiCons, iSwap);
1188                 }
1189         }
1190 }
1191
1192 void btSequentialImpulseConstraintSolverMt::randomizeConstraintOrdering(int iteration, int numIterations)
1193 {
1194         // randomize ordering of joint constraints
1195         randomizeBatchedConstraintOrdering(&m_batchedJointConstraints);
1196
1197         //contact/friction constraints are not solved more than numIterations
1198         if (iteration < numIterations)
1199         {
1200                 randomizeBatchedConstraintOrdering(&m_batchedContactConstraints);
1201         }
1202 }
1203
1204 struct JointSolverLoop : public btIParallelSumBody
1205 {
1206         btSequentialImpulseConstraintSolverMt* m_solver;
1207         const btBatchedConstraints* m_bc;
1208         int m_iteration;
1209
1210         JointSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc, int iteration)
1211         {
1212                 m_solver = solver;
1213                 m_bc = bc;
1214                 m_iteration = iteration;
1215         }
1216         btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1217         {
1218                 BT_PROFILE("JointSolverLoop");
1219                 btScalar sum = 0;
1220                 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1221                 {
1222                         const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1223                         sum += m_solver->resolveMultipleJointConstraints(m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration);
1224                 }
1225                 return sum;
1226         }
1227 };
1228
1229 btScalar btSequentialImpulseConstraintSolverMt::resolveAllJointConstraints(int iteration)
1230 {
1231         BT_PROFILE("resolveAllJointConstraints");
1232         const btBatchedConstraints& batchedCons = m_batchedJointConstraints;
1233         JointSolverLoop loop(this, &batchedCons, iteration);
1234         btScalar leastSquaresResidual = 0.f;
1235         for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1236         {
1237                 int iPhase = batchedCons.m_phaseOrder[iiPhase];
1238                 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1239                 int grainSize = 1;
1240                 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1241         }
1242         return leastSquaresResidual;
1243 }
1244
1245 struct ContactSolverLoop : public btIParallelSumBody
1246 {
1247         btSequentialImpulseConstraintSolverMt* m_solver;
1248         const btBatchedConstraints* m_bc;
1249
1250         ContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
1251         {
1252                 m_solver = solver;
1253                 m_bc = bc;
1254         }
1255         btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1256         {
1257                 BT_PROFILE("ContactSolverLoop");
1258                 btScalar sum = 0;
1259                 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1260                 {
1261                         const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1262                         sum += m_solver->resolveMultipleContactConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
1263                 }
1264                 return sum;
1265         }
1266 };
1267
1268 btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraints()
1269 {
1270         BT_PROFILE("resolveAllContactConstraints");
1271         const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
1272         ContactSolverLoop loop(this, &batchedCons);
1273         btScalar leastSquaresResidual = 0.f;
1274         for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1275         {
1276                 int iPhase = batchedCons.m_phaseOrder[iiPhase];
1277                 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1278                 int grainSize = batchedCons.m_phaseGrainSize[iPhase];
1279                 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1280         }
1281         return leastSquaresResidual;
1282 }
1283
1284 struct ContactFrictionSolverLoop : public btIParallelSumBody
1285 {
1286         btSequentialImpulseConstraintSolverMt* m_solver;
1287         const btBatchedConstraints* m_bc;
1288
1289         ContactFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
1290         {
1291                 m_solver = solver;
1292                 m_bc = bc;
1293         }
1294         btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1295         {
1296                 BT_PROFILE("ContactFrictionSolverLoop");
1297                 btScalar sum = 0;
1298                 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1299                 {
1300                         const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1301                         sum += m_solver->resolveMultipleContactFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
1302                 }
1303                 return sum;
1304         }
1305 };
1306
1307 btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactFrictionConstraints()
1308 {
1309         BT_PROFILE("resolveAllContactFrictionConstraints");
1310         const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
1311         ContactFrictionSolverLoop loop(this, &batchedCons);
1312         btScalar leastSquaresResidual = 0.f;
1313         for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1314         {
1315                 int iPhase = batchedCons.m_phaseOrder[iiPhase];
1316                 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1317                 int grainSize = batchedCons.m_phaseGrainSize[iPhase];
1318                 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1319         }
1320         return leastSquaresResidual;
1321 }
1322
1323 struct InterleavedContactSolverLoop : public btIParallelSumBody
1324 {
1325         btSequentialImpulseConstraintSolverMt* m_solver;
1326         const btBatchedConstraints* m_bc;
1327
1328         InterleavedContactSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
1329         {
1330                 m_solver = solver;
1331                 m_bc = bc;
1332         }
1333         btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1334         {
1335                 BT_PROFILE("InterleavedContactSolverLoop");
1336                 btScalar sum = 0;
1337                 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1338                 {
1339                         const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1340                         sum += m_solver->resolveMultipleContactConstraintsInterleaved(m_bc->m_constraintIndices, batch.begin, batch.end);
1341                 }
1342                 return sum;
1343         }
1344 };
1345
1346 btScalar btSequentialImpulseConstraintSolverMt::resolveAllContactConstraintsInterleaved()
1347 {
1348         BT_PROFILE("resolveAllContactConstraintsInterleaved");
1349         const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
1350         InterleavedContactSolverLoop loop(this, &batchedCons);
1351         btScalar leastSquaresResidual = 0.f;
1352         for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1353         {
1354                 int iPhase = batchedCons.m_phaseOrder[iiPhase];
1355                 const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1356                 int grainSize = 1;
1357                 leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1358         }
1359         return leastSquaresResidual;
1360 }
1361
1362 struct ContactRollingFrictionSolverLoop : public btIParallelSumBody
1363 {
1364         btSequentialImpulseConstraintSolverMt* m_solver;
1365         const btBatchedConstraints* m_bc;
1366
1367         ContactRollingFrictionSolverLoop(btSequentialImpulseConstraintSolverMt* solver, const btBatchedConstraints* bc)
1368         {
1369                 m_solver = solver;
1370                 m_bc = bc;
1371         }
1372         btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
1373         {
1374                 BT_PROFILE("ContactFrictionSolverLoop");
1375                 btScalar sum = 0;
1376                 for (int iBatch = iBegin; iBatch < iEnd; ++iBatch)
1377                 {
1378                         const btBatchedConstraints::Range& batch = m_bc->m_batches[iBatch];
1379                         sum += m_solver->resolveMultipleContactRollingFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
1380                 }
1381                 return sum;
1382         }
1383 };
1384
1385 btScalar btSequentialImpulseConstraintSolverMt::resolveAllRollingFrictionConstraints()
1386 {
1387         BT_PROFILE("resolveAllRollingFrictionConstraints");
1388         btScalar leastSquaresResidual = 0.f;
1389         //
1390         // We do not generate batches for rolling friction constraints. We assume that
1391         // one of two cases is true:
1392         //
1393         //  1. either most bodies in the simulation have rolling friction, in which case we can use the
1394         //     batches for contacts and use a lookup table to translate contact indices to rolling friction
1395         //     (ignoring any contact indices that don't map to a rolling friction constraint). As long as
1396         //     most contacts have a corresponding rolling friction constraint, this should parallelize well.
1397         //
1398         //  -OR-
1399         //
1400         //  2. few bodies in the simulation have rolling friction, so it is not worth trying to use the
1401         //     batches from contacts as most of the contacts won't have corresponding rolling friction
1402         //     constraints and most threads would end up doing very little work. Most of the time would
1403         //     go to threading overhead, so we don't bother with threading.
1404         //
1405         int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1406         if (numRollingFrictionPoolConstraints >= m_tmpSolverContactConstraintPool.size())
1407         {
1408                 // use batching if there are many rolling friction constraints
1409                 const btBatchedConstraints& batchedCons = m_batchedContactConstraints;
1410                 ContactRollingFrictionSolverLoop loop(this, &batchedCons);
1411                 btScalar leastSquaresResidual = 0.f;
1412                 for (int iiPhase = 0; iiPhase < batchedCons.m_phases.size(); ++iiPhase)
1413                 {
1414                         int iPhase = batchedCons.m_phaseOrder[iiPhase];
1415                         const btBatchedConstraints::Range& phase = batchedCons.m_phases[iPhase];
1416                         int grainSize = 1;
1417                         leastSquaresResidual += btParallelSum(phase.begin, phase.end, grainSize, loop);
1418                 }
1419         }
1420         else
1421         {
1422                 // no batching, also ignores SOLVER_RANDMIZE_ORDER
1423                 for (int j = 0; j < numRollingFrictionPoolConstraints; j++)
1424                 {
1425                         btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1426                         if (rollingFrictionConstraint.m_frictionIndex >= 0)
1427                         {
1428                                 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1429                                 if (totalImpulse > 0.0f)
1430                                 {
1431                                         btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction * totalImpulse;
1432                                         if (rollingFrictionMagnitude > rollingFrictionConstraint.m_friction)
1433                                                 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1434
1435                                         rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1436                                         rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1437
1438                                         btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA], m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB], rollingFrictionConstraint);
1439                                         leastSquaresResidual += residual * residual;
1440                                 }
1441                         }
1442                 }
1443         }
1444         return leastSquaresResidual;
1445 }
1446
1447 void btSequentialImpulseConstraintSolverMt::internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
1448 {
1449         BT_PROFILE("internalWriteBackContacts");
1450         writeBackContacts(iBegin, iEnd, infoGlobal);
1451         //for ( int iContact = iBegin; iContact < iEnd; ++iContact)
1452         //{
1453         //    const btSolverConstraint& contactConstraint = m_tmpSolverContactConstraintPool[ iContact ];
1454         //    btManifoldPoint* pt = (btManifoldPoint*) contactConstraint.m_originalContactPoint;
1455         //    btAssert( pt );
1456         //    pt->m_appliedImpulse = contactConstraint.m_appliedImpulse;
1457         //    pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex ].m_appliedImpulse;
1458         //    if ( m_numFrictionDirections == 2 )
1459         //    {
1460         //        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[ contactConstraint.m_frictionIndex + 1 ].m_appliedImpulse;
1461         //    }
1462         //}
1463 }
1464
1465 void btSequentialImpulseConstraintSolverMt::internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
1466 {
1467         BT_PROFILE("internalWriteBackJoints");
1468         writeBackJoints(iBegin, iEnd, infoGlobal);
1469 }
1470
1471 void btSequentialImpulseConstraintSolverMt::internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal)
1472 {
1473         BT_PROFILE("internalWriteBackBodies");
1474         writeBackBodies(iBegin, iEnd, infoGlobal);
1475 }
1476
1477 struct WriteContactPointsLoop : public btIParallelForBody
1478 {
1479         btSequentialImpulseConstraintSolverMt* m_solver;
1480         const btContactSolverInfo* m_infoGlobal;
1481
1482         WriteContactPointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
1483         {
1484                 m_solver = solver;
1485                 m_infoGlobal = &infoGlobal;
1486         }
1487         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1488         {
1489                 m_solver->internalWriteBackContacts(iBegin, iEnd, *m_infoGlobal);
1490         }
1491 };
1492
1493 struct WriteJointsLoop : public btIParallelForBody
1494 {
1495         btSequentialImpulseConstraintSolverMt* m_solver;
1496         const btContactSolverInfo* m_infoGlobal;
1497
1498         WriteJointsLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
1499         {
1500                 m_solver = solver;
1501                 m_infoGlobal = &infoGlobal;
1502         }
1503         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1504         {
1505                 m_solver->internalWriteBackJoints(iBegin, iEnd, *m_infoGlobal);
1506         }
1507 };
1508
1509 struct WriteBodiesLoop : public btIParallelForBody
1510 {
1511         btSequentialImpulseConstraintSolverMt* m_solver;
1512         const btContactSolverInfo* m_infoGlobal;
1513
1514         WriteBodiesLoop(btSequentialImpulseConstraintSolverMt* solver, const btContactSolverInfo& infoGlobal)
1515         {
1516                 m_solver = solver;
1517                 m_infoGlobal = &infoGlobal;
1518         }
1519         void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1520         {
1521                 m_solver->internalWriteBackBodies(iBegin, iEnd, *m_infoGlobal);
1522         }
1523 };
1524
1525 btScalar btSequentialImpulseConstraintSolverMt::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
1526 {
1527         BT_PROFILE("solveGroupCacheFriendlyFinish");
1528
1529         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1530         {
1531                 WriteContactPointsLoop loop(this, infoGlobal);
1532                 int grainSize = 500;
1533                 btParallelFor(0, m_tmpSolverContactConstraintPool.size(), grainSize, loop);
1534         }
1535
1536         {
1537                 WriteJointsLoop loop(this, infoGlobal);
1538                 int grainSize = 400;
1539                 btParallelFor(0, m_tmpSolverNonContactConstraintPool.size(), grainSize, loop);
1540         }
1541         {
1542                 WriteBodiesLoop loop(this, infoGlobal);
1543                 int grainSize = 100;
1544                 btParallelFor(0, m_tmpSolverBodyPool.size(), grainSize, loop);
1545         }
1546
1547         m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1548         m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1549         m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1550         m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1551
1552         m_tmpSolverBodyPool.resizeNoInitialize(0);
1553         return 0.f;
1554 }