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