Bullet Collision Detection & Physics Library
btSequentialImpulseConstraintSolverMt.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
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 
17 
18 #include "LinearMath/btQuickprof.h"
19 
21 
24 
25 bool btSequentialImpulseConstraintSolverMt::s_allowNestedParallelForLoops = false; // some task schedulers don't like nested loops
31 
33 {
35  m_useBatching = false;
37 }
38 
40 {
41 }
42 
44 {
45  BT_PROFILE("setupBatchedContactConstraints");
52 }
53 
55 {
56  BT_PROFILE("setupBatchedJointConstraints");
63 }
64 
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,
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 
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,
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  {
171 
182  btSolverConstraint* frictionConstraint1 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex];
183  btAssert(frictionConstraint1->m_frictionIndex == iContactConstraint);
184 
185  btSolverConstraint* frictionConstraint2 = NULL;
187  {
188  frictionConstraint2 = &m_tmpSolverContactFrictionConstraintPool[contactConstraint.m_frictionIndex + 1];
189  btAssert(frictionConstraint2->m_frictionIndex == iContactConstraint);
190  }
191 
193  {
194  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
195  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
197  {
198  cp.m_lateralFrictionDir1 *= 1.f / btSqrt(lat_rel_vel);
201  setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
202 
203  if (frictionConstraint2)
204  {
209  setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
210  }
211  }
212  else
213  {
215 
218  setupFrictionConstraint(*frictionConstraint1, cp.m_lateralFrictionDir1, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
219 
220  if (frictionConstraint2)
221  {
224  setupFrictionConstraint(*frictionConstraint2, cp.m_lateralFrictionDir2, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, infoGlobal);
225  }
226 
228  {
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 
247 {
251 
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];
268  }
269  }
270  }
271 };
272 
274 {
275  BT_PROFILE("setupAllContactConstraints");
276  if (m_useBatching)
277  {
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 
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  {
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  }
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;
338  {
340  // now that we have the lock, check again
342  {
343  m_kinematicBodyUniqueIdToSolverBodyTable.resize(uniqueId + 1, INVALID_SOLVER_BODY_ID);
344  }
346  }
348  // if no table entry yet,
349  if (INVALID_SOLVER_BODY_ID == solverBodyId)
350  {
351  // need to acquire both locks
354  // now that we have the lock, check again
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);
363  }
366  }
367  }
368  else
369  {
370  // all fixed bodies (inf mass) get mapped to a single solver id
371  if (m_fixedBodyId < 0)
372  {
374  // now that we have the lock, check again
375  if (m_fixedBodyId < 0)
376  {
378  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
379  initSolverBody(&fixedBody, 0, timeStep);
380  }
382  }
383  solverBodyId = m_fixedBodyId;
384  }
385  btAssert(solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size());
386  return solverBodyId;
387 }
388 
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 
431 {
436 
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  {
447  }
448 };
449 
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 
498 {
501 
503  {
504  m_solver = solver;
505  m_cachedInfoArray = cachedInfoArray;
506  }
507  void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
508  {
510  }
511 };
512 
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);
558  m_tmpSolverContactRollingFrictionConstraintPool.reserve(numRollingFrictionConstraints + extraReserve);
559  }
564  }
565  }
566  {
567  AllocContactConstraintsLoop loop(this, &cachedInfoArray[0]);
568  int grainSize = 200;
569  btParallelFor(0, numManifolds, grainSize, loop);
570  }
571 }
572 
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  {
586  btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
587  initSolverBody(&fixedBody, 0, infoGlobal.m_timeStep);
588  }
589  allocAllContactConstraints(manifoldPtr, numManifolds, infoGlobal);
590  if (m_useBatching)
591  {
593  }
594  setupAllContactConstraints(infoGlobal);
595  }
596 }
597 
599 {
600  BT_PROFILE("internalInitMultipleJoints");
601  for (int i = iBegin; i < iEnd; i++)
602  {
603  btTypedConstraint* constraint = constraints[i];
605  if (constraint->isEnabled())
606  {
607  constraint->buildJacobian();
608  constraint->internalSetAppliedImpulse(0.0f);
609  btJointFeedback* fb = constraint->getJointFeedback();
610  if (fb)
611  {
616  }
617  constraint->getInfo1(&info1);
618  }
619  else
620  {
621  info1.m_numConstraintRows = 0;
622  info1.nub = 0;
623  }
624  }
625 }
626 
628 {
631 
633  {
634  m_solver = solver;
635  m_constraints = constraints;
636  }
637  void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
638  {
640  }
641 };
642 
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  {
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 
665 {
670 
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  {
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;
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];
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  }
732 
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  }
745 }
746 
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);
762  {
763  gyroForce = body->computeGyroscopicForceExplicit(infoGlobal.m_maxGyroscopicForce);
764  solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
765  }
767  {
768  gyroForce = body->computeGyroscopicImpulseImplicit_World(infoGlobal.m_timeStep);
769  solverBody.m_externalTorqueImpulse += gyroForce;
770  }
772  {
773  gyroForce = body->computeGyroscopicImpulseImplicit_Body(infoGlobal.m_timeStep);
774  solverBody.m_externalTorqueImpulse += gyroForce;
775  }
776  }
777  }
778 }
779 
781 {
786 
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  {
799  }
800 };
801 
803 {
804  BT_PROFILE("convertBodies");
806 
808 
809  m_fixedBodyId = numBodies;
810  {
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 
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 {
839  m_useBatching = false;
840  if (numManifolds >= s_minimumContactManifoldsForBatching &&
842  {
843  m_useBatching = true;
846  }
848  numBodies,
849  manifoldPtr,
850  numManifolds,
851  constraints,
852  numConstraints,
853  infoGlobal,
854  debugDrawer);
855  return 0.0f;
856 }
857 
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 
874 {
877 
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];
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  {
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
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  {
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
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 
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 
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 
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 
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 
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 
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];
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  {
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 
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 
1193 {
1194  // randomize ordering of joint constraints
1196 
1197  //contact/friction constraints are not solved more than numIterations
1198  if (iteration < numIterations)
1199  {
1201  }
1202 }
1203 
1205 {
1209 
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];
1224  }
1225  return sum;
1226  }
1227 };
1228 
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 
1246 {
1249 
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];
1263  }
1264  return sum;
1265  }
1266 };
1267 
1269 {
1270  BT_PROFILE("resolveAllContactConstraints");
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 
1285 {
1288 
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];
1302  }
1303  return sum;
1304  }
1305 };
1306 
1308 {
1309  BT_PROFILE("resolveAllContactFrictionConstraints");
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 
1324 {
1327 
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];
1341  }
1342  return sum;
1343  }
1344 };
1345 
1347 {
1348  BT_PROFILE("resolveAllContactConstraintsInterleaved");
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 
1363 {
1366 
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];
1380  }
1381  return sum;
1382  }
1383 };
1384 
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
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  {
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 
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 
1466 {
1467  BT_PROFILE("internalWriteBackJoints");
1468  writeBackJoints(iBegin, iEnd, infoGlobal);
1469 }
1470 
1472 {
1473  BT_PROFILE("internalWriteBackBodies");
1474  writeBackBodies(iBegin, iEnd, infoGlobal);
1475 }
1476 
1478 {
1481 
1483  {
1484  m_solver = solver;
1485  m_infoGlobal = &infoGlobal;
1486  }
1487  void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1488  {
1490  }
1491 };
1492 
1494 {
1497 
1499  {
1500  m_solver = solver;
1501  m_infoGlobal = &infoGlobal;
1502  }
1503  void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1504  {
1506  }
1507 };
1508 
1510 {
1513 
1515  {
1516  m_solver = solver;
1517  m_infoGlobal = &infoGlobal;
1518  }
1519  void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1520  {
1522  }
1523 };
1524 
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;
1540  }
1541  {
1542  WriteBodiesLoop loop(this, infoGlobal);
1543  int grainSize = 100;
1544  btParallelFor(0, m_tmpSolverBodyPool.size(), grainSize, loop);
1545  }
1546 
1551 
1553  return 0.f;
1554 }
static T sum(const btAlignedObjectArray< T > &items)
static const btRigidBody * upcast(const btCollisionObject *colObj)
to keep collision detection and dynamics separate we don&#39;t store a rigidbody pointer but a rigidbody ...
Definition: btRigidBody.h:189
void internalAllocContactConstraints(const btContactManifoldCachedInfo *cachedInfoArray, int numManifolds)
#define SIMD_EPSILON
Definition: btScalar.h:523
btScalar resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd)
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
WriteJointsLoop(btSequentialImpulseConstraintSolverMt *solver, const btContactSolverInfo &infoGlobal)
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal) BT_OVERRIDE
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
void setupFrictionConstraint(btSolverConstraint &solverConstraint, const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
virtual void getInfo1(btConstraintInfo1 *info)=0
internal method used by the constraint solver, don&#39;t use them directly
btSequentialImpulseConstraintSolverMt * m_solver
const btVector3 & getPositionWorldOnA() const
btVector3 m_lateralFrictionDir1
btSequentialImpulseConstraintSolverMt * m_solver
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer) BT_OVERRIDE
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
explicit version is best avoided, it gains energy
btScalar resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd)
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
ContactSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
virtual void convertBodies(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal) BT_OVERRIDE
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
const btRigidBody & getRigidBodyA() const
void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo &infoGlobal)
ContactSplitPenetrationImpulseSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
void resizeNoInitialize(int newsize)
resize changes the number of elements in the array.
btScalar btSqrt(btScalar y)
Definition: btScalar.h:446
#define btAssert(x)
Definition: btScalar.h:133
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
virtual void randomizeConstraintOrdering(int iteration, int numIterations)
bool isKinematicObject() const
btSequentialImpulseConstraintSolverMt * m_solver
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btScalar m_frictionCFM
const btJointFeedback * getJointFeedback() const
btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo * m_cachedInfoArray
ManifoldContactPoint collects and maintains persistent contactpoints.
btAlignedObjectArray< int > m_phaseOrder
btScalar m_contactMotion1
const btContactSolverInfo * m_infoGlobal
ContactFrictionSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
static btBatchedConstraints::BatchingMethod s_jointBatchingMethod
const btRigidBody & getRigidBodyB() const
static btBatchedConstraints::BatchingMethod s_contactBatchingMethod
void initSolverBody(btSolverBody *solverBody, btCollisionObject *collisionObject, btScalar timeStep)
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
btSequentialImpulseConstraintSolverMt * m_solver
bool isStaticOrKinematicObject() const
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
void swap(int index0, int index1)
WriteBodiesLoop(btSequentialImpulseConstraintSolverMt *solver, const btContactSolverInfo &infoGlobal)
btVector3 m_externalTorqueImpulse
Definition: btSolverBody.h:118
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btVector3 m_appliedForceBodyB
btSequentialImpulseConstraintSolverMt * m_solver
void internalConvertMultipleJoints(const btAlignedObjectArray< JointParams > &jointParamsArray, btTypedConstraint **constraints, int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
const btManifoldPoint & getContactPoint(int index) const
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
BT_ENABLE_GYROPSCOPIC_FORCE flags is enabled by default in Bullet 2.83 and onwards.
Definition: btRigidBody.h:45
const btAlignedObjectArray< btSequentialImpulseConstraintSolverMt::JointParams > & m_jointParamsArray
btScalar m_combinedRollingFriction
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
SetupContactConstraintsLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc, const btContactSolverInfo &infoGlobal)
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
const btContactSolverInfo & m_infoGlobal
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const
perform implicit force computation in world space
void setupTorsionalFrictionConstraint(btSolverConstraint &solverConstraint, const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.)
btVector3 m_normalWorldOnB
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btVector3 m_appliedForceBodyA
ContactRollingFrictionSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
bool btThreadsAreRunning()
Definition: btThreads.cpp:381
void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo *cachedInfoArray, btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo * m_cachedInfoArray
#define BT_OVERRIDE
Definition: btThreads.h:26
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
void setFrictionConstraintImpulse(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal)
InterleavedContactSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
const btVector3 & getPositionWorldOnB() const
void lock()
Definition: btThreads.cpp:196
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
const btCollisionObject * getBody0() const
void btSwap(T &a, T &b)
Definition: btScalar.h:623
bool isEnabled() const
btScalar getInvMass() const
Definition: btRigidBody.h:261
const btContactSolverInfo * m_infoGlobal
void randomizeBatchedConstraintOrdering(btBatchedConstraints *batchedConstraints)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
void internalConvertBodies(btCollisionObject **bodies, int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
void unlock()
Definition: btThreads.cpp:201
btCollisionObject can be used to manage collision detection objects.
int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:26
void setZero()
Definition: btVector3.h:671
void internalInitMultipleJoints(btTypedConstraint **constraints, int iBegin, int iEnd)
CollectContactManifoldCachedInfoLoop(btSequentialImpulseConstraintSolverMt *solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo *cachedInfoArray, btPersistentManifold **manifoldPtr, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< Range > m_phases
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
int getFlags() const
Definition: btRigidBody.h:516
btScalar getContactProcessingThreshold() const
btAlignedObjectArray< int > m_constraintIndices
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
ConvertJointsLoop(btSequentialImpulseConstraintSolverMt *solver, const btAlignedObjectArray< btSequentialImpulseConstraintSolverMt::JointParams > &jointParamsArray, btTypedConstraint **srcConstraints, const btContactSolverInfo &infoGlobal)
void setCompanionId(int id)
btScalar resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray< int > &contactIndices, int batchBegin, int batchEnd)
int getWorldArrayIndex() const
btSequentialImpulseConstraintSolverMt * m_solver
btScalar resolveMultipleContactFrictionConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd)
btSequentialImpulseConstraintSolverMt * m_solver
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
void getVelocityInLocalPointNoDelta(const btVector3 &rel_pos, btVector3 &velocity) const
Definition: btSolverBody.h:131
int size() const
return the number of elements in the array
btScalar resolveSplitPenetrationImpulse(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
virtual void solveConstraintObsolete(btSolverBody &, btSolverBody &, btScalar)
internal method used by the constraint solver, don&#39;t use them directly
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
btSequentialImpulseConstraintSolverMt * m_solver
btAlignedObjectArray< char > m_phaseGrainSize
btScalar resolveMultipleContactConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd)
btVector3 m_appliedTorqueBodyB
void setup(btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, BatchingMethod batchingMethod, int minBatchSize, int maxBatchSize, btAlignedObjectArray< char > *scratchMemory)
const btBatchedConstraints * m_bc
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:103
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
JointSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc, int iteration)
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:126
TypedConstraint is the baseclass for Bullet constraints and vehicles.
void resize(int newsize, const T &fillData=T())
btRigidBody * m_originalBody
Definition: btSolverBody.h:120
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer) BT_OVERRIDE
const btCollisionObject * getBody1() const
void setupAllContactConstraints(const btContactSolverInfo &infoGlobal)
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
int getOrInitSolverBodyThreadsafe(btCollisionObject &body, btScalar timeStep)
btScalar btParallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody &body)
Definition: btThreads.cpp:439
btIDebugDraw * m_debugDrawer
btScalar m_contactMotion2
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal) BT_OVERRIDE
AllocContactConstraintsLoop(btSequentialImpulseConstraintSolverMt *solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo *cachedInfoArray)
void allocAllContactConstraints(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
virtual void convertJoints(btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal)
T & expand(const T &fillValue=T())
btVector3 m_appliedTorqueBodyA
btSequentialImpulseConstraintSolverMt * m_solver
btSequentialImpulseConstraintSolverMt * m_solver
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
const btContactSolverInfo & m_infoGlobal
void setupContactConstraint(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, const btVector3 &rel_pos1, const btVector3 &rel_pos2)
InitJointsLoop(btSequentialImpulseConstraintSolverMt *solver, btTypedConstraint **constraints)
void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody &body)
Definition: btThreads.cpp:412
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:262
virtual void convertJoints(btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal) BT_OVERRIDE
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don&#39;t use them directly
WriteContactPointsLoop(btSequentialImpulseConstraintSolverMt *solver, const btContactSolverInfo &infoGlobal)
btVector3 m_lateralFrictionDir2
btSimdScalar m_appliedImpulse
btScalar resolveMultipleJointConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd, int iteration)
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
btSequentialImpulseConstraintSolverMt * m_solver
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
btSequentialImpulseConstraintSolverMt * m_solver
int getCompanionId() const
void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btScalar getDistance() const
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const
perform implicit force computation in body space (inertial frame)
virtual void buildJacobian()
internal method used by the constraint solver, don&#39;t use them directly
btAlignedObjectArray< Range > m_batches
void convertJoint(btSolverConstraint *currentConstraintRow, btTypedConstraint *constraint, const btTypedConstraint::btConstraintInfo1 &info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo &infoGlobal)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btSequentialImpulseConstraintSolverMt * m_solver
btScalar m_combinedSpinningFriction
ConvertBodiesLoop(btSequentialImpulseConstraintSolverMt *solver, btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer) BT_OVERRIDE
btAlignedObjectArray< int > m_kinematicBodyUniqueIdToSolverBodyTable
static int uniqueId
Definition: btRigidBody.cpp:27
void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)