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
solveSingleIteration(int iteration,btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)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
solveGroupCacheFriendlySetup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)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
applyDeltaVee(btScalar * delta_vee,btScalar impulse,int velocityIndex,int ndof)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
resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint & c)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
resolveConeFrictionConstraintRows(const btMultiBodySolverConstraint & cA1,const btMultiBodySolverConstraint & cB)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
setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint,const btVector3 & contactNormal,const btScalar & appliedImpulse,btManifoldPoint & cp,const btContactSolverInfo & infoGlobal,btScalar & relaxation,bool isFriction,btScalar desiredVelocity,btScalar cfmSlip)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
setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint & solverConstraint,const btVector3 & constraintNormal,btManifoldPoint & cp,btScalar combinedTorsionalFriction,const btContactSolverInfo & infoGlobal,btScalar & relaxation,bool isFriction,btScalar desiredVelocity,btScalar cfmSlip)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
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)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
addMultiBodyTorsionalFrictionConstraint(const btVector3 & normalAxis,btPersistentManifold * manifold,int frictionIndex,btManifoldPoint & cp,btScalar combinedTorsionalFriction,btCollisionObject * colObj0,btCollisionObject * colObj1,btScalar relaxation,const btContactSolverInfo & infoGlobal,btScalar desiredVelocity,btScalar cfmSlip)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
addMultiBodySpinningFrictionConstraint(const btVector3 & normalAxis,btPersistentManifold * manifold,int frictionIndex,btManifoldPoint & cp,btScalar combinedTorsionalFriction,btCollisionObject * colObj0,btCollisionObject * colObj1,btScalar relaxation,const btContactSolverInfo & infoGlobal,btScalar desiredVelocity,btScalar cfmSlip)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 }
convertMultiBodyContact(btPersistentManifold * manifold,const btContactSolverInfo & infoGlobal)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
1268 ///avoid collision response between two static objects
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
1309 /////setup the friction constraints
1310 #define ENABLE_FRICTION
1311 #ifdef ENABLE_FRICTION
1312 solverConstraint.m_frictionIndex = m_multiBodyFrictionContactConstraints.size();
1313
1314 ///Bullet has several options to set the friction directions
1315 ///By default, each contact has only a single friction direction that is recomputed automatically every frame
1316 ///based on the relative linear velocity.
1317 ///If the relative velocity is zero, it will automatically compute a friction direction.
1318
1319 ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
1320 ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
1321 ///
1322 ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
1323 ///
1324 ///The user can manually override the friction directions for certain contacts using a contact callback,
1325 ///and set the cp.m_lateralFrictionInitialized to true
1326 ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
1327 ///this will give a conveyor belt effect
1328 ///
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
convertContacts(btPersistentManifold ** manifoldPtr,int numManifolds,const btContactSolverInfo & infoGlobal)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
solveGroup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifold,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & info,btIDebugDraw * debugDrawer,btDispatcher * dispatcher)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
writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & c,btScalar deltaTime)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
solveGroupCacheFriendlyFinish(btCollisionObject ** bodies,int numBodies,const btContactSolverInfo & infoGlobal)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
solveMultiBodyGroup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifold,int numManifolds,btTypedConstraint ** constraints,int numConstraints,btMultiBodyConstraint ** multiBodyConstraints,int numMultiBodyConstraints,const btContactSolverInfo & info,btIDebugDraw * debugDrawer,btDispatcher * dispatcher)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 }
1748