Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletDynamics / Featherstone / btMultiBodyConstraintSolver.cpp
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                 
38                 resolveSingleConstraintRowGeneric(constraint);
39                 if(constraint.m_multiBodyA) 
40                         constraint.m_multiBodyA->setPosUpdated(false);
41                 if(constraint.m_multiBodyB) 
42                         constraint.m_multiBodyB->setPosUpdated(false);
43         }
44
45         //solve featherstone normal contact
46         for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
47         {
48                 btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
49                 if (iteration < infoGlobal.m_numIterations)
50                         resolveSingleConstraintRowGeneric(constraint);
51
52                 if(constraint.m_multiBodyA) 
53                         constraint.m_multiBodyA->setPosUpdated(false);
54                 if(constraint.m_multiBodyB) 
55                         constraint.m_multiBodyB->setPosUpdated(false);
56         }
57         
58         //solve featherstone frictional contact
59
60         for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++)
61         {
62                 if (iteration < infoGlobal.m_numIterations)
63                 {
64                         btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j];
65                         btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
66                         //adjust friction limits here
67                         if (totalImpulse>btScalar(0))
68                         {
69                                 frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
70                                 frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
71                                 resolveSingleConstraintRowGeneric(frictionConstraint);
72
73                                 if(frictionConstraint.m_multiBodyA) 
74                                         frictionConstraint.m_multiBodyA->setPosUpdated(false);
75                                 if(frictionConstraint.m_multiBodyB) 
76                                         frictionConstraint.m_multiBodyB->setPosUpdated(false);
77                         }
78                 }
79         }
80         return val;
81 }
82
83 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
84 {
85         m_multiBodyNonContactConstraints.resize(0);
86         m_multiBodyNormalContactConstraints.resize(0);
87         m_multiBodyFrictionContactConstraints.resize(0);
88         m_data.m_jacobians.resize(0);
89         m_data.m_deltaVelocitiesUnitImpulse.resize(0);
90         m_data.m_deltaVelocities.resize(0);
91
92         for (int i=0;i<numBodies;i++)
93         {
94                 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(bodies[i]);
95                 if (fcA)
96                 {
97                         fcA->m_multiBody->setCompanionId(-1);
98                 }
99         }
100
101         btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
102
103         return val;
104 }
105
106 void    btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
107 {
108     for (int i = 0; i < ndof; ++i) 
109                 m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
110 }
111
112 void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
113 {
114
115         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
116         btScalar deltaVelADotn=0;
117         btScalar deltaVelBDotn=0;
118         btSolverBody* bodyA = 0;
119         btSolverBody* bodyB = 0;
120         int ndofA=0;
121         int ndofB=0;
122
123         if (c.m_multiBodyA)
124         {
125                 ndofA  = c.m_multiBodyA->getNumDofs() + 6;
126                 for (int i = 0; i < ndofA; ++i) 
127                         deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
128         } else if(c.m_solverBodyIdA >= 0)
129         {
130                 bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
131                 deltaVelADotn += c.m_contactNormal1.dot(bodyA->internalGetDeltaLinearVelocity())        + c.m_relpos1CrossNormal.dot(bodyA->internalGetDeltaAngularVelocity());
132         }
133
134         if (c.m_multiBodyB)
135         {
136                 ndofB  = c.m_multiBodyB->getNumDofs() + 6;
137                 for (int i = 0; i < ndofB; ++i) 
138                         deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
139         } else if(c.m_solverBodyIdB >= 0)
140         {
141                 bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
142                 deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity())  + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
143         }
144
145         
146         deltaImpulse    -=      deltaVelADotn*c.m_jacDiagABInv;//m_jacDiagABInv = 1./denom
147         deltaImpulse    -=      deltaVelBDotn*c.m_jacDiagABInv;
148         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
149         
150         if (sum < c.m_lowerLimit)
151         {
152                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
153                 c.m_appliedImpulse = c.m_lowerLimit;
154         }
155         else if (sum > c.m_upperLimit) 
156         {
157                 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
158                 c.m_appliedImpulse = c.m_upperLimit;
159         }
160         else
161         {
162                 c.m_appliedImpulse = sum;
163         }
164
165         if (c.m_multiBodyA)
166         {
167                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
168 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
169                 //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
170                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
171                 c.m_multiBodyA->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
172 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
173         } else if(c.m_solverBodyIdA >= 0)
174         {
175                 bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
176
177         }
178         if (c.m_multiBodyB)
179         {
180                 applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
181 #ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
182                 //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
183                 //it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
184                 c.m_multiBodyB->applyDeltaVeeMultiDof2(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
185 #endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
186         } else if(c.m_solverBodyIdB >= 0)
187         {
188                 bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
189         }
190
191 }
192
193
194
195
196 void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, 
197                                                                                                                                  const btVector3& contactNormal,
198                                                                                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
199                                                                                                                                  btScalar& relaxation,
200                                                                                                                                  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
201 {
202                         
203         BT_PROFILE("setupMultiBodyContactConstraint");
204         btVector3 rel_pos1;
205         btVector3 rel_pos2;
206
207         btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
208         btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
209
210         const btVector3& pos1 = cp.getPositionWorldOnA();
211         const btVector3& pos2 = cp.getPositionWorldOnB();
212
213         btSolverBody* bodyA = multiBodyA ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA];
214         btSolverBody* bodyB = multiBodyB ? 0 : &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB];
215
216         btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
217         btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
218
219         if (bodyA)
220                 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
221         if (bodyB)
222                 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
223
224         relaxation = 1.f;
225
226         
227
228
229         if (multiBodyA)
230         {
231                 if (solverConstraint.m_linkA<0)
232                 {
233                         rel_pos1 = pos1 - multiBodyA->getBasePos();
234                 } else
235                 {
236                         rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
237                 }
238                 const int ndofA  = multiBodyA->getNumDofs() + 6;
239
240                 solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
241
242                 if (solverConstraint.m_deltaVelAindex <0)
243                 {
244                         solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
245                         multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
246                         m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
247                 } else
248                 {
249                         btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
250                 }
251
252                 solverConstraint.m_jacAindex = m_data.m_jacobians.size();
253                 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
254                 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
255                 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
256
257                 btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
258                 multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
259                 btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
260                 multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
261
262                 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
263                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
264                 solverConstraint.m_contactNormal1 = contactNormal;
265         } else
266         {
267                 btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
268                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
269                 solverConstraint.m_contactNormal1 = contactNormal;
270                 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
271         }
272
273         
274
275         if (multiBodyB)
276         {
277                 if (solverConstraint.m_linkB<0)
278                 {
279                         rel_pos2 = pos2 - multiBodyB->getBasePos();
280                 } else
281                 {
282                         rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
283                 }
284
285                 const int ndofB  = multiBodyB->getNumDofs() + 6;
286
287                 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
288                 if (solverConstraint.m_deltaVelBindex <0)
289                 {
290                         solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
291                         multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
292                         m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
293                 }
294
295                 solverConstraint.m_jacBindex = m_data.m_jacobians.size();
296
297                 m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
298                 m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
299                 btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
300
301                 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);
302                 multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
303                 
304                 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);          
305                 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
306                 solverConstraint.m_contactNormal2 = -contactNormal;
307         
308         } else
309         {
310                 btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);          
311                 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
312                 solverConstraint.m_contactNormal2 = -contactNormal;
313         
314                 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
315         }
316
317         {
318                                                 
319                 btVector3 vec;
320                 btScalar denom0 = 0.f;
321                 btScalar denom1 = 0.f;
322                 btScalar* jacB = 0;
323                 btScalar* jacA = 0;
324                 btScalar* lambdaA =0;
325                 btScalar* lambdaB =0;
326                 int ndofA  = 0;
327                 if (multiBodyA)
328                 {
329                         ndofA  = multiBodyA->getNumDofs() + 6;
330                         jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
331                         lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
332                         for (int i = 0; i < ndofA; ++i)
333                         {
334                                 btScalar j = jacA[i] ;
335                                 btScalar l =lambdaA[i];
336                                 denom0 += j*l;
337                         }
338                 } else
339                 {
340                         if (rb0)
341                         {
342                                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
343                                 denom0 = rb0->getInvMass() + contactNormal.dot(vec);
344                         }
345                 }
346                 if (multiBodyB)
347                 {
348                         const int ndofB  = multiBodyB->getNumDofs() + 6;
349                         jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
350                         lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
351                         for (int i = 0; i < ndofB; ++i)
352                         {
353                                 btScalar j = jacB[i] ;
354                                 btScalar l =lambdaB[i];
355                                 denom1 += j*l;
356                         }
357
358                 } else
359                 {
360                         if (rb1)
361                         {
362                                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
363                                 denom1 = rb1->getInvMass() + contactNormal.dot(vec);
364                         }
365                 }
366
367                  
368
369                  btScalar d = denom0+denom1;
370                  if (d>SIMD_EPSILON)
371                  {
372                         solverConstraint.m_jacDiagABInv = relaxation/(d);
373                  } else
374                  {
375                         //disable the constraint row to handle singularity/redundant constraint
376                         solverConstraint.m_jacDiagABInv  = 0.f;
377                  }
378                 
379         }
380
381         
382         //compute rhs and remaining solverConstraint fields
383
384         
385
386         btScalar restitution = 0.f;
387         btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
388
389         btScalar rel_vel = 0.f;
390         int ndofA  = 0;
391         int ndofB  = 0;
392         {
393
394                 btVector3 vel1,vel2;
395                 if (multiBodyA)
396                 {
397                         ndofA  = multiBodyA->getNumDofs() + 6;
398                         btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
399                         for (int i = 0; i < ndofA ; ++i) 
400                                 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
401                 } else
402                 {
403                         if (rb0)
404                         {
405                                 rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
406                         }
407                 }
408                 if (multiBodyB)
409                 {
410                         ndofB  = multiBodyB->getNumDofs() + 6;
411                         btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
412                         for (int i = 0; i < ndofB ; ++i) 
413                                 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
414
415                 } else
416                 {
417                         if (rb1)
418                         {
419                                 rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
420                         }
421                 }
422
423                 solverConstraint.m_friction = cp.m_combinedFriction;
424
425                 if(!isFriction)
426                 {
427                         restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);     
428                         if (restitution <= btScalar(0.))
429                         {
430                                 restitution = 0.f;
431                         }
432                 }
433         }
434
435
436         ///warm starting (or zero if disabled)
437         //disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
438         if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
439         {
440                 solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
441
442                 if (solverConstraint.m_appliedImpulse)
443                 {
444                         if (multiBodyA)
445                         {
446                                 btScalar impulse = solverConstraint.m_appliedImpulse;
447                                 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
448                                 multiBodyA->applyDeltaVeeMultiDof(deltaV,impulse);
449                                 
450                                 applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
451                         } else
452                         {
453                                 if (rb0)
454                                         bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
455                         }
456                         if (multiBodyB)
457                         {
458                                 btScalar impulse = solverConstraint.m_appliedImpulse;
459                                 btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
460                                 multiBodyB->applyDeltaVeeMultiDof(deltaV,impulse);
461                                 applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
462                         } else
463                         {
464                                 if (rb1)
465                                         bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
466                         }
467                 }
468         } else
469         {
470                 solverConstraint.m_appliedImpulse = 0.f;
471         }
472
473         solverConstraint.m_appliedPushImpulse = 0.f;
474
475         {
476
477                 btScalar positionalError = 0.f;
478                 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
479
480                 btScalar erp = infoGlobal.m_erp2;
481                 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
482                 {
483                         erp = infoGlobal.m_erp;
484                 }
485
486                 if (penetration>0)
487                 {
488                         positionalError = 0;
489                         velocityError -= penetration / infoGlobal.m_timeStep;
490
491                 } else
492                 {
493                         positionalError = -penetration * erp/infoGlobal.m_timeStep;
494                 }
495
496                 btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
497                 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
498
499                 if(!isFriction)
500                 {
501                         if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
502                         {
503                                 //combine position and velocity into rhs
504                                 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
505                                 solverConstraint.m_rhsPenetration = 0.f;
506
507                         } else
508                         {
509                                 //split position and velocity into rhs and m_rhsPenetration
510                                 solverConstraint.m_rhs = velocityImpulse;
511                                 solverConstraint.m_rhsPenetration = penetrationImpulse;
512                         }
513
514                         solverConstraint.m_lowerLimit = 0;
515                         solverConstraint.m_upperLimit = 1e10f;
516                 }
517                 else
518                 {
519                         solverConstraint.m_rhs = velocityImpulse;
520                         solverConstraint.m_rhsPenetration = 0.f;
521                         solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
522                         solverConstraint.m_upperLimit = solverConstraint.m_friction;
523                 }
524
525                 solverConstraint.m_cfm = 0.f;                   //why not use cfmSlip?
526         }
527
528 }
529
530
531
532
533 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)
534 {
535         BT_PROFILE("addMultiBodyFrictionConstraint");
536         btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
537     solverConstraint.m_orgConstraint = 0;
538     solverConstraint.m_orgDofIndex = -1;
539     
540         solverConstraint.m_frictionIndex = frictionIndex;
541         bool isFriction = true;
542
543         const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
544         const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
545         
546         btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
547         btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
548
549         int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
550         int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
551
552         solverConstraint.m_solverBodyIdA = solverBodyIdA;
553         solverConstraint.m_solverBodyIdB = solverBodyIdB;
554         solverConstraint.m_multiBodyA = mbA;
555         if (mbA)
556                 solverConstraint.m_linkA = fcA->m_link;
557
558         solverConstraint.m_multiBodyB = mbB;
559         if (mbB)
560                 solverConstraint.m_linkB = fcB->m_link;
561
562         solverConstraint.m_originalContactPoint = &cp;
563
564         setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip);
565         return solverConstraint;
566 }
567
568 void    btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
569 {
570         const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
571         const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
572         
573         btMultiBody* mbA = fcA? fcA->m_multiBody : 0;
574         btMultiBody* mbB = fcB? fcB->m_multiBody : 0;
575
576         btCollisionObject* colObj0=0,*colObj1=0;
577
578         colObj0 = (btCollisionObject*)manifold->getBody0();
579         colObj1 = (btCollisionObject*)manifold->getBody1();
580
581         int solverBodyIdA = mbA? -1 : getOrInitSolverBody(*colObj0,infoGlobal.m_timeStep);
582         int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1,infoGlobal.m_timeStep);
583
584 //      btSolverBody* solverBodyA = mbA ? 0 : &m_tmpSolverBodyPool[solverBodyIdA];
585 //      btSolverBody* solverBodyB = mbB ? 0 : &m_tmpSolverBodyPool[solverBodyIdB];
586
587
588         ///avoid collision response between two static objects
589 //      if (!solverBodyA || (solverBodyA->m_invMass.isZero() && (!solverBodyB || solverBodyB->m_invMass.isZero())))
590         //      return;
591
592
593
594         for (int j=0;j<manifold->getNumContacts();j++)
595         {
596
597                 btManifoldPoint& cp = manifold->getContactPoint(j);
598
599                 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
600                 {
601                 
602                         btScalar relaxation;
603
604                         int frictionIndex = m_multiBodyNormalContactConstraints.size();
605
606                         btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
607
608         //              btRigidBody* rb0 = btRigidBody::upcast(colObj0);
609         //              btRigidBody* rb1 = btRigidBody::upcast(colObj1);
610             solverConstraint.m_orgConstraint = 0;
611             solverConstraint.m_orgDofIndex = -1;
612                         solverConstraint.m_solverBodyIdA = solverBodyIdA;
613                         solverConstraint.m_solverBodyIdB = solverBodyIdB;
614                         solverConstraint.m_multiBodyA = mbA;
615                         if (mbA)
616                                 solverConstraint.m_linkA = fcA->m_link;
617
618                         solverConstraint.m_multiBodyB = mbB;
619                         if (mbB)
620                                 solverConstraint.m_linkB = fcB->m_link;
621
622                         solverConstraint.m_originalContactPoint = &cp;
623
624                         bool isFriction = false;
625                         setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB,cp, infoGlobal, relaxation, isFriction);
626
627 //                      const btVector3& pos1 = cp.getPositionWorldOnA();
628 //                      const btVector3& pos2 = cp.getPositionWorldOnB();
629
630                         /////setup the friction constraints
631 #define ENABLE_FRICTION
632 #ifdef ENABLE_FRICTION
633                         solverConstraint.m_frictionIndex = frictionIndex;
634 #if ROLLING_FRICTION
635         int rollingFriction=1;
636                         btVector3 angVelA(0,0,0),angVelB(0,0,0);
637                         if (rb0)
638                                 angVelA = rb0->getAngularVelocity();
639                         if (rb1)
640                                 angVelB = rb1->getAngularVelocity();
641                         btVector3 relAngVel = angVelB-angVelA;
642
643                         if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
644                         {
645                                 //only a single rollingFriction per manifold
646                                 rollingFriction--;
647                                 if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
648                                 {
649                                         relAngVel.normalize();
650                                         applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
651                                         applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
652                                         if (relAngVel.length()>0.001)
653                                                 addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
654
655                                 } else
656                                 {
657                                         addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
658                                         btVector3 axis0,axis1;
659                                         btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
660                                         applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
661                                         applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
662                                         applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
663                                         applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
664                                         if (axis0.length()>0.001)
665                                                 addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
666                                         if (axis1.length()>0.001)
667                                                 addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
668                 
669                                 }
670                         }
671 #endif //ROLLING_FRICTION
672
673                         ///Bullet has several options to set the friction directions
674                         ///By default, each contact has only a single friction direction that is recomputed automatically very frame 
675                         ///based on the relative linear velocity.
676                         ///If the relative velocity it zero, it will automatically compute a friction direction.
677                         
678                         ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
679                         ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
680                         ///
681                         ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
682                         ///
683                         ///The user can manually override the friction directions for certain contacts using a contact callback, 
684                         ///and set the cp.m_lateralFrictionInitialized to true
685                         ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
686                         ///this will give a conveyor belt effect
687                         ///
688                         if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
689                         {/*
690                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
691                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
692                                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
693                                 {
694                                         cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
695                                         if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
696                                         {
697                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
698                                                 cp.m_lateralFrictionDir2.normalize();//??
699                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
700                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
701                                                 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
702
703                                         }
704
705                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
706                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
707                                         addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
708
709                                 } else
710                                 */
711                                 {
712                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
713
714                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
715                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
716                                         addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
717
718                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
719                                         {
720                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
721                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
722                                                 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
723                                         }
724
725                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
726                                         {
727                                                 cp.m_lateralFrictionInitialized = true;
728                                         }
729                                 }
730
731                         } else
732                         {
733                                 addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
734
735                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
736                                         addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
737
738                                 //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
739                                 //todo:
740                                 solverConstraint.m_appliedImpulse = 0.f;
741                                 solverConstraint.m_appliedPushImpulse = 0.f;
742                         }
743                 
744
745 #endif //ENABLE_FRICTION
746
747                 }
748         }
749 }
750
751 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal)
752 {
753         //btPersistentManifold* manifold = 0;
754
755         for (int i=0;i<numManifolds;i++)
756         {
757                 btPersistentManifold* manifold= manifoldPtr[i];
758                 const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
759                 const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
760                 if (!fcA && !fcB)
761                 {
762                         //the contact doesn't involve any Featherstone btMultiBody, so deal with the regular btRigidBody/btCollisionObject case
763                         convertContact(manifold,infoGlobal);
764                 } else
765                 {
766                         convertMultiBodyContact(manifold,infoGlobal);
767                 }
768         }
769
770         //also convert the multibody constraints, if any
771
772         
773         for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
774         {
775                 btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
776                 m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
777                 m_data.m_fixedBodyId = m_fixedBodyId;
778                 
779                 c->createConstraintRows(m_multiBodyNonContactConstraints,m_data,        infoGlobal);
780         }
781
782 }
783
784
785
786 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
787 {
788         return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
789 }
790
791 #if 0
792 static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
793 {
794         if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
795         {
796                 //todo: get rid of those temporary memory allocations for the joint feedback
797                 btAlignedObjectArray<btScalar> forceVector;
798                 int numDofsPlusBase = 6+mb->getNumDofs();
799                 forceVector.resize(numDofsPlusBase);
800                 for (int i=0;i<numDofsPlusBase;i++)
801                 {
802                         forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
803                 }
804                 btAlignedObjectArray<btScalar> output;
805                 output.resize(numDofsPlusBase);
806                 bool applyJointFeedback = true;
807                 mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
808         }
809 }
810 #endif
811
812 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
813 {
814 #if 1 
815         
816         //bod->addBaseForce(m_gravity * bod->getBaseMass());
817         //bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
818
819         if (c.m_orgConstraint)
820         {
821                 c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
822         }
823         
824
825         if (c.m_multiBodyA)
826         {
827                 
828                 c.m_multiBodyA->setCompanionId(-1);
829                 btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
830                 btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
831                 if (c.m_linkA<0)
832                 {
833                         c.m_multiBodyA->addBaseConstraintForce(force);
834                         c.m_multiBodyA->addBaseConstraintTorque(torque);
835                 } else
836                 {
837                         c.m_multiBodyA->addLinkConstraintForce(c.m_linkA,force);
838                                 //b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
839                         c.m_multiBodyA->addLinkConstraintTorque(c.m_linkA,torque);
840                 }
841         }
842         
843         if (c.m_multiBodyB)
844         {
845                 {
846                         c.m_multiBodyB->setCompanionId(-1);
847                         btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
848                         btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
849                         if (c.m_linkB<0)
850                         {
851                                 c.m_multiBodyB->addBaseConstraintForce(force);
852                                 c.m_multiBodyB->addBaseConstraintTorque(torque);
853                         } else
854                         {
855                                 {
856                                         c.m_multiBodyB->addLinkConstraintForce(c.m_linkB,force);
857                                         //b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
858                                         c.m_multiBodyB->addLinkConstraintTorque(c.m_linkB,torque);
859                                 }
860                                         
861                         }
862                 }
863         }
864 #endif
865
866 #ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
867
868         if (c.m_multiBodyA)
869         {
870                 
871                 if(c.m_multiBodyA->isMultiDof())
872                 {
873                         c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
874                 }
875                 else
876                 {
877                         c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
878                 }
879         }
880         
881         if (c.m_multiBodyB)
882         {
883                 if(c.m_multiBodyB->isMultiDof())
884                 {
885                         c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
886                 }
887                 else
888                 {
889                         c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
890                 }
891         }
892 #endif
893
894
895
896 }
897
898 btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
899 {
900         BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
901         int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
902         
903
904         //write back the delta v to the multi bodies, either as applied impulse (direct velocity change) 
905         //or as applied force, so we can measure the joint reaction forces easier
906         for (int i=0;i<numPoolConstraints;i++)
907         {
908                 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
909                 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
910
911                 writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
912
913                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
914                 {
915                         writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
916                 }
917         }
918
919
920         for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
921         {
922                 btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
923                 writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
924         }
925
926         
927         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
928         {
929                 BT_PROFILE("warm starting write back");
930                 for (int j=0;j<numPoolConstraints;j++)
931                 {
932                         const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
933                         btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
934                         btAssert(pt);
935                         pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
936                         pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
937                         
938                         //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
939                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
940                         {
941                                 pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
942                         }
943                         //do a callback here?
944                 }
945         }
946 #if 0
947         //multibody joint feedback
948         {
949                 BT_PROFILE("multi body joint feedback");
950                 for (int j=0;j<numPoolConstraints;j++)
951                 {
952                         const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
953                 
954                         //apply the joint feedback into all links of the btMultiBody
955                         //todo: double-check the signs of the applied impulse
956
957                         if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
958                         {
959                                 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
960                         }
961                         if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
962                         {
963                                 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
964                         }
965 #if 0
966                         if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
967                         {
968                                 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
969                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
970                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
971                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
972
973                         }
974                         if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
975                         {
976                                 applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
977                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
978                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
979                                         m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
980                         }
981                 
982                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
983                         {
984                                 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
985                                 {
986                                         applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
987                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
988                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
989                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
990                                 }
991
992                                 if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
993                                 {
994                                         applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
995                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
996                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
997                                                 m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
998                                 }
999                         }
1000 #endif
1001                 }
1002         
1003                 for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
1004                 {
1005                         const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
1006                         if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
1007                         {
1008                                 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1009                         }
1010                         if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
1011                         {
1012                                 applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
1013                         }
1014                 }
1015         }
1016
1017         numPoolConstraints = m_multiBodyNonContactConstraints.size();
1018
1019 #if 0
1020         //@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
1021         for (int i=0;i<numPoolConstraints;i++)
1022         {
1023                 const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
1024
1025                 btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
1026                 btJointFeedback* fb = constr->getJointFeedback();
1027                 if (fb)
1028                 {
1029                         fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1030                         fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1031                         fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
1032                         fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
1033                         
1034                 }
1035
1036                 constr->internalSetAppliedImpulse(c.m_appliedImpulse);
1037                 if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1038                 {
1039                         constr->setEnabled(false);
1040                 }
1041
1042         }
1043 #endif 
1044 #endif
1045
1046         return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
1047 }
1048
1049
1050 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)
1051 {
1052         //printf("solveMultiBodyGroup start\n");
1053         m_tmpMultiBodyConstraints = multiBodyConstraints;
1054         m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
1055         
1056         btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
1057
1058         m_tmpMultiBodyConstraints = 0;
1059         m_tmpNumMultiBodyConstraints = 0;
1060         
1061
1062 }