Bullet Collision Detection & Physics Library
btMultiBodyConstraintSolver.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
19 
21 #include "btMultiBodyConstraint.h"
23 
24 #include "LinearMath/btQuickprof.h"
26 #include "LinearMath/btScalar.h"
27 
28 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
29 {
30  btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
31 
32  //solve featherstone non-contact constraints
33 
34  //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
35 
36  for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
37  {
38  int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
39 
41 
42  btScalar residual = resolveSingleConstraintRowGeneric(constraint);
43  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
44 
45  if (constraint.m_multiBodyA)
46  constraint.m_multiBodyA->setPosUpdated(false);
47  if (constraint.m_multiBodyB)
48  constraint.m_multiBodyB->setPosUpdated(false);
49  }
50 
51  //solve featherstone normal contact
52  for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
53  {
54  int index = j0; //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
55 
57  btScalar residual = 0.f;
58 
59  if (iteration < infoGlobal.m_numIterations)
60  {
61  residual = resolveSingleConstraintRowGeneric(constraint);
62  }
63 
64  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
65 
66  if (constraint.m_multiBodyA)
67  constraint.m_multiBodyA->setPosUpdated(false);
68  if (constraint.m_multiBodyB)
69  constraint.m_multiBodyB->setPosUpdated(false);
70  }
71 
72  //solve featherstone frictional contact
74  {
75  for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++)
76  {
77  if (iteration < infoGlobal.m_numIterations)
78  {
79  int index = j1;
80 
82  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
83  //adjust friction limits here
84  if (totalImpulse > btScalar(0))
85  {
86  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
87  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
88  btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
89  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
90 
91  if (frictionConstraint.m_multiBodyA)
92  frictionConstraint.m_multiBodyA->setPosUpdated(false);
93  if (frictionConstraint.m_multiBodyB)
94  frictionConstraint.m_multiBodyB->setPosUpdated(false);
95  }
96  }
97  }
98 
99  for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
100  {
101  if (iteration < infoGlobal.m_numIterations)
102  {
103  int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
104 
106  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
107  j1++;
108  int index2 = j1;
110  //adjust friction limits here
111  if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
112  {
113  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
114  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
115  frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
116  frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
117 
118  btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
119  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
120 
121  if (frictionConstraint.m_multiBodyA)
122  frictionConstraint.m_multiBodyA->setPosUpdated(false);
123  if (frictionConstraint.m_multiBodyB)
124  frictionConstraint.m_multiBodyB->setPosUpdated(false);
125 
126  if (frictionConstraintB.m_multiBodyA)
127  frictionConstraintB.m_multiBodyA->setPosUpdated(false);
128  if (frictionConstraintB.m_multiBodyB)
129  frictionConstraintB.m_multiBodyB->setPosUpdated(false);
130  }
131  }
132  }
133 
134  for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
135  {
136  if (iteration < infoGlobal.m_numIterations)
137  {
138  int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
140 
141  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
142  j1++;
143  int index2 = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
145  btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
146 
147  if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
148  {
149  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
150  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
151  frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
152  frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
153  btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
154  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
155 
156  if (frictionConstraintB.m_multiBodyA)
157  frictionConstraintB.m_multiBodyA->setPosUpdated(false);
158  if (frictionConstraintB.m_multiBodyB)
159  frictionConstraintB.m_multiBodyB->setPosUpdated(false);
160 
161  if (frictionConstraint.m_multiBodyA)
162  frictionConstraint.m_multiBodyA->setPosUpdated(false);
163  if (frictionConstraint.m_multiBodyB)
164  frictionConstraint.m_multiBodyB->setPosUpdated(false);
165  }
166  }
167  }
168  }
169  else
170  {
171  for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
172  {
173  if (iteration < infoGlobal.m_numIterations)
174  {
175  int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
176 
178  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
179  //adjust friction limits here
180  if (totalImpulse > btScalar(0))
181  {
182  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
183  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
184  btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
185  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
186 
187  if (frictionConstraint.m_multiBodyA)
188  frictionConstraint.m_multiBodyA->setPosUpdated(false);
189  if (frictionConstraint.m_multiBodyB)
190  frictionConstraint.m_multiBodyB->setPosUpdated(false);
191  }
192  }
193  }
194  }
195  return leastSquaredResidual;
196 }
197 
198 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
199 {
205 
209 
210  for (int i = 0; i < numBodies; i++)
211  {
213  if (fcA)
214  {
215  fcA->m_multiBody->setCompanionId(-1);
216  }
217  }
218 
219  btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
220 
221  return val;
222 }
223 
224 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
225 {
226  for (int i = 0; i < ndof; ++i)
227  m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
228 }
229 
231 {
232  btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
233  btScalar deltaVelADotn = 0;
234  btScalar deltaVelBDotn = 0;
235  btSolverBody* bodyA = 0;
236  btSolverBody* bodyB = 0;
237  int ndofA = 0;
238  int ndofB = 0;
239 
240  if (c.m_multiBodyA)
241  {
242  ndofA = c.m_multiBodyA->getNumDofs() + 6;
243  for (int i = 0; i < ndofA; ++i)
244  deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
245  }
246  else if (c.m_solverBodyIdA >= 0)
247  {
250  }
251 
252  if (c.m_multiBodyB)
253  {
254  ndofB = c.m_multiBodyB->getNumDofs() + 6;
255  for (int i = 0; i < ndofB; ++i)
256  deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
257  }
258  else if (c.m_solverBodyIdB >= 0)
259  {
262  }
263 
264  deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
265  deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv;
266  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
267 
268  if (sum < c.m_lowerLimit)
269  {
270  deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
272  }
273  else if (sum > c.m_upperLimit)
274  {
275  deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
277  }
278  else
279  {
280  c.m_appliedImpulse = sum;
281  }
282 
283  if (c.m_multiBodyA)
284  {
286 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
287  //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
288  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
290 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
291  }
292  else if (c.m_solverBodyIdA >= 0)
293  {
294  bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
295  }
296  if (c.m_multiBodyB)
297  {
299 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
300  //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
301  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
303 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
304  }
305  else if (c.m_solverBodyIdB >= 0)
306  {
307  bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
308  }
309  btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv;
310  return deltaVel;
311 }
312 
314 {
315  int ndofA = 0;
316  int ndofB = 0;
317  btSolverBody* bodyA = 0;
318  btSolverBody* bodyB = 0;
319  btScalar deltaImpulseB = 0.f;
320  btScalar sumB = 0.f;
321  {
322  deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm;
323  btScalar deltaVelADotn = 0;
324  btScalar deltaVelBDotn = 0;
325  if (cB.m_multiBodyA)
326  {
327  ndofA = cB.m_multiBodyA->getNumDofs() + 6;
328  for (int i = 0; i < ndofA; ++i)
329  deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i];
330  }
331  else if (cB.m_solverBodyIdA >= 0)
332  {
335  }
336 
337  if (cB.m_multiBodyB)
338  {
339  ndofB = cB.m_multiBodyB->getNumDofs() + 6;
340  for (int i = 0; i < ndofB; ++i)
341  deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i];
342  }
343  else if (cB.m_solverBodyIdB >= 0)
344  {
347  }
348 
349  deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
350  deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv;
351  sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
352  }
353 
354  btScalar deltaImpulseA = 0.f;
355  btScalar sumA = 0.f;
356  const btMultiBodySolverConstraint& cA = cA1;
357  {
358  {
359  deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm;
360  btScalar deltaVelADotn = 0;
361  btScalar deltaVelBDotn = 0;
362  if (cA.m_multiBodyA)
363  {
364  ndofA = cA.m_multiBodyA->getNumDofs() + 6;
365  for (int i = 0; i < ndofA; ++i)
366  deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i];
367  }
368  else if (cA.m_solverBodyIdA >= 0)
369  {
372  }
373 
374  if (cA.m_multiBodyB)
375  {
376  ndofB = cA.m_multiBodyB->getNumDofs() + 6;
377  for (int i = 0; i < ndofB; ++i)
378  deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i];
379  }
380  else if (cA.m_solverBodyIdB >= 0)
381  {
384  }
385 
386  deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
387  deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv;
388  sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
389  }
390  }
391 
392  if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit)
393  {
394  btScalar angle = btAtan2(sumA, sumB);
395  btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle));
396  btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle));
397 
398  if (sumA < -sumAclipped)
399  {
400  deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
401  cA.m_appliedImpulse = -sumAclipped;
402  }
403  else if (sumA > sumAclipped)
404  {
405  deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
406  cA.m_appliedImpulse = sumAclipped;
407  }
408  else
409  {
410  cA.m_appliedImpulse = sumA;
411  }
412 
413  if (sumB < -sumBclipped)
414  {
415  deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
416  cB.m_appliedImpulse = -sumBclipped;
417  }
418  else if (sumB > sumBclipped)
419  {
420  deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
421  cB.m_appliedImpulse = sumBclipped;
422  }
423  else
424  {
425  cB.m_appliedImpulse = sumB;
426  }
427  //deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
428  //cA.m_appliedImpulse = sumAclipped;
429  //deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
430  //cB.m_appliedImpulse = sumBclipped;
431  }
432  else
433  {
434  cA.m_appliedImpulse = sumA;
435  cB.m_appliedImpulse = sumB;
436  }
437 
438  if (cA.m_multiBodyA)
439  {
441 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
442  //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
443  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
445 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
446  }
447  else if (cA.m_solverBodyIdA >= 0)
448  {
449  bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA);
450  }
451  if (cA.m_multiBodyB)
452  {
454 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
455  //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
456  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
458 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
459  }
460  else if (cA.m_solverBodyIdB >= 0)
461  {
462  bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA);
463  }
464 
465  if (cB.m_multiBodyA)
466  {
468 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
469  //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
470  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
472 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
473  }
474  else if (cB.m_solverBodyIdA >= 0)
475  {
476  bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB);
477  }
478  if (cB.m_multiBodyB)
479  {
481 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
482  //note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
483  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
485 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
486  }
487  else if (cB.m_solverBodyIdB >= 0)
488  {
489  bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB);
490  }
491 
492  btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv;
493  return deltaVel;
494 }
495 
496 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
497 {
498  BT_PROFILE("setupMultiBodyContactConstraint");
499  btVector3 rel_pos1;
500  btVector3 rel_pos2;
501 
502  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
503  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
504 
505  const btVector3& pos1 = cp.getPositionWorldOnA();
506  const btVector3& pos2 = cp.getPositionWorldOnB();
507 
508  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
509  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
510 
511  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
512  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
513 
514  if (bodyA)
515  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
516  if (bodyB)
517  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
518 
519  relaxation = infoGlobal.m_sor;
520 
521  btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
522 
523  //cfm = 1 / ( dt * kp + kd )
524  //erp = dt * kp / ( dt * kp + kd )
525 
526  btScalar cfm;
527  btScalar erp;
528  if (isFriction)
529  {
530  cfm = infoGlobal.m_frictionCFM;
531  erp = infoGlobal.m_frictionERP;
532  }
533  else
534  {
535  cfm = infoGlobal.m_globalCfm;
536  erp = infoGlobal.m_erp2;
537 
539  {
541  cfm = cp.m_contactCFM;
543  erp = cp.m_contactERP;
544  }
545  else
546  {
548  {
550  if (denom < SIMD_EPSILON)
551  {
552  denom = SIMD_EPSILON;
553  }
554  cfm = btScalar(1) / denom;
555  erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
556  }
557  }
558  }
559 
560  cfm *= invTimeStep;
561 
562  if (multiBodyA)
563  {
564  if (solverConstraint.m_linkA < 0)
565  {
566  rel_pos1 = pos1 - multiBodyA->getBasePos();
567  }
568  else
569  {
570  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
571  }
572  const int ndofA = multiBodyA->getNumDofs() + 6;
573 
574  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
575 
576  if (solverConstraint.m_deltaVelAindex < 0)
577  {
578  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
579  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
581  }
582  else
583  {
584  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
585  }
586 
587  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
591 
592  btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
593  multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
594  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
596 
597  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
598  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
599  solverConstraint.m_contactNormal1 = contactNormal;
600  }
601  else
602  {
603  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
604  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
605  solverConstraint.m_contactNormal1 = contactNormal;
606  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
607  }
608 
609  if (multiBodyB)
610  {
611  if (solverConstraint.m_linkB < 0)
612  {
613  rel_pos2 = pos2 - multiBodyB->getBasePos();
614  }
615  else
616  {
617  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
618  }
619 
620  const int ndofB = multiBodyB->getNumDofs() + 6;
621 
622  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
623  if (solverConstraint.m_deltaVelBindex < 0)
624  {
625  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
626  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
628  }
629 
630  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
631 
635 
636  multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
638 
639  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
640  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
641  solverConstraint.m_contactNormal2 = -contactNormal;
642  }
643  else
644  {
645  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
646  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
647  solverConstraint.m_contactNormal2 = -contactNormal;
648 
649  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
650  }
651 
652  {
653  btVector3 vec;
654  btScalar denom0 = 0.f;
655  btScalar denom1 = 0.f;
656  btScalar* jacB = 0;
657  btScalar* jacA = 0;
658  btScalar* lambdaA = 0;
659  btScalar* lambdaB = 0;
660  int ndofA = 0;
661  if (multiBodyA)
662  {
663  ndofA = multiBodyA->getNumDofs() + 6;
664  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
665  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
666  for (int i = 0; i < ndofA; ++i)
667  {
668  btScalar j = jacA[i];
669  btScalar l = lambdaA[i];
670  denom0 += j * l;
671  }
672  }
673  else
674  {
675  if (rb0)
676  {
677  vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
678  denom0 = rb0->getInvMass() + contactNormal.dot(vec);
679  }
680  }
681  if (multiBodyB)
682  {
683  const int ndofB = multiBodyB->getNumDofs() + 6;
684  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
685  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
686  for (int i = 0; i < ndofB; ++i)
687  {
688  btScalar j = jacB[i];
689  btScalar l = lambdaB[i];
690  denom1 += j * l;
691  }
692  }
693  else
694  {
695  if (rb1)
696  {
697  vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
698  denom1 = rb1->getInvMass() + contactNormal.dot(vec);
699  }
700  }
701 
702  btScalar d = denom0 + denom1 + cfm;
703  if (d > SIMD_EPSILON)
704  {
705  solverConstraint.m_jacDiagABInv = relaxation / (d);
706  }
707  else
708  {
709  //disable the constraint row to handle singularity/redundant constraint
710  solverConstraint.m_jacDiagABInv = 0.f;
711  }
712  }
713 
714  //compute rhs and remaining solverConstraint fields
715 
716  btScalar restitution = 0.f;
717  btScalar distance = 0;
718  if (!isFriction)
719  {
720  distance = cp.getDistance() + infoGlobal.m_linearSlop;
721  }
722  else
723  {
725  {
726  distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
727  }
728  }
729 
730  btScalar rel_vel = 0.f;
731  int ndofA = 0;
732  int ndofB = 0;
733  {
734  btVector3 vel1, vel2;
735  if (multiBodyA)
736  {
737  ndofA = multiBodyA->getNumDofs() + 6;
738  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
739  for (int i = 0; i < ndofA; ++i)
740  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
741  }
742  else
743  {
744  if (rb0)
745  {
746  rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) +
747  (rb0->getTotalTorque() * rb0->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos1) +
748  rb0->getTotalForce() * rb0->getInvMass() * infoGlobal.m_timeStep)
749  .dot(solverConstraint.m_contactNormal1);
750  }
751  }
752  if (multiBodyB)
753  {
754  ndofB = multiBodyB->getNumDofs() + 6;
755  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
756  for (int i = 0; i < ndofB; ++i)
757  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
758  }
759  else
760  {
761  if (rb1)
762  {
763  rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2) +
764  (rb1->getTotalTorque() * rb1->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos2) +
765  rb1->getTotalForce() * rb1->getInvMass() * infoGlobal.m_timeStep)
766  .dot(solverConstraint.m_contactNormal2);
767  }
768  }
769 
770  solverConstraint.m_friction = cp.m_combinedFriction;
771 
772  if (!isFriction)
773  {
774  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
775  if (restitution <= btScalar(0.))
776  {
777  restitution = 0.f;
778  }
779  }
780  }
781 
782  {
783  btScalar positionalError = 0.f;
784  btScalar velocityError = restitution - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
785  if (isFriction)
786  {
787  positionalError = -distance * erp / infoGlobal.m_timeStep;
788  }
789  else
790  {
791  if (distance > 0)
792  {
793  positionalError = 0;
794  velocityError -= distance / infoGlobal.m_timeStep;
795  }
796  else
797  {
798  positionalError = -distance * erp / infoGlobal.m_timeStep;
799  }
800  }
801 
802  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
803  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
804 
805  if (!isFriction)
806  {
807  // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
808  {
809  //combine position and velocity into rhs
810  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
811  solverConstraint.m_rhsPenetration = 0.f;
812  }
813  /*else
814  {
815  //split position and velocity into rhs and m_rhsPenetration
816  solverConstraint.m_rhs = velocityImpulse;
817  solverConstraint.m_rhsPenetration = penetrationImpulse;
818  }
819  */
820  solverConstraint.m_lowerLimit = 0;
821  solverConstraint.m_upperLimit = 1e10f;
822  }
823  else
824  {
825  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
826  solverConstraint.m_rhsPenetration = 0.f;
827  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
828  solverConstraint.m_upperLimit = solverConstraint.m_friction;
829  }
830 
831  solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
832  }
833 
835  {
836  if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS)
837  {
838  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor;
839  if (solverConstraint.m_appliedImpulse < 0)
840  solverConstraint.m_appliedImpulse = 0;
841  }
842  else
843  {
844  solverConstraint.m_appliedImpulse = 0.f;
845  }
846 
847  if (solverConstraint.m_appliedImpulse)
848  {
849  if (multiBodyA)
850  {
851  btScalar impulse = solverConstraint.m_appliedImpulse;
852  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
853  multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
854 
855  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
856  }
857  else
858  {
859  if (rb0)
860  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
861  }
862  if (multiBodyB)
863  {
864  btScalar impulse = solverConstraint.m_appliedImpulse;
865  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
866  multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
867  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
868  }
869  else
870  {
871  if (rb1)
872  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
873  }
874  }
875  }
876  else
877  {
878  solverConstraint.m_appliedImpulse = 0.f;
879  solverConstraint.m_appliedPushImpulse = 0.f;
880  }
881 }
882 
884  const btVector3& constraintNormal,
885  btManifoldPoint& cp,
886  btScalar combinedTorsionalFriction,
887  const btContactSolverInfo& infoGlobal,
888  btScalar& relaxation,
889  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
890 {
891  BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
892  btVector3 rel_pos1;
893  btVector3 rel_pos2;
894 
895  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
896  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
897 
898  const btVector3& pos1 = cp.getPositionWorldOnA();
899  const btVector3& pos2 = cp.getPositionWorldOnB();
900 
901  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
902  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
903 
904  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
905  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
906 
907  if (bodyA)
908  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
909  if (bodyB)
910  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
911 
912  relaxation = infoGlobal.m_sor;
913 
914  // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
915 
916  if (multiBodyA)
917  {
918  if (solverConstraint.m_linkA < 0)
919  {
920  rel_pos1 = pos1 - multiBodyA->getBasePos();
921  }
922  else
923  {
924  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
925  }
926  const int ndofA = multiBodyA->getNumDofs() + 6;
927 
928  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
929 
930  if (solverConstraint.m_deltaVelAindex < 0)
931  {
932  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
933  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
935  }
936  else
937  {
938  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
939  }
940 
941  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
945 
946  btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
947  multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0, 0, 0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
948  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
950 
951  btVector3 torqueAxis0 = constraintNormal;
952  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
953  solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
954  }
955  else
956  {
957  btVector3 torqueAxis0 = constraintNormal;
958  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
959  solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
960  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
961  }
962 
963  if (multiBodyB)
964  {
965  if (solverConstraint.m_linkB < 0)
966  {
967  rel_pos2 = pos2 - multiBodyB->getBasePos();
968  }
969  else
970  {
971  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
972  }
973 
974  const int ndofB = multiBodyB->getNumDofs() + 6;
975 
976  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
977  if (solverConstraint.m_deltaVelBindex < 0)
978  {
979  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
980  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
982  }
983 
984  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
985 
989 
990  multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0, 0, 0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
992 
993  btVector3 torqueAxis1 = -constraintNormal;
994  solverConstraint.m_relpos2CrossNormal = torqueAxis1;
995  solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
996  }
997  else
998  {
999  btVector3 torqueAxis1 = -constraintNormal;
1000  solverConstraint.m_relpos2CrossNormal = torqueAxis1;
1001  solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
1002 
1003  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
1004  }
1005 
1006  {
1007  btScalar denom0 = 0.f;
1008  btScalar denom1 = 0.f;
1009  btScalar* jacB = 0;
1010  btScalar* jacA = 0;
1011  btScalar* lambdaA = 0;
1012  btScalar* lambdaB = 0;
1013  int ndofA = 0;
1014  if (multiBodyA)
1015  {
1016  ndofA = multiBodyA->getNumDofs() + 6;
1017  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
1018  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1019  for (int i = 0; i < ndofA; ++i)
1020  {
1021  btScalar j = jacA[i];
1022  btScalar l = lambdaA[i];
1023  denom0 += j * l;
1024  }
1025  }
1026  else
1027  {
1028  if (rb0)
1029  {
1030  btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
1031  denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1032  }
1033  }
1034  if (multiBodyB)
1035  {
1036  const int ndofB = multiBodyB->getNumDofs() + 6;
1037  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
1038  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1039  for (int i = 0; i < ndofB; ++i)
1040  {
1041  btScalar j = jacB[i];
1042  btScalar l = lambdaB[i];
1043  denom1 += j * l;
1044  }
1045  }
1046  else
1047  {
1048  if (rb1)
1049  {
1050  btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
1051  denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1052  }
1053  }
1054 
1055  btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm;
1056  if (d > SIMD_EPSILON)
1057  {
1058  solverConstraint.m_jacDiagABInv = relaxation / (d);
1059  }
1060  else
1061  {
1062  //disable the constraint row to handle singularity/redundant constraint
1063  solverConstraint.m_jacDiagABInv = 0.f;
1064  }
1065  }
1066 
1067  //compute rhs and remaining solverConstraint fields
1068 
1069  btScalar restitution = 0.f;
1070  btScalar penetration = isFriction ? 0 : cp.getDistance();
1071 
1072  btScalar rel_vel = 0.f;
1073  int ndofA = 0;
1074  int ndofB = 0;
1075  {
1076  btVector3 vel1, vel2;
1077  if (multiBodyA)
1078  {
1079  ndofA = multiBodyA->getNumDofs() + 6;
1080  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
1081  for (int i = 0; i < ndofA; ++i)
1082  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
1083  }
1084  else
1085  {
1086  if (rb0)
1087  {
1088  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
1089  rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0));
1090  }
1091  }
1092  if (multiBodyB)
1093  {
1094  ndofB = multiBodyB->getNumDofs() + 6;
1095  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
1096  for (int i = 0; i < ndofB; ++i)
1097  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
1098  }
1099  else
1100  {
1101  if (rb1)
1102  {
1103  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
1104  rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0));
1105  }
1106  }
1107 
1108  solverConstraint.m_friction = combinedTorsionalFriction;
1109 
1110  if (!isFriction)
1111  {
1112  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
1113  if (restitution <= btScalar(0.))
1114  {
1115  restitution = 0.f;
1116  }
1117  }
1118  }
1119 
1120  solverConstraint.m_appliedImpulse = 0.f;
1121  solverConstraint.m_appliedPushImpulse = 0.f;
1122 
1123  {
1124  btScalar velocityError = 0 - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
1125 
1126  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1127 
1128  solverConstraint.m_rhs = velocityImpulse;
1129  solverConstraint.m_rhsPenetration = 0.f;
1130  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
1131  solverConstraint.m_upperLimit = solverConstraint.m_friction;
1132 
1133  solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv;
1134  }
1135 }
1136 
1137 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1138 {
1139  BT_PROFILE("addMultiBodyFrictionConstraint");
1141  solverConstraint.m_orgConstraint = 0;
1142  solverConstraint.m_orgDofIndex = -1;
1143 
1144  solverConstraint.m_frictionIndex = frictionIndex;
1145  bool isFriction = true;
1146 
1149 
1150  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1151  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1152 
1153  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1154  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1155 
1156  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1157  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1158  solverConstraint.m_multiBodyA = mbA;
1159  if (mbA)
1160  solverConstraint.m_linkA = fcA->m_link;
1161 
1162  solverConstraint.m_multiBodyB = mbB;
1163  if (mbB)
1164  solverConstraint.m_linkB = fcB->m_link;
1165 
1166  solverConstraint.m_originalContactPoint = &cp;
1167 
1168  setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1169  return solverConstraint;
1170 }
1171 
1173  btScalar combinedTorsionalFriction,
1174  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1175 {
1176  BT_PROFILE("addMultiBodyRollingFrictionConstraint");
1177 
1178  bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0));
1179 
1181  solverConstraint.m_orgConstraint = 0;
1182  solverConstraint.m_orgDofIndex = -1;
1183 
1184  solverConstraint.m_frictionIndex = frictionIndex;
1185  bool isFriction = true;
1186 
1189 
1190  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1191  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1192 
1193  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1194  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1195 
1196  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1197  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1198  solverConstraint.m_multiBodyA = mbA;
1199  if (mbA)
1200  solverConstraint.m_linkA = fcA->m_link;
1201 
1202  solverConstraint.m_multiBodyB = mbB;
1203  if (mbB)
1204  solverConstraint.m_linkB = fcB->m_link;
1205 
1206  solverConstraint.m_originalContactPoint = &cp;
1207 
1208  setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1209  return solverConstraint;
1210 }
1211 
1213  btScalar combinedTorsionalFriction,
1214  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1215 {
1216  BT_PROFILE("addMultiBodyRollingFrictionConstraint");
1217 
1219  solverConstraint.m_orgConstraint = 0;
1220  solverConstraint.m_orgDofIndex = -1;
1221 
1222  solverConstraint.m_frictionIndex = frictionIndex;
1223  bool isFriction = true;
1224 
1227 
1228  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1229  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1230 
1231  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1232  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1233 
1234  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1235  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1236  solverConstraint.m_multiBodyA = mbA;
1237  if (mbA)
1238  solverConstraint.m_linkA = fcA->m_link;
1239 
1240  solverConstraint.m_multiBodyB = mbB;
1241  if (mbB)
1242  solverConstraint.m_linkB = fcB->m_link;
1243 
1244  solverConstraint.m_originalContactPoint = &cp;
1245 
1246  setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1247  return solverConstraint;
1248 }
1250 {
1253 
1254  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1255  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1256 
1257  btCollisionObject *colObj0 = 0, *colObj1 = 0;
1258 
1259  colObj0 = (btCollisionObject*)manifold->getBody0();
1260  colObj1 = (btCollisionObject*)manifold->getBody1();
1261 
1262  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1263  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1264 
1265  // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
1266  // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
1267 
1269  // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
1270  // return;
1271 
1272  //only a single rollingFriction per manifold
1273  int rollingFriction = 1;
1274 
1275  for (int j = 0; j < manifold->getNumContacts(); j++)
1276  {
1277  btManifoldPoint& cp = manifold->getContactPoint(j);
1278 
1279  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1280  {
1281  btScalar relaxation;
1282 
1283  int frictionIndex = m_multiBodyNormalContactConstraints.size();
1284 
1286 
1287  // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
1288  // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
1289  solverConstraint.m_orgConstraint = 0;
1290  solverConstraint.m_orgDofIndex = -1;
1291  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1292  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1293  solverConstraint.m_multiBodyA = mbA;
1294  if (mbA)
1295  solverConstraint.m_linkA = fcA->m_link;
1296 
1297  solverConstraint.m_multiBodyB = mbB;
1298  if (mbB)
1299  solverConstraint.m_linkB = fcB->m_link;
1300 
1301  solverConstraint.m_originalContactPoint = &cp;
1302 
1303  bool isFriction = false;
1304  setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
1305 
1306  // const btVector3& pos1 = cp.getPositionWorldOnA();
1307  // const btVector3& pos2 = cp.getPositionWorldOnB();
1308 
1310 #define ENABLE_FRICTION
1311 #ifdef ENABLE_FRICTION
1313 
1318 
1329 
1333 
1334  if (rollingFriction > 0)
1335  {
1336  if (cp.m_combinedSpinningFriction > 0)
1337  {
1338  addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
1339  }
1340  if (cp.m_combinedRollingFriction > 0)
1341  {
1346 
1347  addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1348  addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1349  }
1350  rollingFriction--;
1351  }
1353  { /*
1354  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1355  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1356  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
1357  {
1358  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1359  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1360  {
1361  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
1362  cp.m_lateralFrictionDir2.normalize();//??
1363  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1364  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1365  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1366 
1367  }
1368 
1369  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1370  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1371  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1372 
1373  } else
1374  */
1375  {
1378  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1379 
1381  {
1384  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1385  }
1386 
1388  {
1390  }
1391  }
1392  }
1393  else
1394  {
1395  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1396 
1398  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1399  solverConstraint.m_appliedImpulse = 0.f;
1400  solverConstraint.m_appliedPushImpulse = 0.f;
1401  }
1402 
1403 #endif //ENABLE_FRICTION
1404  }
1405  else
1406  {
1407  // Reset quantities related to warmstart as 0.
1408  cp.m_appliedImpulse = 0;
1409  cp.m_prevRHS = 0;
1410  }
1411  }
1412 }
1413 
1414 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
1415 {
1416  for (int i = 0; i < numManifolds; i++)
1417  {
1418  btPersistentManifold* manifold = manifoldPtr[i];
1421  if (!fcA && !fcB)
1422  {
1423  //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
1424  convertContact(manifold, infoGlobal);
1425  }
1426  else
1427  {
1428  convertMultiBodyContact(manifold, infoGlobal);
1429  }
1430  }
1431 
1432  //also convert the multibody constraints, if any
1433 
1434  for (int i = 0; i < m_tmpNumMultiBodyConstraints; i++)
1435  {
1439 
1441  }
1442 
1443  // Warmstart for noncontact constraints
1445  {
1446  for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1447  {
1448  btMultiBodySolverConstraint& solverConstraint =
1450  solverConstraint.m_appliedImpulse =
1451  solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
1453 
1454  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
1455  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
1456  if (solverConstraint.m_appliedImpulse)
1457  {
1458  if (multiBodyA)
1459  {
1460  int ndofA = multiBodyA->getNumDofs() + 6;
1461  btScalar* deltaV =
1462  &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1463  btScalar impulse = solverConstraint.m_appliedImpulse;
1464  multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
1465  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
1466  }
1467  if (multiBodyB)
1468  {
1469  int ndofB = multiBodyB->getNumDofs() + 6;
1470  btScalar* deltaV =
1471  &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1472  btScalar impulse = solverConstraint.m_appliedImpulse;
1473  multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
1474  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
1475  }
1476  }
1477  }
1478  }
1479  else
1480  {
1481  for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1482  {
1484  solverConstraint.m_appliedImpulse = 0;
1485  }
1486  }
1487 }
1488 
1489 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
1490 {
1491  //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
1492  return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1493 }
1494 
1495 #if 0
1496 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
1497 {
1498  if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
1499  {
1500  //todo: get rid of those temporary memory allocations for the joint feedback
1501  btAlignedObjectArray<btScalar> forceVector;
1502  int numDofsPlusBase = 6+mb->getNumDofs();
1503  forceVector.resize(numDofsPlusBase);
1504  for (int i=0;i<numDofsPlusBase;i++)
1505  {
1506  forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
1507  }
1509  output.resize(numDofsPlusBase);
1510  bool applyJointFeedback = true;
1511  mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
1512  }
1513 }
1514 #endif
1515 
1517 {
1518 #if 1
1519 
1520  //bod->addBaseForce(m_gravity * bod->getBaseMass());
1521  //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
1522 
1523  if (c.m_orgConstraint)
1524  {
1526  }
1527 
1528  if (c.m_multiBodyA)
1529  {
1531  btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime);
1532  btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime);
1533  if (c.m_linkA < 0)
1534  {
1537  }
1538  else
1539  {
1541  //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1543  }
1544  }
1545 
1546  if (c.m_multiBodyB)
1547  {
1548  {
1550  btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime);
1551  btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime);
1552  if (c.m_linkB < 0)
1553  {
1556  }
1557  else
1558  {
1559  {
1561  //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1563  }
1564  }
1565  }
1566  }
1567 #endif
1568 
1569 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
1570 
1571  if (c.m_multiBodyA)
1572  {
1574  }
1575 
1576  if (c.m_multiBodyB)
1577  {
1579  }
1580 #endif
1581 }
1582 
1584 {
1585  BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1586  int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
1587 
1588  //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
1589  //or as applied force, so we can measure the joint reaction forces easier
1590  for (int i = 0; i < numPoolConstraints; i++)
1591  {
1593  writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
1594 
1596 
1598  {
1600  }
1601  }
1602 
1603  for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1604  {
1606  writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
1607  }
1608 
1609 
1610  {
1611  BT_PROFILE("warm starting write back");
1612  for (int j = 0; j < numPoolConstraints; j++)
1613  {
1615  btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
1616  btAssert(pt);
1617  pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1618  pt->m_prevRHS = solverConstraint.m_rhs;
1619  pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1620 
1621  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1623  {
1624  pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse;
1625  } else
1626  {
1627  pt->m_appliedImpulseLateral2 = 0;
1628  }
1629  }
1630  }
1631 
1632 #if 0
1633  //multibody joint feedback
1634  {
1635  BT_PROFILE("multi body joint feedback");
1636  for (int j=0;j<numPoolConstraints;j++)
1637  {
1639 
1640  //apply the joint feedback into all links of the btMultiBody
1641  //todo: double-check the signs of the applied impulse
1642 
1643  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1644  {
1645  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1646  }
1647  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1648  {
1649  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1650  }
1651 #if 0
1652  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
1653  {
1654  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1655  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
1656  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
1657  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1658 
1659  }
1660  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
1661  {
1662  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1663  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
1664  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
1665  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1666  }
1667 
1669  {
1670  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
1671  {
1672  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1673  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
1674  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
1675  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1676  }
1677 
1678  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
1679  {
1680  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1681  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
1682  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
1683  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1684  }
1685  }
1686 #endif
1687  }
1688 
1689  for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1690  {
1692  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1693  {
1694  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1695  }
1696  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1697  {
1698  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1699  }
1700  }
1701  }
1702 
1703  numPoolConstraints = m_multiBodyNonContactConstraints.size();
1704 
1705 #if 0
1706  //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1707  for (int i=0;i<numPoolConstraints;i++)
1708  {
1710 
1712  btJointFeedback* fb = constr->getJointFeedback();
1713  if (fb)
1714  {
1716  fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1717  fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1718  fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1719 
1720  }
1721 
1724  {
1725  constr->setEnabled(false);
1726  }
1727 
1728  }
1729 #endif
1730 #endif
1731 
1732  return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1733 }
1734 
1735 void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
1736 {
1737  //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
1738 
1739  //printf("solveMultiBodyGroup start\n");
1740  m_tmpMultiBodyConstraints = multiBodyConstraints;
1741  m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1742 
1743  btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1744 
1747 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btTypedConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.h:181
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultiBodyConstraintSolver::convertMultiBodyContact
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:1249
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:103
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:74
btContactSolverInfoData::m_linearSlop
btScalar m_linearSlop
Definition: btContactSolverInfo.h:57
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:48
btMultiBodySolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btMultiBodySolverConstraint.h:50
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition: btSolverBody.h:120
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
btManifoldPoint::m_lateralFrictionDir2
btVector3 m_lateralFrictionDir2
Definition: btManifoldPoint.h:140
btManifoldPoint::m_contactMotion2
btScalar m_contactMotion2
Definition: btManifoldPoint.h:123
btRigidBody::getTotalTorque
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:284
btMultiBody::addBaseConstraintTorque
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:339
btSolverBody::m_linearVelocity
btVector3 m_linearVelocity
Definition: btSolverBody.h:115
btSequentialImpulseConstraintSolver::solveGroup
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
btSequentialImpulseConstraintSolver Sequentially applies impulses
Definition: btSequentialImpulseConstraintSolver.cpp:1854
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
btMultiBody::addLinkConstraintForce
void addLinkConstraintForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:674
btManifoldPoint::m_contactCFM
btScalar m_contactCFM
Definition: btManifoldPoint.h:126
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:61
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:48
btManifoldPoint::m_contactERP
btScalar m_contactERP
Definition: btManifoldPoint.h:131
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:212
btMultiBody::getCompanionId
int getCompanionId() const
Definition: btMultiBody.h:515
btManifoldPoint::m_contactPointFlags
int m_contactPointFlags
Definition: btManifoldPoint.h:116
BT_CONTACT_FLAG_HAS_CONTACT_CFM
Definition: btManifoldPoint.h:43
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btContactSolverInfo
Definition: btContactSolverInfo.h:72
btManifoldPoint::getPositionWorldOnA
const btVector3 & getPositionWorldOnA() const
Definition: btManifoldPoint.h:151
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btMultiBody::internalNeedsJointFeedback
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:598
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:76
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
Definition: btContactSolverInfo.h:28
btTypedConstraint::getBreakingImpulseThreshold
btScalar getBreakingImpulseThreshold() const
Definition: btTypedConstraint.h:191
btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:1583
btMultiBodySolverConstraint::m_deltaVelBindex
int m_deltaVelBindex
Definition: btMultiBodySolverConstraint.h:39
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:30
btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
Definition: btMultiBodyConstraintSolver.cpp:1516
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:100
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btMultiBodyConstraintSolver.h
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:40
SOLVER_USE_ARTICULATED_WARMSTARTING
Definition: btContactSolverInfo.h:34
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btMultiBodySolverConstraint.h:60
btSequentialImpulseConstraintSolver::restitutionCurve
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
Definition: btSequentialImpulseConstraintSolver.cpp:492
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:105
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:47
btSequentialImpulseConstraintSolver::applyAnisotropicFriction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
Definition: btSequentialImpulseConstraintSolver.cpp:502
btMultiBodyConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
Definition: btMultiBodyConstraint.h:126
btTypedConstraint::getJointFeedback
const btJointFeedback * getJointFeedback() const
Definition: btTypedConstraint.h:267
btSolverBody::m_externalForceImpulse
btVector3 m_externalForceImpulse
Definition: btSolverBody.h:117
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:120
btContactSolverInfoData::m_sor
btScalar m_sor
Definition: btContactSolverInfo.h:46
btMultiBody::addLinkConstraintTorque
void addLinkConstraintTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:679
btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:883
btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint
btMultiBodySolverConstraint & addMultiBodyFrictionConstraint(const btVector3 &normalAxis, const btScalar &appliedImpulse, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:1137
btMultiBodyConstraintSolver::m_data
btMultiBodyJacobianData m_data
Definition: btMultiBodyConstraintSolver.h:39
btJointFeedback
Definition: btTypedConstraint.h:63
btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION
Definition: btCollisionObject.h:160
btMultiBodyConstraintSolver::m_multiBodySpinningFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints
Definition: btMultiBodyConstraintSolver.h:37
btContactSolverInfoData::m_frictionERP
btScalar m_frictionERP
Definition: btContactSolverInfo.h:51
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:243
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:142
btMultiBodySolverConstraint::m_deltaVelAindex
int m_deltaVelAindex
Definition: btMultiBodySolverConstraint.h:37
btScalar.h
btManifoldPoint::m_combinedRestitution
btScalar m_combinedRestitution
Definition: btManifoldPoint.h:106
btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint
btMultiBodySolverConstraint & addMultiBodyTorsionalFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btScalar combinedTorsionalFriction, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:1172
btMultiBodyConstraintSolver::m_multiBodyNormalContactConstraints
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
Definition: btMultiBodyConstraintSolver.h:34
btMultiBodyConstraintSolver::solveSingleIteration
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btMultiBodyConstraintSolver.cpp:28
btMultiBodyConstraintSolver::solveGroup
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
this method should not be called, it was just used during porting/integration of Featherstone btMulti...
Definition: btMultiBodyConstraintSolver.cpp:1489
btSequentialImpulseConstraintSolver::solveSingleIteration
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1522
btMultiBodySolverConstraint.h
btMultiBodyConstraintSolver::applyDeltaVee
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
Definition: btMultiBodyConstraintSolver.cpp:224
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
BT_CONTACT_FLAG_FRICTION_ANCHOR
Definition: btManifoldPoint.h:46
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:218
btSolverBody::m_angularVelocity
btVector3 m_angularVelocity
Definition: btSolverBody.h:116
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:417
btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btMultiBodyConstraintSolver.cpp:198
btSolverBody.h
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:74
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:499
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btRigidBody::getLinearFactor
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:254
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btJointFeedback::m_appliedForceBodyA
btVector3 m_appliedForceBodyA
Definition: btTypedConstraint.h:67
btManifoldPoint::m_combinedContactDamping1
btScalar m_combinedContactDamping1
Definition: btManifoldPoint.h:132
btMultiBodyConstraintSolver::m_tmpNumMultiBodyConstraints
int m_tmpNumMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:43
btSimdScalar
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
Definition: btSolverBody.h:99
btMultiBodySolverConstraint::m_linkA
int m_linkA
Definition: btMultiBodySolverConstraint.h:71
btMultiBodyConstraintSolver::convertContacts
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:1414
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:26
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition: btManifoldPoint.h:51
btManifoldPoint::m_contactMotion1
btScalar m_contactMotion1
Definition: btManifoldPoint.h:122
btManifoldPoint::m_combinedSpinningFriction
btScalar m_combinedSpinningFriction
Definition: btManifoldPoint.h:105
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
Definition: btMultiBody.cpp:1438
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btMultiBodyLinkCollider.h
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btPersistentManifold::getContactProcessingThreshold
btScalar getContactProcessingThreshold() const
Definition: btPersistentManifold.h:145
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:532
btMultiBodyConstraintSolver::m_multiBodyFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
Definition: btMultiBodyConstraintSolver.h:35
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1832
btContactSolverInfoData::m_frictionCFM
btScalar m_frictionCFM
Definition: btContactSolverInfo.h:52
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:498
btSolverBody::internalGetDeltaAngularVelocity
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:202
btManifoldPoint::m_frictionCFM
btScalar m_frictionCFM
Definition: btManifoldPoint.h:135
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
Definition: btManifoldPoint.h:45
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:32
btManifoldPoint::m_prevRHS
btScalar m_prevRHS
Definition: btManifoldPoint.h:119
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btSolverBody::internalGetDeltaLinearVelocity
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
Definition: btSolverBody.h:197
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btMultiBodyConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
btManifoldPoint::getPositionWorldOnB
const btVector3 & getPositionWorldOnB() const
Definition: btManifoldPoint.h:157
btContactSolverInfoData::m_globalCfm
btScalar m_globalCfm
Definition: btContactSolverInfo.h:50
btMultiBodySolverConstraint::m_jacBindex
int m_jacBindex
Definition: btMultiBodySolverConstraint.h:40
btManifoldPoint::m_combinedRollingFriction
btScalar m_combinedRollingFriction
Definition: btManifoldPoint.h:104
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:60
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:63
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btMultiBody
Definition: btMultiBody.h:49
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
btSequentialImpulseConstraintSolver::convertContact
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:995
BT_CONTACT_FLAG_HAS_CONTACT_ERP
Definition: btManifoldPoint.h:44
btSequentialImpulseConstraintSolver::m_fixedBodyId
int m_fixedBodyId
Definition: btSequentialImpulseConstraintSolver.h:67
btMultiBody::setCompanionId
void setCompanionId(int id)
Definition: btMultiBody.h:519
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:70
btMultiBodyConstraintSolver::m_multiBodyNonContactConstraints
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
Definition: btMultiBodyConstraintSolver.h:32
btManifoldPoint::m_appliedImpulseLateral1
btScalar m_appliedImpulseLateral1
Definition: btManifoldPoint.h:120
output
#define output
btAlignedObjectArray< btScalar >
btMultiBodyJacobianData::m_deltaVelocities
btAlignedObjectArray< btScalar > m_deltaVelocities
Definition: btMultiBodyConstraint.h:32
btMultiBodySolverConstraint::m_originalContactPoint
void * m_originalContactPoint
Definition: btMultiBodySolverConstraint.h:62
btMultiBodyConstraint::getAppliedImpulse
btScalar getAppliedImpulse(int dof)
Definition: btMultiBodyConstraint.h:133
btPersistentManifold.h
btMultiBodyJacobianData::m_solverBodyPool
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
Definition: btMultiBodyConstraint.h:36
btContactSolverInfo.h
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:592
btMultiBodyConstraintSolver::setupMultiBodyContactConstraint
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, const btScalar &appliedImpulse, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:496
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btMultiBody::fillContactJacobianMultiDof
void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
Definition: btMultiBody.h:456
btMultiBodySolverConstraint::m_linkB
int m_linkB
Definition: btMultiBodySolverConstraint.h:75
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btMultiBodySolverConstraint.h:54
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
btMultiBodySolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btMultiBodySolverConstraint.h:51
btMultiBodySolverConstraint::m_jacAindex
int m_jacAindex
Definition: btMultiBodySolverConstraint.h:38
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:56
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:43
btContactSolverInfoData::m_articulatedWarmstartingFactor
btScalar m_articulatedWarmstartingFactor
Definition: btContactSolverInfo.h:59
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1399
btMultiBodyConstraintSolver::m_tmpMultiBodyConstraints
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:42
btSolverBody::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:126
btMultiBodyLinkCollider::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:37
btQuickprof.h
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
btTypedConstraint::setEnabled
void setEnabled(bool enabled)
Definition: btTypedConstraint.h:206
btContactSolverInfoData::m_restitutionVelocityThreshold
btScalar m_restitutionVelocityThreshold
Definition: btContactSolverInfo.h:66
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
Definition: btContactSolverInfo.h:27
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:55
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btMultiBody::applyDeltaVeeMultiDof2
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
btMultiBody::addBaseConstraintForce
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:335
btMultiBodySolverConstraint::m_friction
btScalar m_friction
Definition: btMultiBodySolverConstraint.h:53
btPersistentManifold::getContactPoint
const btManifoldPoint & getContactPoint(int index) const
Definition: btPersistentManifold.h:130
btMultiBodyLinkCollider::m_multiBody
btMultiBody * m_multiBody
Definition: btMultiBodyLinkCollider.h:36
SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btMultiBodyConstraintSolver::m_multiBodyTorsionalFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints
Definition: btMultiBodyConstraintSolver.h:36
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
btMultiBodySolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btMultiBodySolverConstraint.h:59
btManifoldPoint::m_lateralFrictionDir1
btVector3 m_lateralFrictionDir1
Definition: btManifoldPoint.h:139
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:106
btMultiBodyConstraint.h
btManifoldPoint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btManifoldPoint.h:118
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:44
btRigidBody::getTotalForce
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:279
SOLVER_DISABLE_IMPLICIT_CONE_FRICTION
Definition: btContactSolverInfo.h:33
btMultiBodySolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btMultiBodySolverConstraint.h:58
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
btSequentialImpulseConstraintSolver::getOrInitSolverBody
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:689
btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
Definition: btMultiBodyConstraintSolver.cpp:230
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btCollisionObject::CF_ANISOTROPIC_FRICTION
Definition: btCollisionObject.h:159
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint
btMultiBodySolverConstraint & addMultiBodySpinningFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btScalar combinedTorsionalFriction, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:1212
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:45
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:197
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btManifoldPoint::m_appliedImpulseLateral2
btScalar m_appliedImpulseLateral2
Definition: btManifoldPoint.h:121
btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint &cA1, const btMultiBodySolverConstraint &cB)
Definition: btMultiBodyConstraintSolver.cpp:313
btMultiBodyConstraintSolver::solveMultiBodyGroup
virtual void solveMultiBodyGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, btMultiBodyConstraint **multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
Definition: btMultiBodyConstraintSolver.cpp:1735
btMultiBody::fillConstraintJacobianMultiDof
void fillConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
Definition: btMultiBody.cpp:1891
btManifoldPoint::m_combinedFriction
btScalar m_combinedFriction
Definition: btManifoldPoint.h:103
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
Definition: btManifoldPoint.h:42
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btManifoldPoint::m_combinedContactStiffness1
btScalar m_combinedContactStiffness1
Definition: btManifoldPoint.h:127
btMultiBodySolverConstraint::m_cfm
btScalar m_cfm
Definition: btMultiBodySolverConstraint.h:56
btMultiBodySolverConstraint::m_frictionIndex
int m_frictionIndex
Definition: btMultiBodySolverConstraint.h:67