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
solveSingleIteration(int iteration,btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)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
solveGroupCacheFriendlySetup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifoldPtr,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & infoGlobal,btIDebugDraw * debugDrawer)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
applyDeltaVee(btScalar * delta_vee,btScalar impulse,int velocityIndex,int ndof)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
resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint & c)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
resolveSingleConstraintRowGenericMultiBody(const btMultiBodySolverConstraint & c)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
setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint,const btVector3 & contactNormal,btManifoldPoint & cp,const btContactSolverInfo & infoGlobal,btScalar & relaxation,bool isFriction,btScalar desiredVelocity,btScalar cfmSlip)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
509 ///warm starting (or zero if disabled)
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
addMultiBodyFrictionConstraint(const btVector3 & normalAxis,btPersistentManifold * manifold,int frictionIndex,btManifoldPoint & cp,btCollisionObject * colObj0,btCollisionObject * colObj1,btScalar relaxation,const btContactSolverInfo & infoGlobal,btScalar desiredVelocity,btScalar cfmSlip)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
convertMultiBodyContact(btPersistentManifold * manifold,const btContactSolverInfo & infoGlobal)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
666 ///avoid collision response between two static objects
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
708 /////setup the friction constraints
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
751 ///Bullet has several options to set the friction directions
752 ///By default, each contact has only a single friction direction that is recomputed automatically very frame
753 ///based on the relative linear velocity.
754 ///If the relative velocity it zero, it will automatically compute a friction direction.
755
756 ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
757 ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
758 ///
759 ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
760 ///
761 ///The user can manually override the friction directions for certain contacts using a contact callback,
762 ///and set the cp.m_lateralFrictionInitialized to true
763 ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
764 ///this will give a conveyor belt effect
765 ///
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
convertContacts(btPersistentManifold ** manifoldPtr,int numManifolds,const btContactSolverInfo & infoGlobal)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
solveGroup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifold,int numManifolds,btTypedConstraint ** constraints,int numConstraints,const btContactSolverInfo & info,btIDebugDraw * debugDrawer,btDispatcher * dispatcher)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
writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & c,btScalar deltaTime)890 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
891 {
892 #if 1
893
894 //bod->addBaseForce(m_gravity * bod->getBaseMass());
895 //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
896
897 if (c.m_orgConstraint)
898 {
899 c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
900 }
901
902
903 if (c.m_multiBodyA)
904 {
905
906 if(c.m_multiBodyA->isMultiDof())
907 {
908 c.m_multiBodyA->setCompanionId(-1);
909 btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
910 btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
911 if (c.m_linkA<0)
912 {
913 c.m_multiBodyA->addBaseConstraintForce(force);
914 c.m_multiBodyA->addBaseConstraintTorque(torque);
915 } else
916 {
917 c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
918 //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
919 c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
920 }
921 }
922 }
923
924 if (c.m_multiBodyB)
925 {
926 if(c.m_multiBodyB->isMultiDof())
927 {
928 {
929 c.m_multiBodyB->setCompanionId(-1);
930 btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
931 btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
932 if (c.m_linkB<0)
933 {
934 c.m_multiBodyB->addBaseConstraintForce(force);
935 c.m_multiBodyB->addBaseConstraintTorque(torque);
936 } else
937 {
938 {
939 c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
940 //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
941 c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
942 }
943
944 }
945 }
946 }
947 }
948 #endif
949
950 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
951
952 if (c.m_multiBodyA)
953 {
954
955 if(c.m_multiBodyA->isMultiDof())
956 {
957 c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
958 }
959 else
960 {
961 c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
962 }
963 }
964
965 if (c.m_multiBodyB)
966 {
967 if(c.m_multiBodyB->isMultiDof())
968 {
969 c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
970 }
971 else
972 {
973 c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
974 }
975 }
976 #endif
977
978
979
980 }
981
solveGroupCacheFriendlyFinish(btCollisionObject ** bodies,int numBodies,const btContactSolverInfo & infoGlobal)982 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
983 {
984 BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
985 int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
986
987
988 //write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
989 //or as applied force, so we can measure the joint reaction forces easier
990 for (int i=0;i<numPoolConstraints;i++)
991 {
992 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
993 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
994
995 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
996
997 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
998 {
999 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
1000 }
1001 }
1002
1003
1004 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1005 {
1006 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1007 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
1008 }
1009
1010
1011 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1012 {
1013 BT_PROFILE("warm starting write back");
1014 for (int j=0;j<numPoolConstraints;j++)
1015 {
1016 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
1017 btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
1018 btAssert(pt);
1019 pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
1020 pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
1021
1022 //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1023 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1024 {
1025 pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
1026 }
1027 //do a callback here?
1028 }
1029 }
1030 #if 0
1031 //multibody joint feedback
1032 {
1033 BT_PROFILE("multi body joint feedback");
1034 for (int j=0;j<numPoolConstraints;j++)
1035 {
1036 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
1037
1038 //apply the joint feedback into all links of the btMultiBody
1039 //todo: double-check the signs of the applied impulse
1040
1041 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1042 {
1043 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1044 }
1045 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1046 {
1047 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1048 }
1049 #if 0
1050 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
1051 {
1052 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1053 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
1054 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
1055 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1056
1057 }
1058 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
1059 {
1060 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
1061 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
1062 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
1063 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1064 }
1065
1066 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1067 {
1068 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
1069 {
1070 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1071 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
1072 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
1073 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1074 }
1075
1076 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
1077 {
1078 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
1079 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
1080 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
1081 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
1082 }
1083 }
1084 #endif
1085 }
1086
1087 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1088 {
1089 const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1090 if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1091 {
1092 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1093 }
1094 if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1095 {
1096 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1097 }
1098 }
1099 }
1100
1101 numPoolConstraints = m_multiBodyNonContactConstraints.size();
1102
1103 #if 0
1104 //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1105 for (int i=0;i<numPoolConstraints;i++)
1106 {
1107 const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
1108
1109 btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
1110 btJointFeedback* fb = constr->getJointFeedback();
1111 if (fb)
1112 {
1113 fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1114 fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1115 fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1116 fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1117
1118 }
1119
1120 constr->internalSetAppliedImpulse(c.m_appliedImpulse);
1121 if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1122 {
1123 constr->setEnabled(false);
1124 }
1125
1126 }
1127 #endif
1128 #endif
1129
1130 return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
1131 }
1132
1133
solveMultiBodyGroup(btCollisionObject ** bodies,int numBodies,btPersistentManifold ** manifold,int numManifolds,btTypedConstraint ** constraints,int numConstraints,btMultiBodyConstraint ** multiBodyConstraints,int numMultiBodyConstraints,const btContactSolverInfo & info,btIDebugDraw * debugDrawer,btDispatcher * dispatcher)1134 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)
1135 {
1136 //printf("solveMultiBodyGroup start\n");
1137 m_tmpMultiBodyConstraints = multiBodyConstraints;
1138 m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1139
1140 btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
1141
1142 m_tmpMultiBodyConstraints = 0;
1143 m_tmpNumMultiBodyConstraints = 0;
1144
1145
1146 }
1147