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 leastSquaredResidual = 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 
35  for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
36  {
37  int index = iteration&1? j : m_multiBodyNonContactConstraints.size()-1-j;
38 
40 
41  btScalar residual = resolveSingleConstraintRowGeneric(constraint);
42  leastSquaredResidual += residual*residual;
43 
44  if(constraint.m_multiBodyA)
45  constraint.m_multiBodyA->setPosUpdated(false);
46  if(constraint.m_multiBodyB)
47  constraint.m_multiBodyB->setPosUpdated(false);
48  }
49 
50  //solve featherstone normal contact
51  for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++)
52  {
53  int index = j0;//iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
54 
56  btScalar residual = 0.f;
57 
58  if (iteration < infoGlobal.m_numIterations)
59  {
60  residual = resolveSingleConstraintRowGeneric(constraint);
61  }
62 
63  leastSquaredResidual += residual*residual;
64 
65  if(constraint.m_multiBodyA)
66  constraint.m_multiBodyA->setPosUpdated(false);
67  if(constraint.m_multiBodyB)
68  constraint.m_multiBodyB->setPosUpdated(false);
69  }
70 
71  //solve featherstone frictional contact
72 
73  for (int j1=0;j1<this->m_multiBodyFrictionContactConstraints.size();j1++)
74  {
75  if (iteration < infoGlobal.m_numIterations)
76  {
77  int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
78 
80  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
81  //adjust friction limits here
82  if (totalImpulse>btScalar(0))
83  {
84  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
85  frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
86  btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
87  leastSquaredResidual += residual*residual;
88 
89  if(frictionConstraint.m_multiBodyA)
90  frictionConstraint.m_multiBodyA->setPosUpdated(false);
91  if(frictionConstraint.m_multiBodyB)
92  frictionConstraint.m_multiBodyB->setPosUpdated(false);
93  }
94  }
95  }
96  return leastSquaredResidual;
97 }
98 
99 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
100 {
107 
108  for (int i=0;i<numBodies;i++)
109  {
111  if (fcA)
112  {
113  fcA->m_multiBody->setCompanionId(-1);
114  }
115  }
116 
117  btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
118 
119  return val;
120 }
121 
122 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
123 {
124  for (int i = 0; i < ndof; ++i)
125  m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
126 }
127 
129 {
130 
131  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
132  btScalar deltaVelADotn=0;
133  btScalar deltaVelBDotn=0;
134  btSolverBody* bodyA = 0;
135  btSolverBody* bodyB = 0;
136  int ndofA=0;
137  int ndofB=0;
138 
139  if (c.m_multiBodyA)
140  {
141  ndofA = c.m_multiBodyA->getNumDofs() + 6;
142  for (int i = 0; i < ndofA; ++i)
144  } else if(c.m_solverBodyIdA >= 0)
145  {
148  }
149 
150  if (c.m_multiBodyB)
151  {
152  ndofB = c.m_multiBodyB->getNumDofs() + 6;
153  for (int i = 0; i < ndofB; ++i)
155  } else if(c.m_solverBodyIdB >= 0)
156  {
159  }
160 
161 
162  deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
163  deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
164  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
165 
166  if (sum < c.m_lowerLimit)
167  {
168  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
170  }
171  else if (sum > c.m_upperLimit)
172  {
173  deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
175  }
176  else
177  {
178  c.m_appliedImpulse = sum;
179  }
180 
181  if (c.m_multiBodyA)
182  {
184 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
185  //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
186  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
188 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
189  } else if(c.m_solverBodyIdA >= 0)
190  {
192 
193  }
194  if (c.m_multiBodyB)
195  {
197 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
198  //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
199  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
201 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
202  } else if(c.m_solverBodyIdB >= 0)
203  {
205  }
206  return deltaImpulse;
207 }
208 
209 
210 
211 
213  const btVector3& contactNormal,
214  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
215  btScalar& relaxation,
216  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
217 {
218 
219  BT_PROFILE("setupMultiBodyContactConstraint");
220  btVector3 rel_pos1;
221  btVector3 rel_pos2;
222 
223  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
224  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
225 
226  const btVector3& pos1 = cp.getPositionWorldOnA();
227  const btVector3& pos2 = cp.getPositionWorldOnB();
228 
229  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
230  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
231 
232  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
233  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
234 
235  if (bodyA)
236  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
237  if (bodyB)
238  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
239 
240  relaxation = infoGlobal.m_sor;
241 
242  btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
243 
244  //cfm = 1 / ( dt * kp + kd )
245  //erp = dt * kp / ( dt * kp + kd )
246 
247  btScalar cfm;
248  btScalar erp;
249  if (isFriction)
250  {
251  cfm = infoGlobal.m_frictionCFM;
252  erp = infoGlobal.m_frictionERP;
253  } else
254  {
255  cfm = infoGlobal.m_globalCfm;
256  erp = infoGlobal.m_erp2;
257 
259  {
261  cfm = cp.m_contactCFM;
263  erp = cp.m_contactERP;
264  } else
265  {
267  {
269  if (denom < SIMD_EPSILON)
270  {
271  denom = SIMD_EPSILON;
272  }
273  cfm = btScalar(1) / denom;
274  erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
275  }
276  }
277  }
278 
279  cfm *= invTimeStep;
280 
281  if (multiBodyA)
282  {
283  if (solverConstraint.m_linkA<0)
284  {
285  rel_pos1 = pos1 - multiBodyA->getBasePos();
286  } else
287  {
288  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
289  }
290  const int ndofA = multiBodyA->getNumDofs() + 6;
291 
292  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
293 
294  if (solverConstraint.m_deltaVelAindex <0)
295  {
296  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
297  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
299  } else
300  {
301  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
302  }
303 
304  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
308 
309  btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
310  multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
311  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
313 
314  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
315  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
316  solverConstraint.m_contactNormal1 = contactNormal;
317  } else
318  {
319  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
320  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
321  solverConstraint.m_contactNormal1 = contactNormal;
322  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
323  }
324 
325 
326 
327  if (multiBodyB)
328  {
329  if (solverConstraint.m_linkB<0)
330  {
331  rel_pos2 = pos2 - multiBodyB->getBasePos();
332  } else
333  {
334  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
335  }
336 
337  const int ndofB = multiBodyB->getNumDofs() + 6;
338 
339  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
340  if (solverConstraint.m_deltaVelBindex <0)
341  {
342  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
343  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
345  }
346 
347  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
348 
352 
353  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);
355 
356  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
357  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
358  solverConstraint.m_contactNormal2 = -contactNormal;
359 
360  } else
361  {
362  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
363  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
364  solverConstraint.m_contactNormal2 = -contactNormal;
365 
366  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
367  }
368 
369  {
370 
371  btVector3 vec;
372  btScalar denom0 = 0.f;
373  btScalar denom1 = 0.f;
374  btScalar* jacB = 0;
375  btScalar* jacA = 0;
376  btScalar* lambdaA =0;
377  btScalar* lambdaB =0;
378  int ndofA = 0;
379  if (multiBodyA)
380  {
381  ndofA = multiBodyA->getNumDofs() + 6;
382  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
383  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
384  for (int i = 0; i < ndofA; ++i)
385  {
386  btScalar j = jacA[i] ;
387  btScalar l =lambdaA[i];
388  denom0 += j*l;
389  }
390  } else
391  {
392  if (rb0)
393  {
394  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
395  denom0 = rb0->getInvMass() + contactNormal.dot(vec);
396  }
397  }
398  if (multiBodyB)
399  {
400  const int ndofB = multiBodyB->getNumDofs() + 6;
401  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
402  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
403  for (int i = 0; i < ndofB; ++i)
404  {
405  btScalar j = jacB[i] ;
406  btScalar l =lambdaB[i];
407  denom1 += j*l;
408  }
409 
410  } else
411  {
412  if (rb1)
413  {
414  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
415  denom1 = rb1->getInvMass() + contactNormal.dot(vec);
416  }
417  }
418 
419 
420 
421  btScalar d = denom0+denom1+cfm;
422  if (d>SIMD_EPSILON)
423  {
424  solverConstraint.m_jacDiagABInv = relaxation/(d);
425  } else
426  {
427  //disable the constraint row to handle singularity/redundant constraint
428  solverConstraint.m_jacDiagABInv = 0.f;
429  }
430 
431  }
432 
433 
434  //compute rhs and remaining solverConstraint fields
435 
436 
437 
438  btScalar restitution = 0.f;
439  btScalar distance = 0;
440  if (!isFriction)
441  {
442  distance = cp.getDistance()+infoGlobal.m_linearSlop;
443  } else
444  {
446  {
447  distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
448  }
449  }
450 
451 
452  btScalar rel_vel = 0.f;
453  int ndofA = 0;
454  int ndofB = 0;
455  {
456 
457  btVector3 vel1,vel2;
458  if (multiBodyA)
459  {
460  ndofA = multiBodyA->getNumDofs() + 6;
461  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
462  for (int i = 0; i < ndofA ; ++i)
463  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
464  } else
465  {
466  if (rb0)
467  {
468  rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) +
469  (rb0->getTotalTorque()*rb0->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos1)+
470  rb0->getTotalForce()*rb0->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal1);
471  }
472  }
473  if (multiBodyB)
474  {
475  ndofB = multiBodyB->getNumDofs() + 6;
476  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
477  for (int i = 0; i < ndofB ; ++i)
478  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
479 
480  } else
481  {
482  if (rb1)
483  {
484  rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2)+
485  (rb1->getTotalTorque()*rb1->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos2) +
486  rb1->getTotalForce()*rb1->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal2);
487  }
488  }
489 
490  solverConstraint.m_friction = cp.m_combinedFriction;
491 
492  if(!isFriction)
493  {
494  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
495  if (restitution <= btScalar(0.))
496  {
497  restitution = 0.f;
498  }
499  }
500  }
501 
502 
504  //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
505  if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
506  {
507  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
508 
509  if (solverConstraint.m_appliedImpulse)
510  {
511  if (multiBodyA)
512  {
513  btScalar impulse = solverConstraint.m_appliedImpulse;
514  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
515  multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
516 
517  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
518  } else
519  {
520  if (rb0)
521  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
522  }
523  if (multiBodyB)
524  {
525  btScalar impulse = solverConstraint.m_appliedImpulse;
526  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
527  multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
528  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
529  } else
530  {
531  if (rb1)
532  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
533  }
534  }
535  } else
536  {
537  solverConstraint.m_appliedImpulse = 0.f;
538  }
539 
540  solverConstraint.m_appliedPushImpulse = 0.f;
541 
542  {
543 
544  btScalar positionalError = 0.f;
545  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
546  if (isFriction)
547  {
548  positionalError = -distance * erp/infoGlobal.m_timeStep;
549  } else
550  {
551  if (distance>0)
552  {
553  positionalError = 0;
554  velocityError -= distance / infoGlobal.m_timeStep;
555 
556  } else
557  {
558  positionalError = -distance * erp/infoGlobal.m_timeStep;
559  }
560  }
561 
562  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
563  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
564 
565  if(!isFriction)
566  {
567  // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
568  {
569  //combine position and velocity into rhs
570  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
571  solverConstraint.m_rhsPenetration = 0.f;
572 
573  }
574  /*else
575  {
576  //split position and velocity into rhs and m_rhsPenetration
577  solverConstraint.m_rhs = velocityImpulse;
578  solverConstraint.m_rhsPenetration = penetrationImpulse;
579  }
580  */
581  solverConstraint.m_lowerLimit = 0;
582  solverConstraint.m_upperLimit = 1e10f;
583  }
584  else
585  {
586  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
587  solverConstraint.m_rhsPenetration = 0.f;
588  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
589  solverConstraint.m_upperLimit = solverConstraint.m_friction;
590  }
591 
592  solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
593 
594 
595 
596  }
597 
598 }
599 
601  const btVector3& constraintNormal,
602  btManifoldPoint& cp,
603  btScalar combinedTorsionalFriction,
604  const btContactSolverInfo& infoGlobal,
605  btScalar& relaxation,
606  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
607 {
608 
609  BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
610  btVector3 rel_pos1;
611  btVector3 rel_pos2;
612 
613  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
614  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
615 
616  const btVector3& pos1 = cp.getPositionWorldOnA();
617  const btVector3& pos2 = cp.getPositionWorldOnB();
618 
619  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
620  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
621 
622  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
623  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
624 
625  if (bodyA)
626  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
627  if (bodyB)
628  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
629 
630  relaxation = infoGlobal.m_sor;
631 
632  // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
633 
634 
635  if (multiBodyA)
636  {
637  if (solverConstraint.m_linkA<0)
638  {
639  rel_pos1 = pos1 - multiBodyA->getBasePos();
640  } else
641  {
642  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
643  }
644  const int ndofA = multiBodyA->getNumDofs() + 6;
645 
646  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
647 
648  if (solverConstraint.m_deltaVelAindex <0)
649  {
650  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
651  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
653  } else
654  {
655  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
656  }
657 
658  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
662 
663  btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
664  multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
665  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
667 
668  btVector3 torqueAxis0 = -constraintNormal;
669  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
670  solverConstraint.m_contactNormal1 = btVector3(0,0,0);
671  } else
672  {
673  btVector3 torqueAxis0 = -constraintNormal;
674  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
675  solverConstraint.m_contactNormal1 = btVector3(0,0,0);
676  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
677  }
678 
679 
680 
681  if (multiBodyB)
682  {
683  if (solverConstraint.m_linkB<0)
684  {
685  rel_pos2 = pos2 - multiBodyB->getBasePos();
686  } else
687  {
688  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
689  }
690 
691  const int ndofB = multiBodyB->getNumDofs() + 6;
692 
693  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
694  if (solverConstraint.m_deltaVelBindex <0)
695  {
696  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
697  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
699  }
700 
701  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
702 
706 
707  multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
709 
710  btVector3 torqueAxis1 = constraintNormal;
711  solverConstraint.m_relpos2CrossNormal = torqueAxis1;
712  solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
713 
714  } else
715  {
716  btVector3 torqueAxis1 = constraintNormal;
717  solverConstraint.m_relpos2CrossNormal = torqueAxis1;
718  solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
719 
720  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
721  }
722 
723  {
724 
725  btScalar denom0 = 0.f;
726  btScalar denom1 = 0.f;
727  btScalar* jacB = 0;
728  btScalar* jacA = 0;
729  btScalar* lambdaA =0;
730  btScalar* lambdaB =0;
731  int ndofA = 0;
732  if (multiBodyA)
733  {
734  ndofA = multiBodyA->getNumDofs() + 6;
735  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
736  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
737  for (int i = 0; i < ndofA; ++i)
738  {
739  btScalar j = jacA[i] ;
740  btScalar l =lambdaA[i];
741  denom0 += j*l;
742  }
743  } else
744  {
745  if (rb0)
746  {
747  btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
748  denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
749  }
750  }
751  if (multiBodyB)
752  {
753  const int ndofB = multiBodyB->getNumDofs() + 6;
754  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
755  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
756  for (int i = 0; i < ndofB; ++i)
757  {
758  btScalar j = jacB[i] ;
759  btScalar l =lambdaB[i];
760  denom1 += j*l;
761  }
762 
763  } else
764  {
765  if (rb1)
766  {
767  btVector3 iMJaB = rb1?rb1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
768  denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
769  }
770  }
771 
772 
773 
774  btScalar d = denom0+denom1+infoGlobal.m_globalCfm;
775  if (d>SIMD_EPSILON)
776  {
777  solverConstraint.m_jacDiagABInv = relaxation/(d);
778  } else
779  {
780  //disable the constraint row to handle singularity/redundant constraint
781  solverConstraint.m_jacDiagABInv = 0.f;
782  }
783 
784  }
785 
786 
787  //compute rhs and remaining solverConstraint fields
788 
789 
790 
791  btScalar restitution = 0.f;
792  btScalar penetration = isFriction? 0 : cp.getDistance();
793 
794  btScalar rel_vel = 0.f;
795  int ndofA = 0;
796  int ndofB = 0;
797  {
798 
799  btVector3 vel1,vel2;
800  if (multiBodyA)
801  {
802  ndofA = multiBodyA->getNumDofs() + 6;
803  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
804  for (int i = 0; i < ndofA ; ++i)
805  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
806  } else
807  {
808  if (rb0)
809  {
810  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
811  rel_vel += solverConstraint.m_contactNormal1.dot(rb0?solverBodyA->m_linearVelocity+solverBodyA->m_externalForceImpulse:btVector3(0,0,0))
812  + solverConstraint.m_relpos1CrossNormal.dot(rb0?solverBodyA->m_angularVelocity:btVector3(0,0,0));
813 
814  }
815  }
816  if (multiBodyB)
817  {
818  ndofB = multiBodyB->getNumDofs() + 6;
819  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
820  for (int i = 0; i < ndofB ; ++i)
821  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
822 
823  } else
824  {
825  if (rb1)
826  {
827  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
828  rel_vel += solverConstraint.m_contactNormal2.dot(rb1?solverBodyB->m_linearVelocity+solverBodyB->m_externalForceImpulse:btVector3(0,0,0))
829  + solverConstraint.m_relpos2CrossNormal.dot(rb1?solverBodyB->m_angularVelocity:btVector3(0,0,0));
830 
831  }
832  }
833 
834  solverConstraint.m_friction =combinedTorsionalFriction;
835 
836  if(!isFriction)
837  {
838  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
839  if (restitution <= btScalar(0.))
840  {
841  restitution = 0.f;
842  }
843  }
844  }
845 
846 
847  solverConstraint.m_appliedImpulse = 0.f;
848  solverConstraint.m_appliedPushImpulse = 0.f;
849 
850  {
851 
852  btScalar velocityError = 0 - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
853 
854 
855 
856  btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
857 
858  solverConstraint.m_rhs = velocityImpulse;
859  solverConstraint.m_rhsPenetration = 0.f;
860  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
861  solverConstraint.m_upperLimit = solverConstraint.m_friction;
862 
863  solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv;
864 
865 
866 
867  }
868 
869 }
870 
872 {
873  BT_PROFILE("addMultiBodyFrictionConstraint");
875  solverConstraint.m_orgConstraint = 0;
876  solverConstraint.m_orgDofIndex = -1;
877 
878  solverConstraint.m_frictionIndex = frictionIndex;
879  bool isFriction = true;
880 
883 
884  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
885  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
886 
887  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
888  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
889 
890  solverConstraint.m_solverBodyIdA = solverBodyIdA;
891  solverConstraint.m_solverBodyIdB = solverBodyIdB;
892  solverConstraint.m_multiBodyA = mbA;
893  if (mbA)
894  solverConstraint.m_linkA = fcA->m_link;
895 
896  solverConstraint.m_multiBodyB = mbB;
897  if (mbB)
898  solverConstraint.m_linkB = fcB->m_link;
899 
900  solverConstraint.m_originalContactPoint = &cp;
901 
902  setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
903  return solverConstraint;
904 }
905 
907  btScalar combinedTorsionalFriction,
908  btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
909 {
910  BT_PROFILE("addMultiBodyRollingFrictionConstraint");
912  solverConstraint.m_orgConstraint = 0;
913  solverConstraint.m_orgDofIndex = -1;
914 
915  solverConstraint.m_frictionIndex = frictionIndex;
916  bool isFriction = true;
917 
920 
921  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
922  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
923 
924  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
925  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
926 
927  solverConstraint.m_solverBodyIdA = solverBodyIdA;
928  solverConstraint.m_solverBodyIdB = solverBodyIdB;
929  solverConstraint.m_multiBodyA = mbA;
930  if (mbA)
931  solverConstraint.m_linkA = fcA->m_link;
932 
933  solverConstraint.m_multiBodyB = mbB;
934  if (mbB)
935  solverConstraint.m_linkB = fcB->m_link;
936 
937  solverConstraint.m_originalContactPoint = &cp;
938 
939  setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction,infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
940  return solverConstraint;
941 }
942 
944 {
947 
948  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
949  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
950 
951  btCollisionObject* colObj0=0,*colObj1=0;
952 
953  colObj0 = (btCollisionObject*)manifold->getBody0();
954  colObj1 = (btCollisionObject*)manifold->getBody1();
955 
956  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
957  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
958 
959 // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
960 // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
961 
962 
964 // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
965  // return;
966 
967  //only a single rollingFriction per manifold
968  int rollingFriction=1;
969 
970  for (int j=0;j<manifold->getNumContacts();j++)
971  {
972 
973  btManifoldPoint& cp = manifold->getContactPoint(j);
974 
975  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
976  {
977 
978  btScalar relaxation;
979 
980  int frictionIndex = m_multiBodyNormalContactConstraints.size();
981 
983 
984  // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
985  // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
986  solverConstraint.m_orgConstraint = 0;
987  solverConstraint.m_orgDofIndex = -1;
988  solverConstraint.m_solverBodyIdA = solverBodyIdA;
989  solverConstraint.m_solverBodyIdB = solverBodyIdB;
990  solverConstraint.m_multiBodyA = mbA;
991  if (mbA)
992  solverConstraint.m_linkA = fcA->m_link;
993 
994  solverConstraint.m_multiBodyB = mbB;
995  if (mbB)
996  solverConstraint.m_linkB = fcB->m_link;
997 
998  solverConstraint.m_originalContactPoint = &cp;
999 
1000  bool isFriction = false;
1001  setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
1002 
1003 // const btVector3& pos1 = cp.getPositionWorldOnA();
1004 // const btVector3& pos2 = cp.getPositionWorldOnB();
1005 
1007 #define ENABLE_FRICTION
1008 #ifdef ENABLE_FRICTION
1009  solverConstraint.m_frictionIndex = frictionIndex;
1010 
1015 
1026 
1030 
1031  if (rollingFriction > 0 )
1032  {
1033  if (cp.m_combinedSpinningFriction>0)
1034  {
1035  addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
1036  }
1037  if (cp.m_combinedRollingFriction>0)
1038  {
1039 
1044 
1045  if (cp.m_lateralFrictionDir1.length()>0.001)
1046  addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
1047 
1048  if (cp.m_lateralFrictionDir2.length()>0.001)
1049  addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
1050  }
1051  rollingFriction--;
1052  }
1054  {/*
1055  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1056  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1057  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
1058  {
1059  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1060  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1061  {
1062  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
1063  cp.m_lateralFrictionDir2.normalize();//??
1064  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1065  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1066  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1067 
1068  }
1069 
1070  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1071  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1072  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1073 
1074  } else
1075  */
1076  {
1077 
1078 
1081  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
1082 
1083 
1085  {
1088  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
1089  }
1090 
1092  {
1094  }
1095  }
1096 
1097  } else
1098  {
1099  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM);
1100 
1102  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM);
1103 
1104  //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
1105  //todo:
1106  solverConstraint.m_appliedImpulse = 0.f;
1107  solverConstraint.m_appliedPushImpulse = 0.f;
1108  }
1109 
1110 
1111 #endif //ENABLE_FRICTION
1112 
1113  }
1114  }
1115 }
1116 
1117 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
1118 {
1119  //btPersistentManifold* manifold = 0;
1120 
1121  for (int i=0;i<numManifolds;i++)
1122  {
1123  btPersistentManifold* manifold= manifoldPtr[i];
1126  if (!fcA && !fcB)
1127  {
1128  //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
1129  convertContact(manifold,infoGlobal);
1130  } else
1131  {
1132  convertMultiBodyContact(manifold,infoGlobal);
1133  }
1134  }
1135 
1136  //also convert the multibody constraints, if any
1137 
1138 
1139  for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
1140  {
1144 
1146  }
1147 
1148 }
1149 
1150 
1151 
1152 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
1153 {
1154  return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
1155 }
1156 
1157 #if 0
1158 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
1159 {
1160  if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
1161  {
1162  //todo: get rid of those temporary memory allocations for the joint feedback
1163  btAlignedObjectArray<btScalar> forceVector;
1164  int numDofsPlusBase = 6+mb->getNumDofs();
1165  forceVector.resize(numDofsPlusBase);
1166  for (int i=0;i<numDofsPlusBase;i++)
1167  {
1168  forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
1169  }
1171  output.resize(numDofsPlusBase);
1172  bool applyJointFeedback = true;
1173  mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
1174  }
1175 }
1176 #endif
1177 
1178 
1180 {
1181 #if 1
1182 
1183  //bod->addBaseForce(m_gravity * bod->getBaseMass());
1184  //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
1185 
1186  if (c.m_orgConstraint)
1187  {
1189  }
1190 
1191 
1192  if (c.m_multiBodyA)
1193  {
1194 
1196  btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
1197  btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
1198  if (c.m_linkA<0)
1199  {
1202  } else
1203  {
1205  //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1207  }
1208  }
1209 
1210  if (c.m_multiBodyB)
1211  {
1212  {
1214  btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
1215  btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
1216  if (c.m_linkB<0)
1217  {
1220  } else
1221  {
1222  {
1224  //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1226  }
1227 
1228  }
1229  }
1230  }
1231 #endif
1232 
1233 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
1234 
1235  if (c.m_multiBodyA)
1236  {
1237 
1238  if(c.m_multiBodyA->isMultiDof())
1239  {
1241  }
1242  else
1243  {
1245  }
1246  }
1247 
1248  if (c.m_multiBodyB)
1249  {
1250  if(c.m_multiBodyB->isMultiDof())
1251  {
1253  }
1254  else
1255  {
1257  }
1258  }
1259 #endif
1260 
1261 
1262 
1263 }
1264 
1266 {
1267  BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1268  int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
1269 
1270 
1271  //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
1272  //or as applied force, so we can measure the joint reaction forces easier
1273  for (int i=0;i<numPoolConstraints;i++)
1274  {
1276  writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
1277 
1279 
1281  {
1283  }
1284  }
1285 
1286 
1287  for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1288  {
1290  writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
1291  }
1292 
1293 
1294  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1295  {
1296  BT_PROFILE("warm starting write back");
1297  for (int j=0;j<numPoolConstraints;j++)
1298  {
1300  btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
1301  btAssert(pt);
1302  pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1303  pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1304 
1305  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1307  {
1308  pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
1309  }
1310  //do a callback here?
1311  }
1312  }
1313 #if 0
1314  //multibody joint feedback
1315  {
1316  BT_PROFILE("multi body joint feedback");
1317  for (int j=0;j<numPoolConstraints;j++)
1318  {
1320 
1321  //apply the joint feedback into all links of the btMultiBody
1322  //todo: double-check the signs of the applied impulse
1323 
1324  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1325  {
1326  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1327  }
1328  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1329  {
1330  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1331  }
1332 #if 0
1333  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
1334  {
1335  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1336  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
1337  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
1338  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1339 
1340  }
1341  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
1342  {
1343  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1344  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
1345  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
1346  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1347  }
1348 
1350  {
1351  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
1352  {
1353  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1354  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
1355  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
1356  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1357  }
1358 
1359  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
1360  {
1361  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1362  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
1363  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
1364  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1365  }
1366  }
1367 #endif
1368  }
1369 
1370  for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1371  {
1373  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1374  {
1375  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1376  }
1377  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1378  {
1379  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1380  }
1381  }
1382  }
1383 
1384  numPoolConstraints = m_multiBodyNonContactConstraints.size();
1385 
1386 #if 0
1387  //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1388  for (int i=0;i<numPoolConstraints;i++)
1389  {
1391 
1393  btJointFeedback* fb = constr->getJointFeedback();
1394  if (fb)
1395  {
1397  fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1398  fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1399  fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1400 
1401  }
1402 
1405  {
1406  constr->setEnabled(false);
1407  }
1408 
1409  }
1410 #endif
1411 #endif
1412 
1413  return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
1414 }
1415 
1416 
1417 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)
1418 {
1419  //printf("solveMultiBodyGroup start\n");
1420  m_tmpMultiBodyConstraints = multiBodyConstraints;
1421  m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1422 
1423  btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
1424 
1427 
1428 
1429 }
static T sum(const btAlignedObjectArray< T > &items)
btVector3 m_linearVelocity
Definition: btSolverBody.h:119
btVector3 m_angularVelocity
Definition: btSolverBody.h:120
#define SIMD_EPSILON
Definition: btScalar.h:521
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
const btVector3 & getBasePos() const
Definition: btMultiBody.h:186
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)
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:566
const btVector3 & getPositionWorldOnA() const
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btScalar m_combinedContactStiffness1
btVector3 m_lateralFrictionDir1
btAlignedObjectArray< btScalar > scratch_r
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:292
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
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)
const btRigidBody & getRigidBodyA() const
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1283
btScalar m_appliedImpulse
#define btAssert(x)
Definition: btScalar.h:131
void addLinkConstraintForce(int i, const btVector3 &f)
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:264
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
int getCompanionId() const
Definition: btMultiBody.h:482
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:223
btScalar m_frictionCFM
const btJointFeedback * getJointFeedback() const
ManifoldContactPoint collects and maintains persistent contactpoints.
btScalar m_contactMotion1
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
const btRigidBody & getRigidBodyB() const
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
btScalar getBreakingImpulseThreshold() const
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:504
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
btAlignedObjectArray< btMatrix3x3 > scratch_m
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
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...
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
const btManifoldPoint & getContactPoint(int index) const
btVector3 m_externalForceImpulse
Definition: btSolverBody.h:121
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
btMultiBodySolverConstraint & addMultiBodyTorsionalFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btScalar combinedTorsionalFriction, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btVector3 m_normalWorldOnB
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
const btVector3 & getPositionWorldOnB() const
const btCollisionObject * getBody0() const
void setCompanionId(int id)
Definition: btMultiBody.h:486
btScalar m_appliedImpulseLateral2
btScalar getInvMass() const
Definition: btRigidBody.h:273
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
#define output
int getNumDofs() const
Definition: btMultiBody.h:165
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
btCollisionObject can be used to manage collision detection objects.
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 getContactProcessingThreshold() const
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
btAlignedObjectArray< btScalar > m_jacobians
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
int size() const
return the number of elements in the array
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:382
btAlignedObjectArray< btVector3 > scratch_v
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
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)
btScalar m_combinedContactDamping1
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:130
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 btCollisionObject * getBody1() const
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
void setEnabled(bool enabled)
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:383
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:560
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
btScalar m_contactMotion2
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btScalar m_combinedFriction
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
void fillConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:287
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:213
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:274
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:316
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:439
btVector3 m_lateralFrictionDir2
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:75
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:320
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:258
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
btScalar getDistance() const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btScalar m_combinedSpinningFriction
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btScalar btFabs(btScalar x)
Definition: btScalar.h:475