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"
25 
26 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
27 {
28  btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
29 
30  //solve featherstone non-contact constraints
31 
32  //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
33 
34  for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
35  {
36  int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
37 
39 
40  btScalar residual = resolveSingleConstraintRowGeneric(constraint);
41  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
42 
43  if (constraint.m_multiBodyA)
44  constraint.m_multiBodyA->setPosUpdated(false);
45  if (constraint.m_multiBodyB)
46  constraint.m_multiBodyB->setPosUpdated(false);
47  }
48 
49  //solve featherstone normal contact
50  for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
51  {
52  int index = j0; //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
53 
55  btScalar residual = 0.f;
56 
57  if (iteration < infoGlobal.m_numIterations)
58  {
59  residual = resolveSingleConstraintRowGeneric(constraint);
60  }
61 
62  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
63 
64  if (constraint.m_multiBodyA)
65  constraint.m_multiBodyA->setPosUpdated(false);
66  if (constraint.m_multiBodyB)
67  constraint.m_multiBodyB->setPosUpdated(false);
68  }
69 
70  //solve featherstone frictional contact
72  {
73  for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
74  {
75  if (iteration < infoGlobal.m_numIterations)
76  {
77  int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
78 
80  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
81  //adjust friction limits here
82  if (totalImpulse > btScalar(0))
83  {
84  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
85  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
86  btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
87  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
88 
89  if (frictionConstraint.m_multiBodyA)
90  frictionConstraint.m_multiBodyA->setPosUpdated(false);
91  if (frictionConstraint.m_multiBodyB)
92  frictionConstraint.m_multiBodyB->setPosUpdated(false);
93  }
94  }
95  }
96 
97  for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
98  {
99  if (iteration < infoGlobal.m_numIterations)
100  {
101  int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
103 
104  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
105  j1++;
106  int index2 = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
108  btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
109 
110  if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
111  {
112  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
113  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
114  frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
115  frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
116  btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
117  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
118 
119  if (frictionConstraintB.m_multiBodyA)
120  frictionConstraintB.m_multiBodyA->setPosUpdated(false);
121  if (frictionConstraintB.m_multiBodyB)
122  frictionConstraintB.m_multiBodyB->setPosUpdated(false);
123 
124  if (frictionConstraint.m_multiBodyA)
125  frictionConstraint.m_multiBodyA->setPosUpdated(false);
126  if (frictionConstraint.m_multiBodyB)
127  frictionConstraint.m_multiBodyB->setPosUpdated(false);
128  }
129  }
130  }
131  }
132  else
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;
139 
141  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
142  //adjust friction limits here
143  if (totalImpulse > btScalar(0))
144  {
145  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
146  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
147  btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
148  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
149 
150  if (frictionConstraint.m_multiBodyA)
151  frictionConstraint.m_multiBodyA->setPosUpdated(false);
152  if (frictionConstraint.m_multiBodyB)
153  frictionConstraint.m_multiBodyB->setPosUpdated(false);
154  }
155  }
156  }
157  }
158  return leastSquaredResidual;
159 }
160 
161 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
162 {
167 
171 
172  for (int i = 0; i < numBodies; i++)
173  {
175  if (fcA)
176  {
177  fcA->m_multiBody->setCompanionId(-1);
178  }
179  }
180 
181  btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
182 
183  return val;
184 }
185 
186 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
187 {
188  for (int i = 0; i < ndof; ++i)
189  m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
190 }
191 
193 {
194  btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
195  btScalar deltaVelADotn = 0;
196  btScalar deltaVelBDotn = 0;
197  btSolverBody* bodyA = 0;
198  btSolverBody* bodyB = 0;
199  int ndofA = 0;
200  int ndofB = 0;
201 
202  if (c.m_multiBodyA)
203  {
204  ndofA = c.m_multiBodyA->getNumDofs() + 6;
205  for (int i = 0; i < ndofA; ++i)
206  deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
207  }
208  else if (c.m_solverBodyIdA >= 0)
209  {
212  }
213 
214  if (c.m_multiBodyB)
215  {
216  ndofB = c.m_multiBodyB->getNumDofs() + 6;
217  for (int i = 0; i < ndofB; ++i)
218  deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
219  }
220  else if (c.m_solverBodyIdB >= 0)
221  {
224  }
225 
226  deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
227  deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv;
228  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
229 
230  if (sum < c.m_lowerLimit)
231  {
232  deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
234  }
235  else if (sum > c.m_upperLimit)
236  {
237  deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
239  }
240  else
241  {
242  c.m_appliedImpulse = sum;
243  }
244 
245  if (c.m_multiBodyA)
246  {
248 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
249  //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
250  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
252 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
253  }
254  else if (c.m_solverBodyIdA >= 0)
255  {
256  bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
257  }
258  if (c.m_multiBodyB)
259  {
261 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
262  //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
263  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
265 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
266  }
267  else if (c.m_solverBodyIdB >= 0)
268  {
269  bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
270  }
271  btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv;
272  return deltaVel;
273 }
274 
276 {
277  int ndofA = 0;
278  int ndofB = 0;
279  btSolverBody* bodyA = 0;
280  btSolverBody* bodyB = 0;
281  btScalar deltaImpulseB = 0.f;
282  btScalar sumB = 0.f;
283  {
284  deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm;
285  btScalar deltaVelADotn = 0;
286  btScalar deltaVelBDotn = 0;
287  if (cB.m_multiBodyA)
288  {
289  ndofA = cB.m_multiBodyA->getNumDofs() + 6;
290  for (int i = 0; i < ndofA; ++i)
291  deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i];
292  }
293  else if (cB.m_solverBodyIdA >= 0)
294  {
297  }
298 
299  if (cB.m_multiBodyB)
300  {
301  ndofB = cB.m_multiBodyB->getNumDofs() + 6;
302  for (int i = 0; i < ndofB; ++i)
303  deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i];
304  }
305  else if (cB.m_solverBodyIdB >= 0)
306  {
309  }
310 
311  deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
312  deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv;
313  sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
314  }
315 
316  btScalar deltaImpulseA = 0.f;
317  btScalar sumA = 0.f;
318  const btMultiBodySolverConstraint& cA = cA1;
319  {
320  {
321  deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm;
322  btScalar deltaVelADotn = 0;
323  btScalar deltaVelBDotn = 0;
324  if (cA.m_multiBodyA)
325  {
326  ndofA = cA.m_multiBodyA->getNumDofs() + 6;
327  for (int i = 0; i < ndofA; ++i)
328  deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i];
329  }
330  else if (cA.m_solverBodyIdA >= 0)
331  {
334  }
335 
336  if (cA.m_multiBodyB)
337  {
338  ndofB = cA.m_multiBodyB->getNumDofs() + 6;
339  for (int i = 0; i < ndofB; ++i)
340  deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i];
341  }
342  else if (cA.m_solverBodyIdB >= 0)
343  {
346  }
347 
348  deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
349  deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv;
350  sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
351  }
352  }
353 
354  if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit)
355  {
356  btScalar angle = btAtan2(sumA, sumB);
357  btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle));
358  btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle));
359 
360  if (sumA < -sumAclipped)
361  {
362  deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
363  cA.m_appliedImpulse = -sumAclipped;
364  }
365  else if (sumA > sumAclipped)
366  {
367  deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
368  cA.m_appliedImpulse = sumAclipped;
369  }
370  else
371  {
372  cA.m_appliedImpulse = sumA;
373  }
374 
375  if (sumB < -sumBclipped)
376  {
377  deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
378  cB.m_appliedImpulse = -sumBclipped;
379  }
380  else if (sumB > sumBclipped)
381  {
382  deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
383  cB.m_appliedImpulse = sumBclipped;
384  }
385  else
386  {
387  cB.m_appliedImpulse = sumB;
388  }
389  //deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
390  //cA.m_appliedImpulse = sumAclipped;
391  //deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
392  //cB.m_appliedImpulse = sumBclipped;
393  }
394  else
395  {
396  cA.m_appliedImpulse = sumA;
397  cB.m_appliedImpulse = sumB;
398  }
399 
400  if (cA.m_multiBodyA)
401  {
403 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
404  //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
405  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
407 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
408  }
409  else if (cA.m_solverBodyIdA >= 0)
410  {
411  bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA);
412  }
413  if (cA.m_multiBodyB)
414  {
416 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
417  //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
418  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
420 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
421  }
422  else if (cA.m_solverBodyIdB >= 0)
423  {
424  bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA);
425  }
426 
427  if (cB.m_multiBodyA)
428  {
430 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
431  //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
432  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
434 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
435  }
436  else if (cB.m_solverBodyIdA >= 0)
437  {
438  bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB);
439  }
440  if (cB.m_multiBodyB)
441  {
443 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
444  //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
445  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
447 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
448  }
449  else if (cB.m_solverBodyIdB >= 0)
450  {
451  bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB);
452  }
453 
454  btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv;
455  return deltaVel;
456 }
457 
459  const btVector3& contactNormal,
460  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
461  btScalar& relaxation,
462  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
463 {
464  BT_PROFILE("setupMultiBodyContactConstraint");
465  btVector3 rel_pos1;
466  btVector3 rel_pos2;
467 
468  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
469  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
470 
471  const btVector3& pos1 = cp.getPositionWorldOnA();
472  const btVector3& pos2 = cp.getPositionWorldOnB();
473 
474  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
475  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
476 
477  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
478  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
479 
480  if (bodyA)
481  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
482  if (bodyB)
483  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
484 
485  relaxation = infoGlobal.m_sor;
486 
487  btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
488 
489  //cfm = 1 / ( dt * kp + kd )
490  //erp = dt * kp / ( dt * kp + kd )
491 
492  btScalar cfm;
493  btScalar erp;
494  if (isFriction)
495  {
496  cfm = infoGlobal.m_frictionCFM;
497  erp = infoGlobal.m_frictionERP;
498  }
499  else
500  {
501  cfm = infoGlobal.m_globalCfm;
502  erp = infoGlobal.m_erp2;
503 
505  {
507  cfm = cp.m_contactCFM;
509  erp = cp.m_contactERP;
510  }
511  else
512  {
514  {
516  if (denom < SIMD_EPSILON)
517  {
518  denom = SIMD_EPSILON;
519  }
520  cfm = btScalar(1) / denom;
521  erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
522  }
523  }
524  }
525 
526  cfm *= invTimeStep;
527 
528  if (multiBodyA)
529  {
530  if (solverConstraint.m_linkA < 0)
531  {
532  rel_pos1 = pos1 - multiBodyA->getBasePos();
533  }
534  else
535  {
536  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
537  }
538  const int ndofA = multiBodyA->getNumDofs() + 6;
539 
540  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
541 
542  if (solverConstraint.m_deltaVelAindex < 0)
543  {
544  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
545  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
547  }
548  else
549  {
550  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
551  }
552 
553  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
557 
558  btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
559  multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
560  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
562 
563  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
564  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
565  solverConstraint.m_contactNormal1 = contactNormal;
566  }
567  else
568  {
569  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
570  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
571  solverConstraint.m_contactNormal1 = contactNormal;
572  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
573  }
574 
575  if (multiBodyB)
576  {
577  if (solverConstraint.m_linkB < 0)
578  {
579  rel_pos2 = pos2 - multiBodyB->getBasePos();
580  }
581  else
582  {
583  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
584  }
585 
586  const int ndofB = multiBodyB->getNumDofs() + 6;
587 
588  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
589  if (solverConstraint.m_deltaVelBindex < 0)
590  {
591  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
592  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
594  }
595 
596  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
597 
601 
602  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);
604 
605  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
606  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
607  solverConstraint.m_contactNormal2 = -contactNormal;
608  }
609  else
610  {
611  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
612  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
613  solverConstraint.m_contactNormal2 = -contactNormal;
614 
615  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
616  }
617 
618  {
619  btVector3 vec;
620  btScalar denom0 = 0.f;
621  btScalar denom1 = 0.f;
622  btScalar* jacB = 0;
623  btScalar* jacA = 0;
624  btScalar* lambdaA = 0;
625  btScalar* lambdaB = 0;
626  int ndofA = 0;
627  if (multiBodyA)
628  {
629  ndofA = multiBodyA->getNumDofs() + 6;
630  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
631  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
632  for (int i = 0; i < ndofA; ++i)
633  {
634  btScalar j = jacA[i];
635  btScalar l = lambdaA[i];
636  denom0 += j * l;
637  }
638  }
639  else
640  {
641  if (rb0)
642  {
643  vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
644  denom0 = rb0->getInvMass() + contactNormal.dot(vec);
645  }
646  }
647  if (multiBodyB)
648  {
649  const int ndofB = multiBodyB->getNumDofs() + 6;
650  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
651  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
652  for (int i = 0; i < ndofB; ++i)
653  {
654  btScalar j = jacB[i];
655  btScalar l = lambdaB[i];
656  denom1 += j * l;
657  }
658  }
659  else
660  {
661  if (rb1)
662  {
663  vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
664  denom1 = rb1->getInvMass() + contactNormal.dot(vec);
665  }
666  }
667 
668  btScalar d = denom0 + denom1 + cfm;
669  if (d > SIMD_EPSILON)
670  {
671  solverConstraint.m_jacDiagABInv = relaxation / (d);
672  }
673  else
674  {
675  //disable the constraint row to handle singularity/redundant constraint
676  solverConstraint.m_jacDiagABInv = 0.f;
677  }
678  }
679 
680  //compute rhs and remaining solverConstraint fields
681 
682  btScalar restitution = 0.f;
683  btScalar distance = 0;
684  if (!isFriction)
685  {
686  distance = cp.getDistance() + infoGlobal.m_linearSlop;
687  }
688  else
689  {
691  {
692  distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
693  }
694  }
695 
696  btScalar rel_vel = 0.f;
697  int ndofA = 0;
698  int ndofB = 0;
699  {
700  btVector3 vel1, vel2;
701  if (multiBodyA)
702  {
703  ndofA = multiBodyA->getNumDofs() + 6;
704  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
705  for (int i = 0; i < ndofA; ++i)
706  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
707  }
708  else
709  {
710  if (rb0)
711  {
712  rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) +
713  (rb0->getTotalTorque() * rb0->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos1) +
714  rb0->getTotalForce() * rb0->getInvMass() * infoGlobal.m_timeStep)
715  .dot(solverConstraint.m_contactNormal1);
716  }
717  }
718  if (multiBodyB)
719  {
720  ndofB = multiBodyB->getNumDofs() + 6;
721  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
722  for (int i = 0; i < ndofB; ++i)
723  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
724  }
725  else
726  {
727  if (rb1)
728  {
729  rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2) +
730  (rb1->getTotalTorque() * rb1->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos2) +
731  rb1->getTotalForce() * rb1->getInvMass() * infoGlobal.m_timeStep)
732  .dot(solverConstraint.m_contactNormal2);
733  }
734  }
735 
736  solverConstraint.m_friction = cp.m_combinedFriction;
737 
738  if (!isFriction)
739  {
740  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
741  if (restitution <= btScalar(0.))
742  {
743  restitution = 0.f;
744  }
745  }
746  }
747 
749  //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
750  if (0) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
751  {
752  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
753 
754  if (solverConstraint.m_appliedImpulse)
755  {
756  if (multiBodyA)
757  {
758  btScalar impulse = solverConstraint.m_appliedImpulse;
759  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
760  multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse);
761 
762  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
763  }
764  else
765  {
766  if (rb0)
767  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
768  }
769  if (multiBodyB)
770  {
771  btScalar impulse = solverConstraint.m_appliedImpulse;
772  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
773  multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse);
774  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
775  }
776  else
777  {
778  if (rb1)
779  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
780  }
781  }
782  }
783  else
784  {
785  solverConstraint.m_appliedImpulse = 0.f;
786  }
787 
788  solverConstraint.m_appliedPushImpulse = 0.f;
789 
790  {
791  btScalar positionalError = 0.f;
792  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
793  if (isFriction)
794  {
795  positionalError = -distance * erp / infoGlobal.m_timeStep;
796  }
797  else
798  {
799  if (distance > 0)
800  {
801  positionalError = 0;
802  velocityError -= distance / infoGlobal.m_timeStep;
803  }
804  else
805  {
806  positionalError = -distance * erp / infoGlobal.m_timeStep;
807  }
808  }
809 
810  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
811  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
812 
813  if (!isFriction)
814  {
815  // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
816  {
817  //combine position and velocity into rhs
818  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
819  solverConstraint.m_rhsPenetration = 0.f;
820  }
821  /*else
822  {
823  //split position and velocity into rhs and m_rhsPenetration
824  solverConstraint.m_rhs = velocityImpulse;
825  solverConstraint.m_rhsPenetration = penetrationImpulse;
826  }
827  */
828  solverConstraint.m_lowerLimit = 0;
829  solverConstraint.m_upperLimit = 1e10f;
830  }
831  else
832  {
833  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
834  solverConstraint.m_rhsPenetration = 0.f;
835  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
836  solverConstraint.m_upperLimit = solverConstraint.m_friction;
837  }
838 
839  solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
840  }
841 }
842 
844  const btVector3& constraintNormal,
845  btManifoldPoint& cp,
846  btScalar combinedTorsionalFriction,
847  const btContactSolverInfo& infoGlobal,
848  btScalar& relaxation,
849  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
850 {
851  BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
852  btVector3 rel_pos1;
853  btVector3 rel_pos2;
854 
855  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
856  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
857 
858  const btVector3& pos1 = cp.getPositionWorldOnA();
859  const btVector3& pos2 = cp.getPositionWorldOnB();
860 
861  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
862  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
863 
864  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
865  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
866 
867  if (bodyA)
868  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
869  if (bodyB)
870  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
871 
872  relaxation = infoGlobal.m_sor;
873 
874  // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
875 
876  if (multiBodyA)
877  {
878  if (solverConstraint.m_linkA < 0)
879  {
880  rel_pos1 = pos1 - multiBodyA->getBasePos();
881  }
882  else
883  {
884  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
885  }
886  const int ndofA = multiBodyA->getNumDofs() + 6;
887 
888  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
889 
890  if (solverConstraint.m_deltaVelAindex < 0)
891  {
892  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
893  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
895  }
896  else
897  {
898  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
899  }
900 
901  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
905 
906  btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
907  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);
908  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
910 
911  btVector3 torqueAxis0 = -constraintNormal;
912  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
913  solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
914  }
915  else
916  {
917  btVector3 torqueAxis0 = -constraintNormal;
918  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
919  solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
920  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
921  }
922 
923  if (multiBodyB)
924  {
925  if (solverConstraint.m_linkB < 0)
926  {
927  rel_pos2 = pos2 - multiBodyB->getBasePos();
928  }
929  else
930  {
931  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
932  }
933 
934  const int ndofB = multiBodyB->getNumDofs() + 6;
935 
936  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
937  if (solverConstraint.m_deltaVelBindex < 0)
938  {
939  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
940  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
942  }
943 
944  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
945 
949 
950  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);
952 
953  btVector3 torqueAxis1 = constraintNormal;
954  solverConstraint.m_relpos2CrossNormal = torqueAxis1;
955  solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
956  }
957  else
958  {
959  btVector3 torqueAxis1 = constraintNormal;
960  solverConstraint.m_relpos2CrossNormal = torqueAxis1;
961  solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
962 
963  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
964  }
965 
966  {
967  btScalar denom0 = 0.f;
968  btScalar denom1 = 0.f;
969  btScalar* jacB = 0;
970  btScalar* jacA = 0;
971  btScalar* lambdaA = 0;
972  btScalar* lambdaB = 0;
973  int ndofA = 0;
974  if (multiBodyA)
975  {
976  ndofA = multiBodyA->getNumDofs() + 6;
977  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
978  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
979  for (int i = 0; i < ndofA; ++i)
980  {
981  btScalar j = jacA[i];
982  btScalar l = lambdaA[i];
983  denom0 += j * l;
984  }
985  }
986  else
987  {
988  if (rb0)
989  {
990  btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
991  denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
992  }
993  }
994  if (multiBodyB)
995  {
996  const int ndofB = multiBodyB->getNumDofs() + 6;
997  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
998  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
999  for (int i = 0; i < ndofB; ++i)
1000  {
1001  btScalar j = jacB[i];
1002  btScalar l = lambdaB[i];
1003  denom1 += j * l;
1004  }
1005  }
1006  else
1007  {
1008  if (rb1)
1009  {
1010  btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
1011  denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1012  }
1013  }
1014 
1015  btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm;
1016  if (d > SIMD_EPSILON)
1017  {
1018  solverConstraint.m_jacDiagABInv = relaxation / (d);
1019  }
1020  else
1021  {
1022  //disable the constraint row to handle singularity/redundant constraint
1023  solverConstraint.m_jacDiagABInv = 0.f;
1024  }
1025  }
1026 
1027  //compute rhs and remaining solverConstraint fields
1028 
1029  btScalar restitution = 0.f;
1030  btScalar penetration = isFriction ? 0 : cp.getDistance();
1031 
1032  btScalar rel_vel = 0.f;
1033  int ndofA = 0;
1034  int ndofB = 0;
1035  {
1036  btVector3 vel1, vel2;
1037  if (multiBodyA)
1038  {
1039  ndofA = multiBodyA->getNumDofs() + 6;
1040  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
1041  for (int i = 0; i < ndofA; ++i)
1042  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
1043  }
1044  else
1045  {
1046  if (rb0)
1047  {
1048  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
1049  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));
1050  }
1051  }
1052  if (multiBodyB)
1053  {
1054  ndofB = multiBodyB->getNumDofs() + 6;
1055  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
1056  for (int i = 0; i < ndofB; ++i)
1057  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
1058  }
1059  else
1060  {
1061  if (rb1)
1062  {
1063  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
1064  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));
1065  }
1066  }
1067 
1068  solverConstraint.m_friction = combinedTorsionalFriction;
1069 
1070  if (!isFriction)
1071  {
1072  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
1073  if (restitution <= btScalar(0.))
1074  {
1075  restitution = 0.f;
1076  }
1077  }
1078  }
1079 
1080  solverConstraint.m_appliedImpulse = 0.f;
1081  solverConstraint.m_appliedPushImpulse = 0.f;
1082 
1083  {
1084  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
1085 
1086  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1087 
1088  solverConstraint.m_rhs = velocityImpulse;
1089  solverConstraint.m_rhsPenetration = 0.f;
1090  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
1091  solverConstraint.m_upperLimit = solverConstraint.m_friction;
1092 
1093  solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv;
1094  }
1095 }
1096 
1097 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1098 {
1099  BT_PROFILE("addMultiBodyFrictionConstraint");
1101  solverConstraint.m_orgConstraint = 0;
1102  solverConstraint.m_orgDofIndex = -1;
1103 
1104  solverConstraint.m_frictionIndex = frictionIndex;
1105  bool isFriction = true;
1106 
1109 
1110  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1111  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1112 
1113  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1114  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1115 
1116  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1117  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1118  solverConstraint.m_multiBodyA = mbA;
1119  if (mbA)
1120  solverConstraint.m_linkA = fcA->m_link;
1121 
1122  solverConstraint.m_multiBodyB = mbB;
1123  if (mbB)
1124  solverConstraint.m_linkB = fcB->m_link;
1125 
1126  solverConstraint.m_originalContactPoint = &cp;
1127 
1128  setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1129  return solverConstraint;
1130 }
1131 
1133  btScalar combinedTorsionalFriction,
1134  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1135 {
1136  BT_PROFILE("addMultiBodyRollingFrictionConstraint");
1137 
1138  bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0));
1139 
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  setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1169  return solverConstraint;
1170 }
1171 
1173 {
1176 
1177  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1178  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1179 
1180  btCollisionObject *colObj0 = 0, *colObj1 = 0;
1181 
1182  colObj0 = (btCollisionObject*)manifold->getBody0();
1183  colObj1 = (btCollisionObject*)manifold->getBody1();
1184 
1185  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1186  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1187 
1188  // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
1189  // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
1190 
1192  // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
1193  // return;
1194 
1195  //only a single rollingFriction per manifold
1196  int rollingFriction = 1;
1197 
1198  for (int j = 0; j < manifold->getNumContacts(); j++)
1199  {
1200  btManifoldPoint& cp = manifold->getContactPoint(j);
1201 
1202  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1203  {
1204  btScalar relaxation;
1205 
1206  int frictionIndex = m_multiBodyNormalContactConstraints.size();
1207 
1209 
1210  // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
1211  // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
1212  solverConstraint.m_orgConstraint = 0;
1213  solverConstraint.m_orgDofIndex = -1;
1214  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1215  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1216  solverConstraint.m_multiBodyA = mbA;
1217  if (mbA)
1218  solverConstraint.m_linkA = fcA->m_link;
1219 
1220  solverConstraint.m_multiBodyB = mbB;
1221  if (mbB)
1222  solverConstraint.m_linkB = fcB->m_link;
1223 
1224  solverConstraint.m_originalContactPoint = &cp;
1225 
1226  bool isFriction = false;
1227  setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction);
1228 
1229  // const btVector3& pos1 = cp.getPositionWorldOnA();
1230  // const btVector3& pos2 = cp.getPositionWorldOnB();
1231 
1233 #define ENABLE_FRICTION
1234 #ifdef ENABLE_FRICTION
1236 
1241 
1252 
1256 
1257  if (rollingFriction > 0)
1258  {
1259  if (cp.m_combinedSpinningFriction > 0)
1260  {
1261  addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
1262  }
1263  if (cp.m_combinedRollingFriction > 0)
1264  {
1269 
1270  if (cp.m_lateralFrictionDir1.length() > 0.001)
1271  addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1272 
1273  if (cp.m_lateralFrictionDir2.length() > 0.001)
1274  addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1275  }
1276  rollingFriction--;
1277  }
1279  { /*
1280  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1281  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1282  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
1283  {
1284  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1285  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1286  {
1287  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
1288  cp.m_lateralFrictionDir2.normalize();//??
1289  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1290  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1291  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1292 
1293  }
1294 
1295  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1296  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1297  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1298 
1299  } else
1300  */
1301  {
1304  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1305 
1307  {
1310  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1311  }
1312 
1314  {
1316  }
1317  }
1318  }
1319  else
1320  {
1321  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1322 
1324  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1325 
1326  //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1327  //todo:
1328  solverConstraint.m_appliedImpulse = 0.f;
1329  solverConstraint.m_appliedPushImpulse = 0.f;
1330  }
1331 
1332 #endif //ENABLE_FRICTION
1333  }
1334  }
1335 }
1336 
1337 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
1338 {
1339  //btPersistentManifold* manifold = 0;
1340 
1341  for (int i = 0; i < numManifolds; i++)
1342  {
1343  btPersistentManifold* manifold = manifoldPtr[i];
1346  if (!fcA && !fcB)
1347  {
1348  //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
1349  convertContact(manifold, infoGlobal);
1350  }
1351  else
1352  {
1353  convertMultiBodyContact(manifold, infoGlobal);
1354  }
1355  }
1356 
1357  //also convert the multibody constraints, if any
1358 
1359  for (int i = 0; i < m_tmpNumMultiBodyConstraints; i++)
1360  {
1364 
1366  }
1367 }
1368 
1369 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
1370 {
1371  //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
1372  return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1373 }
1374 
1375 #if 0
1376 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
1377 {
1378  if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
1379  {
1380  //todo: get rid of those temporary memory allocations for the joint feedback
1381  btAlignedObjectArray<btScalar> forceVector;
1382  int numDofsPlusBase = 6+mb->getNumDofs();
1383  forceVector.resize(numDofsPlusBase);
1384  for (int i=0;i<numDofsPlusBase;i++)
1385  {
1386  forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
1387  }
1389  output.resize(numDofsPlusBase);
1390  bool applyJointFeedback = true;
1391  mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
1392  }
1393 }
1394 #endif
1395 
1397 {
1398 #if 1
1399 
1400  //bod->addBaseForce(m_gravity * bod->getBaseMass());
1401  //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
1402 
1403  if (c.m_orgConstraint)
1404  {
1406  }
1407 
1408  if (c.m_multiBodyA)
1409  {
1411  btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime);
1412  btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime);
1413  if (c.m_linkA < 0)
1414  {
1417  }
1418  else
1419  {
1421  //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1423  }
1424  }
1425 
1426  if (c.m_multiBodyB)
1427  {
1428  {
1430  btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime);
1431  btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime);
1432  if (c.m_linkB < 0)
1433  {
1436  }
1437  else
1438  {
1439  {
1441  //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1443  }
1444  }
1445  }
1446  }
1447 #endif
1448 
1449 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
1450 
1451  if (c.m_multiBodyA)
1452  {
1454  }
1455 
1456  if (c.m_multiBodyB)
1457  {
1459  }
1460 #endif
1461 }
1462 
1464 {
1465  BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1466  int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
1467 
1468  //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
1469  //or as applied force, so we can measure the joint reaction forces easier
1470  for (int i = 0; i < numPoolConstraints; i++)
1471  {
1473  writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
1474 
1476 
1478  {
1480  }
1481  }
1482 
1483  for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1484  {
1486  writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
1487  }
1488 
1489  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1490  {
1491  BT_PROFILE("warm starting write back");
1492  for (int j = 0; j < numPoolConstraints; j++)
1493  {
1495  btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
1496  btAssert(pt);
1497  pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1498  pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1499 
1500  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1502  {
1503  pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse;
1504  } else
1505  {
1506  pt->m_appliedImpulseLateral2 = 0;
1507  }
1508  }
1509 
1510  //do a callback here?
1511  }
1512 #if 0
1513  //multibody joint feedback
1514  {
1515  BT_PROFILE("multi body joint feedback");
1516  for (int j=0;j<numPoolConstraints;j++)
1517  {
1519 
1520  //apply the joint feedback into all links of the btMultiBody
1521  //todo: double-check the signs of the applied impulse
1522 
1523  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1524  {
1525  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1526  }
1527  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1528  {
1529  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1530  }
1531 #if 0
1532  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
1533  {
1534  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1535  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
1536  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
1537  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1538 
1539  }
1540  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
1541  {
1542  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1543  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
1544  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
1545  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1546  }
1547 
1549  {
1550  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
1551  {
1552  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1553  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
1554  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
1555  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1556  }
1557 
1558  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
1559  {
1560  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1561  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
1562  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
1563  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1564  }
1565  }
1566 #endif
1567  }
1568 
1569  for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1570  {
1572  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1573  {
1574  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1575  }
1576  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1577  {
1578  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1579  }
1580  }
1581  }
1582 
1583  numPoolConstraints = m_multiBodyNonContactConstraints.size();
1584 
1585 #if 0
1586  //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1587  for (int i=0;i<numPoolConstraints;i++)
1588  {
1590 
1592  btJointFeedback* fb = constr->getJointFeedback();
1593  if (fb)
1594  {
1596  fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1597  fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1598  fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1599 
1600  }
1601 
1604  {
1605  constr->setEnabled(false);
1606  }
1607 
1608  }
1609 #endif
1610 #endif
1611 
1612  return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1613 }
1614 
1615 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)
1616 {
1617  //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
1618 
1619  //printf("solveMultiBodyGroup start\n");
1620  m_tmpMultiBodyConstraints = multiBodyConstraints;
1621  m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1622 
1623  btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1624 
1627 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:523
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:1172
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:55
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:137
btManifoldPoint::m_contactMotion2
btScalar m_contactMotion2
Definition: btManifoldPoint.h:120
btRigidBody::getTotalTorque
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:281
btMultiBody::addBaseConstraintTorque
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:316
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:1872
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:660
btManifoldPoint::m_contactCFM
btScalar m_contactCFM
Definition: btManifoldPoint.h:123
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:57
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:48
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:257
btManifoldPoint::m_contactERP
btScalar m_contactERP
Definition: btManifoldPoint.h:128
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:212
btMultiBody::getCompanionId
int getCompanionId() const
Definition: btMultiBody.h:472
btManifoldPoint::m_contactPointFlags
int m_contactPointFlags
Definition: btManifoldPoint.h:114
BT_CONTACT_FLAG_HAS_CONTACT_CFM
Definition: btManifoldPoint.h:43
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btContactSolverInfo
Definition: btContactSolverInfo.h:69
btManifoldPoint::getPositionWorldOnA
const btVector3 & getPositionWorldOnA() const
Definition: btManifoldPoint.h:148
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btMultiBody::internalNeedsJointFeedback
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:555
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:1463
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:1396
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:98
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
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
SOLVER_USE_WARMSTARTING
Definition: btContactSolverInfo.h:25
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:45
btMultiBody::addLinkConstraintTorque
void addLinkConstraintTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:665
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:843
btMultiBodyConstraintSolver::m_data
btMultiBodyJacobianData m_data
Definition: btMultiBodyConstraintSolver.h:38
btJointFeedback
Definition: btTypedConstraint.h:63
btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION
Definition: btCollisionObject.h:158
btContactSolverInfoData::m_warmstartingFactor
btScalar m_warmstartingFactor
Definition: btContactSolverInfo.h:56
btContactSolverInfoData::m_frictionERP
btScalar m_frictionERP
Definition: btContactSolverInfo.h:49
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:139
btMultiBodySolverConstraint::m_deltaVelAindex
int m_deltaVelAindex
Definition: btMultiBodySolverConstraint.h:37
btManifoldPoint::m_combinedRestitution
btScalar m_combinedRestitution
Definition: btManifoldPoint.h:104
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:1132
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:26
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:1369
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:1548
btMultiBodyConstraintSolver::applyDeltaVee
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
Definition: btMultiBodyConstraintSolver.cpp:186
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:394
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:161
btSolverBody.h
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:74
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:479
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:252
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:477
btJointFeedback::m_appliedForceBodyA
btVector3 m_appliedForceBodyA
Definition: btTypedConstraint.h:67
btManifoldPoint::m_combinedContactDamping1
btScalar m_combinedContactDamping1
Definition: btManifoldPoint.h:129
btMultiBodyConstraintSolver::m_tmpNumMultiBodyConstraints
int m_tmpNumMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:42
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:1337
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:119
btManifoldPoint::m_combinedSpinningFriction
btScalar m_combinedSpinningFriction
Definition: btManifoldPoint.h:103
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:1424
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:210
btMultiBodyLinkCollider.h
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:41
btPersistentManifold::getContactProcessingThreshold
btScalar getContactProcessingThreshold() const
Definition: btPersistentManifold.h:145
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:487
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:1850
btContactSolverInfoData::m_frictionCFM
btScalar m_frictionCFM
Definition: btContactSolverInfo.h:50
btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint
btMultiBodySolverConstraint & addMultiBodyFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:1097
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:478
btSolverBody::internalGetDeltaAngularVelocity
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:202
btManifoldPoint::m_frictionCFM
btScalar m_frictionCFM
Definition: btManifoldPoint.h:132
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:261
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
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:154
btContactSolverInfoData::m_globalCfm
btScalar m_globalCfm
Definition: btContactSolverInfo.h:48
btMultiBodySolverConstraint::m_jacBindex
int m_jacBindex
Definition: btMultiBodySolverConstraint.h:40
btManifoldPoint::m_combinedRollingFriction
btScalar m_combinedRollingFriction
Definition: btManifoldPoint.h:102
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:58
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:374
btSequentialImpulseConstraintSolver::convertContact
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1021
BT_CONTACT_FLAG_HAS_CONTACT_ERP
Definition: btManifoldPoint.h:44
btSequentialImpulseConstraintSolver::m_fixedBodyId
int m_fixedBodyId
Definition: btSequentialImpulseConstraintSolver.h:48
btMultiBody::setCompanionId
void setCompanionId(int id)
Definition: btMultiBody.h:476
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:117
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
btPersistentManifold.h
btMultiBodyJacobianData::m_solverBodyPool
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
Definition: btMultiBodyConstraint.h:36
btContactSolverInfo.h
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:549
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:430
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:37
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:43
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:1425
btMultiBodyConstraintSolver::m_tmpMultiBodyConstraints
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:41
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:256
btTypedConstraint::setEnabled
void setEnabled(bool enabled)
Definition: btTypedConstraint.h:206
btContactSolverInfoData::m_restitutionVelocityThreshold
btScalar m_restitutionVelocityThreshold
Definition: btContactSolverInfo.h:64
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
Definition: btContactSolverInfo.h:27
btMultiBodyConstraintSolver::setupMultiBodyContactConstraint
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:458
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:55
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:262
btMultiBody::applyDeltaVeeMultiDof2
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:377
btMultiBody::addBaseConstraintForce
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:312
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:498
btMultiBodySolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btMultiBodySolverConstraint.h:59
btManifoldPoint::m_lateralFrictionDir1
btVector3 m_lateralFrictionDir1
Definition: btManifoldPoint.h:136
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:106
btMultiBodyConstraint.h
btManifoldPoint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btManifoldPoint.h:116
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:44
btRigidBody::getTotalForce
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:276
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:192
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:237
btCollisionObject::CF_ANISOTROPIC_FRICTION
Definition: btCollisionObject.h:157
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:43
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:47
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:89
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btManifoldPoint::m_appliedImpulseLateral2
btScalar m_appliedImpulseLateral2
Definition: btManifoldPoint.h:118
btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint &cA1, const btMultiBodySolverConstraint &cB)
Definition: btMultiBodyConstraintSolver.cpp:275
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:1615
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:1721
btManifoldPoint::m_combinedFriction
btScalar m_combinedFriction
Definition: btManifoldPoint.h:101
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:149
btManifoldPoint::m_combinedContactStiffness1
btScalar m_combinedContactStiffness1
Definition: btManifoldPoint.h:124
btMultiBodySolverConstraint::m_cfm
btScalar m_cfm
Definition: btMultiBodySolverConstraint.h:56
btMultiBodySolverConstraint::m_frictionIndex
int m_frictionIndex
Definition: btMultiBodySolverConstraint.h:67