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

Bullet Collision Detection & Physics Library

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 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
39 btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
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
55 btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
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
79 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
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
99 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
100 {
101 m_multiBodyNonContactConstraints.resize(0);
102 m_multiBodyNormalContactConstraints.resize(0);
103 m_multiBodyFrictionContactConstraints.resize(0);
104 m_data.m_jacobians.resize(0);
105 m_data.m_deltaVelocitiesUnitImpulse.resize(0);
106 m_data.m_deltaVelocities.resize(0);
107
108 for (int i=0;i<numBodies;i++)
109 {
110 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
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
122 void 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
128 btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
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)
143 deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
144 } else if(c.m_solverBodyIdA >= 0)
145 {
146 bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
147 deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
148 }
149
150 if (c.m_multiBodyB)
151 {
152 ndofB = c.m_multiBodyB->getNumDofs() + 6;
153 for (int i = 0; i < ndofB; ++i)
154 deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
155 } else if(c.m_solverBodyIdB >= 0)
156 {
157 bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
158 deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
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;
169 c.m_appliedImpulse = c.m_lowerLimit;
170 }
171 else if (sum > c.m_upperLimit)
172 {
173 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
174 c.m_appliedImpulse = c.m_upperLimit;
175 }
176 else
177 {
178 c.m_appliedImpulse = sum;
179 }
180
181 if (c.m_multiBodyA)
182 {
183 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
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
187 c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
188 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
189 } else if(c.m_solverBodyIdA >= 0)
190 {
191 bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
192
193 }
194 if (c.m_multiBodyB)
195 {
196 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
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
200 c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
201 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
202 } else if(c.m_solverBodyIdB >= 0)
203 {
204 bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
205 }
206 return deltaImpulse;
207 }
208
209
210
211
212 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
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
258 if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
259 {
260 if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
261 cfm = cp.m_contactCFM;
262 if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
263 erp = cp.m_contactERP;
264 } else
265 {
266 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
267 {
268 btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
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);
298 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
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();
305 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
306 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
307 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.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];
312 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
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);
344 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
345 }
346
347 solverConstraint.m_jacBindex = m_data.m_jacobians.size();
348
349 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
350 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
351 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
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);
354 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
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 {
445 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
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
600 void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
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);
652 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
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();
659 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
660 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
661 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.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];
666 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
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);
698 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
699 }
700
701 solverConstraint.m_jacBindex = m_data.m_jacobians.size();
702
703 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
704 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
705 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
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);
708 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
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
871 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)
872 {
873 BT_PROFILE("addMultiBodyFrictionConstraint");
874 btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
875 solverConstraint.m_orgConstraint = 0;
876 solverConstraint.m_orgDofIndex = -1;
877
878 solverConstraint.m_frictionIndex = frictionIndex;
879 bool isFriction = true;
880
881 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
882 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
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
906 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,
907 btScalar combinedTorsionalFriction,
908 btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
909 {
910 BT_PROFILE("addMultiBodyRollingFrictionConstraint");
911 btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
912 solverConstraint.m_orgConstraint = 0;
913 solverConstraint.m_orgDofIndex = -1;
914
915 solverConstraint.m_frictionIndex = frictionIndex;
916 bool isFriction = true;
917
918 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
919 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
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
943 void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
944 {
945 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
946 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
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
982 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
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
1027 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
1028 cp.m_lateralFrictionDir1.normalize();
1029 cp.m_lateralFrictionDir2.normalize();
1030
1031 if (rollingFriction > 0 )
1032 {
1033 if (cp.m_combinedSpinningFriction>0)
1034 {
1035 addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
1036 }
1037 if (cp.m_combinedRollingFriction>0)
1038 {
1039
1040 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1041 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1042 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1043 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
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 }
1053 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
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
1079 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1080 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1081 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
1082
1083
1084 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1085 {
1086 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1087 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1088 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
1089 }
1090
1091 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
1092 {
1093 cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
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
1101 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
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
1117 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
1118 {
1119 //btPersistentManifold* manifold = 0;
1120
1121 for (int i=0;i<numManifolds;i++)
1122 {
1123 btPersistentManifold* manifold= manifoldPtr[i];
1124 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1125 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
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 {
1141 btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
1142 m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
1143 m_data.m_fixedBodyId = m_fixedBodyId;
1144
1145 c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal);
1146 }
1147
1148 }
1149
1150
1151
1152 btScalar 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
1158 static 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
1163 btAlignedObjectArray<btScalar> forceVector;
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 }
1170 btAlignedObjectArray<btScalar> output;
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
1179 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
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 {
1188 c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
1189 }
1190
1191
1192 if (c.m_multiBodyA)
1193 {
1194
1195 c.m_multiBodyA->setCompanionId(-1);
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 {
1200 c.m_multiBodyA->addBaseConstraintForce(force);
1201 c.m_multiBodyA->addBaseConstraintTorque(torque);
1202 } else
1203 {
1204 c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
1205 //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1206 c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
1207 }
1208 }
1209
1210 if (c.m_multiBodyB)
1211 {
1212 {
1213 c.m_multiBodyB->setCompanionId(-1);
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 {
1218 c.m_multiBodyB->addBaseConstraintForce(force);
1219 c.m_multiBodyB->addBaseConstraintTorque(torque);
1220 } else
1221 {
1222 {
1223 c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
1224 //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1225 c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
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 {
1240 c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
1241 }
1242 else
1243 {
1244 c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
1245 }
1246 }
1247
1248 if (c.m_multiBodyB)
1249 {
1250 if(c.m_multiBodyB->isMultiDof())
1251 {
1252 c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
1253 }
1254 else
1255 {
1256 c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
1257 }
1258 }
1259 #endif
1260
1261
1262
1263 }
1264
1265 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
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 {
1275 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
1276 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
1277
1278 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
1279
1280 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1281 {
1282 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
1283 }
1284 }
1285
1286
1287 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1288 {
1289 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
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 {
1299 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
1300 btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
1301 btAssert(pt);
1302 pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1303 pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1304
1305 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1306 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
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 {
1319 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
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
1349 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
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 {
1372 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
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 {
1390 const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
1391
1392 btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
1393 btJointFeedback* fb = constr->getJointFeedback();
1394 if (fb)
1395 {
1396 fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
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
1403 constr->internalSetAppliedImpulse(c.m_appliedImpulse);
1404 if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1405 {
1406 constr->setEnabled(false);
1407 }
1408
1409 }
1410 #endif
1411 #endif
1412
1413 return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
1414 }
1415
1416
1417 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)
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
1425 m_tmpMultiBodyConstraints = 0;
1426 m_tmpNumMultiBodyConstraints = 0;
1427
1428
1429 }
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:84
btSolverBody::m_linearVelocity
btVector3 m_linearVelocity
Definition: btSolverBody.h:119
btSequentialImpulseConstraintSolver::m_fixedBodyId
int m_fixedBodyId
Definition: btSequentialImpulseConstraintSolver.h:47
btSolverBody::m_angularVelocity
btVector3 m_angularVelocity
Definition: btSolverBody.h:120
btManifoldPoint::m_contactPointFlags
int m_contactPointFlags
Definition: btManifoldPoint.h:116
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:521
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:63
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:50
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:186
btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:1265
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:212
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:40
btMultiBody::internalNeedsJointFeedback
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:566
btManifoldPoint::getPositionWorldOnA
const btVector3 & getPositionWorldOnA() const
Definition: btManifoldPoint.h:155
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_combinedContactStiffness1
btScalar m_combinedContactStiffness1
Definition: btManifoldPoint.h:127
btManifoldPoint::m_lateralFrictionDir1
btVector3 m_lateralFrictionDir1
Definition: btManifoldPoint.h:140
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btRigidBody::getTotalTorque
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:292
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
btManifoldPoint::m_appliedImpulseLateral1
btScalar m_appliedImpulseLateral1
Definition: btManifoldPoint.h:119
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:1869
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:46
btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
Definition: btMultiBodyConstraintSolver.cpp:1179
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:47
BT_CONTACT_FLAG_HAS_CONTACT_ERP
Definition: btManifoldPoint.h:42
btManifoldPoint::m_combinedRestitution
btScalar m_combinedRestitution
Definition: btManifoldPoint.h:106
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
Definition: btManifoldPoint.h:43
btManifoldPoint::m_contactERP
btScalar m_contactERP
Definition: btManifoldPoint.h:132
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:1955
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:122
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:222
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:117
btJointFeedback
Definition: btTypedConstraint.h:67
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1283
btManifoldPoint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btManifoldPoint.h:118
btAssert
#define btAssert(x)
Definition: btScalar.h:131
btMultiBodyConstraint.h
btMultiBody::addLinkConstraintForce
void addLinkConstraintForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:638
btRigidBody::getLinearFactor
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:264
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:1631
btPersistentManifold.h
btMultiBody::getCompanionId
int getCompanionId() const
Definition: btMultiBody.h:482
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:223
btManifoldPoint::m_frictionCFM
btScalar m_frictionCFM
Definition: btManifoldPoint.h:136
btTypedConstraint::getJointFeedback
const btJointFeedback * getJointFeedback() const
Definition: btTypedConstraint.h:275
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition: btManifoldPoint.h:49
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
Definition: btContactSolverInfo.h:28
btManifoldPoint::m_contactMotion1
btScalar m_contactMotion1
Definition: btManifoldPoint.h:121
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
Definition: btMultiBody.cpp:1377
btMultiBodyConstraintSolver::m_tmpNumMultiBodyConstraints
int m_tmpNumMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:44
btMultiBody
Definition: btMultiBody.h:51
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:226
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
btTypedConstraint::getBreakingImpulseThreshold
btScalar getBreakingImpulseThreshold() const
Definition: btTypedConstraint.h:197
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:504
btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
Definition: btMultiBodyConstraintSolver.cpp:128
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btContactSolverInfoData::m_globalCfm
btScalar m_globalCfm
Definition: btContactSolverInfo.h:49
btMultiBodySolverConstraint::m_deltaVelBindex
int m_deltaVelBindex
Definition: btMultiBodySolverConstraint.h:37
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btCollisionObject::CF_ANISOTROPIC_FRICTION
Definition: btCollisionObject.h:162
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:309
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:1152
btMultiBodyConstraintSolver::m_multiBodyNonContactConstraints
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
Definition: btMultiBodyConstraintSolver.h:35
btSequentialImpulseConstraintSolver::getOrInitSolverBody
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:737
btPersistentManifold::getContactPoint
const btManifoldPoint & getContactPoint(int index) const
Definition: btPersistentManifold.h:125
btSolverBody::m_externalForceImpulse
btVector3 m_externalForceImpulse
Definition: btSolverBody.h:121
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btMultiBody::addLinkConstraintTorque
void addLinkConstraintTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:643
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:104
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:36
btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint
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)
Definition: btMultiBodyConstraintSolver.cpp:906
btMultiBodySolverConstraint::m_jacBindex
int m_jacBindex
Definition: btMultiBodySolverConstraint.h:38
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:100
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:1417
btJointFeedback::m_appliedForceBodyA
btVector3 m_appliedForceBodyA
Definition: btTypedConstraint.h:70
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:70
btManifoldPoint::getPositionWorldOnB
const btVector3 & getPositionWorldOnB() const
Definition: btManifoldPoint.h:160
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:102
btMultiBody::setCompanionId
void setCompanionId(int id)
Definition: btMultiBody.h:486
btManifoldPoint::m_appliedImpulseLateral2
btScalar m_appliedImpulseLateral2
Definition: btManifoldPoint.h:120
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:273
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
output
#define output
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:165
btMultiBodyConstraintSolver::convertContacts
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:1117
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:49
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
SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btContactSolverInfoData::m_frictionERP
btScalar m_frictionERP
Definition: btContactSolverInfo.h:50
btPersistentManifold::getContactProcessingThreshold
btScalar getContactProcessingThreshold() const
Definition: btPersistentManifold.h:140
btMultiBodySolverConstraint::m_linkA
int m_linkA
Definition: btMultiBodySolverConstraint.h:71
BT_CONTACT_FLAG_HAS_CONTACT_CFM
Definition: btManifoldPoint.h:41
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
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
btMultiBodySolverConstraint::m_originalContactPoint
void * m_originalContactPoint
Definition: btMultiBodySolverConstraint.h:62
btMultiBodyLinkCollider.h
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:54
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
Definition: btManifoldPoint.h:40
btMultiBodyConstraintSolver.h
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:155
btSequentialImpulseConstraintSolver::convertContact
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1082
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:382
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:215
btContactSolverInfoData::m_frictionCFM
btScalar m_frictionCFM
Definition: btContactSolverInfo.h:51
btManifoldPoint::m_contactCFM
btScalar m_contactCFM
Definition: btManifoldPoint.h:126
btContactSolverInfo
Definition: btContactSolverInfo.h:69
btMultiBodySolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btMultiBodySolverConstraint.h:50
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:871
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btManifoldPoint::m_combinedContactDamping1
btScalar m_combinedContactDamping1
Definition: btManifoldPoint.h:133
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
btMultiBodySolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btMultiBodySolverConstraint.h:58
btSolverBody::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:130
BT_CONTACT_FLAG_FRICTION_ANCHOR
Definition: btManifoldPoint.h:44
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
btSolverBody::internalGetDeltaLinearVelocity
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don&#39;t use them
Definition: btSolverBody.h:208
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:103
btSequentialImpulseConstraintSolver::restitutionCurve
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
Definition: btSequentialImpulseConstraintSolver.cpp:511
btTypedConstraint::setEnabled
void setEnabled(bool enabled)
Definition: btTypedConstraint.h:212
btMultiBodyConstraintSolver::convertMultiBodyContact
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:943
btMultiBody::applyDeltaVeeMultiDof2
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:383
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:1261
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:99
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:560
btMultiBodyConstraintSolver::applyDeltaVee
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
Definition: btMultiBodyConstraintSolver.cpp:122
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:122
btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, btManifoldPoint &cp, btScalar combinedTorsionalFriction, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:600
btManifoldPoint::m_combinedFriction
btScalar m_combinedFriction
Definition: btManifoldPoint.h:103
btMultiBodySolverConstraint::m_linkB
int m_linkB
Definition: btMultiBodySolverConstraint.h:75
btMultiBodyConstraintSolver::m_tmpMultiBodyConstraints
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:43
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:141
btContactSolverInfoData::m_linearSlop
btScalar m_linearSlop
Definition: btContactSolverInfo.h:56
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
btMultiBody::fillConstraintJacobianMultiDof
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
Definition: btMultiBody.cpp:1675
btRigidBody::getTotalForce
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:287
btSolverBody::internalGetDeltaAngularVelocity
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:213
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:274
btTypedConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don&#39;t use them directly
Definition: btTypedConstraint.h:186
btMultiBody::addBaseConstraintForce
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:316
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:439
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
Definition: btContactSolverInfo.h:27
btManifoldPoint::m_lateralFrictionDir2
btVector3 m_lateralFrictionDir2
Definition: btManifoldPoint.h:141
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
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btSolverBody.h
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:75
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
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:320
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:258
btSequentialImpulseConstraintSolver::applyAnisotropicFriction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
Definition: btSequentialImpulseConstraintSolver.cpp:523
btMultiBodySolverConstraint::m_frictionIndex
int m_frictionIndex
Definition: btMultiBodySolverConstraint.h:67
btMultiBodySolverConstraint::m_cfm
btScalar m_cfm
Definition: btMultiBodySolverConstraint.h:55
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:146
btContactSolverInfoData::m_warmstartingFactor
btScalar m_warmstartingFactor
Definition: btContactSolverInfo.h:57
btContactSolverInfoData::m_restitutionVelocityThreshold
btScalar m_restitutionVelocityThreshold
Definition: btContactSolverInfo.h:65
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
btManifoldPoint::m_combinedSpinningFriction
btScalar m_combinedSpinningFriction
Definition: btManifoldPoint.h:105
btMultiBodyConstraintSolver::m_data
btMultiBodyJacobianData m_data
Definition: btMultiBodyConstraintSolver.h:40
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:59
btMultiBodySolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btMultiBodySolverConstraint.h:57
btContactSolverInfoData::m_sor
btScalar m_sor
Definition: btContactSolverInfo.h:46
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION
Definition: btCollisionObject.h:163
btContactSolverInfo.h
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:475