Bullet Collision Detection & Physics Library: btMultiBodyConstraintSolver.cpp Source File

Bullet Collision Detection & Physics Library

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