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