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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btMultiBodyConstraintSolver.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17 #include "btMultiBodyConstraintSolver.h"
18 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
19 #include "btMultiBodyLinkCollider.h"
20
21 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
22 #include "btMultiBodyConstraint.h"
23 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
24
25 #include "LinearMath/btQuickprof.h"
26
27 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
28 {
29 btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
30
31 //solve featherstone non-contact constraints
32
33 //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
34 for (int j=0;j<m_multiBodyNonContactConstraints.size();j++)
35 {
36 btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
37 //if (iteration < constraint.m_overrideNumSolverIterations)
38 //resolveSingleConstraintRowGenericMultiBody(constraint);
39 resolveSingleConstraintRowGeneric(constraint);
40 if(constraint.m_multiBodyA)
41 constraint.m_multiBodyA->setPosUpdated(false);
42 if(constraint.m_multiBodyB)
43 constraint.m_multiBodyB->setPosUpdated(false);
44 }
45
46 //solve featherstone normal contact
47 for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
48 {
49 btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
50 if (iteration < infoGlobal.m_numIterations)
51 resolveSingleConstraintRowGeneric(constraint);
52
53 if(constraint.m_multiBodyA)
54 constraint.m_multiBodyA->setPosUpdated(false);
55 if(constraint.m_multiBodyB)
56 constraint.m_multiBodyB->setPosUpdated(false);
57 }
58
59 //solve featherstone frictional contact
60
61 for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
62 {
63 if (iteration < infoGlobal.m_numIterations)
64 {
65 btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
66 btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
67 //adjust friction limits here
68 if (totalImpulse>btScalar(0))
69 {
70 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
71 frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
72 resolveSingleConstraintRowGeneric(frictionConstraint);
73
74 if(frictionConstraint.m_multiBodyA)
75 frictionConstraint.m_multiBodyA->setPosUpdated(false);
76 if(frictionConstraint.m_multiBodyB)
77 frictionConstraint.m_multiBodyB->setPosUpdated(false);
78 }
79 }
80 }
81 return val;
82 }
83
84 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
85 {
86 m_multiBodyNonContactConstraints.resize(0);
87 m_multiBodyNormalContactConstraints.resize(0);
88 m_multiBodyFrictionContactConstraints.resize(0);
89 m_data.m_jacobians.resize(0);
90 m_data.m_deltaVelocitiesUnitImpulse.resize(0);
91 m_data.m_deltaVelocities.resize(0);
92
93 for (int i=0;i<numBodies;i++)
94 {
95 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
96 if (fcA)
97 {
98 fcA->m_multiBody->setCompanionId(-1);
99 }
100 }
101
102 btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
103
104 return val;
105 }
106
107 void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
108 {
109 for (int i = 0; i < ndof; ++i)
110 m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
111 }
112
113 void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
114 {
115
116 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
117 btScalar deltaVelADotn=0;
118 btScalar deltaVelBDotn=0;
119 btSolverBody* bodyA = 0;
120 btSolverBody* bodyB = 0;
121 int ndofA=0;
122 int ndofB=0;
123
124 if (c.m_multiBodyA)
125 {
126 ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
127 for (int i = 0; i < ndofA; ++i)
128 deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
129 } else if(c.m_solverBodyIdA >= 0)
130 {
131 bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
132 deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
133 }
134
135 if (c.m_multiBodyB)
136 {
137 ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
138 for (int i = 0; i < ndofB; ++i)
139 deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
140 } else if(c.m_solverBodyIdB >= 0)
141 {
142 bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
143 deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
144 }
145
146
147 deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
148 deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
149 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
150
151 if (sum < c.m_lowerLimit)
152 {
153 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
154 c.m_appliedImpulse = c.m_lowerLimit;
155 }
156 else if (sum > c.m_upperLimit)
157 {
158 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
159 c.m_appliedImpulse = c.m_upperLimit;
160 }
161 else
162 {
163 c.m_appliedImpulse = sum;
164 }
165
166 if (c.m_multiBodyA)
167 {
168 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
169 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
170 //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
171 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
172 if(c.m_multiBodyA->isMultiDof())
173 c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
174 else
175 c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
176 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
177 } else if(c.m_solverBodyIdA >= 0)
178 {
179 bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
180
181 }
182 if (c.m_multiBodyB)
183 {
184 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
185 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
186 //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
187 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
188 if(c.m_multiBodyB->isMultiDof())
189 c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
190 else
191 c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
192 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
193 } else if(c.m_solverBodyIdB >= 0)
194 {
195 bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
196 }
197
198 }
199
200
201 void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint& c)
202 {
203
204 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
205 btScalar deltaVelADotn=0;
206 btScalar deltaVelBDotn=0;
207 int ndofA=0;
208 int ndofB=0;
209
210 if (c.m_multiBodyA)
211 {
212 ndofA = (c.m_multiBodyA->isMultiDof() ? c.m_multiBodyA->getNumDofs() : c.m_multiBodyA->getNumLinks()) + 6;
213 for (int i = 0; i < ndofA; ++i)
214 deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
215 }
216
217 if (c.m_multiBodyB)
218 {
219 ndofB = (c.m_multiBodyB->isMultiDof() ? c.m_multiBodyB->getNumDofs() : c.m_multiBodyB->getNumLinks()) + 6;
220 for (int i = 0; i < ndofB; ++i)
221 deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
222 }
223
224
225 deltaImpulse -= deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
226 deltaImpulse -= deltaVelBDotn*c.m_jacDiagABInv;
227 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
228
229 if (sum < c.m_lowerLimit)
230 {
231 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
232 c.m_appliedImpulse = c.m_lowerLimit;
233 }
234 else if (sum > c.m_upperLimit)
235 {
236 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
237 c.m_appliedImpulse = c.m_upperLimit;
238 }
239 else
240 {
241 c.m_appliedImpulse = sum;
242 }
243
244 if (c.m_multiBodyA)
245 {
246 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
247 c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
248 }
249 if (c.m_multiBodyB)
250 {
251 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
252 c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
253 }
254 }
255
256
257 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
258 const btVector3& contactNormal,
259 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
260 btScalar& relaxation,
261 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
262 {
263
264 BT_PROFILE("setupMultiBodyContactConstraint");
265 btVector3 rel_pos1;
266 btVector3 rel_pos2;
267
268 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
269 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
270
271 const btVector3& pos1 = cp.getPositionWorldOnA();
272 const btVector3& pos2 = cp.getPositionWorldOnB();
273
274 btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
275 btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
276
277 btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
278 btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
279
280 if (bodyA)
281 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
282 if (bodyB)
283 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
284
285 relaxation = 1.f;
286
287
288
289
290 if (multiBodyA)
291 {
292 if (solverConstraint.m_linkA<0)
293 {
294 rel_pos1 = pos1 - multiBodyA->getBasePos();
295 } else
296 {
297 rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
298 }
299 const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
300
301 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
302
303 if (solverConstraint.m_deltaVelAindex <0)
304 {
305 solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
306 multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
307 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
308 } else
309 {
310 btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
311 }
312
313 solverConstraint.m_jacAindex = m_data.m_jacobians.size();
314 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
315 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
316 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
317
318 btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
319 if(multiBodyA->isMultiDof())
320 multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
321 else
322 multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
323 btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
324 if(multiBodyA->isMultiDof())
325 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
326 else
327 multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
328
329 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
330 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
331 solverConstraint.m_contactNormal1 = contactNormal;
332 } else
333 {
334 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
335 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
336 solverConstraint.m_contactNormal1 = contactNormal;
337 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
338 }
339
340
341
342 if (multiBodyB)
343 {
344 if (solverConstraint.m_linkB<0)
345 {
346 rel_pos2 = pos2 - multiBodyB->getBasePos();
347 } else
348 {
349 rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
350 }
351
352 const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
353
354 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
355 if (solverConstraint.m_deltaVelBindex <0)
356 {
357 solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
358 multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
359 m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
360 }
361
362 solverConstraint.m_jacBindex = m_data.m_jacobians.size();
363
364 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
365 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
366 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
367
368 if(multiBodyB->isMultiDof())
369 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);
370 else
371 multiBodyB->fillContactJacobian(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);
372 if(multiBodyB->isMultiDof())
373 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
374 else
375 multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
376
377 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
378 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
379 solverConstraint.m_contactNormal2 = -contactNormal;
380
381 } else
382 {
383 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
384 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
385 solverConstraint.m_contactNormal2 = -contactNormal;
386
387 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
388 }
389
390 {
391
392 btVector3 vec;
393 btScalar denom0 = 0.f;
394 btScalar denom1 = 0.f;
395 btScalar* jacB = 0;
396 btScalar* jacA = 0;
397 btScalar* lambdaA =0;
398 btScalar* lambdaB =0;
399 int ndofA = 0;
400 if (multiBodyA)
401 {
402 ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
403 jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
404 lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
405 for (int i = 0; i < ndofA; ++i)
406 {
407 btScalar j = jacA[i] ;
408 btScalar l =lambdaA[i];
409 denom0 += j*l;
410 }
411 } else
412 {
413 if (rb0)
414 {
415 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
416 denom0 = rb0->getInvMass() + contactNormal.dot(vec);
417 }
418 }
419 if (multiBodyB)
420 {
421 const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
422 jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
423 lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
424 for (int i = 0; i < ndofB; ++i)
425 {
426 btScalar j = jacB[i] ;
427 btScalar l =lambdaB[i];
428 denom1 += j*l;
429 }
430
431 } else
432 {
433 if (rb1)
434 {
435 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
436 denom1 = rb1->getInvMass() + contactNormal.dot(vec);
437 }
438 }
439
440
441
442 btScalar d = denom0+denom1;
443 if (d>SIMD_EPSILON)
444 {
445 solverConstraint.m_jacDiagABInv = relaxation/(d);
446 } else
447 {
448 //disable the constraint row to handle singularity/redundant constraint
449 solverConstraint.m_jacDiagABInv = 0.f;
450 }
451
452 }
453
454
455 //compute rhs and remaining solverConstraint fields
456
457
458
459 btScalar restitution = 0.f;
460 btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
461
462 btScalar rel_vel = 0.f;
463 int ndofA = 0;
464 int ndofB = 0;
465 {
466
467 btVector3 vel1,vel2;
468 if (multiBodyA)
469 {
470 ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
471 btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
472 for (int i = 0; i < ndofA ; ++i)
473 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
474 } else
475 {
476 if (rb0)
477 {
478 rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
479 }
480 }
481 if (multiBodyB)
482 {
483 ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
484 btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
485 for (int i = 0; i < ndofB ; ++i)
486 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
487
488 } else
489 {
490 if (rb1)
491 {
492 rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
493 }
494 }
495
496 solverConstraint.m_friction = cp.m_combinedFriction;
497
498 if(!isFriction)
499 {
500 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
501 if (restitution <= btScalar(0.))
502 {
503 restitution = 0.f;
504 }
505 }
506 }
507
508
510 //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
511 if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
512 {
513 solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
514
515 if (solverConstraint.m_appliedImpulse)
516 {
517 if (multiBodyA)
518 {
519 btScalar impulse = solverConstraint.m_appliedImpulse;
520 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
521 if(multiBodyA->isMultiDof())
522 multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
523 else
524 multiBodyA->applyDeltaVee(deltaV,impulse);
525 applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
526 } else
527 {
528 if (rb0)
529 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
530 }
531 if (multiBodyB)
532 {
533 btScalar impulse = solverConstraint.m_appliedImpulse;
534 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
535 if(multiBodyB->isMultiDof())
536 multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
537 else
538 multiBodyB->applyDeltaVee(deltaV,impulse);
539 applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
540 } else
541 {
542 if (rb1)
543 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
544 }
545 }
546 } else
547 {
548 solverConstraint.m_appliedImpulse = 0.f;
549 }
550
551 solverConstraint.m_appliedPushImpulse = 0.f;
552
553 {
554
555 btScalar positionalError = 0.f;
556 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
557
558 btScalar erp = infoGlobal.m_erp2;
559 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
560 {
561 erp = infoGlobal.m_erp;
562 }
563
564 if (penetration>0)
565 {
566 positionalError = 0;
567 velocityError -= penetration / infoGlobal.m_timeStep;
568
569 } else
570 {
571 positionalError = -penetration * erp/infoGlobal.m_timeStep;
572 }
573
574 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
575 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
576
577 if(!isFriction)
578 {
579 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
580 {
581 //combine position and velocity into rhs
582 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
583 solverConstraint.m_rhsPenetration = 0.f;
584
585 } else
586 {
587 //split position and velocity into rhs and m_rhsPenetration
588 solverConstraint.m_rhs = velocityImpulse;
589 solverConstraint.m_rhsPenetration = penetrationImpulse;
590 }
591
592 solverConstraint.m_lowerLimit = 0;
593 solverConstraint.m_upperLimit = 1e10f;
594 }
595 else
596 {
597 solverConstraint.m_rhs = velocityImpulse;
598 solverConstraint.m_rhsPenetration = 0.f;
599 solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
600 solverConstraint.m_upperLimit = solverConstraint.m_friction;
601 }
602
603 solverConstraint.m_cfm = 0.f; //why not use cfmSlip?
604 }
605
606 }
607
608
609
610
611 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)
612 {
613 BT_PROFILE("addMultiBodyFrictionConstraint");
614 btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
615 solverConstraint.m_orgConstraint = 0;
616 solverConstraint.m_orgDofIndex = -1;
617
618 solverConstraint.m_frictionIndex = frictionIndex;
619 bool isFriction = true;
620
621 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
622 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
623
624 btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
625 btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
626
627 int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
628 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
629
630 solverConstraint.m_solverBodyIdA = solverBodyIdA;
631 solverConstraint.m_solverBodyIdB = solverBodyIdB;
632 solverConstraint.m_multiBodyA = mbA;
633 if (mbA)
634 solverConstraint.m_linkA = fcA->m_link;
635
636 solverConstraint.m_multiBodyB = mbB;
637 if (mbB)
638 solverConstraint.m_linkB = fcB->m_link;
639
640 solverConstraint.m_originalContactPoint = &cp;
641
642 setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
643 return solverConstraint;
644 }
645
646 void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
647 {
648 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
649 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
650
651 btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
652 btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
653
654 btCollisionObject* colObj0=0,*colObj1=0;
655
656 colObj0 = (btCollisionObject*)manifold->getBody0();
657 colObj1 = (btCollisionObject*)manifold->getBody1();
658
659 int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
660 int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
661
662 // btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
663 // btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
664
665
667 // if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
668 // return;
669
670
671
672 for (int j=0;j<manifold->getNumContacts();j++)
673 {
674
675 btManifoldPoint& cp = manifold->getContactPoint(j);
676
677 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
678 {
679
680 btScalar relaxation;
681
682 int frictionIndex = m_multiBodyNormalContactConstraints.size();
683
684 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
685
686 // btRigidBody* rb0 = btRigidBody::upcast(colObj0);
687 // btRigidBody* rb1 = btRigidBody::upcast(colObj1);
688 solverConstraint.m_orgConstraint = 0;
689 solverConstraint.m_orgDofIndex = -1;
690 solverConstraint.m_solverBodyIdA = solverBodyIdA;
691 solverConstraint.m_solverBodyIdB = solverBodyIdB;
692 solverConstraint.m_multiBodyA = mbA;
693 if (mbA)
694 solverConstraint.m_linkA = fcA->m_link;
695
696 solverConstraint.m_multiBodyB = mbB;
697 if (mbB)
698 solverConstraint.m_linkB = fcB->m_link;
699
700 solverConstraint.m_originalContactPoint = &cp;
701
702 bool isFriction = false;
703 setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
704
705 // const btVector3& pos1 = cp.getPositionWorldOnA();
706 // const btVector3& pos2 = cp.getPositionWorldOnB();
707
709 #define ENABLE_FRICTION
710 #ifdef ENABLE_FRICTION
711 solverConstraint.m_frictionIndex = frictionIndex;
712 #if ROLLING_FRICTION
713 int rollingFriction=1;
714 btVector3 angVelA(0,0,0),angVelB(0,0,0);
715 if (rb0)
716 angVelA = rb0->getAngularVelocity();
717 if (rb1)
718 angVelB = rb1->getAngularVelocity();
719 btVector3 relAngVel = angVelB-angVelA;
720
721 if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
722 {
723 //only a single rollingFriction per manifold
724 rollingFriction--;
725 if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
726 {
727 relAngVel.normalize();
728 applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
729 applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
730 if (relAngVel.length()>0.001)
731 addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
732
733 } else
734 {
735 addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
736 btVector3 axis0,axis1;
737 btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
738 applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
739 applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
740 applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
741 applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
742 if (axis0.length()>0.001)
743 addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
744 if (axis1.length()>0.001)
745 addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
746
747 }
748 }
749 #endif //ROLLING_FRICTION
750
755
766 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
767 {/*
768 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
769 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
770 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
771 {
772 cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
773 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
774 {
775 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
776 cp.m_lateralFrictionDir2.normalize();//??
777 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
778 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
779 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
780
781 }
782
783 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
784 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
785 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
786
787 } else
788 */
789 {
790 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
791
792 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
793 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
794 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
795
796 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
797 {
798 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
799 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
800 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
801 }
802
803 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
804 {
805 cp.m_lateralFrictionInitialized = true;
806 }
807 }
808
809 } else
810 {
811 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
812
813 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
814 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
815
816 //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
817 //todo:
818 solverConstraint.m_appliedImpulse = 0.f;
819 solverConstraint.m_appliedPushImpulse = 0.f;
820 }
821
822
823 #endif //ENABLE_FRICTION
824
825 }
826 }
827 }
828
829 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
830 {
831 //btPersistentManifold* manifold = 0;
832
833 for (int i=0;i<numManifolds;i++)
834 {
835 btPersistentManifold* manifold= manifoldPtr[i];
836 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
837 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
838 if (!fcA && !fcB)
839 {
840 //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
841 convertContact(manifold,infoGlobal);
842 } else
843 {
844 convertMultiBodyContact(manifold,infoGlobal);
845 }
846 }
847
848 //also convert the multibody constraints, if any
849
850
851 for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
852 {
853 btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
854 m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
855 m_data.m_fixedBodyId = m_fixedBodyId;
856
857 c->createConstraintRows(m_multiBodyNonContactConstraints,m_data, infoGlobal);
858 }
859
860 }
861
862
863
864 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
865 {
866 return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
867 }
868
869 #if 0
870 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
871 {
872 if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
873 {
874 //todo: get rid of those temporary memory allocations for the joint feedback
875 btAlignedObjectArray<btScalar> forceVector;
876 int numDofsPlusBase = 6+mb->getNumDofs();
877 forceVector.resize(numDofsPlusBase);
878 for (int i=0;i<numDofsPlusBase;i++)
879 {
880 forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
881 }
882 btAlignedObjectArray<btScalar> output;
883 output.resize(numDofsPlusBase);
884 bool applyJointFeedback = true;
885 mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
886 }
887 }
888 #endif
889
890 #include "Bullet3Common/b3Logging.h"
891 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
892 {
893 #if 1
894
895 //bod->addBaseForce(m_gravity * bod->getBaseMass());
896 //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
897
898 if (c.m_orgConstraint)
899 {
900 c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
901 }
902
903
904 if (c.m_multiBodyA)
905 {
906
907 if(c.m_multiBodyA->isMultiDof())
908 {
909 c.m_multiBodyA->setCompanionId(-1);
910 btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
911 btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
912 if (c.m_linkA<0)
913 {
914 c.m_multiBodyA->addBaseConstraintForce(force);
915 c.m_multiBodyA->addBaseConstraintTorque(torque);
916 } else
917 {
918 c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
919 //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
920 c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
921 }
922 }
923 }
924
925 if (c.m_multiBodyB)
926 {
927 if(c.m_multiBodyB->isMultiDof())
928 {
929 {
930 c.m_multiBodyB->setCompanionId(-1);
931 btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
932 btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
933 if (c.m_linkB<0)
934 {
935 c.m_multiBodyB->addBaseConstraintForce(force);
936 c.m_multiBodyB->addBaseConstraintTorque(torque);
937 } else
938 {
939 {
940 c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
941 //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
942 c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
943 }
944
945 }
946 }
947 }
948 }
949 #endif
950
951 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
952
953 if (c.m_multiBodyA)
954 {
955
956 if(c.m_multiBodyA->isMultiDof())
957 {
958 c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
959 }
960 else
961 {
962 c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
963 }
964 }
965
966 if (c.m_multiBodyB)
967 {
968 if(c.m_multiBodyB->isMultiDof())
969 {
970 c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
971 }
972 else
973 {
974 c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
975 }
976 }
977 #endif
978
979
980
981 }
982
983 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
984 {
985 BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
986 int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
987
988
989 //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
990 //or as applied force, so we can measure the joint reaction forces easier
991 for (int i=0;i<numPoolConstraints;i++)
992 {
993 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
994 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
995
996 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
997
998 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
999 {
1000 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
1001 }
1002 }
1003
1004
1005 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1006 {
1007 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1008 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
1009 }
1010
1011
1012 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1013 {
1014 BT_PROFILE("warm starting write back");
1015 for (int j=0;j<numPoolConstraints;j++)
1016 {
1017 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
1018 btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
1019 btAssert(pt);
1020 pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1021 pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1022
1023 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1024 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1025 {
1026 pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
1027 }
1028 //do a callback here?
1029 }
1030 }
1031 #if 0
1032 //multibody joint feedback
1033 {
1034 BT_PROFILE("multi body joint feedback");
1035 for (int j=0;j<numPoolConstraints;j++)
1036 {
1037 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
1038
1039 //apply the joint feedback into all links of the btMultiBody
1040 //todo: double-check the signs of the applied impulse
1041
1042 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1043 {
1044 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1045 }
1046 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1047 {
1048 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1049 }
1050 #if 0
1051 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
1052 {
1053 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1054 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
1055 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
1056 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1057
1058 }
1059 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
1060 {
1061 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1062 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
1063 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
1064 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1065 }
1066
1067 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1068 {
1069 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
1070 {
1071 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1072 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
1073 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
1074 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1075 }
1076
1077 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
1078 {
1079 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1080 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
1081 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
1082 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1083 }
1084 }
1085 #endif
1086 }
1087
1088 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1089 {
1090 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1091 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1092 {
1093 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1094 }
1095 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1096 {
1097 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1098 }
1099 }
1100 }
1101
1102 numPoolConstraints = m_multiBodyNonContactConstraints.size();
1103
1104 #if 0
1105 //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1106 for (int i=0;i<numPoolConstraints;i++)
1107 {
1108 const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
1109
1110 btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
1111 btJointFeedback* fb = constr->getJointFeedback();
1112 if (fb)
1113 {
1114 fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1115 fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1116 fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1117 fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1118
1119 }
1120
1121 constr->internalSetAppliedImpulse(c.m_appliedImpulse);
1122 if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1123 {
1124 constr->setEnabled(false);
1125 }
1126
1127 }
1128 #endif
1129 #endif
1130
1131 return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
1132 }
1133
1134
1135 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)
1136 {
1137 //printf("solveMultiBodyGroup start\n");
1138 m_tmpMultiBodyConstraints = multiBodyConstraints;
1139 m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1140
1141 btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
1142
1143 m_tmpMultiBodyConstraints = 0;
1144 m_tmpNumMultiBodyConstraints = 0;
1145
1146
1147 }
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:270
btMultiBody::calcAccelerationDeltas
void calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
Definition: btMultiBody.cpp:1796
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:84
btSequentialImpulseConstraintSolver::m_fixedBodyId
int m_fixedBodyId
Definition: btSequentialImpulseConstraintSolver.h:47
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:494
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btMultiBodySolverConstraint.h:59
btPersistentManifold
btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping...
Definition: btPersistentManifold.h:54
btMultiBodyLinkCollider::upcast
static btMultiBodyLinkCollider * upcast(btCollisionObject *colObj)
Definition: btMultiBodyLinkCollider.h:50
btManifoldPoint::m_contactCFM2
btScalar m_contactCFM2
Definition: btManifoldPoint.h:112
btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:983
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:118
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:257
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:40
btContactSolverInfoData::m_erp
btScalar m_erp
Definition: btContactSolverInfo.h:47
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:43
btManifoldPoint::m_lateralFrictionDir1
btVector3 m_lateralFrictionDir1
Definition: btManifoldPoint.h:116
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btMultiBodyJacobianData::m_deltaVelocities
btAlignedObjectArray< btScalar > m_deltaVelocities
Definition: btMultiBodyConstraint.h:32
btMultiBodyConstraint::createConstraintRows
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)=0
btAlignedObjectArray< btScalar >
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:255
btMultiBodyJacobianData::m_solverBodyPool
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
Definition: btMultiBodyConstraint.h:36
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:501
btManifoldPoint::m_appliedImpulseLateral1
btScalar m_appliedImpulseLateral1
Definition: btManifoldPoint.h:107
btMultiBodyConstraintSolver::solveSingleIteration
virtual btScalar solveSingleIteration(int iteration, btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btMultiBodyConstraintSolver.cpp:27
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject **bodies, int numBodies, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:1825
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:46
btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint &constraint, btScalar deltaTime)
Definition: btMultiBodyConstraintSolver.cpp:891
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:154
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:47
btManifoldPoint::m_combinedRestitution
btScalar m_combinedRestitution
Definition: btManifoldPoint.h:95
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:1911
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:50
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:112
btJointFeedback
Definition: btTypedConstraint.h:67
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1272
btManifoldPoint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btManifoldPoint.h:106
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btMultiBodyConstraint.h
btMultiBody::addLinkConstraintForce
void addLinkConstraintForce(int i, const btVector3 &f)
Definition: btMultiBody.cpp:600
btTypedConstraint::getBreakingImpulseThreshold
btScalar getBreakingImpulseThreshold() const
Definition: btTypedConstraint.h:201
btContactSolverInfoData::m_singleAxisRollingFrictionThreshold
btScalar m_singleAxisRollingFrictionThreshold
Definition: btContactSolverInfo.h:60
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:1529
btPersistentManifold.h
btMultiBody::internalNeedsJointFeedback
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:597
btManifoldPoint
ManifoldContactPoint collects and maintains persistent contactpoints.
Definition: btManifoldPoint.h:42
SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION
Definition: btContactSolverInfo.h:28
btPersistentManifold::getBody0
const btCollisionObject * getBody0() const
Definition: btPersistentManifold.h:94
btManifoldPoint::m_contactMotion1
btScalar m_contactMotion1
Definition: btManifoldPoint.h:109
btMultiBodyConstraintSolver::m_tmpNumMultiBodyConstraints
int m_tmpNumMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:44
btMultiBody
Definition: btMultiBody.h:48
btMultiBody::applyDeltaVeeMultiDof
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:422
btSequentialImpulseConstraintSolver::restitutionCurve
btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
Definition: btSequentialImpulseConstraintSolver.cpp:505
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btMultiBodySolverConstraint::m_deltaVelBindex
int m_deltaVelBindex
Definition: btMultiBodySolverConstraint.h:37
btMultiBody::applyDeltaVee
void applyDeltaVee(const btScalar *delta_vee)
Definition: btMultiBody.h:366
btMultiBodyConstraint
Definition: btMultiBodyConstraint.h:42
btCollisionObject::CF_ANISOTROPIC_FRICTION
Definition: btCollisionObject.h:149
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
btMultiBodySolverConstraint::m_deltaVelAindex
int m_deltaVelAindex
Definition: btMultiBodySolverConstraint.h:35
btMultiBodyConstraintSolver::solveGroup
virtual btScalar solveGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
this method should not be called, it was just used during porting/integration of Featherstone btMulti...
Definition: btMultiBodyConstraintSolver.cpp:864
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:379
btTypedConstraint::getJointFeedback
const btJointFeedback * getJointFeedback() const
Definition: btTypedConstraint.h:279
btMultiBodyConstraintSolver::m_multiBodyNonContactConstraints
btMultiBodyConstraintArray m_multiBodyNonContactConstraints
Definition: btMultiBodyConstraintSolver.h:35
btSequentialImpulseConstraintSolver::getOrInitSolverBody
int getOrInitSolverBody(btCollisionObject &body, btScalar timeStep)
Definition: btSequentialImpulseConstraintSolver.cpp:717
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btMultiBody::addLinkConstraintTorque
void addLinkConstraintTorque(int i, const btVector3 &t)
Definition: btMultiBody.cpp:605
btSimdScalar
#define btSimdScalar
Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later...
Definition: btSolverBody.h:104
btManifoldPoint::m_combinedRollingFriction
btScalar m_combinedRollingFriction
Definition: btManifoldPoint.h:94
btSequentialImpulseConstraintSolver::m_tmpSolverBodyPool
btAlignedObjectArray< btSolverBody > m_tmpSolverBodyPool
Definition: btSequentialImpulseConstraintSolver.h:36
btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint &c)
Definition: btMultiBodyConstraintSolver.cpp:113
btMultiBodySolverConstraint::m_jacBindex
int m_jacBindex
Definition: btMultiBodySolverConstraint.h:38
btManifoldPoint::m_normalWorldOnB
btVector3 m_normalWorldOnB
Definition: btManifoldPoint.h:90
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btMultiBodyConstraintSolver::solveMultiBodyGroup
virtual void solveMultiBodyGroup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, btMultiBodyConstraint **multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btDispatcher *dispatcher)
Definition: btMultiBodyConstraintSolver.cpp:1135
btJointFeedback::m_appliedForceBodyA
btVector3 m_appliedForceBodyA
Definition: btTypedConstraint.h:69
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:70
btMultiBody::setCompanionId
void setCompanionId(int id)
Definition: btMultiBody.h:517
btManifoldPoint::m_appliedImpulseLateral2
btScalar m_appliedImpulseLateral2
Definition: btManifoldPoint.h:108
btMultiBodyJacobianData::m_fixedBodyId
int m_fixedBodyId
Definition: btMultiBodyConstraint.h:37
output
#define output
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btMultiBodyConstraintSolver::convertContacts
void convertContacts(btPersistentManifold **manifoldPtr, int numManifolds, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:829
btManifoldPoint::m_contactCFM1
btScalar m_contactCFM1
Definition: btManifoldPoint.h:111
btCollisionObject
btCollisionObject can be used to manage collision detection objects.
Definition: btCollisionObject.h:49
btPersistentManifold::getContactProcessingThreshold
btScalar getContactProcessingThreshold() const
Definition: btPersistentManifold.h:132
btIDebugDraw
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:74
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
SOLVER_USE_2_FRICTION_DIRECTIONS
Definition: btContactSolverInfo.h:26
btMultiBodySolverConstraint::m_linkA
int m_linkA
Definition: btMultiBodySolverConstraint.h:71
btPersistentManifold::getContactPoint
const btManifoldPoint & getContactPoint(int index) const
Definition: btPersistentManifold.h:117
btMultiBodySolverConstraint::m_jacAindex
int m_jacAindex
Definition: btMultiBodySolverConstraint.h:36
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyConstraintSolver::m_multiBodyFrictionContactConstraints
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints
Definition: btMultiBodyConstraintSolver.h:38
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:30
btSolverBody::internalGetInvMass
const btVector3 & internalGetInvMass() const
Definition: btSolverBody.h:223
btContactSolverInfoData::m_numIterations
int m_numIterations
Definition: btContactSolverInfo.h:44
btMultiBodySolverConstraint::m_originalContactPoint
void * m_originalContactPoint
Definition: btMultiBodySolverConstraint.h:62
btMultiBodyLinkCollider.h
btManifoldPoint::getPositionWorldOnB
const btVector3 & getPositionWorldOnB() const
Definition: btManifoldPoint.h:136
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:54
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btMultiBodyConstraintSolver.h
btSequentialImpulseConstraintSolver::convertContact
void convertContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btSequentialImpulseConstraintSolver.cpp:976
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
BT_PROFILE
#define BT_PROFILE(name)
Definition: btQuickprof.h:196
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodySolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btMultiBodySolverConstraint.h:50
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
Definition: btMultiBody.cpp:1633
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:611
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:23
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btSequentialImpulseConstraintSolver::addRollingFrictionConstraint
btSolverConstraint & addRollingFrictionConstraint(const btVector3 &normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint &cp, const btVector3 &rel_pos1, const btVector3 &rel_pos2, btCollisionObject *colObj0, btCollisionObject *colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f)
Definition: btSequentialImpulseConstraintSolver.cpp:707
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
btMultiBody::getCompanionId
int getCompanionId() const
Definition: btMultiBody.h:513
btMultiBodySolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btMultiBodySolverConstraint.h:58
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:470
btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody
void resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint &c)
Definition: btMultiBodyConstraintSolver.cpp:201
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:77
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:212
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition: btSolverBody.h:124
btQuickprof.h
btPersistentManifold::getNumContacts
int getNumContacts() const
Definition: btPersistentManifold.h:109
btSolverBody::internalGetDeltaLinearVelocity
btVector3 & internalGetDeltaLinearVelocity()
some internal methods, don't use them
Definition: btSolverBody.h:208
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:226
btTypedConstraint::setEnabled
void setEnabled(bool enabled)
Definition: btTypedConstraint.h:216
btMultiBodyConstraintSolver::convertMultiBodyContact
void convertMultiBodyContact(btPersistentManifold *manifold, const btContactSolverInfo &infoGlobal)
Definition: btMultiBodyConstraintSolver.cpp:646
btMultiBody::applyDeltaVeeMultiDof2
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:405
btMultiBodySolverConstraint::m_friction
btScalar m_friction
Definition: btMultiBodySolverConstraint.h:52
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer)
Definition: btSequentialImpulseConstraintSolver.cpp:1170
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:84
btMultiBody::setPosUpdated
void setPosUpdated(bool updated)
Definition: btMultiBody.h:591
btMultiBodyConstraintSolver::applyDeltaVee
void applyDeltaVee(btScalar *deltaV, btScalar impulse, int velocityIndex, int ndof)
Definition: btMultiBodyConstraintSolver.cpp:107
btContactSolverInfoData::m_splitImpulsePenetrationThreshold
btScalar m_splitImpulsePenetrationThreshold
Definition: btContactSolverInfo.h:51
btMultiBodyConstraintSolver::m_multiBodyNormalContactConstraints
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints
Definition: btMultiBodyConstraintSolver.h:37
btMultiBodySolverConstraint::m_orgDofIndex
int m_orgDofIndex
Definition: btMultiBodySolverConstraint.h:79
btManifoldPoint::m_contactMotion2
btScalar m_contactMotion2
Definition: btManifoldPoint.h:110
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:271
btManifoldPoint::m_combinedFriction
btScalar m_combinedFriction
Definition: btManifoldPoint.h:93
btManifoldPoint::m_lateralFrictionInitialized
bool m_lateralFrictionInitialized
Definition: btManifoldPoint.h:104
btMultiBodySolverConstraint::m_linkB
int m_linkB
Definition: btMultiBodySolverConstraint.h:75
btMultiBodyConstraintSolver::m_tmpMultiBodyConstraints
btMultiBodyConstraint ** m_tmpMultiBodyConstraints
Definition: btMultiBodyConstraintSolver.h:43
btMultiBody::fillContactJacobian
void fillContactJacobian(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.cpp:2241
btManifoldPoint::getPositionWorldOnA
const btVector3 & getPositionWorldOnA() const
Definition: btManifoldPoint.h:131
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultiBodyLinkCollider::m_link
int m_link
Definition: btMultiBodyLinkCollider.h:29
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:131
btContactSolverInfoData::m_linearSlop
btScalar m_linearSlop
Definition: btContactSolverInfo.h:53
btMultiBody::isMultiDof
bool isMultiDof()
Definition: btMultiBody.h:579
btSolverBody::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:130
btSolverBody::internalGetDeltaAngularVelocity
btVector3 & internalGetDeltaAngularVelocity()
Definition: btSolverBody.h:213
btTypedConstraint::internalSetAppliedImpulse
void internalSetAppliedImpulse(btScalar appliedImpulse)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.h:190
btMultiBody::addBaseConstraintForce
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:302
SOLVER_ENABLE_FRICTION_DIRECTION_CACHING
Definition: btContactSolverInfo.h:27
btManifoldPoint::m_lateralFrictionDir2
btVector3 m_lateralFrictionDir2
Definition: btManifoldPoint.h:117
btMultiBodyLinkCollider::m_multiBody
btMultiBody * m_multiBody
Definition: btMultiBodyLinkCollider.h:28
btMultiBodySolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btMultiBodySolverConstraint.h:49
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btMultiBodySolverConstraint.h:53
btManifoldPoint::getDistance
btScalar getDistance() const
Definition: btManifoldPoint.h:122
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:176
btSolverBody.h
btDispatcher
The btDispatcher interface class can be used in combination with broadphase to dispatch calculations ...
Definition: btDispatcher.h:69
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
btRigidBody::getLinearFactor
const btVector3 & getLinearFactor() const
Definition: btRigidBody.h:261
btAlignedObjectArray::expandNonInitializing
T & expandNonInitializing()
Definition: btAlignedObjectArray.h:239
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:41
btMultiBody::addBaseConstraintTorque
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:306
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:230
btSequentialImpulseConstraintSolver::applyAnisotropicFriction
static void applyAnisotropicFriction(btCollisionObject *colObj, btVector3 &frictionDirection, int frictionMode)
Definition: btSequentialImpulseConstraintSolver.cpp:513
btMultiBodySolverConstraint::m_frictionIndex
int m_frictionIndex
Definition: btMultiBodySolverConstraint.h:67
btMultiBodySolverConstraint::m_cfm
btScalar m_cfm
Definition: btMultiBodySolverConstraint.h:55
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:155
btPersistentManifold::getBody1
const btCollisionObject * getBody1() const
Definition: btPersistentManifold.h:95
btContactSolverInfoData::m_warmstartingFactor
btScalar m_warmstartingFactor
Definition: btContactSolverInfo.h:54
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:248
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btMultiBodyConstraintSolver::m_data
btMultiBodyJacobianData m_data
Definition: btMultiBodyConstraintSolver.h:40
btContactSolverInfoData::m_solverMode
int m_solverMode
Definition: btContactSolverInfo.h:56
btMultiBodySolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btMultiBodySolverConstraint.h:57
btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION
Definition: btCollisionObject.h:150
btContactSolverInfo.h
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:449