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