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 #include "btMultiBodyConstraintSolver.h"
17 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
18 #include "btMultiBodyLinkCollider.h"
19 
20 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
21 #include "btMultiBodyConstraint.h"
22 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
23 
24 #include "LinearMath/btQuickprof.h"
25 
26 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
27 {
28  btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
29 
30  //solve featherstone non-contact constraints
31 
32  //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
33  for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
34  {
35  btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
36  //if (iteration < constraint.m_overrideNumSolverIterations)
37  //resolveSingleConstraintRowGenericMultiBody(constraint);
38  resolveSingleConstraintRowGeneric(constraint);
39  }
40 
41  //solve featherstone normal contact
42  for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
43  {
44  btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
45  if (iteration < infoGlobal.m_numIterations)
46  resolveSingleConstraintRowGeneric(constraint);
47  }
48 
49  //solve featherstone frictional contact
50 
51  for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
52  {
53  if (iteration < infoGlobal.m_numIterations)
54  {
55  btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
56  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
57  //adjust friction limits here
58  if (totalImpulse>btScalar(0))
59  {
60  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
61  frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
62  resolveSingleConstraintRowGeneric(frictionConstraint);
63  }
64  }
65  }
66  return val;
67 }
68 
69 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
70 {
71  m_multiBodyNonContactConstraints.resize(0);
72  m_multiBodyNormalContactConstraints.resize(0);
73  m_multiBodyFrictionContactConstraints.resize(0);
74  m_data.m_jacobians.resize(0);
75  m_data.m_deltaVelocitiesUnitImpulse.resize(0);
76  m_data.m_deltaVelocities.resize(0);
77 
78  for (int i=0;i<numBodies;i++)
79  {
80  const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
81  if (fcA)
82  {
83  fcA->m_multiBody->setCompanionId(-1);
84  }
85  }
86 
87  btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
88 
89  return val;
90 }
91 
92 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
93 {
94  for (int i = 0; i < ndof; ++i)
95  m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
96 }
97 
98 void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
99 {
100 
101  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
102  btScalar deltaVelADotn=0;
103  btScalar deltaVelBDotn=0;
104  btSolverBody* bodyA = 0;
105  btSolverBody* bodyB = 0;
106  int ndofA=0;
107  int ndofB=0;
108 
109  if (c.m_multiBodyA)
110  {
111  ndofA = c.m_multiBodyA->getNumLinks() + 6;
112  for (int i = 0; i < ndofA; ++i)
113  deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
114  } else
115  {
116  bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
117  deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
118  }
119 
120  if (c.m_multiBodyB)
121  {
122  ndofB = c.m_multiBodyB->getNumLinks() + 6;
123  for (int i = 0; i < ndofB; ++i)
124  deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
125  } else
126  {
127  bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
128  deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
129  }
130 
131 
132  deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
133  deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
134  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
135 
136  if (sum < c.m_lowerLimit)
137  {
138  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
139  c.m_appliedImpulse = c.m_lowerLimit;
140  }
141  else if (sum > c.m_upperLimit)
142  {
143  deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
144  c.m_appliedImpulse = c.m_upperLimit;
145  }
146  else
147  {
148  c.m_appliedImpulse = sum;
149  }
150 
151  if (c.m_multiBodyA)
152  {
153  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
154  c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
155  } else
156  {
157  bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
158 
159  }
160  if (c.m_multiBodyB)
161  {
162  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
163  c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
164  } else
165  {
166  bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
167  }
168 
169 }
170 
171 
172 void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c)
173 {
174 
175  btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
176  btScalar deltaVelADotn=0;
177  btScalar deltaVelBDotn=0;
178  int ndofA=0;
179  int ndofB=0;
180 
181  if (c.m_multiBodyA)
182  {
183  ndofA = c.m_multiBodyA->getNumLinks() + 6;
184  for (int i = 0; i < ndofA; ++i)
185  deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
186  }
187 
188  if (c.m_multiBodyB)
189  {
190  ndofB = c.m_multiBodyB->getNumLinks() + 6;
191  for (int i = 0; i < ndofB; ++i)
192  deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
193  }
194 
195 
196  deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
197  deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
198  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
199 
200  if (sum < c.m_lowerLimit)
201  {
202  deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
203  c.m_appliedImpulse = c.m_lowerLimit;
204  }
205  else if (sum > c.m_upperLimit)
206  {
207  deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
208  c.m_appliedImpulse = c.m_upperLimit;
209  }
210  else
211  {
212  c.m_appliedImpulse = sum;
213  }
214 
215  if (c.m_multiBodyA)
216  {
217  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
218  c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
219  }
220  if (c.m_multiBodyB)
221  {
222  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
223  c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
224  }
225 }
226 
227 
228 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
229  const btVector3& contactNormal,
230  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
231  btScalar& relaxation,
232  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
233 {
234 
235  BT_PROFILE("setupMultiBodyContactConstraint");
236  btVector3 rel_pos1;
237  btVector3 rel_pos2;
238 
239  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
240  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
241 
242  const btVector3& pos1 = cp.getPositionWorldOnA();
243  const btVector3& pos2 = cp.getPositionWorldOnB();
244 
245  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
246  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
247 
248  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
249  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
250 
251  if (bodyA)
252  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
253  if (bodyB)
254  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
255 
256  relaxation = 1.f;
257 
258  if (multiBodyA)
259  {
260  const int ndofA = multiBodyA->getNumLinks() + 6;
261 
262  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
263 
264  if (solverConstraint.m_deltaVelAindex <0)
265  {
266  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
267  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
268  m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
269  } else
270  {
271  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
272  }
273 
274  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
275  m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
276  m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
277  btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
278 
279  btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
280  multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
281  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
282  multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
283  } else
284  {
285  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
286  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
287  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
288  solverConstraint.m_contactNormal1 = contactNormal;
289  }
290 
291  if (multiBodyB)
292  {
293  const int ndofB = multiBodyB->getNumLinks() + 6;
294 
295  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
296  if (solverConstraint.m_deltaVelBindex <0)
297  {
298  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
299  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
300  m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
301  }
302 
303  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
304 
305  m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
306  m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
307  btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
308 
309  multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
310  multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
311  } else
312  {
313  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
314  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
315  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
316  solverConstraint.m_contactNormal2 = -contactNormal;
317  }
318 
319  {
320 
321  btVector3 vec;
322  btScalar denom0 = 0.f;
323  btScalar denom1 = 0.f;
324  btScalar* jacB = 0;
325  btScalar* jacA = 0;
326  btScalar* lambdaA =0;
327  btScalar* lambdaB =0;
328  int ndofA = 0;
329  if (multiBodyA)
330  {
331  ndofA = multiBodyA->getNumLinks() + 6;
332  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
333  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
334  for (int i = 0; i < ndofA; ++i)
335  {
336  btScalar j = jacA[i] ;
337  btScalar l =lambdaA[i];
338  denom0 += j*l;
339  }
340  } else
341  {
342  if (rb0)
343  {
344  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
345  denom0 = rb0->getInvMass() + contactNormal.dot(vec);
346  }
347  }
348  if (multiBodyB)
349  {
350  const int ndofB = multiBodyB->getNumLinks() + 6;
351  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
352  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
353  for (int i = 0; i < ndofB; ++i)
354  {
355  btScalar j = jacB[i] ;
356  btScalar l =lambdaB[i];
357  denom1 += j*l;
358  }
359 
360  } else
361  {
362  if (rb1)
363  {
364  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
365  denom1 = rb1->getInvMass() + contactNormal.dot(vec);
366  }
367  }
368 
369  if (multiBodyA && (multiBodyA==multiBodyB))
370  {
371  // ndof1 == ndof2 in this case
372  for (int i = 0; i < ndofA; ++i)
373  {
374  denom1 += jacB[i] * lambdaA[i];
375  denom1 += jacA[i] * lambdaB[i];
376  }
377  }
378 
379  btScalar d = denom0+denom1;
380  if (btFabs(d)>SIMD_EPSILON)
381  {
382 
383  solverConstraint.m_jacDiagABInv = relaxation/(d);
384  } else
385  {
386  solverConstraint.m_jacDiagABInv = 1.f;
387  }
388 
389  }
390 
391 
392  //compute rhs and remaining solverConstraint fields
393 
394 
395 
396  btScalar restitution = 0.f;
397  btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
398 
399  btScalar rel_vel = 0.f;
400  int ndofA = 0;
401  int ndofB = 0;
402  {
403 
404  btVector3 vel1,vel2;
405  if (multiBodyA)
406  {
407  ndofA = multiBodyA->getNumLinks() + 6;
408  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
409  for (int i = 0; i < ndofA ; ++i)
410  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
411  } else
412  {
413  if (rb0)
414  {
415  rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
416  }
417  }
418  if (multiBodyB)
419  {
420  ndofB = multiBodyB->getNumLinks() + 6;
421  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
422  for (int i = 0; i < ndofB ; ++i)
423  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
424 
425  } else
426  {
427  if (rb1)
428  {
429  rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
430  }
431  }
432 
433  solverConstraint.m_friction = cp.m_combinedFriction;
434 
435 
436  restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
437  if (restitution <= btScalar(0.))
438  {
439  restitution = 0.f;
440  };
441  }
442 
443 
445  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
446  {
447  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
448 
449  if (solverConstraint.m_appliedImpulse)
450  {
451  if (multiBodyA)
452  {
453  btScalar impulse = solverConstraint.m_appliedImpulse;
454  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
455  multiBodyA->applyDeltaVee(deltaV,impulse);
456  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
457  } else
458  {
459  if (rb0)
460  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
461  }
462  if (multiBodyB)
463  {
464  btScalar impulse = solverConstraint.m_appliedImpulse;
465  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
466  multiBodyB->applyDeltaVee(deltaV,impulse);
467  applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
468  } else
469  {
470  if (rb1)
471  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
472  }
473  }
474  } else
475  {
476  solverConstraint.m_appliedImpulse = 0.f;
477  }
478 
479  solverConstraint.m_appliedPushImpulse = 0.f;
480 
481  {
482 
483 
484  btScalar positionalError = 0.f;
485  btScalar velocityError = restitution - rel_vel;// * damping;
486 
487 
488  btScalar erp = infoGlobal.m_erp2;
489  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
490  {
491  erp = infoGlobal.m_erp;
492  }
493 
494  if (penetration>0)
495  {
496  positionalError = 0;
497  velocityError = -penetration / infoGlobal.m_timeStep;
498 
499  } else
500  {
501  positionalError = -penetration * erp/infoGlobal.m_timeStep;
502  }
503 
504  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
505  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
506 
507  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
508  {
509  //combine position and velocity into rhs
510  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
511  solverConstraint.m_rhsPenetration = 0.f;
512 
513  } else
514  {
515  //split position and velocity into rhs and m_rhsPenetration
516  solverConstraint.m_rhs = velocityImpulse;
517  solverConstraint.m_rhsPenetration = penetrationImpulse;
518  }
519 
520  solverConstraint.m_cfm = 0.f;
521  solverConstraint.m_lowerLimit = 0;
522  solverConstraint.m_upperLimit = 1e10f;
523  }
524 
525 }
526 
527 
528 
529 
530 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
531 {
532  BT_PROFILE("addMultiBodyFrictionConstraint");
533  btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
534  solverConstraint.m_frictionIndex = frictionIndex;
535  bool isFriction = true;
536 
537  const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
538  const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
539 
540  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
541  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
542 
543  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
544  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
545 
546  solverConstraint.m_solverBodyIdA = solverBodyIdA;
547  solverConstraint.m_solverBodyIdB = solverBodyIdB;
548  solverConstraint.m_multiBodyA = mbA;
549  if (mbA)
550  solverConstraint.m_linkA = fcA->m_link;
551 
552  solverConstraint.m_multiBodyB = mbB;
553  if (mbB)
554  solverConstraint.m_linkB = fcB->m_link;
555 
556  solverConstraint.m_originalContactPoint = &cp;
557 
558  setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
559  return solverConstraint;
560 }
561 
562 void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
563 {
564  const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
565  const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
566 
567  btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
568  btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
569 
570  btCollisionObject* colObj0=0,*colObj1=0;
571 
572  colObj0 = (btCollisionObject*)manifold->getBody0();
573  colObj1 = (btCollisionObject*)manifold->getBody1();
574 
575  int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
576  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
577 
578  btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
579  btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
580 
581 
583 // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
584  // return;
585 
586  int rollingFriction=1;
587 
588  for (int j=0;j<manifold->getNumContacts();j++)
589  {
590 
591  btManifoldPoint& cp = manifold->getContactPoint(j);
592 
593  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
594  {
595 
596  btScalar relaxation;
597 
598  int frictionIndex = m_multiBodyNormalContactConstraints.size();
599 
600  btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
601 
602  btRigidBody* rb0 = btRigidBody::upcast(colObj0);
603  btRigidBody* rb1 = btRigidBody::upcast(colObj1);
604  solverConstraint.m_solverBodyIdA = solverBodyIdA;
605  solverConstraint.m_solverBodyIdB = solverBodyIdB;
606  solverConstraint.m_multiBodyA = mbA;
607  if (mbA)
608  solverConstraint.m_linkA = fcA->m_link;
609 
610  solverConstraint.m_multiBodyB = mbB;
611  if (mbB)
612  solverConstraint.m_linkB = fcB->m_link;
613 
614  solverConstraint.m_originalContactPoint = &cp;
615 
616  bool isFriction = false;
617  setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
618 
619 // const btVector3& pos1 = cp.getPositionWorldOnA();
620 // const btVector3& pos2 = cp.getPositionWorldOnB();
621 
623 #define ENABLE_FRICTION
624 #ifdef ENABLE_FRICTION
625  solverConstraint.m_frictionIndex = frictionIndex;
626 #if ROLLING_FRICTION
627  btVector3 angVelA(0,0,0),angVelB(0,0,0);
628  if (rb0)
629  angVelA = rb0->getAngularVelocity();
630  if (rb1)
631  angVelB = rb1->getAngularVelocity();
632  btVector3 relAngVel = angVelB-angVelA;
633 
634  if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
635  {
636  //only a single rollingFriction per manifold
637  rollingFriction--;
638  if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
639  {
640  relAngVel.normalize();
641  applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
642  applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
643  if (relAngVel.length()>0.001)
644  addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
645 
646  } else
647  {
648  addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
649  btVector3 axis0,axis1;
650  btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
651  applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
652  applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
653  applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
654  applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
655  if (axis0.length()>0.001)
656  addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
657  if (axis1.length()>0.001)
658  addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
659 
660  }
661  }
662 #endif //ROLLING_FRICTION
663 
668 
679  if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
680  {/*
681  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
682  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
683  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
684  {
685  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
686  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
687  {
688  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
689  cp.m_lateralFrictionDir2.normalize();//??
690  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
691  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
692  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
693 
694  }
695 
696  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
697  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
698  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
699 
700  } else
701  */
702  {
703  btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
704 
705  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
706  {
707  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
708  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
709  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
710  }
711 
712  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
713  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
714  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
715 
716  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
717  {
718  cp.m_lateralFrictionInitialized = true;
719  }
720  }
721 
722  } else
723  {
724  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
725 
726  if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
727  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
728 
729  //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
730  //todo:
731  solverConstraint.m_appliedImpulse = 0.f;
732  solverConstraint.m_appliedPushImpulse = 0.f;
733  }
734 
735 
736 #endif //ENABLE_FRICTION
737 
738  }
739  }
740 }
741 
742 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
743 {
744  btPersistentManifold* manifold = 0;
745 
746  for (int i=0;i<numManifolds;i++)
747  {
748  btPersistentManifold* manifold= manifoldPtr[i];
749  const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
750  const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
751  if (!fcA && !fcB)
752  {
753  //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
754  convertContact(manifold,infoGlobal);
755  } else
756  {
757  convertMultiBodyContact(manifold,infoGlobal);
758  }
759  }
760 
761  //also convert the multibody constraints, if any
762 
763 
764  for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
765  {
766  btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
767  m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
768  m_data.m_fixedBodyId = m_fixedBodyId;
769 
770  c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal);
771  }
772 
773 }
774 
775 
776 
777 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
778 {
779  return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
780 }
781 
782 
783 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)
784 {
785  //printf("solveMultiBodyGroup start\n");
786  m_tmpMultiBodyConstraints = multiBodyConstraints;
787  m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
788 
789  btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
790 
791  m_tmpMultiBodyConstraints = 0;
792  m_tmpNumMultiBodyConstraints = 0;
793 
794 
795 }
Generated on Wed Apr 9 2014 02:38:23 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2