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  //if (iteration < constraint.m_overrideNumSolverIterations)
38  //resolveSingleConstraintRowGenericMultiBody(constraint);
40  if(constraint.m_multiBodyA)
41  constraint.m_multiBodyA->setPosUpdated(false);
42  if(constraint.m_multiBodyB)
43  constraint.m_multiBodyB->setPosUpdated(false);
44  }
45 
46  //solve featherstone normal contact
47  for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
48  {
50  if (iteration < infoGlobal.m_numIterations)
52 
53  if(constraint.m_multiBodyA)
54  constraint.m_multiBodyA->setPosUpdated(false);
55  if(constraint.m_multiBodyB)
56  constraint.m_multiBodyB->setPosUpdated(false);
57  }
58 
59  //solve featherstone frictional contact
60 
61  for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
62  {
63  if (iteration < infoGlobal.m_numIterations)
64  {
66  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
67  //adjust friction limits here
68  if (totalImpulse>btScalar(0))
69  {
70  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
71  frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
72  resolveSingleConstraintRowGeneric(frictionConstraint);
73 
74  if(frictionConstraint.m_multiBodyA)
75  frictionConstraint.m_multiBodyA->setPosUpdated(false);
76  if(frictionConstraint.m_multiBodyB)
77  frictionConstraint.m_multiBodyB->setPosUpdated(false);
78  }
79  }
80  }
81  return val;
82 }
83 
84 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
85 {
92 
93  for (int i=0;i<numBodies;i++)
94  {
96  if (fcA)
97  {
98  fcA->m_multiBody->setCompanionId(-1);
99  }
100  }
101 
102  btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
103 
104  return val;
105 }
106 
107 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
108 {
109  for (int i = 0; i < ndof; ++i)
110  m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
111 }
112 
114 {
115 
116  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
117  btScalar deltaVelADotn=0;
118  btScalar deltaVelBDotn=0;
119  btSolverBody* bodyA = 0;
120  btSolverBody* bodyB = 0;
121  int ndofA=0;
122  int ndofB=0;
123 
124  if (c.m_multiBodyA)
125  {
126  ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
127  for (int i = 0; i < ndofA; ++i)
129  } else if(c.m_solverBodyIdA >= 0)
130  {
133  }
134 
135  if (c.m_multiBodyB)
136  {
137  ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
138  for (int i = 0; i < ndofB; ++i)
140  } else if(c.m_solverBodyIdB >= 0)
141  {
144  }
145 
146 
147  deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
148  deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
149  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
150 
151  if (sum < c.m_lowerLimit)
152  {
153  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
155  }
156  else if (sum > c.m_upperLimit)
157  {
158  deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
160  }
161  else
162  {
163  c.m_appliedImpulse = sum;
164  }
165 
166  if (c.m_multiBodyA)
167  {
169 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
170  //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
171  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
172  if(c.m_multiBodyA->isMultiDof())
174  else
176 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
177  } else if(c.m_solverBodyIdA >= 0)
178  {
180 
181  }
182  if (c.m_multiBodyB)
183  {
185 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
186  //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
187  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
188  if(c.m_multiBodyB->isMultiDof())
190  else
192 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
193  } else if(c.m_solverBodyIdB >= 0)
194  {
196  }
197 
198 }
199 
200 
202 {
203 
204  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
205  btScalar deltaVelADotn=0;
206  btScalar deltaVelBDotn=0;
207  int ndofA=0;
208  int ndofB=0;
209 
210  if (c.m_multiBodyA)
211  {
212  ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
213  for (int i = 0; i < ndofA; ++i)
215  }
216 
217  if (c.m_multiBodyB)
218  {
219  ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
220  for (int i = 0; i < ndofB; ++i)
222  }
223 
224 
225  deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
226  deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
227  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
228 
229  if (sum < c.m_lowerLimit)
230  {
231  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
233  }
234  else if (sum > c.m_upperLimit)
235  {
236  deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
238  }
239  else
240  {
241  c.m_appliedImpulse = sum;
242  }
243 
244  if (c.m_multiBodyA)
245  {
248  }
249  if (c.m_multiBodyB)
250  {
253  }
254 }
255 
256 
258  const btVector3& contactNormal,
259  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
260  btScalar& relaxation,
261  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
262 {
263 
264  BT_PROFILE("setupMultiBodyContactConstraint");
265  btVector3 rel_pos1;
266  btVector3 rel_pos2;
267 
268  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
269  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
270 
271  const btVector3& pos1 = cp.getPositionWorldOnA();
272  const btVector3& pos2 = cp.getPositionWorldOnB();
273 
274  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
275  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
276 
277  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
278  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
279 
280  if (bodyA)
281  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
282  if (bodyB)
283  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
284 
285  relaxation = 1.f;
286 
287 
288 
289 
290  if (multiBodyA)
291  {
292  if (solverConstraint.m_linkA<0)
293  {
294  rel_pos1 = pos1 - multiBodyA->getBasePos();
295  } else
296  {
297  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
298  }
299  const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
300 
301  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
302 
303  if (solverConstraint.m_deltaVelAindex <0)
304  {
305  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
306  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
308  } else
309  {
310  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
311  }
312 
313  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
317 
318  btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
319  if(multiBodyA->isMultiDof())
320  multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
321  else
322  multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
323  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
324  if(multiBodyA->isMultiDof())
326  else
327  multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
328 
329  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
330  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
331  solverConstraint.m_contactNormal1 = contactNormal;
332  } else
333  {
334  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
335  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
336  solverConstraint.m_contactNormal1 = contactNormal;
337  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
338  }
339 
340 
341 
342  if (multiBodyB)
343  {
344  if (solverConstraint.m_linkB<0)
345  {
346  rel_pos2 = pos2 - multiBodyB->getBasePos();
347  } else
348  {
349  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
350  }
351 
352  const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
353 
354  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
355  if (solverConstraint.m_deltaVelBindex <0)
356  {
357  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
358  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
360  }
361 
362  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
363 
367 
368  if(multiBodyB->isMultiDof())
369  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);
370  else
371  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);
372  if(multiBodyB->isMultiDof())
374  else
376 
377  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
378  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
379  solverConstraint.m_contactNormal2 = -contactNormal;
380 
381  } else
382  {
383  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
384  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
385  solverConstraint.m_contactNormal2 = -contactNormal;
386 
387  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
388  }
389 
390  {
391 
392  btVector3 vec;
393  btScalar denom0 = 0.f;
394  btScalar denom1 = 0.f;
395  btScalar* jacB = 0;
396  btScalar* jacA = 0;
397  btScalar* lambdaA =0;
398  btScalar* lambdaB =0;
399  int ndofA = 0;
400  if (multiBodyA)
401  {
402  ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
403  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
404  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
405  for (int i = 0; i < ndofA; ++i)
406  {
407  btScalar j = jacA[i] ;
408  btScalar l =lambdaA[i];
409  denom0 += j*l;
410  }
411  } else
412  {
413  if (rb0)
414  {
415  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
416  denom0 = rb0->getInvMass() + contactNormal.dot(vec);
417  }
418  }
419  if (multiBodyB)
420  {
421  const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
422  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
423  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
424  for (int i = 0; i < ndofB; ++i)
425  {
426  btScalar j = jacB[i] ;
427  btScalar l =lambdaB[i];
428  denom1 += j*l;
429  }
430 
431  } else
432  {
433  if (rb1)
434  {
435  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
436  denom1 = rb1->getInvMass() + contactNormal.dot(vec);
437  }
438  }
439 
440 
441 
442  btScalar d = denom0+denom1;
443  if (d>SIMD_EPSILON)
444  {
445  solverConstraint.m_jacDiagABInv = relaxation/(d);
446  } else
447  {
448  //disable the constraint row to handle singularity/redundant constraint
449  solverConstraint.m_jacDiagABInv = 0.f;
450  }
451 
452  }
453 
454 
455  //compute rhs and remaining solverConstraint fields
456 
457 
458 
459  btScalar restitution = 0.f;
460  btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
461 
462  btScalar rel_vel = 0.f;
463  int ndofA = 0;
464  int ndofB = 0;
465  {
466 
467  btVector3 vel1,vel2;
468  if (multiBodyA)
469  {
470  ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
471  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
472  for (int i = 0; i < ndofA ; ++i)
473  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
474  } else
475  {
476  if (rb0)
477  {
478  rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
479  }
480  }
481  if (multiBodyB)
482  {
483  ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
484  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
485  for (int i = 0; i < ndofB ; ++i)
486  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
487 
488  } else
489  {
490  if (rb1)
491  {
492  rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
493  }
494  }
495 
496  solverConstraint.m_friction = cp.m_combinedFriction;
497 
498  if(!isFriction)
499  {
500  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
501  if (restitution <= btScalar(0.))
502  {
503  restitution = 0.f;
504  }
505  }
506  }
507 
508 
510  //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
511  if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
512  {
513  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
514 
515  if (solverConstraint.m_appliedImpulse)
516  {
517  if (multiBodyA)
518  {
519  btScalar impulse = solverConstraint.m_appliedImpulse;
520  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
521  if(multiBodyA->isMultiDof())
522  multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
523  else
524  multiBodyA->applyDeltaVee(deltaV,impulse);
525  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
526  } else
527  {
528  if (rb0)
529  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
530  }
531  if (multiBodyB)
532  {
533  btScalar impulse = solverConstraint.m_appliedImpulse;
534  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
535  if(multiBodyB->isMultiDof())
536  multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
537  else
538  multiBodyB->applyDeltaVee(deltaV,impulse);
539  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
540  } else
541  {
542  if (rb1)
543  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
544  }
545  }
546  } else
547  {
548  solverConstraint.m_appliedImpulse = 0.f;
549  }
550 
551  solverConstraint.m_appliedPushImpulse = 0.f;
552 
553  {
554 
555  btScalar positionalError = 0.f;
556  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
557 
558  btScalar erp = infoGlobal.m_erp2;
559  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
560  {
561  erp = infoGlobal.m_erp;
562  }
563 
564  if (penetration>0)
565  {
566  positionalError = 0;
567  velocityError -= penetration / infoGlobal.m_timeStep;
568 
569  } else
570  {
571  positionalError = -penetration * erp/infoGlobal.m_timeStep;
572  }
573 
574  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
575  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
576 
577  if(!isFriction)
578  {
579  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
580  {
581  //combine position and velocity into rhs
582  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
583  solverConstraint.m_rhsPenetration = 0.f;
584 
585  } else
586  {
587  //split position and velocity into rhs and m_rhsPenetration
588  solverConstraint.m_rhs = velocityImpulse;
589  solverConstraint.m_rhsPenetration = penetrationImpulse;
590  }
591 
592  solverConstraint.m_lowerLimit = 0;
593  solverConstraint.m_upperLimit = 1e10f;
594  }
595  else
596  {
597  solverConstraint.m_rhs = velocityImpulse;
598  solverConstraint.m_rhsPenetration = 0.f;
599  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
600  solverConstraint.m_upperLimit = solverConstraint.m_friction;
601  }
602 
603  solverConstraint.m_cfm = 0.f; //why not use cfmSlip?
604  }
605 
606 }
607 
608 
609 
610 
612 {
613  BT_PROFILE("addMultiBodyFrictionConstraint");
615  solverConstraint.m_orgConstraint = 0;
616  solverConstraint.m_orgDofIndex = -1;
617 
618  solverConstraint.m_frictionIndex = frictionIndex;
619  bool isFriction = true;
620 
623 
624  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
625  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
626 
627  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
628  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
629 
630  solverConstraint.m_solverBodyIdA = solverBodyIdA;
631  solverConstraint.m_solverBodyIdB = solverBodyIdB;
632  solverConstraint.m_multiBodyA = mbA;
633  if (mbA)
634  solverConstraint.m_linkA = fcA->m_link;
635 
636  solverConstraint.m_multiBodyB = mbB;
637  if (mbB)
638  solverConstraint.m_linkB = fcB->m_link;
639 
640  solverConstraint.m_originalContactPoint = &cp;
641 
642  setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
643  return solverConstraint;
644 }
645 
647 {
650 
651  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
652  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
653 
654  btCollisionObject* colObj0=0,*colObj1=0;
655 
656  colObj0 = (btCollisionObject*)manifold->getBody0();
657  colObj1 = (btCollisionObject*)manifold->getBody1();
658 
659  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
660  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
661 
662 // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
663 // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
664 
665 
667 // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
668  // return;
669 
670 
671 
672  for (int j=0;j<manifold->getNumContacts();j++)
673  {
674 
675  btManifoldPoint& cp = manifold->getContactPoint(j);
676 
677  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
678  {
679 
680  btScalar relaxation;
681 
682  int frictionIndex = m_multiBodyNormalContactConstraints.size();
683 
685 
686  // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
687  // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
688  solverConstraint.m_orgConstraint = 0;
689  solverConstraint.m_orgDofIndex = -1;
690  solverConstraint.m_solverBodyIdA = solverBodyIdA;
691  solverConstraint.m_solverBodyIdB = solverBodyIdB;
692  solverConstraint.m_multiBodyA = mbA;
693  if (mbA)
694  solverConstraint.m_linkA = fcA->m_link;
695 
696  solverConstraint.m_multiBodyB = mbB;
697  if (mbB)
698  solverConstraint.m_linkB = fcB->m_link;
699 
700  solverConstraint.m_originalContactPoint = &cp;
701 
702  bool isFriction = false;
703  setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
704 
705 // const btVector3& pos1 = cp.getPositionWorldOnA();
706 // const btVector3& pos2 = cp.getPositionWorldOnB();
707 
709 #define ENABLE_FRICTION
710 #ifdef ENABLE_FRICTION
711  solverConstraint.m_frictionIndex = frictionIndex;
712 #if ROLLING_FRICTION
713  int rollingFriction=1;
714  btVector3 angVelA(0,0,0),angVelB(0,0,0);
715  if (rb0)
716  angVelA = rb0->getAngularVelocity();
717  if (rb1)
718  angVelB = rb1->getAngularVelocity();
719  btVector3 relAngVel = angVelB-angVelA;
720 
721  if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
722  {
723  //only a single rollingFriction per manifold
724  rollingFriction--;
725  if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
726  {
727  relAngVel.normalize();
730  if (relAngVel.length()>0.001)
731  addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
732 
733  } else
734  {
735  addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
736  btVector3 axis0,axis1;
737  btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
742  if (axis0.length()>0.001)
743  addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
744  if (axis1.length()>0.001)
745  addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
746 
747  }
748  }
749 #endif //ROLLING_FRICTION
750 
755 
767  {/*
768  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
769  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
770  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
771  {
772  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
773  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
774  {
775  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
776  cp.m_lateralFrictionDir2.normalize();//??
777  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
778  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
779  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
780 
781  }
782 
783  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
784  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
785  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
786 
787  } else
788  */
789  {
791 
794  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
795 
797  {
800  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
801  }
802 
804  {
806  }
807  }
808 
809  } else
810  {
811  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
812 
814  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
815 
816  //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
817  //todo:
818  solverConstraint.m_appliedImpulse = 0.f;
819  solverConstraint.m_appliedPushImpulse = 0.f;
820  }
821 
822 
823 #endif //ENABLE_FRICTION
824 
825  }
826  }
827 }
828 
829 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
830 {
831  //btPersistentManifold* manifold = 0;
832 
833  for (int i=0;i<numManifolds;i++)
834  {
835  btPersistentManifold* manifold= manifoldPtr[i];
838  if (!fcA && !fcB)
839  {
840  //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
841  convertContact(manifold,infoGlobal);
842  } else
843  {
844  convertMultiBodyContact(manifold,infoGlobal);
845  }
846  }
847 
848  //also convert the multibody constraints, if any
849 
850 
851  for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
852  {
856 
858  }
859 
860 }
861 
862 
863 
864 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
865 {
866  return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
867 }
868 
869 #if 0
870 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
871 {
872  if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
873  {
874  //todo: get rid of those temporary memory allocations for the joint feedback
875  btAlignedObjectArray<btScalar> forceVector;
876  int numDofsPlusBase = 6+mb->getNumDofs();
877  forceVector.resize(numDofsPlusBase);
878  for (int i=0;i<numDofsPlusBase;i++)
879  {
880  forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
881  }
883  output.resize(numDofsPlusBase);
884  bool applyJointFeedback = true;
885  mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
886  }
887 }
888 #endif
889 
890 #include "Bullet3Common/b3Logging.h"
892 {
893 #if 1
894 
895  //bod->addBaseForce(m_gravity * bod->getBaseMass());
896  //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
897 
898  if (c.m_orgConstraint)
899  {
901  }
902 
903 
904  if (c.m_multiBodyA)
905  {
906 
907  if(c.m_multiBodyA->isMultiDof())
908  {
910  btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
911  btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
912  if (c.m_linkA<0)
913  {
916  } else
917  {
919  //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
921  }
922  }
923  }
924 
925  if (c.m_multiBodyB)
926  {
927  if(c.m_multiBodyB->isMultiDof())
928  {
929  {
931  btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
932  btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
933  if (c.m_linkB<0)
934  {
937  } else
938  {
939  {
941  //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
943  }
944 
945  }
946  }
947  }
948  }
949 #endif
950 
951 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
952 
953  if (c.m_multiBodyA)
954  {
955 
956  if(c.m_multiBodyA->isMultiDof())
957  {
959  }
960  else
961  {
963  }
964  }
965 
966  if (c.m_multiBodyB)
967  {
968  if(c.m_multiBodyB->isMultiDof())
969  {
971  }
972  else
973  {
975  }
976  }
977 #endif
978 
979 
980 
981 }
982 
984 {
985  BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
986  int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
987 
988 
989  //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
990  //or as applied force, so we can measure the joint reaction forces easier
991  for (int i=0;i<numPoolConstraints;i++)
992  {
994  writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
995 
997 
999  {
1001  }
1002  }
1003 
1004 
1005  for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1006  {
1008  writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
1009  }
1010 
1011 
1012  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1013  {
1014  BT_PROFILE("warm starting write back");
1015  for (int j=0;j<numPoolConstraints;j++)
1016  {
1018  btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
1019  btAssert(pt);
1020  pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1021  pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1022 
1023  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1025  {
1026  pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
1027  }
1028  //do a callback here?
1029  }
1030  }
1031 #if 0
1032  //multibody joint feedback
1033  {
1034  BT_PROFILE("multi body joint feedback");
1035  for (int j=0;j<numPoolConstraints;j++)
1036  {
1038 
1039  //apply the joint feedback into all links of the btMultiBody
1040  //todo: double-check the signs of the applied impulse
1041 
1042  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1043  {
1044  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1045  }
1046  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1047  {
1048  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1049  }
1050 #if 0
1051  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
1052  {
1053  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1054  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
1055  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
1056  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1057 
1058  }
1059  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
1060  {
1061  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1062  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
1063  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
1064  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1065  }
1066 
1068  {
1069  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
1070  {
1071  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1072  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
1073  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
1074  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1075  }
1076 
1077  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
1078  {
1079  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1080  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
1081  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
1082  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1083  }
1084  }
1085 #endif
1086  }
1087 
1088  for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1089  {
1091  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1092  {
1093  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1094  }
1095  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1096  {
1097  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1098  }
1099  }
1100  }
1101 
1102  numPoolConstraints = m_multiBodyNonContactConstraints.size();
1103 
1104 #if 0
1105  //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1106  for (int i=0;i<numPoolConstraints;i++)
1107  {
1109 
1111  btJointFeedback* fb = constr->getJointFeedback();
1112  if (fb)
1113  {
1115  fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1116  fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1117  fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1118 
1119  }
1120 
1123  {
1124  constr->setEnabled(false);
1125  }
1126 
1127  }
1128 #endif
1129 #endif
1130 
1131  return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
1132 }
1133 
1134 
1135 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)
1136 {
1137  //printf("solveMultiBodyGroup start\n");
1138  m_tmpMultiBodyConstraints = multiBodyConstraints;
1139  m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1140 
1141  btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
1142 
1145 
1146 
1147 }
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)
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:118
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)
int getNumLinks() const
Definition: btMultiBody.h:154
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:597
ManifoldContactPoint collects and maintains persistent contactpoints.
const btCollisionObject * getBody0() const
btScalar m_contactMotion1
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:422
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:366
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:517
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:513
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:470
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'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:405
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:591
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:579
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't use them directly
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:302
btVector3 m_lateralFrictionDir2
btScalar getDistance() const
const btVector3 & getBasePos() const
Definition: btMultiBody.h:176
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:306
const btRigidBody & getRigidBodyB() const
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
int getNumDofs() const
Definition: btMultiBody.h:155
const btCollisionObject * getBody1() const
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:248
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