Blender  V3.3
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 
19 
21 #include "btMultiBodyConstraint.h"
23 
24 #include "LinearMath/btQuickprof.h"
26 #include "LinearMath/btScalar.h"
27 
29 {
31 
32  //solve featherstone non-contact constraints
33  btScalar nonContactResidual = 0;
34  //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
35  for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i)
36  {
37  // reset the nonContactResdual to 0 at start of each inner iteration
38  nonContactResidual = 0;
39  for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
40  {
41  int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
42 
43  btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
44 
45  btScalar residual = resolveSingleConstraintRowGeneric(constraint);
46  nonContactResidual = btMax(nonContactResidual, residual * residual);
47 
48  if (constraint.m_multiBodyA)
49  constraint.m_multiBodyA->setPosUpdated(false);
50  if (constraint.m_multiBodyB)
51  constraint.m_multiBodyB->setPosUpdated(false);
52  }
53  }
54  leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual);
55 
56  //solve featherstone normal contact
57  for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
58  {
59  int index = j0; //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
60 
62  btScalar residual = 0.f;
63 
64  if (iteration < infoGlobal.m_numIterations)
65  {
66  residual = resolveSingleConstraintRowGeneric(constraint);
67  }
68 
69  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
70 
71  if (constraint.m_multiBodyA)
72  constraint.m_multiBodyA->setPosUpdated(false);
73  if (constraint.m_multiBodyB)
74  constraint.m_multiBodyB->setPosUpdated(false);
75  }
76 
77  //solve featherstone frictional contact
79  {
80  for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++)
81  {
82  if (iteration < infoGlobal.m_numIterations)
83  {
84  int index = j1;
85 
87  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
88  //adjust friction limits here
89  if (totalImpulse > btScalar(0))
90  {
91  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
92  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
93  btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
94  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
95 
96  if (frictionConstraint.m_multiBodyA)
97  frictionConstraint.m_multiBodyA->setPosUpdated(false);
98  if (frictionConstraint.m_multiBodyB)
99  frictionConstraint.m_multiBodyB->setPosUpdated(false);
100  }
101  }
102  }
103 
104  for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
105  {
106  if (iteration < infoGlobal.m_numIterations)
107  {
108  int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
109 
111  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
112  j1++;
113  int index2 = j1;
115  //adjust friction limits here
116  if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
117  {
118  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
119  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
120  frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
121  frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
122 
123  btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
124  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
125 
126  if (frictionConstraint.m_multiBodyA)
127  frictionConstraint.m_multiBodyA->setPosUpdated(false);
128  if (frictionConstraint.m_multiBodyB)
129  frictionConstraint.m_multiBodyB->setPosUpdated(false);
130 
131  if (frictionConstraintB.m_multiBodyA)
132  frictionConstraintB.m_multiBodyA->setPosUpdated(false);
133  if (frictionConstraintB.m_multiBodyB)
134  frictionConstraintB.m_multiBodyB->setPosUpdated(false);
135  }
136  }
137  }
138 
139  for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
140  {
141  if (iteration < infoGlobal.m_numIterations)
142  {
143  int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
145 
146  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
147  j1++;
148  int index2 = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
150  btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
151 
152  if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
153  {
154  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
155  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
156  frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
157  frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
158  btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
159  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
160 
161  if (frictionConstraintB.m_multiBodyA)
162  frictionConstraintB.m_multiBodyA->setPosUpdated(false);
163  if (frictionConstraintB.m_multiBodyB)
164  frictionConstraintB.m_multiBodyB->setPosUpdated(false);
165 
166  if (frictionConstraint.m_multiBodyA)
167  frictionConstraint.m_multiBodyA->setPosUpdated(false);
168  if (frictionConstraint.m_multiBodyB)
169  frictionConstraint.m_multiBodyB->setPosUpdated(false);
170  }
171  }
172  }
173  }
174  else
175  {
176  for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
177  {
178  if (iteration < infoGlobal.m_numIterations)
179  {
180  int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
181 
183  btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
184  //adjust friction limits here
185  if (totalImpulse > btScalar(0))
186  {
187  frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
188  frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
189  btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
190  leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
191 
192  if (frictionConstraint.m_multiBodyA)
193  frictionConstraint.m_multiBodyA->setPosUpdated(false);
194  if (frictionConstraint.m_multiBodyB)
195  frictionConstraint.m_multiBodyB->setPosUpdated(false);
196  }
197  }
198  }
199  }
200  return leastSquaredResidual;
201 }
202 
204 {
205  m_multiBodyNonContactConstraints.resize(0);
210 
211  m_data.m_jacobians.resize(0);
212  m_data.m_deltaVelocitiesUnitImpulse.resize(0);
213  m_data.m_deltaVelocities.resize(0);
214 
215  for (int i = 0; i < numBodies; i++)
216  {
218  if (fcA)
219  {
220  fcA->m_multiBody->setCompanionId(-1);
221  }
222  }
223 
225 
226  return val;
227 }
228 
229 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
230 {
231  for (int i = 0; i < ndof; ++i)
232  m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
233 }
234 
236 {
237  btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
238  btScalar deltaVelADotn = 0;
239  btScalar deltaVelBDotn = 0;
240  btSolverBody* bodyA = 0;
241  btSolverBody* bodyB = 0;
242  int ndofA = 0;
243  int ndofB = 0;
244 
245  if (c.m_multiBodyA)
246  {
247  ndofA = c.m_multiBodyA->getNumDofs() + 6;
248  for (int i = 0; i < ndofA; ++i)
249  deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
250  }
251  else if (c.m_solverBodyIdA >= 0)
252  {
253  bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
254  deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
255  }
256 
257  if (c.m_multiBodyB)
258  {
259  ndofB = c.m_multiBodyB->getNumDofs() + 6;
260  for (int i = 0; i < ndofB; ++i)
261  deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
262  }
263  else if (c.m_solverBodyIdB >= 0)
264  {
265  bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
266  deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
267  }
268 
269  deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
270  deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv;
271  const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
272 
273  if (sum < c.m_lowerLimit)
274  {
275  deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
276  c.m_appliedImpulse = c.m_lowerLimit;
277  }
278  else if (sum > c.m_upperLimit)
279  {
280  deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
281  c.m_appliedImpulse = c.m_upperLimit;
282  }
283  else
284  {
285  c.m_appliedImpulse = sum;
286  }
287 
288  if (c.m_multiBodyA)
289  {
290  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
291 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
292  //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
293  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
294  c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
295 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
296  }
297  else if (c.m_solverBodyIdA >= 0)
298  {
299  bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
300  }
301  if (c.m_multiBodyB)
302  {
303  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
304 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
305  //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
306  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
307  c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
308 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
309  }
310  else if (c.m_solverBodyIdB >= 0)
311  {
312  bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
313  }
314  btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv;
315  return deltaVel;
316 }
317 
319 {
320  int ndofA = 0;
321  int ndofB = 0;
322  btSolverBody* bodyA = 0;
323  btSolverBody* bodyB = 0;
324  btScalar deltaImpulseB = 0.f;
325  btScalar sumB = 0.f;
326  {
327  deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm;
328  btScalar deltaVelADotn = 0;
329  btScalar deltaVelBDotn = 0;
330  if (cB.m_multiBodyA)
331  {
332  ndofA = cB.m_multiBodyA->getNumDofs() + 6;
333  for (int i = 0; i < ndofA; ++i)
334  deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i];
335  }
336  else if (cB.m_solverBodyIdA >= 0)
337  {
338  bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
339  deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
340  }
341 
342  if (cB.m_multiBodyB)
343  {
344  ndofB = cB.m_multiBodyB->getNumDofs() + 6;
345  for (int i = 0; i < ndofB; ++i)
346  deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i];
347  }
348  else if (cB.m_solverBodyIdB >= 0)
349  {
350  bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
351  deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
352  }
353 
354  deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
355  deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv;
356  sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
357  }
358 
359  btScalar deltaImpulseA = 0.f;
360  btScalar sumA = 0.f;
361  const btMultiBodySolverConstraint& cA = cA1;
362  {
363  {
364  deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm;
365  btScalar deltaVelADotn = 0;
366  btScalar deltaVelBDotn = 0;
367  if (cA.m_multiBodyA)
368  {
369  ndofA = cA.m_multiBodyA->getNumDofs() + 6;
370  for (int i = 0; i < ndofA; ++i)
371  deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i];
372  }
373  else if (cA.m_solverBodyIdA >= 0)
374  {
375  bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
376  deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
377  }
378 
379  if (cA.m_multiBodyB)
380  {
381  ndofB = cA.m_multiBodyB->getNumDofs() + 6;
382  for (int i = 0; i < ndofB; ++i)
383  deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i];
384  }
385  else if (cA.m_solverBodyIdB >= 0)
386  {
387  bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
388  deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
389  }
390 
391  deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
392  deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv;
393  sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
394  }
395  }
396 
397  if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit)
398  {
399  btScalar angle = btAtan2(sumA, sumB);
400  btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle));
401  btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle));
402 
403  if (sumA < -sumAclipped)
404  {
405  deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
406  cA.m_appliedImpulse = -sumAclipped;
407  }
408  else if (sumA > sumAclipped)
409  {
410  deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
411  cA.m_appliedImpulse = sumAclipped;
412  }
413  else
414  {
415  cA.m_appliedImpulse = sumA;
416  }
417 
418  if (sumB < -sumBclipped)
419  {
420  deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
421  cB.m_appliedImpulse = -sumBclipped;
422  }
423  else if (sumB > sumBclipped)
424  {
425  deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
426  cB.m_appliedImpulse = sumBclipped;
427  }
428  else
429  {
430  cB.m_appliedImpulse = sumB;
431  }
432  //deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
433  //cA.m_appliedImpulse = sumAclipped;
434  //deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
435  //cB.m_appliedImpulse = sumBclipped;
436  }
437  else
438  {
439  cA.m_appliedImpulse = sumA;
440  cB.m_appliedImpulse = sumB;
441  }
442 
443  if (cA.m_multiBodyA)
444  {
445  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA);
446 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
447  //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
448  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
449  cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA);
450 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
451  }
452  else if (cA.m_solverBodyIdA >= 0)
453  {
454  bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA);
455  }
456  if (cA.m_multiBodyB)
457  {
458  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB);
459 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
460  //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
461  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
462  cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA);
463 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
464  }
465  else if (cA.m_solverBodyIdB >= 0)
466  {
467  bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA);
468  }
469 
470  if (cB.m_multiBodyA)
471  {
472  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA);
473 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
474  //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
475  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
476  cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB);
477 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
478  }
479  else if (cB.m_solverBodyIdA >= 0)
480  {
481  bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB);
482  }
483  if (cB.m_multiBodyB)
484  {
485  applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB);
486 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
487  //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
488  //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
489  cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB);
490 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
491  }
492  else if (cB.m_solverBodyIdB >= 0)
493  {
494  bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB);
495  }
496 
497  btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv;
498  return deltaVel;
499 }
500 
501 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
502 {
503  BT_PROFILE("setupMultiBodyContactConstraint");
504  btVector3 rel_pos1;
505  btVector3 rel_pos2;
506 
507  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
508  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
509 
510  const btVector3& pos1 = cp.getPositionWorldOnA();
511  const btVector3& pos2 = cp.getPositionWorldOnB();
512 
513  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
514  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
515 
516  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
517  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
518 
519  if (bodyA)
520  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
521  if (bodyB)
522  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
523 
524  relaxation = infoGlobal.m_sor;
525 
526  btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
527 
528  //cfm = 1 / ( dt * kp + kd )
529  //erp = dt * kp / ( dt * kp + kd )
530 
531  btScalar cfm;
532  btScalar erp;
533  if (isFriction)
534  {
537  }
538  else
539  {
540  cfm = infoGlobal.m_globalCfm;
541  erp = infoGlobal.m_erp2;
542 
544  {
546  cfm = cp.m_contactCFM;
548  erp = cp.m_contactERP;
549  }
550  else
551  {
553  {
555  if (denom < SIMD_EPSILON)
556  {
557  denom = SIMD_EPSILON;
558  }
559  cfm = btScalar(1) / denom;
561  }
562  }
563  }
564 
565  cfm *= invTimeStep;
566 
567  if (multiBodyA)
568  {
569  if (solverConstraint.m_linkA < 0)
570  {
571  rel_pos1 = pos1 - multiBodyA->getBasePos();
572  }
573  else
574  {
575  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
576  }
577  const int ndofA = multiBodyA->getNumDofs() + 6;
578 
579  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
580 
581  if (solverConstraint.m_deltaVelAindex < 0)
582  {
583  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
584  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
585  m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
586  }
587  else
588  {
589  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
590  }
591 
592  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
593  m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
594  m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
595  btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
596 
597  btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
598  multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
599  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
600  multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
601 
602  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
603  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
604  solverConstraint.m_contactNormal1 = contactNormal;
605  }
606  else
607  {
608  btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
609  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
610  solverConstraint.m_contactNormal1 = contactNormal;
611  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
612  }
613 
614  if (multiBodyB)
615  {
616  if (solverConstraint.m_linkB < 0)
617  {
618  rel_pos2 = pos2 - multiBodyB->getBasePos();
619  }
620  else
621  {
622  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
623  }
624 
625  const int ndofB = multiBodyB->getNumDofs() + 6;
626 
627  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
628  if (solverConstraint.m_deltaVelBindex < 0)
629  {
630  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
631  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
632  m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
633  }
634 
635  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
636 
637  m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
638  m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
639  btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
640 
641  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);
642  multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
643 
644  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
645  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
646  solverConstraint.m_contactNormal2 = -contactNormal;
647  }
648  else
649  {
650  btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
651  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
652  solverConstraint.m_contactNormal2 = -contactNormal;
653 
654  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
655  }
656 
657  {
658  btVector3 vec;
659  btScalar denom0 = 0.f;
660  btScalar denom1 = 0.f;
661  btScalar* jacB = 0;
662  btScalar* jacA = 0;
663  btScalar* lambdaA = 0;
664  btScalar* lambdaB = 0;
665  int ndofA = 0;
666  if (multiBodyA)
667  {
668  ndofA = multiBodyA->getNumDofs() + 6;
669  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
670  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
671  for (int i = 0; i < ndofA; ++i)
672  {
673  btScalar j = jacA[i];
674  btScalar l = lambdaA[i];
675  denom0 += j * l;
676  }
677  }
678  else
679  {
680  if (rb0)
681  {
682  vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
683  denom0 = rb0->getInvMass() + contactNormal.dot(vec);
684  }
685  }
686  if (multiBodyB)
687  {
688  const int ndofB = multiBodyB->getNumDofs() + 6;
689  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
690  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
691  for (int i = 0; i < ndofB; ++i)
692  {
693  btScalar j = jacB[i];
694  btScalar l = lambdaB[i];
695  denom1 += j * l;
696  }
697  }
698  else
699  {
700  if (rb1)
701  {
702  vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
703  denom1 = rb1->getInvMass() + contactNormal.dot(vec);
704  }
705  }
706 
707  btScalar d = denom0 + denom1 + cfm;
708  if (d > SIMD_EPSILON)
709  {
710  solverConstraint.m_jacDiagABInv = relaxation / (d);
711  }
712  else
713  {
714  //disable the constraint row to handle singularity/redundant constraint
715  solverConstraint.m_jacDiagABInv = 0.f;
716  }
717  }
718 
719  //compute rhs and remaining solverConstraint fields
720 
721  btScalar restitution = 0.f;
722  btScalar distance = 0;
723  if (!isFriction)
724  {
726  }
727  else
728  {
730  {
731  distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
732  }
733  }
734 
735  btScalar rel_vel = 0.f;
736  int ndofA = 0;
737  int ndofB = 0;
738  {
739  btVector3 vel1, vel2;
740  if (multiBodyA)
741  {
742  ndofA = multiBodyA->getNumDofs() + 6;
743  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
744  for (int i = 0; i < ndofA; ++i)
745  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
746  }
747  else
748  {
749  if (rb0)
750  {
751  rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) +
752  (rb0->getTotalTorque() * rb0->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos1) +
753  rb0->getTotalForce() * rb0->getInvMass() * infoGlobal.m_timeStep)
754  .dot(solverConstraint.m_contactNormal1);
755  }
756  }
757  if (multiBodyB)
758  {
759  ndofB = multiBodyB->getNumDofs() + 6;
760  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
761  for (int i = 0; i < ndofB; ++i)
762  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
763  }
764  else
765  {
766  if (rb1)
767  {
768  rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2) +
769  (rb1->getTotalTorque() * rb1->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos2) +
770  rb1->getTotalForce() * rb1->getInvMass() * infoGlobal.m_timeStep)
771  .dot(solverConstraint.m_contactNormal2);
772  }
773  }
774 
775  solverConstraint.m_friction = cp.m_combinedFriction;
776 
777  if (!isFriction)
778  {
780  if (restitution <= btScalar(0.))
781  {
782  restitution = 0.f;
783  }
784  }
785  }
786 
787  {
788  btScalar positionalError = 0.f;
789  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
790  if (isFriction)
791  {
792  positionalError = -distance * erp / infoGlobal.m_timeStep;
793  }
794  else
795  {
796  if (distance > 0)
797  {
798  positionalError = 0;
799  velocityError -= distance / infoGlobal.m_timeStep;
800  }
801  else
802  {
803  positionalError = -distance * erp / infoGlobal.m_timeStep;
804  }
805  }
806 
807  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
808  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
809 
810  if (!isFriction)
811  {
812  // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
813  {
814  //combine position and velocity into rhs
815  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
816  solverConstraint.m_rhsPenetration = 0.f;
817  }
818  /*else
819  {
820  //split position and velocity into rhs and m_rhsPenetration
821  solverConstraint.m_rhs = velocityImpulse;
822  solverConstraint.m_rhsPenetration = penetrationImpulse;
823  }
824  */
825  solverConstraint.m_lowerLimit = 0;
826  solverConstraint.m_upperLimit = 1e10f;
827  }
828  else
829  {
830  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
831  solverConstraint.m_rhsPenetration = 0.f;
832  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
833  solverConstraint.m_upperLimit = solverConstraint.m_friction;
834  }
835 
836  solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
837  }
838 
840  {
841  if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS)
842  {
843  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor;
844  if (solverConstraint.m_appliedImpulse < 0)
845  solverConstraint.m_appliedImpulse = 0;
846  }
847  else
848  {
849  solverConstraint.m_appliedImpulse = 0.f;
850  }
851 
852  if (solverConstraint.m_appliedImpulse)
853  {
854  if (multiBodyA)
855  {
856  btScalar impulse = solverConstraint.m_appliedImpulse;
857  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
858  multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
859 
860  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
861  }
862  else
863  {
864  if (rb0)
865  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
866  }
867  if (multiBodyB)
868  {
869  btScalar impulse = solverConstraint.m_appliedImpulse;
870  btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
871  multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
872  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
873  }
874  else
875  {
876  if (rb1)
877  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
878  }
879  }
880  }
881  else
882  {
883  solverConstraint.m_appliedImpulse = 0.f;
884  solverConstraint.m_appliedPushImpulse = 0.f;
885  }
886 }
887 
889  const btVector3& constraintNormal,
890  btManifoldPoint& cp,
891  btScalar combinedTorsionalFriction,
893  btScalar& relaxation,
894  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
895 {
896  BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
897  btVector3 rel_pos1;
898  btVector3 rel_pos2;
899 
900  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
901  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
902 
903  const btVector3& pos1 = cp.getPositionWorldOnA();
904  const btVector3& pos2 = cp.getPositionWorldOnB();
905 
906  btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
907  btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
908 
909  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
910  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
911 
912  if (bodyA)
913  rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
914  if (bodyB)
915  rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
916 
917  relaxation = infoGlobal.m_sor;
918 
919  // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
920 
921  if (multiBodyA)
922  {
923  if (solverConstraint.m_linkA < 0)
924  {
925  rel_pos1 = pos1 - multiBodyA->getBasePos();
926  }
927  else
928  {
929  rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
930  }
931  const int ndofA = multiBodyA->getNumDofs() + 6;
932 
933  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
934 
935  if (solverConstraint.m_deltaVelAindex < 0)
936  {
937  solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
938  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
939  m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
940  }
941  else
942  {
943  btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
944  }
945 
946  solverConstraint.m_jacAindex = m_data.m_jacobians.size();
947  m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
948  m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
949  btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
950 
951  btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
952  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);
953  btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
954  multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
955 
956  btVector3 torqueAxis0 = constraintNormal;
957  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
958  solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
959  }
960  else
961  {
962  btVector3 torqueAxis0 = constraintNormal;
963  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
964  solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
965  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
966  }
967 
968  if (multiBodyB)
969  {
970  if (solverConstraint.m_linkB < 0)
971  {
972  rel_pos2 = pos2 - multiBodyB->getBasePos();
973  }
974  else
975  {
976  rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
977  }
978 
979  const int ndofB = multiBodyB->getNumDofs() + 6;
980 
981  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
982  if (solverConstraint.m_deltaVelBindex < 0)
983  {
984  solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
985  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
986  m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
987  }
988 
989  solverConstraint.m_jacBindex = m_data.m_jacobians.size();
990 
991  m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
992  m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
993  btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
994 
995  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);
996  multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
997 
998  btVector3 torqueAxis1 = -constraintNormal;
999  solverConstraint.m_relpos2CrossNormal = torqueAxis1;
1000  solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
1001  }
1002  else
1003  {
1004  btVector3 torqueAxis1 = -constraintNormal;
1005  solverConstraint.m_relpos2CrossNormal = torqueAxis1;
1006  solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
1007 
1008  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
1009  }
1010 
1011  {
1012  btScalar denom0 = 0.f;
1013  btScalar denom1 = 0.f;
1014  btScalar* jacB = 0;
1015  btScalar* jacA = 0;
1016  btScalar* lambdaA = 0;
1017  btScalar* lambdaB = 0;
1018  int ndofA = 0;
1019  if (multiBodyA)
1020  {
1021  ndofA = multiBodyA->getNumDofs() + 6;
1022  jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
1023  lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1024  for (int i = 0; i < ndofA; ++i)
1025  {
1026  btScalar j = jacA[i];
1027  btScalar l = lambdaA[i];
1028  denom0 += j * l;
1029  }
1030  }
1031  else
1032  {
1033  if (rb0)
1034  {
1035  btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
1036  denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1037  }
1038  }
1039  if (multiBodyB)
1040  {
1041  const int ndofB = multiBodyB->getNumDofs() + 6;
1042  jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
1043  lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1044  for (int i = 0; i < ndofB; ++i)
1045  {
1046  btScalar j = jacB[i];
1047  btScalar l = lambdaB[i];
1048  denom1 += j * l;
1049  }
1050  }
1051  else
1052  {
1053  if (rb1)
1054  {
1055  btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
1056  denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1057  }
1058  }
1059 
1060  btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm;
1061  if (d > SIMD_EPSILON)
1062  {
1063  solverConstraint.m_jacDiagABInv = relaxation / (d);
1064  }
1065  else
1066  {
1067  //disable the constraint row to handle singularity/redundant constraint
1068  solverConstraint.m_jacDiagABInv = 0.f;
1069  }
1070  }
1071 
1072  //compute rhs and remaining solverConstraint fields
1073 
1074  btScalar restitution = 0.f;
1075  btScalar penetration = isFriction ? 0 : cp.getDistance();
1076 
1077  btScalar rel_vel = 0.f;
1078  int ndofA = 0;
1079  int ndofB = 0;
1080  {
1081  btVector3 vel1, vel2;
1082  if (multiBodyA)
1083  {
1084  ndofA = multiBodyA->getNumDofs() + 6;
1085  btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
1086  for (int i = 0; i < ndofA; ++i)
1087  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
1088  }
1089  else
1090  {
1091  if (rb0)
1092  {
1093  btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
1094  rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0));
1095  }
1096  }
1097  if (multiBodyB)
1098  {
1099  ndofB = multiBodyB->getNumDofs() + 6;
1100  btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
1101  for (int i = 0; i < ndofB; ++i)
1102  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
1103  }
1104  else
1105  {
1106  if (rb1)
1107  {
1108  btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
1109  rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0));
1110  }
1111  }
1112 
1113  solverConstraint.m_friction = combinedTorsionalFriction;
1114 
1115  if (!isFriction)
1116  {
1118  if (restitution <= btScalar(0.))
1119  {
1120  restitution = 0.f;
1121  }
1122  }
1123  }
1124 
1125  solverConstraint.m_appliedImpulse = 0.f;
1126  solverConstraint.m_appliedPushImpulse = 0.f;
1127 
1128  {
1129  btScalar velocityError = 0 - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
1130 
1131  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1132 
1133  solverConstraint.m_rhs = velocityImpulse;
1134  solverConstraint.m_rhsPenetration = 0.f;
1135  solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
1136  solverConstraint.m_upperLimit = solverConstraint.m_friction;
1137 
1138  solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv;
1139  }
1140 }
1141 
1142 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1143 {
1144  BT_PROFILE("addMultiBodyFrictionConstraint");
1146  solverConstraint.m_orgConstraint = 0;
1147  solverConstraint.m_orgDofIndex = -1;
1148 
1149  solverConstraint.m_frictionIndex = frictionIndex;
1150  bool isFriction = true;
1151 
1152  const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1153  const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1154 
1155  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1156  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1157 
1158  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1159  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1160 
1161  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1162  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1163  solverConstraint.m_multiBodyA = mbA;
1164  if (mbA)
1165  solverConstraint.m_linkA = fcA->m_link;
1166 
1167  solverConstraint.m_multiBodyB = mbB;
1168  if (mbB)
1169  solverConstraint.m_linkB = fcB->m_link;
1170 
1171  solverConstraint.m_originalContactPoint = &cp;
1172 
1173  setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1174  return solverConstraint;
1175 }
1176 
1178  btScalar combinedTorsionalFriction,
1179  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1180 {
1181  BT_PROFILE("addMultiBodyRollingFrictionConstraint");
1182 
1184 
1186  solverConstraint.m_orgConstraint = 0;
1187  solverConstraint.m_orgDofIndex = -1;
1188 
1189  solverConstraint.m_frictionIndex = frictionIndex;
1190  bool isFriction = true;
1191 
1192  const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1193  const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1194 
1195  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1196  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1197 
1198  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1199  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1200 
1201  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1202  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1203  solverConstraint.m_multiBodyA = mbA;
1204  if (mbA)
1205  solverConstraint.m_linkA = fcA->m_link;
1206 
1207  solverConstraint.m_multiBodyB = mbB;
1208  if (mbB)
1209  solverConstraint.m_linkB = fcB->m_link;
1210 
1211  solverConstraint.m_originalContactPoint = &cp;
1212 
1213  setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1214  return solverConstraint;
1215 }
1216 
1218  btScalar combinedTorsionalFriction,
1219  btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1220 {
1221  BT_PROFILE("addMultiBodyRollingFrictionConstraint");
1222 
1224  solverConstraint.m_orgConstraint = 0;
1225  solverConstraint.m_orgDofIndex = -1;
1226 
1227  solverConstraint.m_frictionIndex = frictionIndex;
1228  bool isFriction = true;
1229 
1230  const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1231  const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1232 
1233  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1234  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1235 
1236  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1237  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1238 
1239  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1240  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1241  solverConstraint.m_multiBodyA = mbA;
1242  if (mbA)
1243  solverConstraint.m_linkA = fcA->m_link;
1244 
1245  solverConstraint.m_multiBodyB = mbB;
1246  if (mbB)
1247  solverConstraint.m_linkB = fcB->m_link;
1248 
1249  solverConstraint.m_originalContactPoint = &cp;
1250 
1251  setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1252  return solverConstraint;
1253 }
1255 {
1256  const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1257  const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1258 
1259  btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1260  btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1261 
1262  btCollisionObject *colObj0 = 0, *colObj1 = 0;
1263 
1264  colObj0 = (btCollisionObject*)manifold->getBody0();
1265  colObj1 = (btCollisionObject*)manifold->getBody1();
1266 
1267  int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1268  int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1269 
1270  // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
1271  // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
1272 
1274  // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
1275  // return;
1276 
1277  //only a single rollingFriction per manifold
1278  int rollingFriction = 4;
1279 
1280  for (int j = 0; j < manifold->getNumContacts(); j++)
1281  {
1282  btManifoldPoint& cp = manifold->getContactPoint(j);
1283 
1284  if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1285  {
1286  btScalar relaxation;
1287 
1288  int frictionIndex = m_multiBodyNormalContactConstraints.size();
1289 
1291 
1292  // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
1293  // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
1294  solverConstraint.m_orgConstraint = 0;
1295  solverConstraint.m_orgDofIndex = -1;
1296  solverConstraint.m_solverBodyIdA = solverBodyIdA;
1297  solverConstraint.m_solverBodyIdB = solverBodyIdB;
1298  solverConstraint.m_multiBodyA = mbA;
1299  if (mbA)
1300  solverConstraint.m_linkA = fcA->m_link;
1301 
1302  solverConstraint.m_multiBodyB = mbB;
1303  if (mbB)
1304  solverConstraint.m_linkB = fcB->m_link;
1305 
1306  solverConstraint.m_originalContactPoint = &cp;
1307 
1308  bool isFriction = false;
1309  setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
1310 
1311  // const btVector3& pos1 = cp.getPositionWorldOnA();
1312  // const btVector3& pos2 = cp.getPositionWorldOnB();
1313 
1315 #define ENABLE_FRICTION
1316 #ifdef ENABLE_FRICTION
1317  solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size();
1318 
1323 
1334 
1336  cp.m_lateralFrictionDir1.normalize();
1337  cp.m_lateralFrictionDir2.normalize();
1338 
1339  if (rollingFriction > 0)
1340  {
1341  if (cp.m_combinedSpinningFriction > 0)
1342  {
1343  addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
1344  }
1345  if (cp.m_combinedRollingFriction > 0)
1346  {
1351 
1352  addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1353  addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1354  }
1355  rollingFriction--;
1356  }
1358  { /*
1359  cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1360  btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1361  if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
1362  {
1363  cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1364  if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1365  {
1366  cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
1367  cp.m_lateralFrictionDir2.normalize();//??
1368  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1369  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1370  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1371 
1372  }
1373 
1374  applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1375  applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1376  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1377 
1378  } else
1379  */
1380  {
1383  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1384 
1386  {
1389  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1390  }
1391 
1393  {
1395  }
1396  }
1397  }
1398  else
1399  {
1400  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1401 
1403  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1404  solverConstraint.m_appliedImpulse = 0.f;
1405  solverConstraint.m_appliedPushImpulse = 0.f;
1406  }
1407 
1408 #endif //ENABLE_FRICTION
1409  }
1410  else
1411  {
1412  // Reset quantities related to warmstart as 0.
1413  cp.m_appliedImpulse = 0;
1414  cp.m_prevRHS = 0;
1415  }
1416  }
1417 }
1418 
1420 {
1421  for (int i = 0; i < numManifolds; i++)
1422  {
1423  btPersistentManifold* manifold = manifoldPtr[i];
1424  const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1425  const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1426  if (!fcA && !fcB)
1427  {
1428  //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
1429  convertContact(manifold, infoGlobal);
1430  }
1431  else
1432  {
1434  }
1435  }
1436 
1437  //also convert the multibody constraints, if any
1438 
1439  for (int i = 0; i < m_tmpNumMultiBodyConstraints; i++)
1440  {
1442  m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
1443  m_data.m_fixedBodyId = m_fixedBodyId;
1444 
1445  c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal);
1446  }
1447 
1448  // Warmstart for noncontact constraints
1450  {
1451  for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1452  {
1453  btMultiBodySolverConstraint& solverConstraint =
1454  m_multiBodyNonContactConstraints[i];
1455  solverConstraint.m_appliedImpulse =
1456  solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
1458 
1459  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
1460  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
1461  if (solverConstraint.m_appliedImpulse)
1462  {
1463  if (multiBodyA)
1464  {
1465  int ndofA = multiBodyA->getNumDofs() + 6;
1466  btScalar* deltaV =
1467  &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1468  btScalar impulse = solverConstraint.m_appliedImpulse;
1469  multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
1470  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
1471  }
1472  if (multiBodyB)
1473  {
1474  int ndofB = multiBodyB->getNumDofs() + 6;
1475  btScalar* deltaV =
1476  &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1477  btScalar impulse = solverConstraint.m_appliedImpulse;
1478  multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
1479  applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
1480  }
1481  }
1482  }
1483  }
1484  else
1485  {
1486  for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1487  {
1488  btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1489  solverConstraint.m_appliedImpulse = 0;
1490  }
1491  }
1492 }
1493 
1495 {
1496  //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
1497  return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1498 }
1499 
1500 #if 0
1501 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
1502 {
1503  if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
1504  {
1505  //todo: get rid of those temporary memory allocations for the joint feedback
1506  btAlignedObjectArray<btScalar> forceVector;
1507  int numDofsPlusBase = 6+mb->getNumDofs();
1508  forceVector.resize(numDofsPlusBase);
1509  for (int i=0;i<numDofsPlusBase;i++)
1510  {
1511  forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
1512  }
1514  output.resize(numDofsPlusBase);
1515  bool applyJointFeedback = true;
1516  mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
1517  }
1518 }
1519 #endif
1520 
1522 {
1523 #if 1
1524 
1525  //bod->addBaseForce(m_gravity * bod->getBaseMass());
1526  //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
1527 
1528  if (c.m_orgConstraint)
1529  {
1530  c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex, c.m_appliedImpulse);
1531  }
1532 
1533  if (c.m_multiBodyA)
1534  {
1535  c.m_multiBodyA->setCompanionId(-1);
1536  btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime);
1537  btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime);
1538  if (c.m_linkA < 0)
1539  {
1540  c.m_multiBodyA->addBaseConstraintForce(force);
1541  c.m_multiBodyA->addBaseConstraintTorque(torque);
1542  }
1543  else
1544  {
1545  c.m_multiBodyA->addLinkConstraintForce(c.m_linkA, force);
1546  //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1547  c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA, torque);
1548  }
1549  }
1550 
1551  if (c.m_multiBodyB)
1552  {
1553  {
1554  c.m_multiBodyB->setCompanionId(-1);
1555  btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime);
1556  btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime);
1557  if (c.m_linkB < 0)
1558  {
1559  c.m_multiBodyB->addBaseConstraintForce(force);
1560  c.m_multiBodyB->addBaseConstraintTorque(torque);
1561  }
1562  else
1563  {
1564  {
1565  c.m_multiBodyB->addLinkConstraintForce(c.m_linkB, force);
1566  //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1567  c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB, torque);
1568  }
1569  }
1570  }
1571  }
1572 #endif
1573 
1574 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
1575 
1576  if (c.m_multiBodyA)
1577  {
1578  c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], c.m_appliedImpulse);
1579  }
1580 
1581  if (c.m_multiBodyB)
1582  {
1583  c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], c.m_appliedImpulse);
1584  }
1585 #endif
1586 }
1587 
1589 {
1590  BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1591  int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
1592 
1593  //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
1594  //or as applied force, so we can measure the joint reaction forces easier
1595  for (int i = 0; i < numPoolConstraints; i++)
1596  {
1599 
1601 
1603  {
1605  }
1606  }
1607 
1608  for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1609  {
1610  btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1612  }
1613 
1614 
1615  {
1616  BT_PROFILE("warm starting write back");
1617  for (int j = 0; j < numPoolConstraints; j++)
1618  {
1620  btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
1621  btAssert(pt);
1622  pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1623  pt->m_prevRHS = solverConstraint.m_rhs;
1624  pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1625 
1626  //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1628  {
1629  pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse;
1630  } else
1631  {
1632  pt->m_appliedImpulseLateral2 = 0;
1633  }
1634  }
1635  }
1636 
1637 #if 0
1638  //multibody joint feedback
1639  {
1640  BT_PROFILE("multi body joint feedback");
1641  for (int j=0;j<numPoolConstraints;j++)
1642  {
1644 
1645  //apply the joint feedback into all links of the btMultiBody
1646  //todo: double-check the signs of the applied impulse
1647 
1648  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1649  {
1650  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1651  }
1652  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1653  {
1654  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1655  }
1656 #if 0
1657  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
1658  {
1659  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1660  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
1661  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
1662  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1663 
1664  }
1665  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
1666  {
1667  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1668  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
1669  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
1670  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1671  }
1672 
1674  {
1675  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
1676  {
1677  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1678  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
1679  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
1680  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1681  }
1682 
1683  if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
1684  {
1685  applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1686  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
1687  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
1688  m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1689  }
1690  }
1691 #endif
1692  }
1693 
1694  for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1695  {
1696  const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1697  if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1698  {
1699  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1700  }
1701  if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1702  {
1703  applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1704  }
1705  }
1706  }
1707 
1708  numPoolConstraints = m_multiBodyNonContactConstraints.size();
1709 
1710 #if 0
1711  //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1712  for (int i=0;i<numPoolConstraints;i++)
1713  {
1714  const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
1715 
1716  btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
1717  btJointFeedback* fb = constr->getJointFeedback();
1718  if (fb)
1719  {
1720  fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1721  fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1722  fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1723  fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1724 
1725  }
1726 
1727  constr->internalSetAppliedImpulse(c.m_appliedImpulse);
1728  if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1729  {
1730  constr->setEnabled(false);
1731  }
1732 
1733  }
1734 #endif
1735 #endif
1736 
1738 }
1739 
1740 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)
1741 {
1742  //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
1743 
1744  //printf("solveMultiBodyGroup start\n");
1745  m_tmpMultiBodyConstraints = multiBodyConstraints;
1746  m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1747 
1748  btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1749 
1752 }
ATTR_WARN_UNUSED_RESULT const BMLoop * l
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
btCollisionObject
@ CF_ANISOTROPIC_FRICTION
@ CF_ANISOTROPIC_ROLLING_FRICTION
@ SOLVER_DISABLE_IMPLICIT_CONE_FRICTION
@ SOLVER_USE_ARTICULATED_WARMSTARTING
@ SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
@ SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
@ SOLVER_USE_2_FRICTION_DIRECTIONS
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
@ BT_CONTACT_FLAG_HAS_CONTACT_ERP
@ BT_CONTACT_FLAG_HAS_CONTACT_CFM
@ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
@ BT_CONTACT_FLAG_FRICTION_ANCHOR
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
int m_tmpNumMultiBodyConstraints
btMultiBodySolverConstraint & addMultiBodySpinningFrictionConstraint(const btVector3 &normalAxis, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btScalar combinedTorsionalFriction, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, const btScalar &appliedImpulse, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints
btMultiBodySolverConstraint & addMultiBodyFrictionConstraint(const btVector3 &normalAxis, const btScalar &appliedImpulse, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
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)
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint &cA1, const btMultiBodySolverConstraint &cB)
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
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)
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
btAlignedObjectArray< btScalar > m_data
btMultiBodyConstraint
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
btMultiBody
Definition: btMultiBody.h:51
virtual btScalar solveGroupCacheFriendlySetup(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)
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
btPersistentManifold()
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
SIMD_FORCE_INLINE btScalar btCos(btScalar x)
Definition: btScalar.h:498
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
SIMD_FORCE_INLINE btScalar btSin(btScalar x)
Definition: btScalar.h:499
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:295
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint ** constraints
btSequentialImpulseConstraintSolverMt int btPersistentManifold ** manifoldPtr
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int numConstraints
btSequentialImpulseConstraintSolverMt int numBodies
btSequentialImpulseConstraintSolverMt int btPersistentManifold int btTypedConstraint int const btContactSolverInfo & infoGlobal
btSequentialImpulseConstraintSolverMt int btPersistentManifold int numManifolds
virtual void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
btScalar resolveSingleConstraintRowGeneric(btSolverBody &bodyA, btSolverBody &bodyB, const btSolverConstraint &contactConstraint)
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
static T sum(const btAlignedObjectArray< T > &items)
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
Definition: btSolverBody.h:99
btJointFeedback
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition: btVector3.h:356
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE T & expandNonInitializing()
btScalar m_frictionCFM
btScalar m_combinedSpinningFriction
btScalar m_combinedRollingFriction
btScalar getDistance() const
btScalar m_combinedContactStiffness1
const btVector3 & getPositionWorldOnB() const
btScalar m_combinedRestitution
btVector3 m_lateralFrictionDir2
btScalar m_combinedContactDamping1
btScalar m_appliedImpulseLateral2
btScalar m_appliedImpulse
btScalar m_appliedImpulseLateral1
const btVector3 & getPositionWorldOnA() const
btVector3 m_normalWorldOnB
btScalar m_combinedFriction
btScalar m_contactMotion2
btVector3 m_lateralFrictionDir1
btScalar m_contactMotion1
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:254
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:284
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:279
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:579
BLI_INLINE float fb(float length, float L)
ccl_global KernelShaderEvalInput ccl_global float * output
static unsigned c
Definition: RandGen.cpp:83
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
vec_base< T, 3 > cross(const vec_base< T, 3 > &a, const vec_base< T, 3 > &b)
T distance(const T &a, const T &b)