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