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