Bullet Collision Detection & Physics Library: btMultiBodyConstraintSolver.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btMultiBodyConstraintSolver.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17 #include "btMultiBodyConstraintSolver.h"
18 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
19 #include "btMultiBodyLinkCollider.h"
20
21 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
22 #include "btMultiBodyConstraint.h"
23 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
24
25 #include "LinearMath/btQuickprof.h"
26
27 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
28 {
29 btScalar val = 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 for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
35 {
36 btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
37
38 resolveSingleConstraintRowGeneric(constraint);
39 if(constraint.m_multiBodyA)
40 constraint.m_multiBodyA->setPosUpdated(false);
41 if(constraint.m_multiBodyB)
42 constraint.m_multiBodyB->setPosUpdated(false);
43 }
44
45 //solve featherstone normal contact
46 for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
47 {
48 btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
49 if (iteration < infoGlobal.m_numIterations)
50 resolveSingleConstraintRowGeneric(constraint);
51
52 if(constraint.m_multiBodyA)
53 constraint.m_multiBodyA->setPosUpdated(false);
54 if(constraint.m_multiBodyB)
55 constraint.m_multiBodyB->setPosUpdated(false);
56 }
57
58 //solve featherstone frictional contact
59
60 for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
61 {
62 if (iteration < infoGlobal.m_numIterations)
63 {
64 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
65 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
66 //adjust friction limits here
67 if (totalImpulse>btScalar(0))
68 {
69 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
70 frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
71 resolveSingleConstraintRowGeneric(frictionConstraint);
72
73 if(frictionConstraint.m_multiBodyA)
74 frictionConstraint.m_multiBodyA->setPosUpdated(false);
75 if(frictionConstraint.m_multiBodyB)
76 frictionConstraint.m_multiBodyB->setPosUpdated(false);
77 }
78 }
79 }
80 return val;
81 }
82
83 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
84 {
85 m_multiBodyNonContactConstraints.resize(0);
86 m_multiBodyNormalContactConstraints.resize(0);
87 m_multiBodyFrictionContactConstraints.resize(0);
88 m_data.m_jacobians.resize(0);
89 m_data.m_deltaVelocitiesUnitImpulse.resize(0);
90 m_data.m_deltaVelocities.resize(0);
91
92 for (int i=0;i<numBodies;i++)
93 {
94 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
95 if (fcA)
96 {
97 fcA->m_multiBody->setCompanionId(-1);
98 }
99 }
100
101 btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
102
103 return val;
104 }
105
106 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
107 {
108 for (int i = 0; i < ndof; ++i)
109 m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
110 }
111
112 void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
113 {
114
115 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
116 btScalar deltaVelADotn=0;
117 btScalar deltaVelBDotn=0;
118 btSolverBody* bodyA = 0;
119 btSolverBody* bodyB = 0;
120 int ndofA=0;
121 int ndofB=0;
122
123 if (c.m_multiBodyA)
124 {
125 ndofA = c.m_multiBodyA->getNumDofs() + 6;
126 for (int i = 0; i < ndofA; ++i)
127 deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
128 } else if(c.m_solverBodyIdA >= 0)
129 {
130 bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
131 deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
132 }
133
134 if (c.m_multiBodyB)
135 {
136 ndofB = c.m_multiBodyB->getNumDofs() + 6;
137 for (int i = 0; i < ndofB; ++i)
138 deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
139 } else if(c.m_solverBodyIdB >= 0)
140 {
141 bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
142 deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
143 }
144
145
146 deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
147 deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
148 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
149
150 if (sum < c.m_lowerLimit)
151 {
152 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
153 c.m_appliedImpulse = c.m_lowerLimit;
154 }
155 else if (sum > c.m_upperLimit)
156 {
157 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
158 c.m_appliedImpulse = c.m_upperLimit;
159 }
160 else
161 {
162 c.m_appliedImpulse = sum;
163 }
164
165 if (c.m_multiBodyA)
166 {
167 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
168 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
169 //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
170 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
171 c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
172 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
173 } else if(c.m_solverBodyIdA >= 0)
174 {
175 bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
176
177 }
178 if (c.m_multiBodyB)
179 {
180 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
181 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
182 //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
183 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
184 c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
185 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
186 } else if(c.m_solverBodyIdB >= 0)
187 {
188 bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
189 }
190
191 }
192
193
194
195
196 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
197 const btVector3& contactNormal,
198 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
199 btScalar& relaxation,
200 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
201 {
202
203 BT_PROFILE("setupMultiBodyContactConstraint");
204 btVector3 rel_pos1;
205 btVector3 rel_pos2;
206
207 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
208 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
209
210 const btVector3& pos1 = cp.getPositionWorldOnA();
211 const btVector3& pos2 = cp.getPositionWorldOnB();
212
213 btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
214 btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
215
216 btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
217 btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
218
219 if (bodyA)
220 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
221 if (bodyB)
222 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
223
224 relaxation = 1.f;
225
226
227
228
229 if (multiBodyA)
230 {
231 if (solverConstraint.m_linkA<0)
232 {
233 rel_pos1 = pos1 - multiBodyA->getBasePos();
234 } else
235 {
236 rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
237 }
238 const int ndofA = multiBodyA->getNumDofs() + 6;
239
240 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
241
242 if (solverConstraint.m_deltaVelAindex <0)
243 {
244 solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
245 multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
246 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
247 } else
248 {
249 btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
250 }
251
252 solverConstraint.m_jacAindex = m_data.m_jacobians.size();
253 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
254 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
255 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
256
257 btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
258 multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
259 btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
260 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
261
262 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
263 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
264 solverConstraint.m_contactNormal1 = contactNormal;
265 } else
266 {
267 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
268 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
269 solverConstraint.m_contactNormal1 = contactNormal;
270 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
271 }
272
273
274
275 if (multiBodyB)
276 {
277 if (solverConstraint.m_linkB<0)
278 {
279 rel_pos2 = pos2 - multiBodyB->getBasePos();
280 } else
281 {
282 rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
283 }
284
285 const int ndofB = multiBodyB->getNumDofs() + 6;
286
287 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
288 if (solverConstraint.m_deltaVelBindex <0)
289 {
290 solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
291 multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
292 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
293 }
294
295 solverConstraint.m_jacBindex = m_data.m_jacobians.size();
296
297 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
298 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
299 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
300
301 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);
302 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
303
304 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
305 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
306 solverConstraint.m_contactNormal2 = -contactNormal;
307
308 } else
309 {
310 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
311 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
312 solverConstraint.m_contactNormal2 = -contactNormal;
313
314 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
315 }
316
317 {
318
319 btVector3 vec;
320 btScalar denom0 = 0.f;
321 btScalar denom1 = 0.f;
322 btScalar* jacB = 0;
323 btScalar* jacA = 0;
324 btScalar* lambdaA =0;
325 btScalar* lambdaB =0;
326 int ndofA = 0;
327 if (multiBodyA)
328 {
329 ndofA = multiBodyA->getNumDofs() + 6;
330 jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
331 lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
332 for (int i = 0; i < ndofA; ++i)
333 {
334 btScalar j = jacA[i] ;
335 btScalar l =lambdaA[i];
336 denom0 += j*l;
337 }
338 } else
339 {
340 if (rb0)
341 {
342 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
343 denom0 = rb0->getInvMass() + contactNormal.dot(vec);
344 }
345 }
346 if (multiBodyB)
347 {
348 const int ndofB = multiBodyB->getNumDofs() + 6;
349 jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
350 lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
351 for (int i = 0; i < ndofB; ++i)
352 {
353 btScalar j = jacB[i] ;
354 btScalar l =lambdaB[i];
355 denom1 += j*l;
356 }
357
358 } else
359 {
360 if (rb1)
361 {
362 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
363 denom1 = rb1->getInvMass() + contactNormal.dot(vec);
364 }
365 }
366
367
368
369 btScalar d = denom0+denom1;
370 if (d>SIMD_EPSILON)
371 {
372 solverConstraint.m_jacDiagABInv = relaxation/(d);
373 } else
374 {
375 //disable the constraint row to handle singularity/redundant constraint
376 solverConstraint.m_jacDiagABInv = 0.f;
377 }
378
379 }
380
381
382 //compute rhs and remaining solverConstraint fields
383
384
385
386 btScalar restitution = 0.f;
387 btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
388
389 btScalar rel_vel = 0.f;
390 int ndofA = 0;
391 int ndofB = 0;
392 {
393
394 btVector3 vel1,vel2;
395 if (multiBodyA)
396 {
397 ndofA = multiBodyA->getNumDofs() + 6;
398 btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
399 for (int i = 0; i < ndofA ; ++i)
400 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
401 } else
402 {
403 if (rb0)
404 {
405 rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
406 }
407 }
408 if (multiBodyB)
409 {
410 ndofB = multiBodyB->getNumDofs() + 6;
411 btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
412 for (int i = 0; i < ndofB ; ++i)
413 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
414
415 } else
416 {
417 if (rb1)
418 {
419 rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
420 }
421 }
422
423 solverConstraint.m_friction = cp.m_combinedFriction;
424
425 if(!isFriction)
426 {
427 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
428 if (restitution <= btScalar(0.))
429 {
430 restitution = 0.f;
431 }
432 }
433 }
434
435
437 //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
438 if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
439 {
440 solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
441
442 if (solverConstraint.m_appliedImpulse)
443 {
444 if (multiBodyA)
445 {
446 btScalar impulse = solverConstraint.m_appliedImpulse;
447 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
448 multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
449
450 applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
451 } else
452 {
453 if (rb0)
454 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
455 }
456 if (multiBodyB)
457 {
458 btScalar impulse = solverConstraint.m_appliedImpulse;
459 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
460 multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
461 applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
462 } else
463 {
464 if (rb1)
465 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
466 }
467 }
468 } else
469 {
470 solverConstraint.m_appliedImpulse = 0.f;
471 }
472
473 solverConstraint.m_appliedPushImpulse = 0.f;
474
475 {
476
477 btScalar positionalError = 0.f;
478 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
479
480 btScalar erp = infoGlobal.m_erp2;
481 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
482 {
483 erp = infoGlobal.m_erp;
484 }
485
486 if (penetration>0)
487 {
488 positionalError = 0;
489 velocityError -= penetration / infoGlobal.m_timeStep;
490
491 } else
492 {
493 positionalError = -penetration * erp/infoGlobal.m_timeStep;
494 }
495
496 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
497 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
498
499 if(!isFriction)
500 {
501 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
502 {
503 //combine position and velocity into rhs
504 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
505 solverConstraint.m_rhsPenetration = 0.f;
506
507 } else
508 {
509 //split position and velocity into rhs and m_rhsPenetration
510 solverConstraint.m_rhs = velocityImpulse;
511 solverConstraint.m_rhsPenetration = penetrationImpulse;
512 }
513
514 solverConstraint.m_lowerLimit = 0;
515 solverConstraint.m_upperLimit = 1e10f;
516 }
517 else
518 {
519 solverConstraint.m_rhs = velocityImpulse;
520 solverConstraint.m_rhsPenetration = 0.f;
521 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
522 solverConstraint.m_upperLimit = solverConstraint.m_friction;
523 }
524
525 solverConstraint.m_cfm = 0.f; //why not use cfmSlip?
526 }
527
528 }
529
530
531
532
533 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
534 {
535 BT_PROFILE("addMultiBodyFrictionConstraint");
536 btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
537 solverConstraint.m_orgConstraint = 0;
538 solverConstraint.m_orgDofIndex = -1;
539
540 solverConstraint.m_frictionIndex = frictionIndex;
541 bool isFriction = true;
542
543 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
544 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
545
546 btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
547 btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
548
549 int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
550 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
551
552 solverConstraint.m_solverBodyIdA = solverBodyIdA;
553 solverConstraint.m_solverBodyIdB = solverBodyIdB;
554 solverConstraint.m_multiBodyA = mbA;
555 if (mbA)
556 solverConstraint.m_linkA = fcA->m_link;
557
558 solverConstraint.m_multiBodyB = mbB;
559 if (mbB)
560 solverConstraint.m_linkB = fcB->m_link;
561
562 solverConstraint.m_originalContactPoint = &cp;
563
564 setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
565 return solverConstraint;
566 }
567
568 void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
569 {
570 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
571 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
572
573 btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
574 btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
575
576 btCollisionObject* colObj0=0,*colObj1=0;
577
578 colObj0 = (btCollisionObject*)manifold->getBody0();
579 colObj1 = (btCollisionObject*)manifold->getBody1();
580
581 int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
582 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
583
584 // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
585 // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
586
587
589 // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
590 // return;
591
592
593
594 for (int j=0;j<manifold->getNumContacts();j++)
595 {
596
597 btManifoldPoint& cp = manifold->getContactPoint(j);
598
599 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
600 {
601
602 btScalar relaxation;
603
604 int frictionIndex = m_multiBodyNormalContactConstraints.size();
605
606 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
607
608 // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
609 // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
610 solverConstraint.m_orgConstraint = 0;
611 solverConstraint.m_orgDofIndex = -1;
612 solverConstraint.m_solverBodyIdA = solverBodyIdA;
613 solverConstraint.m_solverBodyIdB = solverBodyIdB;
614 solverConstraint.m_multiBodyA = mbA;
615 if (mbA)
616 solverConstraint.m_linkA = fcA->m_link;
617
618 solverConstraint.m_multiBodyB = mbB;
619 if (mbB)
620 solverConstraint.m_linkB = fcB->m_link;
621
622 solverConstraint.m_originalContactPoint = &cp;
623
624 bool isFriction = false;
625 setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
626
627 // const btVector3& pos1 = cp.getPositionWorldOnA();
628 // const btVector3& pos2 = cp.getPositionWorldOnB();
629
631 #define ENABLE_FRICTION
632 #ifdef ENABLE_FRICTION
633 solverConstraint.m_frictionIndex = frictionIndex;
634 #if ROLLING_FRICTION
635 int rollingFriction=1;
636 btVector3 angVelA(0,0,0),angVelB(0,0,0);
637 if (rb0)
638 angVelA = rb0->getAngularVelocity();
639 if (rb1)
640 angVelB = rb1->getAngularVelocity();
641 btVector3 relAngVel = angVelB-angVelA;
642
643 if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
644 {
645 //only a single rollingFriction per manifold
646 rollingFriction--;
647 if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
648 {
649 relAngVel.normalize();
650 applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
651 applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
652 if (relAngVel.length()>0.001)
653 addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
654
655 } else
656 {
657 addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
658 btVector3 axis0,axis1;
659 btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
660 applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
661 applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
662 applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
663 applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
664 if (axis0.length()>0.001)
665 addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
666 if (axis1.length()>0.001)
667 addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
668
669 }
670 }
671 #endif //ROLLING_FRICTION
672
677
688 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
689 {/*
690 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
691 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
692 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
693 {
694 cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
695 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
696 {
697 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
698 cp.m_lateralFrictionDir2.normalize();//??
699 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
700 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
701 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
702
703 }
704
705 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
706 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
707 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
708
709 } else
710 */
711 {
712 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
713
714 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
715 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
716 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
717
718 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
719 {
720 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
721 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
722 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
723 }
724
725 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
726 {
727 cp.m_lateralFrictionInitialized = true;
728 }
729 }
730
731 } else
732 {
733 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
734
735 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
736 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
737
738 //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
739 //todo:
740 solverConstraint.m_appliedImpulse = 0.f;
741 solverConstraint.m_appliedPushImpulse = 0.f;
742 }
743
744
745 #endif //ENABLE_FRICTION
746
747 }
748 }
749 }
750
751 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
752 {
753 //btPersistentManifold* manifold = 0;
754
755 for (int i=0;i<numManifolds;i++)
756 {
757 btPersistentManifold* manifold= manifoldPtr[i];
758 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
759 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
760 if (!fcA && !fcB)
761 {
762 //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
763 convertContact(manifold,infoGlobal);
764 } else
765 {
766 convertMultiBodyContact(manifold,infoGlobal);
767 }
768 }
769
770 //also convert the multibody constraints, if any
771
772
773 for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
774 {
775 btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
776 m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
777 m_data.m_fixedBodyId = m_fixedBodyId;
778
779 c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal);
780 }
781
782 }
783
784
785
786 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
787 {
788 return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
789 }
790
791 #if 0
792 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
793 {
794 if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
795 {
796 //todo: get rid of those temporary memory allocations for the joint feedback
797 btAlignedObjectArray<btScalar> forceVector;
798 int numDofsPlusBase = 6+mb->getNumDofs();
799 forceVector.resize(numDofsPlusBase);
800 for (int i=0;i<numDofsPlusBase;i++)
801 {
802 forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
803 }
804 btAlignedObjectArray<btScalar> output;
805 output.resize(numDofsPlusBase);
806 bool applyJointFeedback = true;
807 mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
808 }
809 }
810 #endif
811
812 #include "Bullet3Common/b3Logging.h"
813 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
814 {
815 #if 1
816
817 //bod->addBaseForce(m_gravity * bod->getBaseMass());
818 //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
819
820 if (c.m_orgConstraint)
821 {
822 c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
823 }
824
825
826 if (c.m_multiBodyA)
827 {
828
829 c.m_multiBodyA->setCompanionId(-1);
830 btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
831 btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
832 if (c.m_linkA<0)
833 {
834 c.m_multiBodyA->addBaseConstraintForce(force);
835 c.m_multiBodyA->addBaseConstraintTorque(torque);
836 } else
837 {
838 c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
839 //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
840 c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
841 }
842 }
843
844 if (c.m_multiBodyB)
845 {
846 {
847 c.m_multiBodyB->setCompanionId(-1);
848 btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
849 btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
850 if (c.m_linkB<0)
851 {
852 c.m_multiBodyB->addBaseConstraintForce(force);
853 c.m_multiBodyB->addBaseConstraintTorque(torque);
854 } else
855 {
856 {
857 c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
858 //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
859 c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
860 }
861
862 }
863 }
864 }
865 #endif
866
867 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
868
869 if (c.m_multiBodyA)
870 {
871
872 if(c.m_multiBodyA->isMultiDof())
873 {
874 c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
875 }
876 else
877 {
878 c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
879 }
880 }
881
882 if (c.m_multiBodyB)
883 {
884 if(c.m_multiBodyB->isMultiDof())
885 {
886 c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
887 }
888 else
889 {
890 c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
891 }
892 }
893 #endif
894
895
896
897 }
898
899 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
900 {
901 BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
902 int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
903
904
905 //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
906 //or as applied force, so we can measure the joint reaction forces easier
907 for (int i=0;i<numPoolConstraints;i++)
908 {
909 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
910 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
911
912 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
913
914 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
915 {
916 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
917 }
918 }
919
920
921 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
922 {
923 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
924 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
925 }
926
927
928 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
929 {
930 BT_PROFILE("warm starting write back");
931 for (int j=0;j<numPoolConstraints;j++)
932 {
933 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
934 btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
935 btAssert(pt);
936 pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
937 pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
938
939 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
940 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
941 {
942 pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
943 }
944 //do a callback here?
945 }
946 }
947 #if 0
948 //multibody joint feedback
949 {
950 BT_PROFILE("multi body joint feedback");
951 for (int j=0;j<numPoolConstraints;j++)
952 {
953 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
954
955 //apply the joint feedback into all links of the btMultiBody
956 //todo: double-check the signs of the applied impulse
957
958 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
959 {
960 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
961 }
962 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
963 {
964 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
965 }
966 #if 0
967 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
968 {
969 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
970 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
971 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
972 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
973
974 }
975 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
976 {
977 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
978 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
979 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
980 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
981 }
982
983 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
984 {
985 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
986 {
987 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
988 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
989 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
990 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
991 }
992
993 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
994 {
995 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
996 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
997 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
998 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
999 }
1000 }
1001 #endif
1002 }
1003
1004 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1005 {
1006 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1007 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1008 {
1009 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1010 }
1011 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1012 {
1013 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1014 }
1015 }
1016 }
1017
1018 numPoolConstraints = m_multiBodyNonContactConstraints.size();
1019
1020 #if 0
1021 //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1022 for (int i=0;i<numPoolConstraints;i++)
1023 {
1024 const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
1025
1026 btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
1027 btJointFeedback* fb = constr->getJointFeedback();
1028 if (fb)
1029 {
1030 fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1031 fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1032 fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1033 fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1034
1035 }
1036
1037 constr->internalSetAppliedImpulse(c.m_appliedImpulse);
1038 if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1039 {
1040 constr->setEnabled(false);
1041 }
1042
1043 }
1044 #endif
1045 #endif
1046
1047 return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
1048 }
1049
1050
1051 void btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
1052 {
1053 //printf("solveMultiBodyGroup start\n");
1054 m_tmpMultiBodyConstraints = multiBodyConstraints;
1055 m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1056
1057 btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
1058
1059 m_tmpMultiBodyConstraints = 0;
1060 m_tmpNumMultiBodyConstraints = 0;
1061
1062
1063 }
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:270
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:84
btSequentialImpulseConstraintSolver::m_fixedBodyId
int m_fixedBodyId
Definition: btSequentialImpulseConstraintSolver.h:47
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:494
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btMultiBodySolverConstraint.h:59
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:54
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:50
btManifoldPoint::m_contactCFM2
btScalar m_contactCFM2
Definition: btManifoldPoint.h:112
btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:899
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btMultiBodyConstraintSolver::setupMultiBodyContactConstraint
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:196
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:40
btContactSolverInfoData::m_erp
btScalar m_erp
Definition: btContactSolverInfo.h:47
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:43
btManifoldPoint::m_lateralFrictionDir1
btVector3 m_lateralFrictionDir1
Definition: btManifoldPoint.h:116
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btMultiBodyJacobianData::m_deltaVelocities
btAlignedObjectArray< btScalar > m_deltaVelocities
Definition: btMultiBodyConstraint.h:32
btMultiBodyConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
btAlignedObjectArray< btScalar >
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:255
btMultiBodyJacobianData::m_solverBodyPool
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
Definition: btMultiBodyConstraint.h:36
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:501
btManifoldPoint::m_appliedImpulseLateral1
btScalar m_appliedImpulseLateral1
Definition: btManifoldPoint.h:107
btMultiBodyConstraintSolver::solveSingleIteration
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btMultiBodyConstraintSolver.cpp:27
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1825
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:46
btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
Definition: btMultiBodyConstraintSolver.cpp:813
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:47
btManifoldPoint::m_combinedRestitution
btScalar m_combinedRestitution
Definition: btManifoldPoint.h:95
btSequentialImpulseConstraintSolver::solveGroup
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
Definition: btSequentialImpulseConstraintSolver.cpp:1911
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:50
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
SOLVER_USE_WARMSTARTING
Definition: btContactSolverInfo.h:25
btMultiBodyConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
Definition: btMultiBodyConstraint.h:112
btJointFeedback
Definition: btTypedConstraint.h:67
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1272
btManifoldPoint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btManifoldPoint.h:106
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btMultiBodyConstraint.h
btMultiBody::addLinkConstraintForce
void addLinkConstraintForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:571
btTypedConstraint::getBreakingImpulseThreshold
btScalar getBreakingImpulseThreshold() const
Definition: btTypedConstraint.h:202
btContactSolverInfoData::m_singleAxisRollingFrictionThreshold
btScalar m_singleAxisRollingFrictionThreshold
Definition: btContactSolverInfo.h:60
btSequentialImpulseConstraintSolver::solveSingleIteration
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1529
btPersistentManifold.h
btMultiBody::internalNeedsJointFeedback
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:553
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition: btManifoldPoint.h:42
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
Definition: btContactSolverInfo.h:28
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:94
btManifoldPoint::m_contactMotion1
btScalar m_contactMotion1
Definition: btManifoldPoint.h:109
btMultiBodyConstraintSolver::m_tmpNumMultiBodyConstraints
int m_tmpNumMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:44
btMultiBody
Definition: btMultiBody.h:51
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:387
btSequentialImpulseConstraintSolver::restitutionCurve
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
Definition: btSequentialImpulseConstraintSolver.cpp:505
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btMultiBodySolverConstraint::m_deltaVelBindex
int m_deltaVelBindex
Definition: btMultiBodySolverConstraint.h:37
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btCollisionObject::CF_ANISOTROPIC_FRICTION
Definition: btCollisionObject.h:149
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
btMultiBodySolverConstraint::m_deltaVelAindex
int m_deltaVelAindex
Definition: btMultiBodySolverConstraint.h:35
btMultiBodyConstraintSolver::solveGroup
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...
Definition: btMultiBodyConstraintSolver.cpp:786
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:379
btTypedConstraint::getJointFeedback
const btJointFeedback * getJointFeedback() const
Definition: btTypedConstraint.h:280
btMultiBodyConstraintSolver::m_multiBodyNonContactConstraints
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
Definition: btMultiBodyConstraintSolver.h:35
btSequentialImpulseConstraintSolver::getOrInitSolverBody
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:717
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btMultiBody::addLinkConstraintTorque
void addLinkConstraintTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:576
btSimdScalar
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later...
Definition: btSolverBody.h:104
btManifoldPoint::m_combinedRollingFriction
btScalar m_combinedRollingFriction
Definition: btManifoldPoint.h:94
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:36
btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
Definition: btMultiBodyConstraintSolver.cpp:112
btMultiBodySolverConstraint::m_jacBindex
int m_jacBindex
Definition: btMultiBodySolverConstraint.h:38
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:90
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:155
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btMultiBodyConstraintSolver::solveMultiBodyGroup
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)
Definition: btMultiBodyConstraintSolver.cpp:1051
btJointFeedback::m_appliedForceBodyA
btVector3 m_appliedForceBodyA
Definition: btTypedConstraint.h:70
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:70
btMultiBody::setCompanionId
void setCompanionId(int id)
Definition: btMultiBody.h:473
btManifoldPoint::m_appliedImpulseLateral2
btScalar m_appliedImpulseLateral2
Definition: btManifoldPoint.h:108
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
output
#define output
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btMultiBodyConstraintSolver::convertContacts
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:751
btManifoldPoint::m_contactCFM1
btScalar m_contactCFM1
Definition: btManifoldPoint.h:111
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:49
btPersistentManifold::getContactProcessingThreshold
btScalar getContactProcessingThreshold() const
Definition: btPersistentManifold.h:132
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:74
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btMultiBodySolverConstraint::m_linkA
int m_linkA
Definition: btMultiBodySolverConstraint.h:71
btPersistentManifold::getContactPoint
const btManifoldPoint & getContactPoint(int index) const
Definition: btPersistentManifold.h:117
btMultiBodySolverConstraint::m_jacAindex
int m_jacAindex
Definition: btMultiBodySolverConstraint.h:36
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyConstraintSolver::m_multiBodyFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
Definition: btMultiBodyConstraintSolver.h:38
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:30
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:223
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
btMultiBodySolverConstraint::m_originalContactPoint
void * m_originalContactPoint
Definition: btMultiBodySolverConstraint.h:62
btMultiBodyLinkCollider.h
btManifoldPoint::getPositionWorldOnB
const btVector3 & getPositionWorldOnB() const
Definition: btManifoldPoint.h:136
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:54
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btMultiBodyConstraintSolver.h
btSequentialImpulseConstraintSolver::convertContact
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:976
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:196
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodySolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btMultiBodySolverConstraint.h:50
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
Definition: btMultiBody.cpp:1294
btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint
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)
Definition: btMultiBodyConstraintSolver.cpp:533
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btSequentialImpulseConstraintSolver::addRollingFrictionConstraint
btSolverConstraint & addRollingFrictionConstraint(const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint &cp, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f)
Definition: btSequentialImpulseConstraintSolver.cpp:707
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
btMultiBody::getCompanionId
int getCompanionId() const
Definition: btMultiBody.h:469
btMultiBodySolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btMultiBodySolverConstraint.h:58
btMultiBody::fillContactJacobianMultiDof
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:426
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:78
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:218
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition: btSolverBody.h:124
btQuickprof.h
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:109
btSolverBody::internalGetDeltaLinearVelocity
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don&#39;t use them
Definition: btSolverBody.h:208
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:227
btTypedConstraint::setEnabled
void setEnabled(bool enabled)
Definition: btTypedConstraint.h:217
btMultiBodyConstraintSolver::convertMultiBodyContact
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:568
btMultiBody::applyDeltaVeeMultiDof2
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:370
btMultiBodySolverConstraint::m_friction
btScalar m_friction
Definition: btMultiBodySolverConstraint.h:52
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1170
btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btMultiBodyConstraintSolver.cpp:83
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:547
btMultiBodyConstraintSolver::applyDeltaVee
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
Definition: btMultiBodyConstraintSolver.cpp:106
btContactSolverInfoData::m_splitImpulsePenetrationThreshold
btScalar m_splitImpulsePenetrationThreshold
Definition: btContactSolverInfo.h:51
btMultiBodyConstraintSolver::m_multiBodyNormalContactConstraints
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
Definition: btMultiBodyConstraintSolver.h:37
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btManifoldPoint::m_contactMotion2
btScalar m_contactMotion2
Definition: btManifoldPoint.h:110
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:271
btManifoldPoint::m_combinedFriction
btScalar m_combinedFriction
Definition: btManifoldPoint.h:93
btManifoldPoint::m_lateralFrictionInitialized
bool m_lateralFrictionInitialized
Definition: btManifoldPoint.h:104
btMultiBodySolverConstraint::m_linkB
int m_linkB
Definition: btMultiBodySolverConstraint.h:75
btMultiBodyConstraintSolver::m_tmpMultiBodyConstraints
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:43
btManifoldPoint::getPositionWorldOnA
const btVector3 & getPositionWorldOnA() const
Definition: btManifoldPoint.h:131
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultiBodyLinkCollider::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:29
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:131
btContactSolverInfoData::m_linearSlop
btScalar m_linearSlop
Definition: btContactSolverInfo.h:53
btSolverBody::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:130
btSolverBody::internalGetDeltaAngularVelocity
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:213
btTypedConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don&#39;t use them directly
Definition: btTypedConstraint.h:191
btMultiBody::addBaseConstraintForce
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:303
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
Definition: btContactSolverInfo.h:27
btManifoldPoint::m_lateralFrictionDir2
btVector3 m_lateralFrictionDir2
Definition: btManifoldPoint.h:117
btMultiBodyLinkCollider::m_multiBody
btMultiBody * m_multiBody
Definition: btMultiBodyLinkCollider.h:28
btMultiBodySolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btMultiBodySolverConstraint.h:49
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btMultiBodySolverConstraint.h:53
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:122
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:177
btSolverBody.h
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:69
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
btRigidBody::getLinearFactor
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:261
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:245
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:41
btMultiBody::addBaseConstraintTorque
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:307
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:231
btSequentialImpulseConstraintSolver::applyAnisotropicFriction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
Definition: btSequentialImpulseConstraintSolver.cpp:513
btMultiBodySolverConstraint::m_frictionIndex
int m_frictionIndex
Definition: btMultiBodySolverConstraint.h:67
btMultiBodySolverConstraint::m_cfm
btScalar m_cfm
Definition: btMultiBodySolverConstraint.h:55
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:156
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:95
btContactSolverInfoData::m_warmstartingFactor
btScalar m_warmstartingFactor
Definition: btContactSolverInfo.h:54
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:249
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btMultiBodyConstraintSolver::m_data
btMultiBodyJacobianData m_data
Definition: btMultiBodyConstraintSolver.h:40
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:56
btMultiBodySolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btMultiBodySolverConstraint.h:57
btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION
Definition: btCollisionObject.h:150
btContactSolverInfo.h
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:449