Blender  V3.3
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");
47  m_tmpSolverBodyPool,
52 }
53 
55 {
56  BT_PROFILE("setupBatchedJointConstraints");
58  m_tmpSolverBodyPool,
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  {
206  cp.m_lateralFrictionDir2.normalize(); //??
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;
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  {
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 
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  {
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  {
377  m_fixedBodyId = m_tmpSolverBodyPool.size();
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 {
433  btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* m_cachedInfoArray;
436 
437  CollectContactManifoldCachedInfoLoop(btSequentialImpulseConstraintSolverMt* solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo* cachedInfoArray, btPersistentManifold** manifoldPtr, const btContactSolverInfo& infoGlobal)
438  {
439  m_solver = solver;
440  m_cachedInfoArray = cachedInfoArray;
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 
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 {
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 
514 {
515  BT_PROFILE("allocAllContactConstraints");
516  btAlignedObjectArray<btContactManifoldCachedInfo> cachedInfoArray; // = m_manifoldCachedInfoArray;
517  cachedInfoArray.resizeNoInitialize(numManifolds);
518  if (/* DISABLES CODE */ (false))
519  {
520  // sequential
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  {
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  }
590  if (m_useBatching)
591  {
593  }
595  }
596 }
597 
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 
628 {
631 
633  {
634  m_solver = solver;
636  }
637  void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
638  {
639  m_solver->internalInitMultipleJoints(m_constraints, iBegin, iEnd);
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  {
652  const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
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),
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 
687 {
688  if (!m_useBatching)
689  {
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  {
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  }
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  {
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  {
764  solverBody.m_externalTorqueImpulse -= gyroForce * body->getInvInertiaTensorWorld() * infoGlobal.m_timeStep;
765  }
767  {
769  solverBody.m_externalTorqueImpulse += gyroForce;
770  }
772  {
774  solverBody.m_externalTorqueImpulse += gyroForce;
775  }
776  }
777  }
778 }
779 
781 {
786 
788  btCollisionObject** bodies,
789  int numBodies,
791  {
792  m_solver = solver;
793  m_bodies = bodies;
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 
803 {
804  BT_PROFILE("convertBodies");
806 
807  m_tmpSolverBodyPool.resizeNoInitialize(numBodies + 1);
808 
810  {
811  btSolverBody& fixedBody = m_tmpSolverBodyPool[m_fixedBodyId];
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  {
825  }
826 }
827 
829  btCollisionObject** bodies,
830  int numBodies,
832  int numManifolds,
834  int numConstraints,
836  btIDebugDraw* debugDrawer)
837 {
839  m_useBatching = false;
842  {
843  m_useBatching = true;
846  }
848  numBodies,
849  manifoldPtr,
850  numManifolds,
851  constraints,
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  {
890  sum += m_solver->resolveMultipleContactSplitPenetrationImpulseConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
891  }
892  return sum;
893  }
894 };
895 
897 {
898  BT_PROFILE("solveGroupCacheFriendlySplitImpulseIterations");
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 
934 {
935  if (!m_useBatching)
936  {
938  }
939  BT_PROFILE("solveSingleIterationMt");
940  btScalar leastSquaresResidual = 0.f;
941 
943  {
944  if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
945  {
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  {
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];
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  {
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];
1223  sum += m_solver->resolveMultipleJointConstraints(m_bc->m_constraintIndices, batch.begin, batch.end, m_iteration);
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];
1262  sum += m_solver->resolveMultipleContactConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
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];
1301  sum += m_solver->resolveMultipleContactFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
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];
1340  sum += m_solver->resolveMultipleContactConstraintsInterleaved(m_bc->m_constraintIndices, batch.begin, batch.end);
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];
1379  sum += m_solver->resolveMultipleContactRollingFrictionConstraints(m_bc->m_constraintIndices, batch.begin, batch.end);
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;
1486  }
1487  void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1488  {
1489  m_solver->internalWriteBackContacts(iBegin, iEnd, *m_infoGlobal);
1490  }
1491 };
1492 
1494 {
1497 
1499  {
1500  m_solver = solver;
1502  }
1503  void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1504  {
1505  m_solver->internalWriteBackJoints(iBegin, iEnd, *m_infoGlobal);
1506  }
1507 };
1508 
1510 {
1513 
1515  {
1516  m_solver = solver;
1518  }
1519  void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
1520  {
1521  m_solver->internalWriteBackBodies(iBegin, iEnd, *m_infoGlobal);
1522  }
1523 };
1524 
1526 {
1527  BT_PROFILE("solveGroupCacheFriendlyFinish");
1528 
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 
1552  m_tmpSolverBodyPool.resizeNoInitialize(0);
1553  return 0.f;
1554 }
btCollisionObject
@ CF_ANISOTROPIC_FRICTION
@ CF_ANISOTROPIC_ROLLING_FRICTION
const btRigidBody & getRigidBodyA() const
const btRigidBody & getRigidBodyB() const
@ SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
@ SOLVER_USE_WARMSTARTING
@ SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
@ SOLVER_RANDMIZE_ORDER
@ SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
@ SOLVER_USE_2_FRICTION_DIRECTIONS
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btPersistentManifold()
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
static int uniqueId
Definition: btRigidBody.cpp:27
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY
Definition: btRigidBody.h:47
@ BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT
Definition: btRigidBody.h:45
@ BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD
Definition: btRigidBody.h:46
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
SIMD_FORCE_INLINE void btSwap(T &a, T &b)
Definition: btScalar.h:643
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:295
virtual void setupBatchedContactConstraints()
void internalCollectContactManifoldCachedInfo(btContactManifoldCachedInfo *cachedInfoArray, btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
virtual btScalar resolveAllRollingFrictionConstraints()
void internalWriteBackBodies(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btBatchedConstraints m_batchedContactConstraints
btAlignedObjectArray< int > m_rollingFrictionIndexTable
void internalAllocContactConstraints(const btContactManifoldCachedInfo *cachedInfoArray, int numManifolds)
void internalConvertMultipleJoints(const btAlignedObjectArray< JointParams > &jointParamsArray, btTypedConstraint **constraints, int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
virtual btScalar resolveAllJointConstraints(int iteration)
void internalWriteBackContacts(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
virtual void randomizeConstraintOrdering(int iteration, int numIterations)
btSpinMutex m_kinematicBodyUniqueIdToSolverBodyTableMutex
btBatchedConstraints m_batchedJointConstraints
static btBatchedConstraints::BatchingMethod s_contactBatchingMethod
int getOrInitSolverBodyThreadsafe(btCollisionObject &body, btScalar timeStep)
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint ** constraints
btScalar resolveMultipleContactRollingFrictionConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd)
btScalar resolveMultipleContactConstraintsInterleaved(const btAlignedObjectArray< int > &contactIndices, int batchBegin, int batchEnd)
btSequentialImpulseConstraintSolverMt int btPersistentManifold ** manifoldPtr
virtual btScalar resolveAllContactFrictionConstraints()
void internalInitMultipleJoints(btTypedConstraint **constraints, int iBegin, int iEnd)
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo btIDebugDraw *debugDrawer BT_OVERRIDE
btSpinMutex m_bodySolverArrayMutex
void randomizeBatchedConstraintOrdering(btBatchedConstraints *batchedConstraints)
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
virtual void setupBatchedJointConstraints()
void internalWriteBackJoints(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< char > m_scratchMemory
btSequentialImpulseConstraintSolverMt int numBodies
void internalSetupContactConstraints(int iContactConstraint, const btContactSolverInfo &infoGlobal)
btScalar resolveMultipleContactSplitPenetrationImpulseConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd)
static bool s_allowNestedParallelForLoops
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
virtual btScalar resolveAllContactConstraints()
virtual ~btSequentialImpulseConstraintSolverMt()
btScalar resolveMultipleJointConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd, int iteration)
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
void allocAllContactConstraints(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
void setupAllContactConstraints(const btContactSolverInfo &infoGlobal)
virtual btScalar resolveAllContactConstraintsInterleaved()
btScalar resolveMultipleContactFrictionConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd)
btScalar resolveMultipleContactConstraints(const btAlignedObjectArray< int > &consIndices, int batchBegin, int batchEnd)
void internalConvertBodies(btCollisionObject **bodies, int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
static int s_minimumContactManifoldsForBatching
static btBatchedConstraints::BatchingMethod s_jointBatchingMethod
void initSolverBody(btSolverBody *solverBody, btCollisionObject *collisionObject, btScalar timeStep)
virtual void convertJoints(btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal)
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool
btAlignedObjectArray< int > m_orderTmpConstraintPool
virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
void convertJoint(btSolverConstraint *currentConstraintRow, btTypedConstraint *constraint, const btTypedConstraint::btConstraintInfo1 &info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< int > m_kinematicBodyUniqueIdToSolverBodyTable
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
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.)
btScalar resolveSplitPenetrationImpulse(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
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.)
btConstraintArray m_tmpSolverContactFrictionConstraintPool
btConstraintArray m_tmpSolverContactConstraintPool
virtual void convertBodies(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
void setFrictionConstraintImpulse(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal)
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo &infoGlobal)
btConstraintArray m_tmpSolverNonContactConstraintPool
void setupContactConstraint(btSolverConstraint &solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, const btVector3 &rel_pos1, const btVector3 &rel_pos2)
btAlignedObjectArray< btTypedConstraint::btConstraintInfo1 > m_tmpConstraintSizesPool
static T sum(const btAlignedObjectArray< T > &items)
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btVector3 m_invMass
Definition: btSolverBody.h:112
btSolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btScalar btParallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody &body)
Definition: btThreads.cpp:439
bool btThreadsAreRunning()
Definition: btThreads.cpp:381
void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody &body)
Definition: btThreads.cpp:412
bool isEnabled() const
btJointFeedback
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
SIMD_FORCE_INLINE btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE void reserve(int _Count)
SIMD_FORCE_INLINE int capacity() const
return the pre-allocated (reserved) elements, this is at least as large as the total number of elemen...
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
void swap(int index0, int index1)
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
btScalar m_frictionCFM
btScalar m_combinedSpinningFriction
btScalar m_combinedRollingFriction
btScalar getDistance() const
const btVector3 & getPositionWorldOnB() const
btVector3 m_lateralFrictionDir2
const btVector3 & getPositionWorldOnA() const
btVector3 m_normalWorldOnB
btScalar m_contactMotion2
btVector3 m_lateralFrictionDir1
btScalar m_contactMotion1
int getFlags() const
Definition: btRigidBody.h:608
btScalar getInvMass() const
Definition: btRigidBody.h:263
static const btRigidBody * upcast(const btCollisionObject *colObj)
Definition: btRigidBody.h:189
btVector3 computeGyroscopicImpulseImplicit_World(btScalar dt) const
perform implicit force computation in world space
btVector3 computeGyroscopicImpulseImplicit_Body(btScalar step) const
perform implicit force computation in body space (inertial frame)
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btVector3 computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
explicit version is best avoided, it gains energy
void lock()
Definition: btThreads.cpp:196
void unlock()
Definition: btThreads.cpp:201
struct @653::@655 batch
BLI_INLINE float fb(float length, float L)
uiWidgetBaseParameters params[MAX_WIDGET_BASE_BATCH]
const btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo * m_cachedInfoArray
btSequentialImpulseConstraintSolverMt * m_solver
AllocContactConstraintsLoop(btSequentialImpulseConstraintSolverMt *solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo *cachedInfoArray)
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo * m_cachedInfoArray
CollectContactManifoldCachedInfoLoop(btSequentialImpulseConstraintSolverMt *solver, btSequentialImpulseConstraintSolverMt::btContactManifoldCachedInfo *cachedInfoArray, btPersistentManifold **manifoldPtr, const btContactSolverInfo &infoGlobal)
ContactFrictionSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
btSequentialImpulseConstraintSolverMt * m_solver
btSequentialImpulseConstraintSolverMt * m_solver
ContactRollingFrictionSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
btSequentialImpulseConstraintSolverMt * m_solver
ContactSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
ContactSplitPenetrationImpulseSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
btSequentialImpulseConstraintSolverMt * m_solver
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
ConvertBodiesLoop(btSequentialImpulseConstraintSolverMt *solver, btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
const btContactSolverInfo & m_infoGlobal
btSequentialImpulseConstraintSolverMt * m_solver
const btContactSolverInfo & m_infoGlobal
const btAlignedObjectArray< btSequentialImpulseConstraintSolverMt::JointParams > & m_jointParamsArray
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
ConvertJointsLoop(btSequentialImpulseConstraintSolverMt *solver, const btAlignedObjectArray< btSequentialImpulseConstraintSolverMt::JointParams > &jointParamsArray, btTypedConstraint **srcConstraints, const btContactSolverInfo &infoGlobal)
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
InitJointsLoop(btSequentialImpulseConstraintSolverMt *solver, btTypedConstraint **constraints)
btSequentialImpulseConstraintSolverMt * m_solver
InterleavedContactSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc)
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
btSequentialImpulseConstraintSolverMt * m_solver
JointSolverLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc, int iteration)
btScalar sumLoop(int iBegin, int iEnd) const BT_OVERRIDE
btSequentialImpulseConstraintSolverMt * m_solver
const btBatchedConstraints * m_bc
SetupContactConstraintsLoop(btSequentialImpulseConstraintSolverMt *solver, const btBatchedConstraints *bc, const btContactSolverInfo &infoGlobal)
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btSequentialImpulseConstraintSolverMt * m_solver
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
const btContactSolverInfo * m_infoGlobal
WriteBodiesLoop(btSequentialImpulseConstraintSolverMt *solver, const btContactSolverInfo &infoGlobal)
btSequentialImpulseConstraintSolverMt * m_solver
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
WriteContactPointsLoop(btSequentialImpulseConstraintSolverMt *solver, const btContactSolverInfo &infoGlobal)
btSequentialImpulseConstraintSolverMt * m_solver
WriteJointsLoop(btSequentialImpulseConstraintSolverMt *solver, const btContactSolverInfo &infoGlobal)
const btContactSolverInfo * m_infoGlobal
void forLoop(int iBegin, int iEnd) const BT_OVERRIDE
btSequentialImpulseConstraintSolverMt * m_solver
btAlignedObjectArray< Range > m_batches
btAlignedObjectArray< int > m_constraintIndices
void setup(btConstraintArray *constraints, const btAlignedObjectArray< btSolverBody > &bodies, BatchingMethod batchingMethod, int minBatchSize, int maxBatchSize, btAlignedObjectArray< char > *scratchMemory)
btAlignedObjectArray< char > m_phaseGrainSize
btAlignedObjectArray< int > m_phaseOrder
btAlignedObjectArray< Range > m_phases
btIDebugDraw * m_debugDrawer
bool contactHasRollingFriction[MAX_NUM_CONTACT_POINTS]
btManifoldPoint * contactPoints[MAX_NUM_CONTACT_POINTS]