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 val = 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  for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
34  {
36  //if (iteration < constraint.m_overrideNumSolverIterations)
37  //resolveSingleConstraintRowGenericMultiBody(constraint);
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->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 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->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 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  //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
169  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
170  if(c.m_multiBodyA->isMultiDof())
172  else
174  } else if(c.m_solverBodyIdA >= 0)
175  {
177 
178  }
179  if (c.m_multiBodyB)
180  {
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
184  if(c.m_multiBodyB->isMultiDof())
186  else
188  } else if(c.m_solverBodyIdB >= 0)
189  {
191  }
192 
193 }
194 
195 
197 {
198 
199  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
200  btScalar deltaVelADotn=0;
201  btScalar deltaVelBDotn=0;
202  int ndofA=0;
203  int ndofB=0;
204 
205  if (c.m_multiBodyA)
206  {
207  ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
208  for (int i = 0; i < ndofA; ++i)
210  }
211 
212  if (c.m_multiBodyB)
213  {
214  ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
215  for (int i = 0; i < ndofB; ++i)
217  }
218 
219 
220  deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
221  deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
222  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
223 
224  if (sum < c.m_lowerLimit)
225  {
226  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
228  }
229  else if (sum > c.m_upperLimit)
230  {
231  deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
233  }
234  else
235  {
236  c.m_appliedImpulse = sum;
237  }
238 
239  if (c.m_multiBodyA)
240  {
243  }
244  if (c.m_multiBodyB)
245  {
248  }
249 }
250 
251 
253  const btVector3& contactNormal,
254  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
255  btScalar& relaxation,
256  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
257 {
258 
259  BT_PROFILE("setupMultiBodyContactConstraint");
260  btVector3 rel_pos1;
261  btVector3 rel_pos2;
262 
263  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
264  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
265 
266  const btVector3& pos1 = cp.getPositionWorldOnA();
267  const btVector3& pos2 = cp.getPositionWorldOnB();
268 
269  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
270  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
271 
272  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
273  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
274 
275  if (bodyA)
276  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
277  if (bodyB)
278  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
279 
280  relaxation = 1.f;
281 
282  if (multiBodyA)
283  {
284  const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
285 
286  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
287 
288  if (solverConstraint.m_deltaVelAindex <0)
289  {
290  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
291  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
293  } else
294  {
295  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
296  }
297 
298  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
302 
303  btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
304  if(multiBodyA->isMultiDof())
305  multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
306  else
307  multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
308  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
309  if(multiBodyA->isMultiDof())
311  else
312  multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
313  } else
314  {
315  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
316  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
317  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
318  solverConstraint.m_contactNormal1 = contactNormal;
319  }
320 
321  if (multiBodyB)
322  {
323  const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
324 
325  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
326  if (solverConstraint.m_deltaVelBindex <0)
327  {
328  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
329  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
331  }
332 
333  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
334 
338 
339  if(multiBodyB->isMultiDof())
340  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);
341  else
342  multiBodyB->fillContactJacobian(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);
343  if(multiBodyB->isMultiDof())
345  else
347  } else
348  {
349  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
350  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
351  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
352  solverConstraint.m_contactNormal2 = -contactNormal;
353  }
354 
355  {
356 
357  btVector3 vec;
358  btScalar denom0 = 0.f;
359  btScalar denom1 = 0.f;
360  btScalar* jacB = 0;
361  btScalar* jacA = 0;
362  btScalar* lambdaA =0;
363  btScalar* lambdaB =0;
364  int ndofA = 0;
365  if (multiBodyA)
366  {
367  ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
368  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
369  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
370  for (int i = 0; i < ndofA; ++i)
371  {
372  btScalar j = jacA[i] ;
373  btScalar l =lambdaA[i];
374  denom0 += j*l;
375  }
376  } else
377  {
378  if (rb0)
379  {
380  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
381  denom0 = rb0->getInvMass() + contactNormal.dot(vec);
382  }
383  }
384  if (multiBodyB)
385  {
386  const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
387  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
388  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
389  for (int i = 0; i < ndofB; ++i)
390  {
391  btScalar j = jacB[i] ;
392  btScalar l =lambdaB[i];
393  denom1 += j*l;
394  }
395 
396  } else
397  {
398  if (rb1)
399  {
400  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
401  denom1 = rb1->getInvMass() + contactNormal.dot(vec);
402  }
403  }
404 
405 
406 
407  btScalar d = denom0+denom1;
408  if (d>SIMD_EPSILON)
409  {
410  solverConstraint.m_jacDiagABInv = relaxation/(d);
411  } else
412  {
413  //disable the constraint row to handle singularity/redundant constraint
414  solverConstraint.m_jacDiagABInv = 0.f;
415  }
416 
417  }
418 
419 
420  //compute rhs and remaining solverConstraint fields
421 
422 
423 
424  btScalar restitution = 0.f;
425  btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
426 
427  btScalar rel_vel = 0.f;
428  int ndofA = 0;
429  int ndofB = 0;
430  {
431 
432  btVector3 vel1,vel2;
433  if (multiBodyA)
434  {
435  ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
436  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
437  for (int i = 0; i < ndofA ; ++i)
438  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
439  } else
440  {
441  if (rb0)
442  {
443  rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
444  }
445  }
446  if (multiBodyB)
447  {
448  ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
449  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
450  for (int i = 0; i < ndofB ; ++i)
451  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
452 
453  } else
454  {
455  if (rb1)
456  {
457  rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
458  }
459  }
460 
461  solverConstraint.m_friction = cp.m_combinedFriction;
462 
463  if(!isFriction)
464  {
465  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
466  if (restitution <= btScalar(0.))
467  {
468  restitution = 0.f;
469  }
470  }
471  }
472 
473 
475  //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
476  if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
477  {
478  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
479 
480  if (solverConstraint.m_appliedImpulse)
481  {
482  if (multiBodyA)
483  {
484  btScalar impulse = solverConstraint.m_appliedImpulse;
485  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
486  if(multiBodyA->isMultiDof())
487  multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
488  else
489  multiBodyA->applyDeltaVee(deltaV,impulse);
490  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
491  } else
492  {
493  if (rb0)
494  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
495  }
496  if (multiBodyB)
497  {
498  btScalar impulse = solverConstraint.m_appliedImpulse;
499  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
500  if(multiBodyB->isMultiDof())
501  multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
502  else
503  multiBodyB->applyDeltaVee(deltaV,impulse);
504  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
505  } else
506  {
507  if (rb1)
508  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
509  }
510  }
511  } else
512  {
513  solverConstraint.m_appliedImpulse = 0.f;
514  }
515 
516  solverConstraint.m_appliedPushImpulse = 0.f;
517 
518  {
519 
520  btScalar positionalError = 0.f;
521  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
522 
523  btScalar erp = infoGlobal.m_erp2;
524  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
525  {
526  erp = infoGlobal.m_erp;
527  }
528 
529  if (penetration>0)
530  {
531  positionalError = 0;
532  velocityError -= penetration / infoGlobal.m_timeStep;
533 
534  } else
535  {
536  positionalError = -penetration * erp/infoGlobal.m_timeStep;
537  }
538 
539  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
540  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
541 
542  if(!isFriction)
543  {
544  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
545  {
546  //combine position and velocity into rhs
547  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
548  solverConstraint.m_rhsPenetration = 0.f;
549 
550  } else
551  {
552  //split position and velocity into rhs and m_rhsPenetration
553  solverConstraint.m_rhs = velocityImpulse;
554  solverConstraint.m_rhsPenetration = penetrationImpulse;
555  }
556 
557  solverConstraint.m_lowerLimit = 0;
558  solverConstraint.m_upperLimit = 1e10f;
559  }
560  else
561  {
562  solverConstraint.m_rhs = velocityImpulse;
563  solverConstraint.m_rhsPenetration = 0.f;
564  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
565  solverConstraint.m_upperLimit = solverConstraint.m_friction;
566  }
567 
568  solverConstraint.m_cfm = 0.f; //why not use cfmSlip?
569  }
570 
571 }
572 
573 
574 
575 
577 {
578  BT_PROFILE("addMultiBodyFrictionConstraint");
580  solverConstraint.m_frictionIndex = frictionIndex;
581  bool isFriction = true;
582 
585 
586  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
587  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
588 
589  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
590  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
591 
592  solverConstraint.m_solverBodyIdA = solverBodyIdA;
593  solverConstraint.m_solverBodyIdB = solverBodyIdB;
594  solverConstraint.m_multiBodyA = mbA;
595  if (mbA)
596  solverConstraint.m_linkA = fcA->m_link;
597 
598  solverConstraint.m_multiBodyB = mbB;
599  if (mbB)
600  solverConstraint.m_linkB = fcB->m_link;
601 
602  solverConstraint.m_originalContactPoint = &cp;
603 
604  setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
605  return solverConstraint;
606 }
607 
609 {
612 
613  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
614  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
615 
616  btCollisionObject* colObj0=0,*colObj1=0;
617 
618  colObj0 = (btCollisionObject*)manifold->getBody0();
619  colObj1 = (btCollisionObject*)manifold->getBody1();
620 
621  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
622  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
623 
624 // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
625 // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
626 
627 
629 // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
630  // return;
631 
632 
633 
634  for (int j=0;j<manifold->getNumContacts();j++)
635  {
636 
637  btManifoldPoint& cp = manifold->getContactPoint(j);
638 
639  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
640  {
641 
642  btScalar relaxation;
643 
644  int frictionIndex = m_multiBodyNormalContactConstraints.size();
645 
647 
648  // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
649  // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
650  solverConstraint.m_solverBodyIdA = solverBodyIdA;
651  solverConstraint.m_solverBodyIdB = solverBodyIdB;
652  solverConstraint.m_multiBodyA = mbA;
653  if (mbA)
654  solverConstraint.m_linkA = fcA->m_link;
655 
656  solverConstraint.m_multiBodyB = mbB;
657  if (mbB)
658  solverConstraint.m_linkB = fcB->m_link;
659 
660  solverConstraint.m_originalContactPoint = &cp;
661 
662  bool isFriction = false;
663  setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
664 
665 // const btVector3& pos1 = cp.getPositionWorldOnA();
666 // const btVector3& pos2 = cp.getPositionWorldOnB();
667 
669 #define ENABLE_FRICTION
670 #ifdef ENABLE_FRICTION
671  solverConstraint.m_frictionIndex = frictionIndex;
672 #if ROLLING_FRICTION
673  int rollingFriction=1;
674  btVector3 angVelA(0,0,0),angVelB(0,0,0);
675  if (rb0)
676  angVelA = rb0->getAngularVelocity();
677  if (rb1)
678  angVelB = rb1->getAngularVelocity();
679  btVector3 relAngVel = angVelB-angVelA;
680 
681  if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
682  {
683  //only a single rollingFriction per manifold
684  rollingFriction--;
685  if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
686  {
687  relAngVel.normalize();
690  if (relAngVel.length()>0.001)
691  addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
692 
693  } else
694  {
695  addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
696  btVector3 axis0,axis1;
697  btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
702  if (axis0.length()>0.001)
703  addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
704  if (axis1.length()>0.001)
705  addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
706 
707  }
708  }
709 #endif //ROLLING_FRICTION
710 
715 
727  {/*
728  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
729  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
730  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
731  {
732  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
733  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
734  {
735  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
736  cp.m_lateralFrictionDir2.normalize();//??
737  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
738  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
739  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
740 
741  }
742 
743  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
744  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
745  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
746 
747  } else
748  */
749  {
751 
754  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
755 
757  {
760  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
761  }
762 
764  {
766  }
767  }
768 
769  } else
770  {
771  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
772 
774  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
775 
776  //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
777  //todo:
778  solverConstraint.m_appliedImpulse = 0.f;
779  solverConstraint.m_appliedPushImpulse = 0.f;
780  }
781 
782 
783 #endif //ENABLE_FRICTION
784 
785  }
786  }
787 }
788 
789 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
790 {
791  //btPersistentManifold* manifold = 0;
792 
793  for (int i=0;i<numManifolds;i++)
794  {
795  btPersistentManifold* manifold= manifoldPtr[i];
798  if (!fcA && !fcB)
799  {
800  //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
801  convertContact(manifold,infoGlobal);
802  } else
803  {
804  convertMultiBodyContact(manifold,infoGlobal);
805  }
806  }
807 
808  //also convert the multibody constraints, if any
809 
810 
811  for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
812  {
816 
818  }
819 
820 }
821 
822 
823 
824 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
825 {
826  return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
827 }
828 
830 {
831  int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
832  int j;
833 
834  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
835  {
836  for (j=0;j<numPoolConstraints;j++)
837  {
840  btAssert(pt);
841  pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
842 
844  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
846  {
848  }
849  //do a callback here?
850  }
851  }
852 
853 
854  numPoolConstraints = m_multiBodyNonContactConstraints.size();
855 
856 #if 0
857  //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
858  for (int i=0;i<numPoolConstraints;i++)
859  {
861 
863  btJointFeedback* fb = constr->getJointFeedback();
864  if (fb)
865  {
867  fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
868  fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
869  fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
870 
871  }
872 
875  {
876  constr->setEnabled(false);
877  }
878 
879  }
880 #endif
881 
882 
883  return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
884 }
885 
886 
887 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)
888 {
889  //printf("solveMultiBodyGroup start\n");
890  m_tmpMultiBodyConstraints = multiBodyConstraints;
891  m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
892 
893  btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
894 
897 
898 
899 }
btScalar getInvMass() const
Definition: btRigidBody.h:270
void calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
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)
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)
int getNumLinks() const
Definition: btMultiBody.h:145
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
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1272
btScalar m_appliedImpulse
#define btAssert(x)
Definition: btScalar.h:113
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)
ManifoldContactPoint collects and maintains persistent contactpoints.
const btCollisionObject * getBody0() const
btScalar m_contactMotion1
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:367
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
void applyDeltaVee(const btScalar *delta_vee)
Definition: btMultiBody.h:328
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
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:460
btScalar m_appliedImpulseLateral2
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:28
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:456
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:413
void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint &c)
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)
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:534
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
void fillContactJacobian(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
const btVector3 & getPositionWorldOnA() const
bool isMultiDof()
Definition: btMultiBody.h:522
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
btVector3 m_lateralFrictionDir2
btScalar getDistance() const
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
const btRigidBody & getRigidBodyB() const
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
int getNumDofs() const
Definition: btMultiBody.h:146
const btCollisionObject * getBody1() const
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:224
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