Bullet Collision Detection & Physics Library
btMultiBodyConstraint.cpp
Go to the documentation of this file.
1 #include "btMultiBodyConstraint.h"
2 #include "BulletDynamics/Dynamics/btRigidBody.h"
3 
4 btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral)
5  :m_bodyA(bodyA),
6  m_bodyB(bodyB),
7  m_linkA(linkA),
8  m_linkB(linkB),
9  m_num_rows(numRows),
10  m_isUnilateral(isUnilateral),
11  m_maxAppliedImpulse(100)
12 {
13  m_jac_size_A = (6 + bodyA->getNumLinks());
14  m_jac_size_both = (m_jac_size_A + (bodyB ? 6 + bodyB->getNumLinks() : 0));
15  m_pos_offset = ((1 + m_jac_size_both)*m_num_rows);
16  m_data.resize((2 + m_jac_size_both) * m_num_rows);
17 }
18 
19 btMultiBodyConstraint::~btMultiBodyConstraint()
20 {
21 }
22 
23 
24 
25 btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodySolverConstraint& constraintRow,
26  btMultiBodyJacobianData& data,
27  btScalar* jacOrgA,btScalar* jacOrgB,
28  const btContactSolverInfo& infoGlobal,
29  btScalar desiredVelocity,
30  btScalar lowerLimit,
31  btScalar upperLimit)
32 {
33 
34 
35 
36  constraintRow.m_multiBodyA = m_bodyA;
37  constraintRow.m_multiBodyB = m_bodyB;
38 
39  btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
40  btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
41 
42  if (multiBodyA)
43  {
44 
45  const int ndofA = multiBodyA->getNumLinks() + 6;
46 
47  constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
48 
49  if (constraintRow.m_deltaVelAindex <0)
50  {
51  constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
52  multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
53  data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
54  } else
55  {
56  btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
57  }
58 
59  constraintRow.m_jacAindex = data.m_jacobians.size();
60  data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
61  data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
62  btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
63  for (int i=0;i<ndofA;i++)
64  data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
65 
66  btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
67  multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
68  }
69 
70  if (multiBodyB)
71  {
72  const int ndofB = multiBodyB->getNumLinks() + 6;
73 
74  constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
75  if (constraintRow.m_deltaVelBindex <0)
76  {
77  constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
78  multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
79  data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
80  }
81 
82  constraintRow.m_jacBindex = data.m_jacobians.size();
83  data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
84 
85  for (int i=0;i<ndofB;i++)
86  data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
87 
88  data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
89  btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
90  multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
91  }
92  {
93 
94  btVector3 vec;
95  btScalar denom0 = 0.f;
96  btScalar denom1 = 0.f;
97  btScalar* jacB = 0;
98  btScalar* jacA = 0;
99  btScalar* lambdaA =0;
100  btScalar* lambdaB =0;
101  int ndofA = 0;
102  if (multiBodyA)
103  {
104  ndofA = multiBodyA->getNumLinks() + 6;
105  jacA = &data.m_jacobians[constraintRow.m_jacAindex];
106  lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
107  for (int i = 0; i < ndofA; ++i)
108  {
109  btScalar j = jacA[i] ;
110  btScalar l =lambdaA[i];
111  denom0 += j*l;
112  }
113  }
114  if (multiBodyB)
115  {
116  const int ndofB = multiBodyB->getNumLinks() + 6;
117  jacB = &data.m_jacobians[constraintRow.m_jacBindex];
118  lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
119  for (int i = 0; i < ndofB; ++i)
120  {
121  btScalar j = jacB[i] ;
122  btScalar l =lambdaB[i];
123  denom1 += j*l;
124  }
125 
126  }
127 
128  if (multiBodyA && (multiBodyA==multiBodyB))
129  {
130  // ndof1 == ndof2 in this case
131  for (int i = 0; i < ndofA; ++i)
132  {
133  denom1 += jacB[i] * lambdaA[i];
134  denom1 += jacA[i] * lambdaB[i];
135  }
136  }
137 
138  btScalar d = denom0+denom1;
139  if (btFabs(d)>SIMD_EPSILON)
140  {
141 
142  constraintRow.m_jacDiagABInv = 1.f/(d);
143  } else
144  {
145  constraintRow.m_jacDiagABInv = 1.f;
146  }
147 
148  }
149 
150 
151  //compute rhs and remaining constraintRow fields
152 
153 
154 
155 
156  btScalar rel_vel = 0.f;
157  int ndofA = 0;
158  int ndofB = 0;
159  {
160 
161  btVector3 vel1,vel2;
162  if (multiBodyA)
163  {
164  ndofA = multiBodyA->getNumLinks() + 6;
165  btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
166  for (int i = 0; i < ndofA ; ++i)
167  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
168  }
169  if (multiBodyB)
170  {
171  ndofB = multiBodyB->getNumLinks() + 6;
172  btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
173  for (int i = 0; i < ndofB ; ++i)
174  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
175 
176  }
177 
178  constraintRow.m_friction = 0.f;
179 
180  constraintRow.m_appliedImpulse = 0.f;
181  constraintRow.m_appliedPushImpulse = 0.f;
182 
183  btScalar velocityError = desiredVelocity - rel_vel;// * damping;
184 
185  btScalar erp = infoGlobal.m_erp2;
186 
187  btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
188 
189  if (!infoGlobal.m_splitImpulse)
190  {
191  //combine position and velocity into rhs
192  constraintRow.m_rhs = velocityImpulse;
193  constraintRow.m_rhsPenetration = 0.f;
194 
195  } else
196  {
197  //split position and velocity into rhs and m_rhsPenetration
198  constraintRow.m_rhs = velocityImpulse;
199  constraintRow.m_rhsPenetration = 0.f;
200  }
201 
202 
203  constraintRow.m_cfm = 0.f;
204  constraintRow.m_lowerLimit = lowerLimit;
205  constraintRow.m_upperLimit = upperLimit;
206 
207  }
208  return rel_vel;
209 }
210 
211 
212 void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
213 {
214  for (int i = 0; i < ndof; ++i)
215  data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
216 }
217 
218 
219 void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint,
220  btMultiBodyJacobianData& data,
221  const btVector3& contactNormalOnB,
222  const btVector3& posAworld, const btVector3& posBworld,
223  btScalar position,
224  const btContactSolverInfo& infoGlobal,
225  btScalar& relaxation,
226  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
227 {
228 
229 
230  btVector3 rel_pos1 = posAworld;
231  btVector3 rel_pos2 = posBworld;
232 
233  solverConstraint.m_multiBodyA = m_bodyA;
234  solverConstraint.m_multiBodyB = m_bodyB;
235  solverConstraint.m_linkA = m_linkA;
236  solverConstraint.m_linkB = m_linkB;
237 
238 
239  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
240  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
241 
242  const btVector3& pos1 = posAworld;
243  const btVector3& pos2 = posBworld;
244 
245  btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
246  btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(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 = data.m_deltaVelocities.size();
267  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
268  data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
269  } else
270  {
271  btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
272  }
273 
274  solverConstraint.m_jacAindex = data.m_jacobians.size();
275  data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
276  data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
277  btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
278 
279  btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
280  multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
281  btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
282  multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
283  } else
284  {
285  btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
286  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
287  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
288  solverConstraint.m_contactNormal1 = contactNormalOnB;
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 = data.m_deltaVelocities.size();
299  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
300  data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
301  }
302 
303  solverConstraint.m_jacBindex = data.m_jacobians.size();
304 
305  data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
306  data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
307  btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
308 
309  multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
310  multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],data.scratch_r, data.scratch_v);
311  } else
312  {
313  btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
314  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
315  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
316  solverConstraint.m_contactNormal2 = -contactNormalOnB;
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 = &data.m_jacobians[solverConstraint.m_jacAindex];
333  lambdaA = &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() + contactNormalOnB.dot(vec);
346  }
347  }
348  if (multiBodyB)
349  {
350  const int ndofB = multiBodyB->getNumLinks() + 6;
351  jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
352  lambdaB = &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() + contactNormalOnB.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 : position+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 = &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 = &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 = 0.f;//cp.m_combinedFriction;
434 
435 
436  restitution = restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
437  if (restitution <= btScalar(0.))
438  {
439  restitution = 0.f;
440  };
441  }
442 
443 
445  /*
446  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
447  {
448  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
449 
450  if (solverConstraint.m_appliedImpulse)
451  {
452  if (multiBodyA)
453  {
454  btScalar impulse = solverConstraint.m_appliedImpulse;
455  btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
456  multiBodyA->applyDeltaVee(deltaV,impulse);
457  applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
458  } else
459  {
460  if (rb0)
461  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
462  }
463  if (multiBodyB)
464  {
465  btScalar impulse = solverConstraint.m_appliedImpulse;
466  btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
467  multiBodyB->applyDeltaVee(deltaV,impulse);
468  applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
469  } else
470  {
471  if (rb1)
472  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
473  }
474  }
475  } else
476  */
477  {
478  solverConstraint.m_appliedImpulse = 0.f;
479  }
480 
481  solverConstraint.m_appliedPushImpulse = 0.f;
482 
483  {
484 
485 
486  btScalar positionalError = 0.f;
487  btScalar velocityError = restitution - rel_vel;// * damping;
488 
489 
490  btScalar erp = infoGlobal.m_erp2;
491  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
492  {
493  erp = infoGlobal.m_erp;
494  }
495 
496  if (penetration>0)
497  {
498  positionalError = 0;
499  velocityError = -penetration / infoGlobal.m_timeStep;
500 
501  } else
502  {
503  positionalError = -penetration * erp/infoGlobal.m_timeStep;
504  }
505 
506  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
507  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
508 
509  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
510  {
511  //combine position and velocity into rhs
512  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
513  solverConstraint.m_rhsPenetration = 0.f;
514 
515  } else
516  {
517  //split position and velocity into rhs and m_rhsPenetration
518  solverConstraint.m_rhs = velocityImpulse;
519  solverConstraint.m_rhsPenetration = penetrationImpulse;
520  }
521 
522  solverConstraint.m_cfm = 0.f;
523  solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
524  solverConstraint.m_upperLimit = m_maxAppliedImpulse;
525  }
526 
527 }
Generated on Wed Apr 9 2014 02:38:23 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2