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