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