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 #include "btMultiBodyConstraintSolver.h"
17 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
18 #include "btMultiBodyLinkCollider.h"
19
20 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
21 #include "btMultiBodyConstraint.h"
22 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
23
24 #include "LinearMath/btQuickprof.h"
25 #include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
26 #include "LinearMath/btScalar.h"
27
28 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
29 {
30 btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
31
32 //solve featherstone non-contact constraints
33
34 //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
35
36 for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
37 {
38 int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
39
40 btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
41
42 btScalar residual = resolveSingleConstraintRowGeneric(constraint);
43 leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
44
45 if (constraint.m_multiBodyA)
46 constraint.m_multiBodyA->setPosUpdated(false);
47 if (constraint.m_multiBodyB)
48 constraint.m_multiBodyB->setPosUpdated(false);
49 }
50
51 //solve featherstone normal contact
52 for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
53 {
54 int index = j0; //iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
55
56 btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
57 btScalar residual = 0.f;
58
59 if (iteration < infoGlobal.m_numIterations)
60 {
61 residual = resolveSingleConstraintRowGeneric(constraint);
62 }
63
64 leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
65
66 if (constraint.m_multiBodyA)
67 constraint.m_multiBodyA->setPosUpdated(false);
68 if (constraint.m_multiBodyB)
69 constraint.m_multiBodyB->setPosUpdated(false);
70 }
71
72 //solve featherstone frictional contact
73 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
74 {
75 for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++)
76 {
77 if (iteration < infoGlobal.m_numIterations)
78 {
79 int index = j1;
80
81 btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[index];
82 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
83 //adjust friction limits here
84 if (totalImpulse > btScalar(0))
85 {
86 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
87 frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
88 btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
89 leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
90
91 if (frictionConstraint.m_multiBodyA)
92 frictionConstraint.m_multiBodyA->setPosUpdated(false);
93 if (frictionConstraint.m_multiBodyB)
94 frictionConstraint.m_multiBodyB->setPosUpdated(false);
95 }
96 }
97 }
98
99 for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
100 {
101 if (iteration < infoGlobal.m_numIterations)
102 {
103 int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
104
105 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
106 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
107 j1++;
108 int index2 = j1;
109 btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2];
110 //adjust friction limits here
111 if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
112 {
113 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
114 frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
115 frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
116 frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
117
118 btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
119 leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
120
121 if (frictionConstraint.m_multiBodyA)
122 frictionConstraint.m_multiBodyA->setPosUpdated(false);
123 if (frictionConstraint.m_multiBodyB)
124 frictionConstraint.m_multiBodyB->setPosUpdated(false);
125
126 if (frictionConstraintB.m_multiBodyA)
127 frictionConstraintB.m_multiBodyA->setPosUpdated(false);
128 if (frictionConstraintB.m_multiBodyB)
129 frictionConstraintB.m_multiBodyB->setPosUpdated(false);
130 }
131 }
132 }
133
134 for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
135 {
136 if (iteration < infoGlobal.m_numIterations)
137 {
138 int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
139 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
140
141 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
142 j1++;
143 int index2 = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
144 btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2];
145 btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex);
146
147 if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
148 {
149 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
150 frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
151 frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
152 frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
153 btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
154 leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
155
156 if (frictionConstraintB.m_multiBodyA)
157 frictionConstraintB.m_multiBodyA->setPosUpdated(false);
158 if (frictionConstraintB.m_multiBodyB)
159 frictionConstraintB.m_multiBodyB->setPosUpdated(false);
160
161 if (frictionConstraint.m_multiBodyA)
162 frictionConstraint.m_multiBodyA->setPosUpdated(false);
163 if (frictionConstraint.m_multiBodyB)
164 frictionConstraint.m_multiBodyB->setPosUpdated(false);
165 }
166 }
167 }
168 }
169 else
170 {
171 for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
172 {
173 if (iteration < infoGlobal.m_numIterations)
174 {
175 int index = j1; //iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
176
177 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index];
178 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
179 //adjust friction limits here
180 if (totalImpulse > btScalar(0))
181 {
182 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
183 frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
184 btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
185 leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
186
187 if (frictionConstraint.m_multiBodyA)
188 frictionConstraint.m_multiBodyA->setPosUpdated(false);
189 if (frictionConstraint.m_multiBodyB)
190 frictionConstraint.m_multiBodyB->setPosUpdated(false);
191 }
192 }
193 }
194 }
195 return leastSquaredResidual;
196 }
197
198 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
199 {
200 m_multiBodyNonContactConstraints.resize(0);
201 m_multiBodyNormalContactConstraints.resize(0);
202 m_multiBodyFrictionContactConstraints.resize(0);
203 m_multiBodyTorsionalFrictionContactConstraints.resize(0);
204 m_multiBodySpinningFrictionContactConstraints.resize(0);
205
206 m_data.m_jacobians.resize(0);
207 m_data.m_deltaVelocitiesUnitImpulse.resize(0);
208 m_data.m_deltaVelocities.resize(0);
209
210 for (int i = 0; i < numBodies; i++)
211 {
212 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
213 if (fcA)
214 {
215 fcA->m_multiBody->setCompanionId(-1);
216 }
217 }
218
219 btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
220
221 return val;
222 }
223
224 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
225 {
226 for (int i = 0; i < ndof; ++i)
227 m_data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
228 }
229
230 btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
231 {
232 btScalar deltaImpulse = c.m_rhs - btScalar(c.m_appliedImpulse) * c.m_cfm;
233 btScalar deltaVelADotn = 0;
234 btScalar deltaVelBDotn = 0;
235 btSolverBody* bodyA = 0;
236 btSolverBody* bodyB = 0;
237 int ndofA = 0;
238 int ndofB = 0;
239
240 if (c.m_multiBodyA)
241 {
242 ndofA = c.m_multiBodyA->getNumDofs() + 6;
243 for (int i = 0; i < ndofA; ++i)
244 deltaVelADotn += m_data.m_jacobians[c.m_jacAindex + i] * m_data.m_deltaVelocities[c.m_deltaVelAindex + i];
245 }
246 else if (c.m_solverBodyIdA >= 0)
247 {
248 bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
249 deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
250 }
251
252 if (c.m_multiBodyB)
253 {
254 ndofB = c.m_multiBodyB->getNumDofs() + 6;
255 for (int i = 0; i < ndofB; ++i)
256 deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex + i] * m_data.m_deltaVelocities[c.m_deltaVelBindex + i];
257 }
258 else if (c.m_solverBodyIdB >= 0)
259 {
260 bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
261 deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
262 }
263
264 deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
265 deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv;
266 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
267
268 if (sum < c.m_lowerLimit)
269 {
270 deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
271 c.m_appliedImpulse = c.m_lowerLimit;
272 }
273 else if (sum > c.m_upperLimit)
274 {
275 deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
276 c.m_appliedImpulse = c.m_upperLimit;
277 }
278 else
279 {
280 c.m_appliedImpulse = sum;
281 }
282
283 if (c.m_multiBodyA)
284 {
285 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
286 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
287 //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
288 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
289 c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse);
290 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
291 }
292 else if (c.m_solverBodyIdA >= 0)
293 {
294 bodyA->internalApplyImpulse(c.m_contactNormal1 * bodyA->internalGetInvMass(), c.m_angularComponentA, deltaImpulse);
295 }
296 if (c.m_multiBodyB)
297 {
298 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse, c.m_deltaVelBindex, ndofB);
299 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
300 //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
301 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
302 c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], deltaImpulse);
303 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
304 }
305 else if (c.m_solverBodyIdB >= 0)
306 {
307 bodyB->internalApplyImpulse(c.m_contactNormal2 * bodyB->internalGetInvMass(), c.m_angularComponentB, deltaImpulse);
308 }
309 btScalar deltaVel = deltaImpulse / c.m_jacDiagABInv;
310 return deltaVel;
311 }
312
313 btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint& cA1, const btMultiBodySolverConstraint& cB)
314 {
315 int ndofA = 0;
316 int ndofB = 0;
317 btSolverBody* bodyA = 0;
318 btSolverBody* bodyB = 0;
319 btScalar deltaImpulseB = 0.f;
320 btScalar sumB = 0.f;
321 {
322 deltaImpulseB = cB.m_rhs - btScalar(cB.m_appliedImpulse) * cB.m_cfm;
323 btScalar deltaVelADotn = 0;
324 btScalar deltaVelBDotn = 0;
325 if (cB.m_multiBodyA)
326 {
327 ndofA = cB.m_multiBodyA->getNumDofs() + 6;
328 for (int i = 0; i < ndofA; ++i)
329 deltaVelADotn += m_data.m_jacobians[cB.m_jacAindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelAindex + i];
330 }
331 else if (cB.m_solverBodyIdA >= 0)
332 {
333 bodyA = &m_tmpSolverBodyPool[cB.m_solverBodyIdA];
334 deltaVelADotn += cB.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cB.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
335 }
336
337 if (cB.m_multiBodyB)
338 {
339 ndofB = cB.m_multiBodyB->getNumDofs() + 6;
340 for (int i = 0; i < ndofB; ++i)
341 deltaVelBDotn += m_data.m_jacobians[cB.m_jacBindex + i] * m_data.m_deltaVelocities[cB.m_deltaVelBindex + i];
342 }
343 else if (cB.m_solverBodyIdB >= 0)
344 {
345 bodyB = &m_tmpSolverBodyPool[cB.m_solverBodyIdB];
346 deltaVelBDotn += cB.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cB.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
347 }
348
349 deltaImpulseB -= deltaVelADotn * cB.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
350 deltaImpulseB -= deltaVelBDotn * cB.m_jacDiagABInv;
351 sumB = btScalar(cB.m_appliedImpulse) + deltaImpulseB;
352 }
353
354 btScalar deltaImpulseA = 0.f;
355 btScalar sumA = 0.f;
356 const btMultiBodySolverConstraint& cA = cA1;
357 {
358 {
359 deltaImpulseA = cA.m_rhs - btScalar(cA.m_appliedImpulse) * cA.m_cfm;
360 btScalar deltaVelADotn = 0;
361 btScalar deltaVelBDotn = 0;
362 if (cA.m_multiBodyA)
363 {
364 ndofA = cA.m_multiBodyA->getNumDofs() + 6;
365 for (int i = 0; i < ndofA; ++i)
366 deltaVelADotn += m_data.m_jacobians[cA.m_jacAindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelAindex + i];
367 }
368 else if (cA.m_solverBodyIdA >= 0)
369 {
370 bodyA = &m_tmpSolverBodyPool[cA.m_solverBodyIdA];
371 deltaVelADotn += cA.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + cA.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
372 }
373
374 if (cA.m_multiBodyB)
375 {
376 ndofB = cA.m_multiBodyB->getNumDofs() + 6;
377 for (int i = 0; i < ndofB; ++i)
378 deltaVelBDotn += m_data.m_jacobians[cA.m_jacBindex + i] * m_data.m_deltaVelocities[cA.m_deltaVelBindex + i];
379 }
380 else if (cA.m_solverBodyIdB >= 0)
381 {
382 bodyB = &m_tmpSolverBodyPool[cA.m_solverBodyIdB];
383 deltaVelBDotn += cA.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + cA.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
384 }
385
386 deltaImpulseA -= deltaVelADotn * cA.m_jacDiagABInv; //m_jacDiagABInv = 1./denom
387 deltaImpulseA -= deltaVelBDotn * cA.m_jacDiagABInv;
388 sumA = btScalar(cA.m_appliedImpulse) + deltaImpulseA;
389 }
390 }
391
392 if (sumA * sumA + sumB * sumB >= cA.m_lowerLimit * cB.m_lowerLimit)
393 {
394 btScalar angle = btAtan2(sumA, sumB);
395 btScalar sumAclipped = btFabs(cA.m_lowerLimit * btSin(angle));
396 btScalar sumBclipped = btFabs(cB.m_lowerLimit * btCos(angle));
397
398 if (sumA < -sumAclipped)
399 {
400 deltaImpulseA = -sumAclipped - cA.m_appliedImpulse;
401 cA.m_appliedImpulse = -sumAclipped;
402 }
403 else if (sumA > sumAclipped)
404 {
405 deltaImpulseA = sumAclipped - cA.m_appliedImpulse;
406 cA.m_appliedImpulse = sumAclipped;
407 }
408 else
409 {
410 cA.m_appliedImpulse = sumA;
411 }
412
413 if (sumB < -sumBclipped)
414 {
415 deltaImpulseB = -sumBclipped - cB.m_appliedImpulse;
416 cB.m_appliedImpulse = -sumBclipped;
417 }
418 else if (sumB > sumBclipped)
419 {
420 deltaImpulseB = sumBclipped - cB.m_appliedImpulse;
421 cB.m_appliedImpulse = sumBclipped;
422 }
423 else
424 {
425 cB.m_appliedImpulse = sumB;
426 }
427 //deltaImpulseA = sumAclipped-cA.m_appliedImpulse;
428 //cA.m_appliedImpulse = sumAclipped;
429 //deltaImpulseB = sumBclipped-cB.m_appliedImpulse;
430 //cB.m_appliedImpulse = sumBclipped;
431 }
432 else
433 {
434 cA.m_appliedImpulse = sumA;
435 cB.m_appliedImpulse = sumB;
436 }
437
438 if (cA.m_multiBodyA)
439 {
440 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA, cA.m_deltaVelAindex, ndofA);
441 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
442 //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
443 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
444 cA.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacAindex], deltaImpulseA);
445 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
446 }
447 else if (cA.m_solverBodyIdA >= 0)
448 {
449 bodyA->internalApplyImpulse(cA.m_contactNormal1 * bodyA->internalGetInvMass(), cA.m_angularComponentA, deltaImpulseA);
450 }
451 if (cA.m_multiBodyB)
452 {
453 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA, cA.m_deltaVelBindex, ndofB);
454 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
455 //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
456 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
457 cA.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cA.m_jacBindex], deltaImpulseA);
458 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
459 }
460 else if (cA.m_solverBodyIdB >= 0)
461 {
462 bodyB->internalApplyImpulse(cA.m_contactNormal2 * bodyB->internalGetInvMass(), cA.m_angularComponentB, deltaImpulseA);
463 }
464
465 if (cB.m_multiBodyA)
466 {
467 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB, cB.m_deltaVelAindex, ndofA);
468 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
469 //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
470 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
471 cB.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacAindex], deltaImpulseB);
472 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
473 }
474 else if (cB.m_solverBodyIdA >= 0)
475 {
476 bodyA->internalApplyImpulse(cB.m_contactNormal1 * bodyA->internalGetInvMass(), cB.m_angularComponentA, deltaImpulseB);
477 }
478 if (cB.m_multiBodyB)
479 {
480 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB, cB.m_deltaVelBindex, ndofB);
481 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
482 //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
483 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
484 cB.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[cB.m_jacBindex], deltaImpulseB);
485 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
486 }
487 else if (cB.m_solverBodyIdB >= 0)
488 {
489 bodyB->internalApplyImpulse(cB.m_contactNormal2 * bodyB->internalGetInvMass(), cB.m_angularComponentB, deltaImpulseB);
490 }
491
492 btScalar deltaVel = deltaImpulseA / cA.m_jacDiagABInv + deltaImpulseB / cB.m_jacDiagABInv;
493 return deltaVel;
494 }
495
496 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
497 {
498 BT_PROFILE("setupMultiBodyContactConstraint");
499 btVector3 rel_pos1;
500 btVector3 rel_pos2;
501
502 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
503 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
504
505 const btVector3& pos1 = cp.getPositionWorldOnA();
506 const btVector3& pos2 = cp.getPositionWorldOnB();
507
508 btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
509 btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
510
511 btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
512 btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
513
514 if (bodyA)
515 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
516 if (bodyB)
517 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
518
519 relaxation = infoGlobal.m_sor;
520
521 btScalar invTimeStep = btScalar(1) / infoGlobal.m_timeStep;
522
523 //cfm = 1 / ( dt * kp + kd )
524 //erp = dt * kp / ( dt * kp + kd )
525
526 btScalar cfm;
527 btScalar erp;
528 if (isFriction)
529 {
530 cfm = infoGlobal.m_frictionCFM;
531 erp = infoGlobal.m_frictionERP;
532 }
533 else
534 {
535 cfm = infoGlobal.m_globalCfm;
536 erp = infoGlobal.m_erp2;
537
538 if ((cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP))
539 {
540 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_CFM)
541 cfm = cp.m_contactCFM;
542 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_HAS_CONTACT_ERP)
543 erp = cp.m_contactERP;
544 }
545 else
546 {
547 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
548 {
549 btScalar denom = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1);
550 if (denom < SIMD_EPSILON)
551 {
552 denom = SIMD_EPSILON;
553 }
554 cfm = btScalar(1) / denom;
555 erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
556 }
557 }
558 }
559
560 cfm *= invTimeStep;
561
562 if (multiBodyA)
563 {
564 if (solverConstraint.m_linkA < 0)
565 {
566 rel_pos1 = pos1 - multiBodyA->getBasePos();
567 }
568 else
569 {
570 rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
571 }
572 const int ndofA = multiBodyA->getNumDofs() + 6;
573
574 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
575
576 if (solverConstraint.m_deltaVelAindex < 0)
577 {
578 solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
579 multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
580 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
581 }
582 else
583 {
584 btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
585 }
586
587 solverConstraint.m_jacAindex = m_data.m_jacobians.size();
588 m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
589 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
590 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
591
592 btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
593 multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
594 btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
595 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
596
597 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
598 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
599 solverConstraint.m_contactNormal1 = contactNormal;
600 }
601 else
602 {
603 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
604 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
605 solverConstraint.m_contactNormal1 = contactNormal;
606 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
607 }
608
609 if (multiBodyB)
610 {
611 if (solverConstraint.m_linkB < 0)
612 {
613 rel_pos2 = pos2 - multiBodyB->getBasePos();
614 }
615 else
616 {
617 rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
618 }
619
620 const int ndofB = multiBodyB->getNumDofs() + 6;
621
622 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
623 if (solverConstraint.m_deltaVelBindex < 0)
624 {
625 solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
626 multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
627 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
628 }
629
630 solverConstraint.m_jacBindex = m_data.m_jacobians.size();
631
632 m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
633 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
634 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
635
636 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);
637 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
638
639 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
640 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
641 solverConstraint.m_contactNormal2 = -contactNormal;
642 }
643 else
644 {
645 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
646 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
647 solverConstraint.m_contactNormal2 = -contactNormal;
648
649 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
650 }
651
652 {
653 btVector3 vec;
654 btScalar denom0 = 0.f;
655 btScalar denom1 = 0.f;
656 btScalar* jacB = 0;
657 btScalar* jacA = 0;
658 btScalar* lambdaA = 0;
659 btScalar* lambdaB = 0;
660 int ndofA = 0;
661 if (multiBodyA)
662 {
663 ndofA = multiBodyA->getNumDofs() + 6;
664 jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
665 lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
666 for (int i = 0; i < ndofA; ++i)
667 {
668 btScalar j = jacA[i];
669 btScalar l = lambdaA[i];
670 denom0 += j * l;
671 }
672 }
673 else
674 {
675 if (rb0)
676 {
677 vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
678 denom0 = rb0->getInvMass() + contactNormal.dot(vec);
679 }
680 }
681 if (multiBodyB)
682 {
683 const int ndofB = multiBodyB->getNumDofs() + 6;
684 jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
685 lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
686 for (int i = 0; i < ndofB; ++i)
687 {
688 btScalar j = jacB[i];
689 btScalar l = lambdaB[i];
690 denom1 += j * l;
691 }
692 }
693 else
694 {
695 if (rb1)
696 {
697 vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
698 denom1 = rb1->getInvMass() + contactNormal.dot(vec);
699 }
700 }
701
702 btScalar d = denom0 + denom1 + cfm;
703 if (d > SIMD_EPSILON)
704 {
705 solverConstraint.m_jacDiagABInv = relaxation / (d);
706 }
707 else
708 {
709 //disable the constraint row to handle singularity/redundant constraint
710 solverConstraint.m_jacDiagABInv = 0.f;
711 }
712 }
713
714 //compute rhs and remaining solverConstraint fields
715
716 btScalar restitution = 0.f;
717 btScalar distance = 0;
718 if (!isFriction)
719 {
720 distance = cp.getDistance() + infoGlobal.m_linearSlop;
721 }
722 else
723 {
724 if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
725 {
726 distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
727 }
728 }
729
730 btScalar rel_vel = 0.f;
731 int ndofA = 0;
732 int ndofB = 0;
733 {
734 btVector3 vel1, vel2;
735 if (multiBodyA)
736 {
737 ndofA = multiBodyA->getNumDofs() + 6;
738 btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
739 for (int i = 0; i < ndofA; ++i)
740 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
741 }
742 else
743 {
744 if (rb0)
745 {
746 rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) +
747 (rb0->getTotalTorque() * rb0->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos1) +
748 rb0->getTotalForce() * rb0->getInvMass() * infoGlobal.m_timeStep)
749 .dot(solverConstraint.m_contactNormal1);
750 }
751 }
752 if (multiBodyB)
753 {
754 ndofB = multiBodyB->getNumDofs() + 6;
755 btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
756 for (int i = 0; i < ndofB; ++i)
757 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
758 }
759 else
760 {
761 if (rb1)
762 {
763 rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2) +
764 (rb1->getTotalTorque() * rb1->getInvInertiaTensorWorld() * infoGlobal.m_timeStep).cross(rel_pos2) +
765 rb1->getTotalForce() * rb1->getInvMass() * infoGlobal.m_timeStep)
766 .dot(solverConstraint.m_contactNormal2);
767 }
768 }
769
770 solverConstraint.m_friction = cp.m_combinedFriction;
771
772 if (!isFriction)
773 {
774 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
775 if (restitution <= btScalar(0.))
776 {
777 restitution = 0.f;
778 }
779 }
780 }
781
782 {
783 btScalar positionalError = 0.f;
784 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
785 if (isFriction)
786 {
787 positionalError = -distance * erp / infoGlobal.m_timeStep;
788 }
789 else
790 {
791 if (distance > 0)
792 {
793 positionalError = 0;
794 velocityError -= distance / infoGlobal.m_timeStep;
795 }
796 else
797 {
798 positionalError = -distance * erp / infoGlobal.m_timeStep;
799 }
800 }
801
802 btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
803 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
804
805 if (!isFriction)
806 {
807 // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
808 {
809 //combine position and velocity into rhs
810 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
811 solverConstraint.m_rhsPenetration = 0.f;
812 }
813 /*else
814 {
815 //split position and velocity into rhs and m_rhsPenetration
816 solverConstraint.m_rhs = velocityImpulse;
817 solverConstraint.m_rhsPenetration = penetrationImpulse;
818 }
819 */
820 solverConstraint.m_lowerLimit = 0;
821 solverConstraint.m_upperLimit = 1e10f;
822 }
823 else
824 {
825 solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
826 solverConstraint.m_rhsPenetration = 0.f;
827 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
828 solverConstraint.m_upperLimit = solverConstraint.m_friction;
829 }
830
831 solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
832 }
833
834 if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
835 {
836 if (btFabs(cp.m_prevRHS) > 1e-5 && cp.m_prevRHS < 2* solverConstraint.m_rhs && solverConstraint.m_rhs < 2*cp.m_prevRHS)
837 {
838 solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor;
839 if (solverConstraint.m_appliedImpulse < 0)
840 solverConstraint.m_appliedImpulse = 0;
841 }
842 else
843 {
844 solverConstraint.m_appliedImpulse = 0.f;
845 }
846
847 if (solverConstraint.m_appliedImpulse)
848 {
849 if (multiBodyA)
850 {
851 btScalar impulse = solverConstraint.m_appliedImpulse;
852 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
853 multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
854
855 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
856 }
857 else
858 {
859 if (rb0)
860 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
861 }
862 if (multiBodyB)
863 {
864 btScalar impulse = solverConstraint.m_appliedImpulse;
865 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
866 multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
867 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
868 }
869 else
870 {
871 if (rb1)
872 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
873 }
874 }
875 }
876 else
877 {
878 solverConstraint.m_appliedImpulse = 0.f;
879 solverConstraint.m_appliedPushImpulse = 0.f;
880 }
881 }
882
883 void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
884 const btVector3& constraintNormal,
885 btManifoldPoint& cp,
886 btScalar combinedTorsionalFriction,
887 const btContactSolverInfo& infoGlobal,
888 btScalar& relaxation,
889 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
890 {
891 BT_PROFILE("setupMultiBodyRollingFrictionConstraint");
892 btVector3 rel_pos1;
893 btVector3 rel_pos2;
894
895 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
896 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
897
898 const btVector3& pos1 = cp.getPositionWorldOnA();
899 const btVector3& pos2 = cp.getPositionWorldOnB();
900
901 btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
902 btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
903
904 btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
905 btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
906
907 if (bodyA)
908 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
909 if (bodyB)
910 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
911
912 relaxation = infoGlobal.m_sor;
913
914 // btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
915
916 if (multiBodyA)
917 {
918 if (solverConstraint.m_linkA < 0)
919 {
920 rel_pos1 = pos1 - multiBodyA->getBasePos();
921 }
922 else
923 {
924 rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
925 }
926 const int ndofA = multiBodyA->getNumDofs() + 6;
927
928 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
929
930 if (solverConstraint.m_deltaVelAindex < 0)
931 {
932 solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
933 multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
934 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofA);
935 }
936 else
937 {
938 btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
939 }
940
941 solverConstraint.m_jacAindex = m_data.m_jacobians.size();
942 m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofA);
943 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofA);
944 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
945
946 btScalar* jac1 = &m_data.m_jacobians[solverConstraint.m_jacAindex];
947 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);
948 btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
949 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex], delta, m_data.scratch_r, m_data.scratch_v);
950
951 btVector3 torqueAxis0 = constraintNormal;
952 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
953 solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
954 }
955 else
956 {
957 btVector3 torqueAxis0 = constraintNormal;
958 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
959 solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
960 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
961 }
962
963 if (multiBodyB)
964 {
965 if (solverConstraint.m_linkB < 0)
966 {
967 rel_pos2 = pos2 - multiBodyB->getBasePos();
968 }
969 else
970 {
971 rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
972 }
973
974 const int ndofB = multiBodyB->getNumDofs() + 6;
975
976 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
977 if (solverConstraint.m_deltaVelBindex < 0)
978 {
979 solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
980 multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
981 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size() + ndofB);
982 }
983
984 solverConstraint.m_jacBindex = m_data.m_jacobians.size();
985
986 m_data.m_jacobians.resize(m_data.m_jacobians.size() + ndofB);
987 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size() + ndofB);
988 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
989
990 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);
991 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex], &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v);
992
993 btVector3 torqueAxis1 = -constraintNormal;
994 solverConstraint.m_relpos2CrossNormal = torqueAxis1;
995 solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
996 }
997 else
998 {
999 btVector3 torqueAxis1 = -constraintNormal;
1000 solverConstraint.m_relpos2CrossNormal = torqueAxis1;
1001 solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
1002
1003 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
1004 }
1005
1006 {
1007 btScalar denom0 = 0.f;
1008 btScalar denom1 = 0.f;
1009 btScalar* jacB = 0;
1010 btScalar* jacA = 0;
1011 btScalar* lambdaA = 0;
1012 btScalar* lambdaB = 0;
1013 int ndofA = 0;
1014 if (multiBodyA)
1015 {
1016 ndofA = multiBodyA->getNumDofs() + 6;
1017 jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
1018 lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1019 for (int i = 0; i < ndofA; ++i)
1020 {
1021 btScalar j = jacA[i];
1022 btScalar l = lambdaA[i];
1023 denom0 += j * l;
1024 }
1025 }
1026 else
1027 {
1028 if (rb0)
1029 {
1030 btVector3 iMJaA = rb0 ? rb0->getInvInertiaTensorWorld() * solverConstraint.m_relpos1CrossNormal : btVector3(0, 0, 0);
1031 denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1032 }
1033 }
1034 if (multiBodyB)
1035 {
1036 const int ndofB = multiBodyB->getNumDofs() + 6;
1037 jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
1038 lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1039 for (int i = 0; i < ndofB; ++i)
1040 {
1041 btScalar j = jacB[i];
1042 btScalar l = lambdaB[i];
1043 denom1 += j * l;
1044 }
1045 }
1046 else
1047 {
1048 if (rb1)
1049 {
1050 btVector3 iMJaB = rb1 ? rb1->getInvInertiaTensorWorld() * solverConstraint.m_relpos2CrossNormal : btVector3(0, 0, 0);
1051 denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1052 }
1053 }
1054
1055 btScalar d = denom0 + denom1 + infoGlobal.m_globalCfm;
1056 if (d > SIMD_EPSILON)
1057 {
1058 solverConstraint.m_jacDiagABInv = relaxation / (d);
1059 }
1060 else
1061 {
1062 //disable the constraint row to handle singularity/redundant constraint
1063 solverConstraint.m_jacDiagABInv = 0.f;
1064 }
1065 }
1066
1067 //compute rhs and remaining solverConstraint fields
1068
1069 btScalar restitution = 0.f;
1070 btScalar penetration = isFriction ? 0 : cp.getDistance();
1071
1072 btScalar rel_vel = 0.f;
1073 int ndofA = 0;
1074 int ndofB = 0;
1075 {
1076 btVector3 vel1, vel2;
1077 if (multiBodyA)
1078 {
1079 ndofA = multiBodyA->getNumDofs() + 6;
1080 btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
1081 for (int i = 0; i < ndofA; ++i)
1082 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
1083 }
1084 else
1085 {
1086 if (rb0)
1087 {
1088 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
1089 rel_vel += solverConstraint.m_contactNormal1.dot(rb0 ? solverBodyA->m_linearVelocity + solverBodyA->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos1CrossNormal.dot(rb0 ? solverBodyA->m_angularVelocity : btVector3(0, 0, 0));
1090 }
1091 }
1092 if (multiBodyB)
1093 {
1094 ndofB = multiBodyB->getNumDofs() + 6;
1095 btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
1096 for (int i = 0; i < ndofB; ++i)
1097 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
1098 }
1099 else
1100 {
1101 if (rb1)
1102 {
1103 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
1104 rel_vel += solverConstraint.m_contactNormal2.dot(rb1 ? solverBodyB->m_linearVelocity + solverBodyB->m_externalForceImpulse : btVector3(0, 0, 0)) + solverConstraint.m_relpos2CrossNormal.dot(rb1 ? solverBodyB->m_angularVelocity : btVector3(0, 0, 0));
1105 }
1106 }
1107
1108 solverConstraint.m_friction = combinedTorsionalFriction;
1109
1110 if (!isFriction)
1111 {
1112 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution, infoGlobal.m_restitutionVelocityThreshold);
1113 if (restitution <= btScalar(0.))
1114 {
1115 restitution = 0.f;
1116 }
1117 }
1118 }
1119
1120 solverConstraint.m_appliedImpulse = 0.f;
1121 solverConstraint.m_appliedPushImpulse = 0.f;
1122
1123 {
1124 btScalar velocityError = 0 - rel_vel; // * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
1125
1126 btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
1127
1128 solverConstraint.m_rhs = velocityImpulse;
1129 solverConstraint.m_rhsPenetration = 0.f;
1130 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
1131 solverConstraint.m_upperLimit = solverConstraint.m_friction;
1132
1133 solverConstraint.m_cfm = infoGlobal.m_globalCfm * solverConstraint.m_jacDiagABInv;
1134 }
1135 }
1136
1137 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1138 {
1139 BT_PROFILE("addMultiBodyFrictionConstraint");
1140 btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
1141 solverConstraint.m_orgConstraint = 0;
1142 solverConstraint.m_orgDofIndex = -1;
1143
1144 solverConstraint.m_frictionIndex = frictionIndex;
1145 bool isFriction = true;
1146
1147 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1148 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1149
1150 btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1151 btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1152
1153 int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1154 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1155
1156 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1157 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1158 solverConstraint.m_multiBodyA = mbA;
1159 if (mbA)
1160 solverConstraint.m_linkA = fcA->m_link;
1161
1162 solverConstraint.m_multiBodyB = mbB;
1163 if (mbB)
1164 solverConstraint.m_linkB = fcB->m_link;
1165
1166 solverConstraint.m_originalContactPoint = &cp;
1167
1168 setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1169 return solverConstraint;
1170 }
1171
1172 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
1173 btScalar combinedTorsionalFriction,
1174 btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1175 {
1176 BT_PROFILE("addMultiBodyRollingFrictionConstraint");
1177
1178 bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0));
1179
1180 btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction ? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing();
1181 solverConstraint.m_orgConstraint = 0;
1182 solverConstraint.m_orgDofIndex = -1;
1183
1184 solverConstraint.m_frictionIndex = frictionIndex;
1185 bool isFriction = true;
1186
1187 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1188 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1189
1190 btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1191 btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1192
1193 int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1194 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1195
1196 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1197 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1198 solverConstraint.m_multiBodyA = mbA;
1199 if (mbA)
1200 solverConstraint.m_linkA = fcA->m_link;
1201
1202 solverConstraint.m_multiBodyB = mbB;
1203 if (mbB)
1204 solverConstraint.m_linkB = fcB->m_link;
1205
1206 solverConstraint.m_originalContactPoint = &cp;
1207
1208 setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1209 return solverConstraint;
1210 }
1211
1212 btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
1213 btScalar combinedTorsionalFriction,
1214 btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
1215 {
1216 BT_PROFILE("addMultiBodyRollingFrictionConstraint");
1217
1218 btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.expandNonInitializing();
1219 solverConstraint.m_orgConstraint = 0;
1220 solverConstraint.m_orgDofIndex = -1;
1221
1222 solverConstraint.m_frictionIndex = frictionIndex;
1223 bool isFriction = true;
1224
1225 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1226 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1227
1228 btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1229 btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1230
1231 int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1232 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1233
1234 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1235 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1236 solverConstraint.m_multiBodyA = mbA;
1237 if (mbA)
1238 solverConstraint.m_linkA = fcA->m_link;
1239
1240 solverConstraint.m_multiBodyB = mbB;
1241 if (mbB)
1242 solverConstraint.m_linkB = fcB->m_link;
1243
1244 solverConstraint.m_originalContactPoint = &cp;
1245
1246 setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
1247 return solverConstraint;
1248 }
1249 void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
1250 {
1251 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1252 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1253
1254 btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
1255 btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
1256
1257 btCollisionObject *colObj0 = 0, *colObj1 = 0;
1258
1259 colObj0 = (btCollisionObject*)manifold->getBody0();
1260 colObj1 = (btCollisionObject*)manifold->getBody1();
1261
1262 int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
1263 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
1264
1265 // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
1266 // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
1267
1269 // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
1270 // return;
1271
1272 //only a single rollingFriction per manifold
1273 int rollingFriction = 1;
1274
1275 for (int j = 0; j < manifold->getNumContacts(); j++)
1276 {
1277 btManifoldPoint& cp = manifold->getContactPoint(j);
1278
1279 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
1280 {
1281 btScalar relaxation;
1282
1283 int frictionIndex = m_multiBodyNormalContactConstraints.size();
1284
1285 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
1286
1287 // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
1288 // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
1289 solverConstraint.m_orgConstraint = 0;
1290 solverConstraint.m_orgDofIndex = -1;
1291 solverConstraint.m_solverBodyIdA = solverBodyIdA;
1292 solverConstraint.m_solverBodyIdB = solverBodyIdB;
1293 solverConstraint.m_multiBodyA = mbA;
1294 if (mbA)
1295 solverConstraint.m_linkA = fcA->m_link;
1296
1297 solverConstraint.m_multiBodyB = mbB;
1298 if (mbB)
1299 solverConstraint.m_linkB = fcB->m_link;
1300
1301 solverConstraint.m_originalContactPoint = &cp;
1302
1303 bool isFriction = false;
1304 setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
1305
1306 // const btVector3& pos1 = cp.getPositionWorldOnA();
1307 // const btVector3& pos2 = cp.getPositionWorldOnB();
1308
1310 #define ENABLE_FRICTION
1311 #ifdef ENABLE_FRICTION
1312 solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size();
1313
1318
1329
1330 btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
1331 cp.m_lateralFrictionDir1.normalize();
1332 cp.m_lateralFrictionDir2.normalize();
1333
1334 if (rollingFriction > 0)
1335 {
1336 if (cp.m_combinedSpinningFriction > 0)
1337 {
1338 addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
1339 }
1340 if (cp.m_combinedRollingFriction > 0)
1341 {
1342 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1343 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1344 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1345 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
1346
1347 addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1348 addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
1349 }
1350 rollingFriction--;
1351 }
1352 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags & BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
1353 { /*
1354 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
1355 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
1356 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
1357 {
1358 cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
1359 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1360 {
1361 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
1362 cp.m_lateralFrictionDir2.normalize();//??
1363 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1364 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1365 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1366
1367 }
1368
1369 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1370 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
1371 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
1372
1373 } else
1374 */
1375 {
1376 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1377 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1378 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1379
1380 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1381 {
1382 applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1383 applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
1384 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
1385 }
1386
1387 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
1388 {
1389 cp.m_contactPointFlags |= BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
1390 }
1391 }
1392 }
1393 else
1394 {
1395 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
1396
1397 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1398 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
1399 solverConstraint.m_appliedImpulse = 0.f;
1400 solverConstraint.m_appliedPushImpulse = 0.f;
1401 }
1402
1403 #endif //ENABLE_FRICTION
1404 }
1405 else
1406 {
1407 // Reset quantities related to warmstart as 0.
1408 cp.m_appliedImpulse = 0;
1409 cp.m_prevRHS = 0;
1410 }
1411 }
1412 }
1413
1414 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
1415 {
1416 for (int i = 0; i < numManifolds; i++)
1417 {
1418 btPersistentManifold* manifold = manifoldPtr[i];
1419 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
1420 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
1421 if (!fcA && !fcB)
1422 {
1423 //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
1424 convertContact(manifold, infoGlobal);
1425 }
1426 else
1427 {
1428 convertMultiBodyContact(manifold, infoGlobal);
1429 }
1430 }
1431
1432 //also convert the multibody constraints, if any
1433
1434 for (int i = 0; i < m_tmpNumMultiBodyConstraints; i++)
1435 {
1436 btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
1437 m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
1438 m_data.m_fixedBodyId = m_fixedBodyId;
1439
1440 c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal);
1441 }
1442
1443 // Warmstart for noncontact constraints
1444 if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING)
1445 {
1446 for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1447 {
1448 btMultiBodySolverConstraint& solverConstraint =
1449 m_multiBodyNonContactConstraints[i];
1450 solverConstraint.m_appliedImpulse =
1451 solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
1452 infoGlobal.m_articulatedWarmstartingFactor;
1453
1454 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
1455 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
1456 if (solverConstraint.m_appliedImpulse)
1457 {
1458 if (multiBodyA)
1459 {
1460 int ndofA = multiBodyA->getNumDofs() + 6;
1461 btScalar* deltaV =
1462 &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
1463 btScalar impulse = solverConstraint.m_appliedImpulse;
1464 multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
1465 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
1466 }
1467 if (multiBodyB)
1468 {
1469 int ndofB = multiBodyB->getNumDofs() + 6;
1470 btScalar* deltaV =
1471 &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
1472 btScalar impulse = solverConstraint.m_appliedImpulse;
1473 multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
1474 applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
1475 }
1476 }
1477 }
1478 }
1479 else
1480 {
1481 for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1482 {
1483 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1484 solverConstraint.m_appliedImpulse = 0;
1485 }
1486 }
1487 }
1488
1489 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
1490 {
1491 //printf("btMultiBodyConstraintSolver::solveGroup: numBodies=%d, numConstraints=%d\n", numBodies, numConstraints);
1492 return btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1493 }
1494
1495 #if 0
1496 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
1497 {
1498 if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
1499 {
1500 //todo: get rid of those temporary memory allocations for the joint feedback
1501 btAlignedObjectArray<btScalar> forceVector;
1502 int numDofsPlusBase = 6+mb->getNumDofs();
1503 forceVector.resize(numDofsPlusBase);
1504 for (int i=0;i<numDofsPlusBase;i++)
1505 {
1506 forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
1507 }
1508 btAlignedObjectArray<btScalar> output;
1509 output.resize(numDofsPlusBase);
1510 bool applyJointFeedback = true;
1511 mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
1512 }
1513 }
1514 #endif
1515
1516 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
1517 {
1518 #if 1
1519
1520 //bod->addBaseForce(m_gravity * bod->getBaseMass());
1521 //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
1522
1523 if (c.m_orgConstraint)
1524 {
1525 c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex, c.m_appliedImpulse);
1526 }
1527
1528 if (c.m_multiBodyA)
1529 {
1530 c.m_multiBodyA->setCompanionId(-1);
1531 btVector3 force = c.m_contactNormal1 * (c.m_appliedImpulse / deltaTime);
1532 btVector3 torque = c.m_relpos1CrossNormal * (c.m_appliedImpulse / deltaTime);
1533 if (c.m_linkA < 0)
1534 {
1535 c.m_multiBodyA->addBaseConstraintForce(force);
1536 c.m_multiBodyA->addBaseConstraintTorque(torque);
1537 }
1538 else
1539 {
1540 c.m_multiBodyA->addLinkConstraintForce(c.m_linkA, force);
1541 //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1542 c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA, torque);
1543 }
1544 }
1545
1546 if (c.m_multiBodyB)
1547 {
1548 {
1549 c.m_multiBodyB->setCompanionId(-1);
1550 btVector3 force = c.m_contactNormal2 * (c.m_appliedImpulse / deltaTime);
1551 btVector3 torque = c.m_relpos2CrossNormal * (c.m_appliedImpulse / deltaTime);
1552 if (c.m_linkB < 0)
1553 {
1554 c.m_multiBodyB->addBaseConstraintForce(force);
1555 c.m_multiBodyB->addBaseConstraintTorque(torque);
1556 }
1557 else
1558 {
1559 {
1560 c.m_multiBodyB->addLinkConstraintForce(c.m_linkB, force);
1561 //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
1562 c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB, torque);
1563 }
1564 }
1565 }
1566 }
1567 #endif
1568
1569 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
1570
1571 if (c.m_multiBodyA)
1572 {
1573 c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], c.m_appliedImpulse);
1574 }
1575
1576 if (c.m_multiBodyB)
1577 {
1578 c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex], c.m_appliedImpulse);
1579 }
1580 #endif
1581 }
1582
1583 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
1584 {
1585 BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
1586 int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
1587
1588 //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
1589 //or as applied force, so we can measure the joint reaction forces easier
1590 for (int i = 0; i < numPoolConstraints; i++)
1591 {
1592 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
1593 writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
1594
1595 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex], infoGlobal.m_timeStep);
1596
1597 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1598 {
1599 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1], infoGlobal.m_timeStep);
1600 }
1601 }
1602
1603 for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
1604 {
1605 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1606 writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep);
1607 }
1608
1609
1610 {
1611 BT_PROFILE("warm starting write back");
1612 for (int j = 0; j < numPoolConstraints; j++)
1613 {
1614 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
1615 btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint;
1616 btAssert(pt);
1617 pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1618 pt->m_prevRHS = solverConstraint.m_rhs;
1619 pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1620
1621 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1622 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1623 {
1624 pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse;
1625 } else
1626 {
1627 pt->m_appliedImpulseLateral2 = 0;
1628 }
1629 }
1630 }
1631
1632 #if 0
1633 //multibody joint feedback
1634 {
1635 BT_PROFILE("multi body joint feedback");
1636 for (int j=0;j<numPoolConstraints;j++)
1637 {
1638 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
1639
1640 //apply the joint feedback into all links of the btMultiBody
1641 //todo: double-check the signs of the applied impulse
1642
1643 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1644 {
1645 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1646 }
1647 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1648 {
1649 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1650 }
1651 #if 0
1652 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
1653 {
1654 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1655 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
1656 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
1657 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1658
1659 }
1660 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
1661 {
1662 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1663 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
1664 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
1665 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1666 }
1667
1668 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1669 {
1670 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
1671 {
1672 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1673 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
1674 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
1675 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1676 }
1677
1678 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
1679 {
1680 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1681 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
1682 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
1683 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1684 }
1685 }
1686 #endif
1687 }
1688
1689 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1690 {
1691 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1692 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1693 {
1694 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1695 }
1696 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1697 {
1698 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1699 }
1700 }
1701 }
1702
1703 numPoolConstraints = m_multiBodyNonContactConstraints.size();
1704
1705 #if 0
1706 //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1707 for (int i=0;i<numPoolConstraints;i++)
1708 {
1709 const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
1710
1711 btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
1712 btJointFeedback* fb = constr->getJointFeedback();
1713 if (fb)
1714 {
1715 fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1716 fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1717 fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1718 fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1719
1720 }
1721
1722 constr->internalSetAppliedImpulse(c.m_appliedImpulse);
1723 if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1724 {
1725 constr->setEnabled(false);
1726 }
1727
1728 }
1729 #endif
1730 #endif
1731
1732 return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1733 }
1734
1735 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)
1736 {
1737 //printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);
1738
1739 //printf("solveMultiBodyGroup start\n");
1740 m_tmpMultiBodyConstraints = multiBodyConstraints;
1741 m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1742
1743 btSequentialImpulseConstraintSolver::solveGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher);
1744
1745 m_tmpMultiBodyConstraints = 0;
1746 m_tmpNumMultiBodyConstraints = 0;
1747 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btTypedConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.h:181
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultiBodyConstraintSolver::convertMultiBodyContact
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:1249
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:76
btContactSolverInfoData::m_linearSlop
btScalar m_linearSlop
Definition: btContactSolverInfo.h:57
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:50
btMultiBodySolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btMultiBodySolverConstraint.h:50
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition: btSolverBody.h:120
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btManifoldPoint::m_lateralFrictionDir2
btVector3 m_lateralFrictionDir2
Definition: btManifoldPoint.h:140
btManifoldPoint::m_contactMotion2
btScalar m_contactMotion2
Definition: btManifoldPoint.h:123
btRigidBody::getTotalTorque
const btVector3 & getTotalTorque() const
Definition: btRigidBody.h:284
btMultiBody::addBaseConstraintTorque
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:339
btSolverBody::m_linearVelocity
btVector3 m_linearVelocity
Definition: btSolverBody.h:115
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:1854
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
btMultiBody::addLinkConstraintForce
void addLinkConstraintForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:674
btManifoldPoint::m_contactCFM
btScalar m_contactCFM
Definition: btManifoldPoint.h:126
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:61
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:48
btManifoldPoint::m_contactERP
btScalar m_contactERP
Definition: btManifoldPoint.h:131
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:212
btMultiBody::getCompanionId
int getCompanionId() const
Definition: btMultiBody.h:515
btManifoldPoint::m_contactPointFlags
int m_contactPointFlags
Definition: btManifoldPoint.h:116
BT_CONTACT_FLAG_HAS_CONTACT_CFM
@ BT_CONTACT_FLAG_HAS_CONTACT_CFM
Definition: btManifoldPoint.h:43
btMultiBodySolverConstraint::m_orgConstraint
btMultiBodyConstraint * m_orgConstraint
Definition: btMultiBodySolverConstraint.h:78
btContactSolverInfo
Definition: btContactSolverInfo.h:73
btManifoldPoint::getPositionWorldOnA
const btVector3 & getPositionWorldOnA() const
Definition: btManifoldPoint.h:151
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btMultiBody::internalNeedsJointFeedback
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:598
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:77
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
@ SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
Definition: btContactSolverInfo.h:28
btTypedConstraint::getBreakingImpulseThreshold
btScalar getBreakingImpulseThreshold() const
Definition: btTypedConstraint.h:191
btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:1583
btMultiBodySolverConstraint::m_deltaVelBindex
int m_deltaVelBindex
Definition: btMultiBodySolverConstraint.h:39
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:30
btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
Definition: btMultiBodyConstraintSolver.cpp:1516
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:100
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btMultiBodyConstraintSolver.h
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
SOLVER_USE_ARTICULATED_WARMSTARTING
@ SOLVER_USE_ARTICULATED_WARMSTARTING
Definition: btContactSolverInfo.h:34
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btMultiBodySolverConstraint.h:60
btSequentialImpulseConstraintSolver::restitutionCurve
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold)
Definition: btSequentialImpulseConstraintSolver.cpp:492
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:105
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:47
btSequentialImpulseConstraintSolver::applyAnisotropicFriction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
Definition: btSequentialImpulseConstraintSolver.cpp:502
btMultiBodyConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
Definition: btMultiBodyConstraint.h:126
btTypedConstraint::getJointFeedback
const btJointFeedback * getJointFeedback() const
Definition: btTypedConstraint.h:267
btSolverBody::m_externalForceImpulse
btVector3 m_externalForceImpulse
Definition: btSolverBody.h:117
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:120
btContactSolverInfoData::m_sor
btScalar m_sor
Definition: btContactSolverInfo.h:46
btMultiBody::addLinkConstraintTorque
void addLinkConstraintTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:679
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:883
btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint
btMultiBodySolverConstraint & addMultiBodyFrictionConstraint(const btVector3 &normalAxis, const btScalar &appliedImpulse, btPersistentManifold *manifold, int frictionIndex, btManifoldPoint &cp, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, const btContactSolverInfo &infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:1137
btMultiBodyConstraintSolver::m_data
btMultiBodyJacobianData m_data
Definition: btMultiBodyConstraintSolver.h:39
btJointFeedback
Definition: btTypedConstraint.h:65
btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION
@ CF_ANISOTROPIC_ROLLING_FRICTION
Definition: btCollisionObject.h:160
btMultiBodyConstraintSolver::m_multiBodySpinningFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints
Definition: btMultiBodyConstraintSolver.h:37
btContactSolverInfoData::m_frictionERP
btScalar m_frictionERP
Definition: btContactSolverInfo.h:51
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:243
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:142
btMultiBodySolverConstraint::m_deltaVelAindex
int m_deltaVelAindex
Definition: btMultiBodySolverConstraint.h:37
btScalar.h
btManifoldPoint::m_combinedRestitution
btScalar m_combinedRestitution
Definition: btManifoldPoint.h:106
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:1172
btMultiBodyConstraintSolver::m_multiBodyNormalContactConstraints
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
Definition: btMultiBodyConstraintSolver.h:34
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:28
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:1489
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:1522
btMultiBodySolverConstraint.h
btMultiBodyConstraintSolver::applyDeltaVee
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
Definition: btMultiBodyConstraintSolver.cpp:224
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:27
BT_CONTACT_FLAG_FRICTION_ANCHOR
@ BT_CONTACT_FLAG_FRICTION_ANCHOR
Definition: btManifoldPoint.h:46
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:29
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:141
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:218
btSolverBody::m_angularVelocity
btVector3 m_angularVelocity
Definition: btSolverBody.h:116
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:417
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:198
btSolverBody.h
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:74
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:499
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:30
btRigidBody::getLinearFactor
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:254
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
btJointFeedback::m_appliedForceBodyA
btVector3 m_appliedForceBodyA
Definition: btTypedConstraint.h:67
btManifoldPoint::m_combinedContactDamping1
btScalar m_combinedContactDamping1
Definition: btManifoldPoint.h:132
btMultiBodyConstraintSolver::m_tmpNumMultiBodyConstraints
int m_tmpNumMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:43
btSimdScalar
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later,...
Definition: btSolverBody.h:99
btMultiBodySolverConstraint::m_linkA
int m_linkA
Definition: btMultiBodySolverConstraint.h:71
btMultiBodyConstraintSolver::convertContacts
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:1414
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
Definition: btIDebugDraw.h:27
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition: btManifoldPoint.h:52
btManifoldPoint::m_contactMotion1
btScalar m_contactMotion1
Definition: btManifoldPoint.h:122
btManifoldPoint::m_combinedSpinningFriction
btScalar m_combinedSpinningFriction
Definition: btManifoldPoint.h:105
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
Definition: btMultiBody.cpp:1438
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btMultiBodyLinkCollider.h
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btPersistentManifold::getContactProcessingThreshold
btScalar getContactProcessingThreshold() const
Definition: btPersistentManifold.h:145
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:532
btMultiBodyConstraintSolver::m_multiBodyFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
Definition: btMultiBodyConstraintSolver.h:35
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1832
btContactSolverInfoData::m_frictionCFM
btScalar m_frictionCFM
Definition: btContactSolverInfo.h:52
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:498
btSolverBody::internalGetDeltaAngularVelocity
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:202
btManifoldPoint::m_frictionCFM
btScalar m_frictionCFM
Definition: btManifoldPoint.h:135
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
@ BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING
Definition: btManifoldPoint.h:45
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:33
btManifoldPoint::m_prevRHS
btScalar m_prevRHS
Definition: btManifoldPoint.h:119
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btSolverBody::internalGetDeltaLinearVelocity
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
Definition: btSolverBody.h:197
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btMultiBodyConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
btManifoldPoint::getPositionWorldOnB
const btVector3 & getPositionWorldOnB() const
Definition: btManifoldPoint.h:157
btContactSolverInfoData::m_globalCfm
btScalar m_globalCfm
Definition: btContactSolverInfo.h:50
btMultiBodySolverConstraint::m_jacBindex
int m_jacBindex
Definition: btMultiBodySolverConstraint.h:40
btManifoldPoint::m_combinedRollingFriction
btScalar m_combinedRollingFriction
Definition: btManifoldPoint.h:104
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:60
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:65
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btMultiBody
Definition: btMultiBody.h:51
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:419
btSequentialImpulseConstraintSolver::convertContact
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:995
BT_CONTACT_FLAG_HAS_CONTACT_ERP
@ BT_CONTACT_FLAG_HAS_CONTACT_ERP
Definition: btManifoldPoint.h:44
btSequentialImpulseConstraintSolver::m_fixedBodyId
int m_fixedBodyId
Definition: btSequentialImpulseConstraintSolver.h:67
btMultiBody::setCompanionId
void setCompanionId(int id)
Definition: btMultiBody.h:519
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:70
btMultiBodyConstraintSolver::m_multiBodyNonContactConstraints
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
Definition: btMultiBodyConstraintSolver.h:32
btManifoldPoint::m_appliedImpulseLateral1
btScalar m_appliedImpulseLateral1
Definition: btManifoldPoint.h:120
output
#define output
btAlignedObjectArray< btScalar >
btMultiBodyJacobianData::m_deltaVelocities
btAlignedObjectArray< btScalar > m_deltaVelocities
Definition: btMultiBodyConstraint.h:32
btMultiBodySolverConstraint::m_originalContactPoint
void * m_originalContactPoint
Definition: btMultiBodySolverConstraint.h:62
btMultiBodyConstraint::getAppliedImpulse
btScalar getAppliedImpulse(int dof)
Definition: btMultiBodyConstraint.h:133
btPersistentManifold.h
btMultiBodyJacobianData::m_solverBodyPool
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
Definition: btMultiBodyConstraint.h:36
btContactSolverInfo.h
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:592
btMultiBodyConstraintSolver::setupMultiBodyContactConstraint
void setupMultiBodyContactConstraint(btMultiBodySolverConstraint &solverConstraint, const btVector3 &contactNormal, const btScalar &appliedImpulse, btManifoldPoint &cp, const btContactSolverInfo &infoGlobal, btScalar &relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0)
Definition: btMultiBodyConstraintSolver.cpp:496
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
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:456
btMultiBodySolverConstraint::m_linkB
int m_linkB
Definition: btMultiBodySolverConstraint.h:75
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btMultiBodySolverConstraint.h:54
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
btMultiBodySolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btMultiBodySolverConstraint.h:51
btMultiBodySolverConstraint::m_jacAindex
int m_jacAindex
Definition: btMultiBodySolverConstraint.h:38
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:56
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:43
btContactSolverInfoData::m_articulatedWarmstartingFactor
btScalar m_articulatedWarmstartingFactor
Definition: btContactSolverInfo.h:59
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:1399
btMultiBodyConstraintSolver::m_tmpMultiBodyConstraints
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:42
btSolverBody::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:126
btMultiBodyLinkCollider::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:37
btQuickprof.h
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
btTypedConstraint::setEnabled
void setEnabled(bool enabled)
Definition: btTypedConstraint.h:206
btContactSolverInfoData::m_restitutionVelocityThreshold
btScalar m_restitutionVelocityThreshold
Definition: btContactSolverInfo.h:66
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
@ SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
Definition: btContactSolverInfo.h:27
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:55
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btMultiBody::applyDeltaVeeMultiDof2
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:400
btMultiBody::addBaseConstraintForce
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:335
btMultiBodySolverConstraint::m_friction
btScalar m_friction
Definition: btMultiBodySolverConstraint.h:53
btPersistentManifold::getContactPoint
const btManifoldPoint & getContactPoint(int index) const
Definition: btPersistentManifold.h:130
btMultiBodyLinkCollider::m_multiBody
btMultiBody * m_multiBody
Definition: btMultiBodyLinkCollider.h:36
SOLVER_USE_2_FRICTION_DIRECTIONS
@ SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btMultiBodyConstraintSolver::m_multiBodyTorsionalFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints
Definition: btMultiBodyConstraintSolver.h:36
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
btMultiBodySolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btMultiBodySolverConstraint.h:59
btManifoldPoint::m_lateralFrictionDir1
btVector3 m_lateralFrictionDir1
Definition: btManifoldPoint.h:139
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:106
btMultiBodyConstraint.h
btManifoldPoint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btManifoldPoint.h:118
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:44
btRigidBody::getTotalForce
const btVector3 & getTotalForce() const
Definition: btRigidBody.h:279
SOLVER_DISABLE_IMPLICIT_CONE_FRICTION
@ SOLVER_DISABLE_IMPLICIT_CONE_FRICTION
Definition: btContactSolverInfo.h:33
btMultiBodySolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btMultiBodySolverConstraint.h:58
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
btSequentialImpulseConstraintSolver::getOrInitSolverBody
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:689
btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
Definition: btMultiBodyConstraintSolver.cpp:230
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:230
btCollisionObject::CF_ANISOTROPIC_FRICTION
@ CF_ANISOTROPIC_FRICTION
Definition: btCollisionObject.h:159
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint
btMultiBodySolverConstraint & addMultiBodySpinningFrictionConstraint(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:1212
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:45
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:198
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btManifoldPoint::m_appliedImpulseLateral2
btScalar m_appliedImpulseLateral2
Definition: btManifoldPoint.h:121
btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows
btScalar resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint &cA1, const btMultiBodySolverConstraint &cB)
Definition: btMultiBodyConstraintSolver.cpp:313
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:1735
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:1891
btManifoldPoint::m_combinedFriction
btScalar m_combinedFriction
Definition: btManifoldPoint.h:103
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
@ BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED
Definition: btManifoldPoint.h:42
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btManifoldPoint::m_combinedContactStiffness1
btScalar m_combinedContactStiffness1
Definition: btManifoldPoint.h:127
btMultiBodySolverConstraint::m_cfm
btScalar m_cfm
Definition: btMultiBodySolverConstraint.h:56
btMultiBodySolverConstraint::m_frictionIndex
int m_frictionIndex
Definition: btMultiBodySolverConstraint.h:67