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 
16 
20 
22 #include "btMultiBodyConstraint.h"
24 
25 #include "LinearMath/btQuickprof.h"
26 
27 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
28 {
29  btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
30 
31  //solve featherstone non-contact constraints
32 
33  //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
34  for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
35  {
37 
39  if(constraint.m_multiBodyA)
40  constraint.m_multiBodyA->setPosUpdated(false);
41  if(constraint.m_multiBodyB)
42  constraint.m_multiBodyB->setPosUpdated(false);
43  }
44 
45  //solve featherstone normal contact
46  for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
47  {
49  if (iteration < infoGlobal.m_numIterations)
51 
52  if(constraint.m_multiBodyA)
53  constraint.m_multiBodyA->setPosUpdated(false);
54  if(constraint.m_multiBodyB)
55  constraint.m_multiBodyB->setPosUpdated(false);
56  }
57 
58  //solve featherstone frictional contact
59 
60  for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
61  {
62  if (iteration < infoGlobal.m_numIterations)
63  {
65  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
66  //adjust friction limits here
67  if (totalImpulse>btScalar(0))
68  {
69  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
70  frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
71  resolveSingleConstraintRowGeneric(frictionConstraint);
72 
73  if(frictionConstraint.m_multiBodyA)
74  frictionConstraint.m_multiBodyA->setPosUpdated(false);
75  if(frictionConstraint.m_multiBodyB)
76  frictionConstraint.m_multiBodyB->setPosUpdated(false);
77  }
78  }
79  }
80  return val;
81 }
82 
83 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
84 {
91 
92  for (int i=0;i<numBodies;i++)
93  {
95  if (fcA)
96  {
97  fcA->m_multiBody->setCompanionId(-1);
98  }
99  }
100 
101  btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
102 
103  return val;
104 }
105 
106 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
107 {
108  for (int i = 0; i < ndof; ++i)
109  m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
110 }
111 
113 {
114 
115  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
116  btScalar deltaVelADotn=0;
117  btScalar deltaVelBDotn=0;
118  btSolverBody* bodyA = 0;
119  btSolverBody* bodyB = 0;
120  int ndofA=0;
121  int ndofB=0;
122 
123  if (c.m_multiBodyA)
124  {
125  ndofA = c.m_multiBodyA->getNumDofs() + 6;
126  for (int i = 0; i < ndofA; ++i)
128  } else if(c.m_solverBodyIdA >= 0)
129  {
132  }
133 
134  if (c.m_multiBodyB)
135  {
136  ndofB = c.m_multiBodyB->getNumDofs() + 6;
137  for (int i = 0; i < ndofB; ++i)
139  } else if(c.m_solverBodyIdB >= 0)
140  {
143  }
144 
145 
146  deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
147  deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
148  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
149 
150  if (sum < c.m_lowerLimit)
151  {
152  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
154  }
155  else if (sum > c.m_upperLimit)
156  {
157  deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
159  }
160  else
161  {
162  c.m_appliedImpulse = sum;
163  }
164 
165  if (c.m_multiBodyA)
166  {
168 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
169  //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
170  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
172 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
173  } else if(c.m_solverBodyIdA >= 0)
174  {
176 
177  }
178  if (c.m_multiBodyB)
179  {
181 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
182  //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
183  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
185 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
186  } else if(c.m_solverBodyIdB >= 0)
187  {
189  }
190 
191 }
192 
193 
194 
195 
197  const btVector3& contactNormal,
198  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
199  btScalar& relaxation,
200  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
201 {
202 
203  BT_PROFILE("setupMultiBodyContactConstraint");
204  btVector3 rel_pos1;
205  btVector3 rel_pos2;
206 
207  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
208  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
209 
210  const btVector3& pos1 = cp.getPositionWorldOnA();
211  const btVector3& pos2 = cp.getPositionWorldOnB();
212 
213  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
214  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
215 
216  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
217  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
218 
219  if (bodyA)
220  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
221  if (bodyB)
222  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
223 
224  relaxation = 1.f;
225 
226 
227 
228 
229  if (multiBodyA)
230  {
231  if (solverConstraint.m_linkA<0)
232  {
233  rel_pos1 = pos1 - multiBodyA->getBasePos();
234  } else
235  {
236  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
237  }
238  const int ndofA = multiBodyA->getNumDofs() + 6;
239 
240  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
241 
242  if (solverConstraint.m_deltaVelAindex <0)
243  {
244  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
245  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
247  } else
248  {
249  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
250  }
251 
252  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
256 
257  btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
258  multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
259  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
261 
262  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
263  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
264  solverConstraint.m_contactNormal1 = contactNormal;
265  } else
266  {
267  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
268  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
269  solverConstraint.m_contactNormal1 = contactNormal;
270  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
271  }
272 
273 
274 
275  if (multiBodyB)
276  {
277  if (solverConstraint.m_linkB<0)
278  {
279  rel_pos2 = pos2 - multiBodyB->getBasePos();
280  } else
281  {
282  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
283  }
284 
285  const int ndofB = multiBodyB->getNumDofs() + 6;
286 
287  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
288  if (solverConstraint.m_deltaVelBindex <0)
289  {
290  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
291  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
293  }
294 
295  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
296 
300 
301  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);
303 
304  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
305  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
306  solverConstraint.m_contactNormal2 = -contactNormal;
307 
308  } else
309  {
310  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
311  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
312  solverConstraint.m_contactNormal2 = -contactNormal;
313 
314  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
315  }
316 
317  {
318 
319  btVector3 vec;
320  btScalar denom0 = 0.f;
321  btScalar denom1 = 0.f;
322  btScalar* jacB = 0;
323  btScalar* jacA = 0;
324  btScalar* lambdaA =0;
325  btScalar* lambdaB =0;
326  int ndofA = 0;
327  if (multiBodyA)
328  {
329  ndofA = multiBodyA->getNumDofs() + 6;
330  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
331  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
332  for (int i = 0; i < ndofA; ++i)
333  {
334  btScalar j = jacA[i] ;
335  btScalar l =lambdaA[i];
336  denom0 += j*l;
337  }
338  } else
339  {
340  if (rb0)
341  {
342  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
343  denom0 = rb0->getInvMass() + contactNormal.dot(vec);
344  }
345  }
346  if (multiBodyB)
347  {
348  const int ndofB = multiBodyB->getNumDofs() + 6;
349  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
350  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
351  for (int i = 0; i < ndofB; ++i)
352  {
353  btScalar j = jacB[i] ;
354  btScalar l =lambdaB[i];
355  denom1 += j*l;
356  }
357 
358  } else
359  {
360  if (rb1)
361  {
362  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
363  denom1 = rb1->getInvMass() + contactNormal.dot(vec);
364  }
365  }
366 
367 
368 
369  btScalar d = denom0+denom1;
370  if (d>SIMD_EPSILON)
371  {
372  solverConstraint.m_jacDiagABInv = relaxation/(d);
373  } else
374  {
375  //disable the constraint row to handle singularity/redundant constraint
376  solverConstraint.m_jacDiagABInv = 0.f;
377  }
378 
379  }
380 
381 
382  //compute rhs and remaining solverConstraint fields
383 
384 
385 
386  btScalar restitution = 0.f;
387  btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
388 
389  btScalar rel_vel = 0.f;
390  int ndofA = 0;
391  int ndofB = 0;
392  {
393 
394  btVector3 vel1,vel2;
395  if (multiBodyA)
396  {
397  ndofA = multiBodyA->getNumDofs() + 6;
398  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
399  for (int i = 0; i < ndofA ; ++i)
400  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
401  } else
402  {
403  if (rb0)
404  {
405  rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
406  }
407  }
408  if (multiBodyB)
409  {
410  ndofB = multiBodyB->getNumDofs() + 6;
411  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
412  for (int i = 0; i < ndofB ; ++i)
413  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
414 
415  } else
416  {
417  if (rb1)
418  {
419  rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
420  }
421  }
422 
423  solverConstraint.m_friction = cp.m_combinedFriction;
424 
425  if(!isFriction)
426  {
427  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
428  if (restitution <= btScalar(0.))
429  {
430  restitution = 0.f;
431  }
432  }
433  }
434 
435 
437  //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
438  if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
439  {
440  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
441 
442  if (solverConstraint.m_appliedImpulse)
443  {
444  if (multiBodyA)
445  {
446  btScalar impulse = solverConstraint.m_appliedImpulse;
447  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
448  multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
449 
450  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
451  } else
452  {
453  if (rb0)
454  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
455  }
456  if (multiBodyB)
457  {
458  btScalar impulse = solverConstraint.m_appliedImpulse;
459  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
460  multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
461  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
462  } else
463  {
464  if (rb1)
465  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
466  }
467  }
468  } else
469  {
470  solverConstraint.m_appliedImpulse = 0.f;
471  }
472 
473  solverConstraint.m_appliedPushImpulse = 0.f;
474 
475  {
476 
477  btScalar positionalError = 0.f;
478  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
479 
480  btScalar erp = infoGlobal.m_erp2;
481  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
482  {
483  erp = infoGlobal.m_erp;
484  }
485 
486  if (penetration>0)
487  {
488  positionalError = 0;
489  velocityError -= penetration / infoGlobal.m_timeStep;
490 
491  } else
492  {
493  positionalError = -penetration * erp/infoGlobal.m_timeStep;
494  }
495 
496  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
497  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
498 
499  if(!isFriction)
500  {
501  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
502  {
503  //combine position and velocity into rhs
504  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
505  solverConstraint.m_rhsPenetration = 0.f;
506 
507  } else
508  {
509  //split position and velocity into rhs and m_rhsPenetration
510  solverConstraint.m_rhs = velocityImpulse;
511  solverConstraint.m_rhsPenetration = penetrationImpulse;
512  }
513 
514  solverConstraint.m_lowerLimit = 0;
515  solverConstraint.m_upperLimit = 1e10f;
516  }
517  else
518  {
519  solverConstraint.m_rhs = velocityImpulse;
520  solverConstraint.m_rhsPenetration = 0.f;
521  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
522  solverConstraint.m_upperLimit = solverConstraint.m_friction;
523  }
524 
525  solverConstraint.m_cfm = 0.f; //why not use cfmSlip?
526  }
527 
528 }
529 
530 
531 
532 
534 {
535  BT_PROFILE("addMultiBodyFrictionConstraint");
537  solverConstraint.m_orgConstraint = 0;
538  solverConstraint.m_orgDofIndex = -1;
539 
540  solverConstraint.m_frictionIndex = frictionIndex;
541  bool isFriction = true;
542 
545 
546  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
547  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
548 
549  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
550  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
551 
552  solverConstraint.m_solverBodyIdA = solverBodyIdA;
553  solverConstraint.m_solverBodyIdB = solverBodyIdB;
554  solverConstraint.m_multiBodyA = mbA;
555  if (mbA)
556  solverConstraint.m_linkA = fcA->m_link;
557 
558  solverConstraint.m_multiBodyB = mbB;
559  if (mbB)
560  solverConstraint.m_linkB = fcB->m_link;
561 
562  solverConstraint.m_originalContactPoint = &cp;
563 
564  setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
565  return solverConstraint;
566 }
567 
569 {
572 
573  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
574  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
575 
576  btCollisionObject* colObj0=0,*colObj1=0;
577 
578  colObj0 = (btCollisionObject*)manifold->getBody0();
579  colObj1 = (btCollisionObject*)manifold->getBody1();
580 
581  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
582  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
583 
584 // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
585 // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
586 
587 
589 // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
590  // return;
591 
592 
593 
594  for (int j=0;j<manifold->getNumContacts();j++)
595  {
596 
597  btManifoldPoint& cp = manifold->getContactPoint(j);
598 
599  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
600  {
601 
602  btScalar relaxation;
603 
604  int frictionIndex = m_multiBodyNormalContactConstraints.size();
605 
607 
608  // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
609  // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
610  solverConstraint.m_orgConstraint = 0;
611  solverConstraint.m_orgDofIndex = -1;
612  solverConstraint.m_solverBodyIdA = solverBodyIdA;
613  solverConstraint.m_solverBodyIdB = solverBodyIdB;
614  solverConstraint.m_multiBodyA = mbA;
615  if (mbA)
616  solverConstraint.m_linkA = fcA->m_link;
617 
618  solverConstraint.m_multiBodyB = mbB;
619  if (mbB)
620  solverConstraint.m_linkB = fcB->m_link;
621 
622  solverConstraint.m_originalContactPoint = &cp;
623 
624  bool isFriction = false;
625  setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
626 
627 // const btVector3& pos1 = cp.getPositionWorldOnA();
628 // const btVector3& pos2 = cp.getPositionWorldOnB();
629 
631 #define ENABLE_FRICTION
632 #ifdef ENABLE_FRICTION
633  solverConstraint.m_frictionIndex = frictionIndex;
634 #if ROLLING_FRICTION
635  int rollingFriction=1;
636  btVector3 angVelA(0,0,0),angVelB(0,0,0);
637  if (rb0)
638  angVelA = rb0->getAngularVelocity();
639  if (rb1)
640  angVelB = rb1->getAngularVelocity();
641  btVector3 relAngVel = angVelB-angVelA;
642 
643  if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
644  {
645  //only a single rollingFriction per manifold
646  rollingFriction--;
647  if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
648  {
649  relAngVel.normalize();
652  if (relAngVel.length()>0.001)
653  addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
654 
655  } else
656  {
657  addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
658  btVector3 axis0,axis1;
659  btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
664  if (axis0.length()>0.001)
665  addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
666  if (axis1.length()>0.001)
667  addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
668 
669  }
670  }
671 #endif //ROLLING_FRICTION
672 
677 
689  {/*
690  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
691  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
692  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
693  {
694  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
695  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
696  {
697  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
698  cp.m_lateralFrictionDir2.normalize();//??
699  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
700  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
701  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
702 
703  }
704 
705  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
706  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
707  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
708 
709  } else
710  */
711  {
713 
716  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
717 
719  {
722  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
723  }
724 
726  {
728  }
729  }
730 
731  } else
732  {
733  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
734 
736  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
737 
738  //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
739  //todo:
740  solverConstraint.m_appliedImpulse = 0.f;
741  solverConstraint.m_appliedPushImpulse = 0.f;
742  }
743 
744 
745 #endif //ENABLE_FRICTION
746 
747  }
748  }
749 }
750 
751 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
752 {
753  //btPersistentManifold* manifold = 0;
754 
755  for (int i=0;i<numManifolds;i++)
756  {
757  btPersistentManifold* manifold= manifoldPtr[i];
760  if (!fcA && !fcB)
761  {
762  //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
763  convertContact(manifold,infoGlobal);
764  } else
765  {
766  convertMultiBodyContact(manifold,infoGlobal);
767  }
768  }
769 
770  //also convert the multibody constraints, if any
771 
772 
773  for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
774  {
778 
780  }
781 
782 }
783 
784 
785 
786 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
787 {
788  return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
789 }
790 
791 #if 0
792 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
793 {
794  if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
795  {
796  //todo: get rid of those temporary memory allocations for the joint feedback
797  btAlignedObjectArray<btScalar> forceVector;
798  int numDofsPlusBase = 6+mb->getNumDofs();
799  forceVector.resize(numDofsPlusBase);
800  for (int i=0;i<numDofsPlusBase;i++)
801  {
802  forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
803  }
805  output.resize(numDofsPlusBase);
806  bool applyJointFeedback = true;
807  mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
808  }
809 }
810 #endif
811 
812 #include "Bullet3Common/b3Logging.h"
814 {
815 #if 1
816 
817  //bod->addBaseForce(m_gravity * bod->getBaseMass());
818  //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
819 
820  if (c.m_orgConstraint)
821  {
823  }
824 
825 
826  if (c.m_multiBodyA)
827  {
828 
830  btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
831  btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
832  if (c.m_linkA<0)
833  {
836  } else
837  {
839  //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
841  }
842  }
843 
844  if (c.m_multiBodyB)
845  {
846  {
848  btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
849  btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
850  if (c.m_linkB<0)
851  {
854  } else
855  {
856  {
858  //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
860  }
861 
862  }
863  }
864  }
865 #endif
866 
867 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
868 
869  if (c.m_multiBodyA)
870  {
871 
872  if(c.m_multiBodyA->isMultiDof())
873  {
875  }
876  else
877  {
879  }
880  }
881 
882  if (c.m_multiBodyB)
883  {
884  if(c.m_multiBodyB->isMultiDof())
885  {
887  }
888  else
889  {
891  }
892  }
893 #endif
894 
895 
896 
897 }
898 
900 {
901  BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
902  int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
903 
904 
905  //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
906  //or as applied force, so we can measure the joint reaction forces easier
907  for (int i=0;i<numPoolConstraints;i++)
908  {
910  writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
911 
913 
915  {
917  }
918  }
919 
920 
921  for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
922  {
924  writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
925  }
926 
927 
928  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
929  {
930  BT_PROFILE("warm starting write back");
931  for (int j=0;j<numPoolConstraints;j++)
932  {
934  btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
935  btAssert(pt);
936  pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
937  pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
938 
939  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
941  {
942  pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
943  }
944  //do a callback here?
945  }
946  }
947 #if 0
948  //multibody joint feedback
949  {
950  BT_PROFILE("multi body joint feedback");
951  for (int j=0;j<numPoolConstraints;j++)
952  {
954 
955  //apply the joint feedback into all links of the btMultiBody
956  //todo: double-check the signs of the applied impulse
957 
958  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
959  {
960  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
961  }
962  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
963  {
964  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
965  }
966 #if 0
967  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
968  {
969  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
970  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
971  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
972  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
973 
974  }
975  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
976  {
977  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
978  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
979  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
980  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
981  }
982 
984  {
985  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
986  {
987  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
988  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
989  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
990  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
991  }
992 
993  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
994  {
995  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
996  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
997  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
998  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
999  }
1000  }
1001 #endif
1002  }
1003 
1004  for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1005  {
1007  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1008  {
1009  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1010  }
1011  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1012  {
1013  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1014  }
1015  }
1016  }
1017 
1018  numPoolConstraints = m_multiBodyNonContactConstraints.size();
1019 
1020 #if 0
1021  //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1022  for (int i=0;i<numPoolConstraints;i++)
1023  {
1025 
1027  btJointFeedback* fb = constr->getJointFeedback();
1028  if (fb)
1029  {
1031  fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1032  fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1033  fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1034 
1035  }
1036 
1039  {
1040  constr->setEnabled(false);
1041  }
1042 
1043  }
1044 #endif
1045 #endif
1046 
1047  return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
1048 }
1049 
1050 
1051 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)
1052 {
1053  //printf("solveMultiBodyGroup start\n");
1054  m_tmpMultiBodyConstraints = multiBodyConstraints;
1055  m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1056 
1057  btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
1058 
1061 
1062 
1063 }
btScalar getInvMass() const
Definition: btRigidBody.h:270
static T sum(const btAlignedObjectArray< T > &items)
#define SIMD_EPSILON
Definition: btScalar.h:494
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
btScalar m_contactCFM2
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btVector3 m_lateralFrictionDir1
btAlignedObjectArray< btScalar > scratch_r
btAlignedObjectArray< btScalar > m_deltaVelocities
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:255
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:501
btScalar m_appliedImpulseLateral1
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
btScalar m_combinedRestitution
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
btMultiBodyConstraint * m_orgConstraint
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1272
btScalar m_appliedImpulse
#define btAssert(x)
Definition: btScalar.h:113
void addLinkConstraintForce(int i, const btVector3 &f)
btScalar getBreakingImpulseThreshold() const
btScalar m_singleAxisRollingFrictionThreshold
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:553
ManifoldContactPoint collects and maintains persistent contactpoints.
const btCollisionObject * getBody0() const
btScalar m_contactMotion1
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:387
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
btAlignedObjectArray< btMatrix3x3 > scratch_m
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
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...
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:379
const btJointFeedback * getJointFeedback() const
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
void addLinkConstraintTorque(int i, const btVector3 &t)
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later...
Definition: btSolverBody.h:104
btScalar m_combinedRollingFriction
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
btVector3 m_normalWorldOnB
int size() const
return the number of elements in the array
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
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)
btVector3 m_appliedForceBodyA
void setCompanionId(int id)
Definition: btMultiBody.h:473
btScalar m_appliedImpulseLateral2
#define output
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
btScalar m_contactCFM1
btCollisionObject can be used to manage collision detection objects.
btScalar getContactProcessingThreshold() const
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
const btManifoldPoint & getContactPoint(int index) const
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
btAlignedObjectArray< btScalar > m_jacobians
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:223
const btVector3 & getPositionWorldOnB() const
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
btAlignedObjectArray< btVector3 > scratch_v
#define BT_PROFILE(name)
Definition: btQuickprof.h:196
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
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)
btSolverConstraint & addRollingFrictionConstraint(const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint &cp, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f)
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
int getCompanionId() const
Definition: btMultiBody.h:469
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:426
TypedConstraint is the baseclass for Bullet constraints and vehicles.
void resize(int newsize, const T &fillData=T())
btRigidBody * m_originalBody
Definition: btSolverBody.h:124
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don&#39;t use them
Definition: btSolverBody.h:208
const btRigidBody & getRigidBodyA() const
void setEnabled(bool enabled)
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:370
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
void setPosUpdated(bool updated)
Definition: btMultiBody.h:547
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
btScalar m_contactMotion2
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:271
btScalar m_combinedFriction
bool m_lateralFrictionInitialized
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
const btVector3 & getPositionWorldOnA() const
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:130
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:213
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don&#39;t use them directly
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:303
btVector3 m_lateralFrictionDir2
btScalar getDistance() const
const btVector3 & getBasePos() const
Definition: btMultiBody.h:177
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:69
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:261
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:307
const btRigidBody & getRigidBodyB() const
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
int getNumDofs() const
Definition: btMultiBody.h:156
const btCollisionObject * getBody1() const
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:249
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btScalar btFabs(btScalar x)
Definition: btScalar.h:449