2911d52e5ea470e42919ce4fae34fd561550a2e5
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btSequentialImpulseConstraintSolver.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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 //#define COMPUTE_IMPULSE_DENOM 1
17 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
18 //#define FORCE_REFESH_CONTACT_MANIFOLDS 1
19
20 #include "btSequentialImpulseConstraintSolver.h"
21 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
22 #include "BulletDynamics/Dynamics/btRigidBody.h"
23 #include "btContactConstraint.h"
24 #include "btSolve2LinearConstraint.h"
25 #include "btContactSolverInfo.h"
26 #include "LinearMath/btIDebugDraw.h"
27 #include "btJacobianEntry.h"
28 #include "LinearMath/btMinMax.h"
29 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
30 #include <new>
31 #include "LinearMath/btStackAlloc.h"
32 #include "LinearMath/btQuickprof.h"
33 #include "btSolverBody.h"
34 #include "btSolverConstraint.h"
35
36
37 #include "LinearMath/btAlignedObjectArray.h"
38
39
40 int totalCpd = 0;
41
42 int     gTotalContactPoints = 0;
43
44 struct  btOrderIndex
45 {
46         int     m_manifoldIndex;
47         int     m_pointIndex;
48 };
49
50
51
52 #define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
53 static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
54
55
56 unsigned long btSequentialImpulseConstraintSolver::btRand2()
57 {
58   m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
59   return m_btSeed2;
60 }
61
62
63
64 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
65 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
66 {
67   // seems good; xor-fold and modulus
68   const unsigned long un = static_cast<unsigned long>(n);
69   unsigned long r = btRand2();
70
71   // note: probably more aggressive than it needs to be -- might be
72   //       able to get away without one or two of the innermost branches.
73   if (un <= 0x00010000UL) {
74     r ^= (r >> 16);
75     if (un <= 0x00000100UL) {
76       r ^= (r >> 8);
77       if (un <= 0x00000010UL) {
78         r ^= (r >> 4);
79         if (un <= 0x00000004UL) {
80           r ^= (r >> 2);
81           if (un <= 0x00000002UL) {
82             r ^= (r >> 1);
83           }
84         }
85      }
86     }
87    }
88
89   return (int) (r % un);
90 }
91
92
93
94
95 bool  MyContactDestroyedCallback(void* userPersistentData);
96 bool  MyContactDestroyedCallback(void* userPersistentData)
97 {
98         assert (userPersistentData);
99         btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
100         btAlignedFree(cpd);
101         totalCpd--;
102         //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
103         return true;
104 }
105
106
107
108 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
109 :m_btSeed2(0)
110 {
111         gContactDestroyedCallback = &MyContactDestroyedCallback;
112
113         //initialize default friction/contact funcs
114         int i,j;
115         for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
116                 for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
117                 {
118
119                         m_contactDispatch[i][j] = resolveSingleCollision;
120                         m_frictionDispatch[i][j] = resolveSingleFriction;
121                 }
122 }
123
124 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
125 {
126
127 }
128
129 void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
130 void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
131 {
132         btRigidBody* rb = btRigidBody::upcast(collisionObject);
133         if (rb)
134         {
135                 solverBody->m_angularVelocity = rb->getAngularVelocity() ;
136                 solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
137                 solverBody->m_friction = collisionObject->getFriction();
138                 solverBody->m_invMass = rb->getInvMass();
139                 solverBody->m_linearVelocity = rb->getLinearVelocity();
140                 solverBody->m_originalBody = rb;
141                 solverBody->m_angularFactor = rb->getAngularFactor();
142         } else
143         {
144                 solverBody->m_angularVelocity.setValue(0,0,0);
145                 solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
146                 solverBody->m_friction = collisionObject->getFriction();
147                 solverBody->m_invMass = 0.f;
148                 solverBody->m_linearVelocity.setValue(0,0,0);
149                 solverBody->m_originalBody = 0;
150                 solverBody->m_angularFactor = 1.f;
151         }
152         
153         solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
154         solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
155 }
156
157
158 int             gNumSplitImpulseRecoveries = 0;
159
160 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
161 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
162 {
163         btScalar rest = restitution * -rel_vel;
164         return rest;
165 }
166
167
168 void    resolveSplitPenetrationImpulseCacheFriendly(
169         btSolverBody& body1,
170         btSolverBody& body2,
171         const btSolverConstraint& contactConstraint,
172         const btContactSolverInfo& solverInfo);
173
174 //SIMD_FORCE_INLINE
175 void    resolveSplitPenetrationImpulseCacheFriendly(
176         btSolverBody& body1,
177         btSolverBody& body2,
178         const btSolverConstraint& contactConstraint,
179         const btContactSolverInfo& solverInfo)
180 {
181         (void)solverInfo;
182
183                 if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
184         {
185
186                                 gNumSplitImpulseRecoveries++;
187                 btScalar normalImpulse;
188
189                 //  Optimized version of projected relative velocity, use precomputed cross products with normal
190                 //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
191                 //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
192                 //      btVector3 vel = vel1 - vel2;
193                 //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
194
195                 btScalar rel_vel;
196                 btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
197                 + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
198                 btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
199                 + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
200
201                 rel_vel = vel1Dotn-vel2Dotn;
202
203
204                                 btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
205                 //      btScalar positionalError = contactConstraint.m_penetration;
206
207                 btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
208
209                 btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
210                 btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
211                 normalImpulse = penetrationImpulse+velocityImpulse;
212
213                 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
214                 btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
215                 btScalar sum = oldNormalImpulse + normalImpulse;
216                 contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
217
218                 normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
219
220                                 body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
221                
222                                 body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
223                
224         }
225
226 }
227
228
229 //velocity + friction
230 //response  between two dynamic objects with friction
231
232 btScalar resolveSingleCollisionCombinedCacheFriendly(
233         btSolverBody& body1,
234         btSolverBody& body2,
235         const btSolverConstraint& contactConstraint,
236         const btContactSolverInfo& solverInfo);
237
238 //SIMD_FORCE_INLINE 
239 btScalar resolveSingleCollisionCombinedCacheFriendly(
240         btSolverBody& body1,
241         btSolverBody& body2,
242         const btSolverConstraint& contactConstraint,
243         const btContactSolverInfo& solverInfo)
244 {
245         (void)solverInfo;
246
247         btScalar normalImpulse;
248
249         {
250
251                 
252                 //  Optimized version of projected relative velocity, use precomputed cross products with normal
253                 //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
254                 //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
255                 //      btVector3 vel = vel1 - vel2;
256                 //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
257
258                 btScalar rel_vel;
259                 btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
260                                         + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
261                 btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
262                                         + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
263
264                 rel_vel = vel1Dotn-vel2Dotn;
265
266                 btScalar positionalError = 0.f;
267                 if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
268                 {
269                         positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
270                 }
271
272                 btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
273
274                 btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
275                 btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
276                 normalImpulse = penetrationImpulse+velocityImpulse;
277                 
278                 
279                 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
280                 btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
281                 btScalar sum = oldNormalImpulse + normalImpulse;
282                 contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
283
284                 normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
285
286                 body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
287                                 contactConstraint.m_angularComponentA,normalImpulse);
288                 
289                 body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
290                                 contactConstraint.m_angularComponentB,-normalImpulse);
291         }
292
293         return normalImpulse;
294 }
295
296 //#define NO_FRICTION_TANGENTIALS 1
297 #ifndef NO_FRICTION_TANGENTIALS
298
299 btScalar resolveSingleFrictionCacheFriendly(
300         btSolverBody& body1,
301         btSolverBody& body2,
302         const btSolverConstraint& contactConstraint,
303         const btContactSolverInfo& solverInfo,
304         btScalar appliedNormalImpulse);
305
306 //SIMD_FORCE_INLINE 
307 btScalar resolveSingleFrictionCacheFriendly(
308         btSolverBody& body1,
309         btSolverBody& body2,
310         const btSolverConstraint& contactConstraint,
311         const btContactSolverInfo& solverInfo,
312         btScalar appliedNormalImpulse)
313 {
314         (void)solverInfo;
315
316         
317         const btScalar combinedFriction = contactConstraint.m_friction;
318         
319         const btScalar limit = appliedNormalImpulse * combinedFriction;
320         
321         if (appliedNormalImpulse>btScalar(0.))
322         //friction
323         {
324                 
325                 btScalar j1;
326                 {
327
328                         btScalar rel_vel;
329                         const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
330                                                 + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
331                         const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
332                                 + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
333                         rel_vel = vel1Dotn-vel2Dotn;
334
335                         // calculate j that moves us to zero relative velocity
336                         j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
337 #define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
338 #ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
339                         btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
340                         contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
341                         
342                         if (limit < contactConstraint.m_appliedImpulse)
343                         {
344                                 contactConstraint.m_appliedImpulse = limit;
345                         } else
346                         {
347                                 if (contactConstraint.m_appliedImpulse < -limit)
348                                         contactConstraint.m_appliedImpulse = -limit;
349                         }
350                         j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
351 #else
352                         if (limit < j1)
353                         {
354                                 j1 = limit;
355                         } else
356                         {
357                                 if (j1 < -limit)
358                                         j1 = -limit;
359                         }
360
361 #endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
362
363                         //GEN_set_min(contactConstraint.m_appliedImpulse, limit);
364                         //GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
365
366                         
367
368                 }
369         
370                 body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
371                 
372                 body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
373
374         } 
375         return 0.f;
376 }
377
378
379 #else
380
381 //velocity + friction
382 //response  between two dynamic objects with friction
383 btScalar resolveSingleFrictionCacheFriendly(
384         btSolverBody& body1,
385         btSolverBody& body2,
386         btSolverConstraint& contactConstraint,
387         const btContactSolverInfo& solverInfo)
388 {
389
390         btVector3 vel1;
391         btVector3 vel2;
392         btScalar normalImpulse(0.f);
393
394         {
395                 const btVector3& normal = contactConstraint.m_contactNormal;
396                 if (contactConstraint.m_penetration < 0.f)
397                         return 0.f;
398
399
400                 body1.getVelocityInLocalPoint(contactConstraint.m_relpos1CrossNormal,vel1);
401                 body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
402                 btVector3 vel = vel1 - vel2;
403                 btScalar rel_vel;
404                 rel_vel = normal.dot(vel);
405
406                 btVector3 lat_vel = vel - normal * rel_vel;
407                 btScalar lat_rel_vel = lat_vel.length2();
408
409                 btScalar combinedFriction = contactConstraint.m_friction;
410                 const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
411                 const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
412
413
414                 if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
415                 {
416                         lat_rel_vel = btSqrt(lat_rel_vel);
417
418                         lat_vel /= lat_rel_vel;
419                         btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
420                         btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
421                         btScalar friction_impulse = lat_rel_vel /
422                                 (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
423                         btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
424
425                         btSetMin(friction_impulse, normal_impulse);
426                         btSetMin(friction_impulse, -normal_impulse);
427                         body1.internalApplyImpulse(lat_vel * -friction_impulse, rel_pos1);
428                         body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
429                 }
430         }
431
432         return normalImpulse;
433 }
434
435 #endif //NO_FRICTION_TANGENTIALS
436
437
438
439
440
441 void    btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation)
442 {
443
444         btRigidBody* body0=btRigidBody::upcast(colObj0);
445         btRigidBody* body1=btRigidBody::upcast(colObj1);
446
447         btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
448         solverConstraint.m_contactNormal = normalAxis;
449
450         solverConstraint.m_solverBodyIdA = solverBodyIdA;
451         solverConstraint.m_solverBodyIdB = solverBodyIdB;
452         solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
453         solverConstraint.m_frictionIndex = frictionIndex;
454
455         solverConstraint.m_friction = cp.m_combinedFriction;
456         solverConstraint.m_originalContactPoint = 0;
457
458         solverConstraint.m_appliedImpulse = btScalar(0.);
459         solverConstraint.m_appliedPushImpulse = 0.f;
460         solverConstraint.m_penetration = 0.f;
461         {
462                 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
463                 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
464                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
465         }
466         {
467                 btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
468                 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
469                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
470         }
471
472 #ifdef COMPUTE_IMPULSE_DENOM
473         btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
474         btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
475 #else
476         btVector3 vec;
477         btScalar denom0 = 0.f;
478         btScalar denom1 = 0.f;
479         if (body0)
480         {
481                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
482                 denom0 = body0->getInvMass() + normalAxis.dot(vec);
483         }
484         if (body1)
485         {
486                 vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
487                 denom1 = body1->getInvMass() + normalAxis.dot(vec);
488         }
489
490
491 #endif //COMPUTE_IMPULSE_DENOM
492         btScalar denom = relaxation/(denom0+denom1);
493         solverConstraint.m_jacDiagABInv = denom;
494
495
496 }
497
498
499 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
500 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
501 {
502         if (colObj && colObj->hasAnisotropicFriction())
503         {
504                 // transform to local coordinates
505                 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
506                 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
507                 //apply anisotropic friction
508                 loc_lateral *= friction_scaling;
509                 // ... and transform it back to global coordinates
510                 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
511         }
512 }
513
514
515
516
517 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
518 {
519         BT_PROFILE("solveGroupCacheFriendlySetup");
520         (void)stackAlloc;
521         (void)debugDrawer;
522
523
524         if (!(numConstraints + numManifolds))
525         {
526 //              printf("empty\n");
527                 return 0.f;
528         }
529         btPersistentManifold* manifold = 0;
530         btCollisionObject* colObj0=0,*colObj1=0;
531
532         //btRigidBody* rb0=0,*rb1=0;
533
534
535 #ifdef FORCE_REFESH_CONTACT_MANIFOLDS
536
537         BEGIN_PROFILE("refreshManifolds");
538
539         int i;
540         
541         
542
543         for (i=0;i<numManifolds;i++)
544         {
545                 manifold = manifoldPtr[i];
546                 rb1 = (btRigidBody*)manifold->getBody1();
547                 rb0 = (btRigidBody*)manifold->getBody0();
548                 
549                 manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
550
551         }
552
553         END_PROFILE("refreshManifolds");
554 #endif //FORCE_REFESH_CONTACT_MANIFOLDS
555
556         
557
558
559
560         //int sizeofSB = sizeof(btSolverBody);
561         //int sizeofSC = sizeof(btSolverConstraint);
562
563
564         //if (1)
565         {
566                 //if m_stackAlloc, try to pack bodies/constraints to speed up solving
567 //              btBlock*                                        sablock;
568 //              sablock = stackAlloc->beginBlock();
569
570         //      int memsize = 16;
571 //              unsigned char* stackMemory = stackAlloc->allocate(memsize);
572
573                 
574                 //todo: use stack allocator for this temp memory
575 //              int minReservation = numManifolds*2;
576
577                 //m_tmpSolverBodyPool.reserve(minReservation);
578
579                 //don't convert all bodies, only the one we need so solver the constraints
580 /*
581                 {
582                         for (int i=0;i<numBodies;i++)
583                         {
584                                 btRigidBody* rb = btRigidBody::upcast(bodies[i]);
585                                 if (rb &&       (rb->getIslandTag() >= 0))
586                                 {
587                                         btAssert(rb->getCompanionId() < 0);
588                                         int solverBodyId = m_tmpSolverBodyPool.size();
589                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
590                                         initSolverBody(&solverBody,rb);
591                                         rb->setCompanionId(solverBodyId);
592                                 } 
593                         }
594                 }
595 */
596                 
597                 //m_tmpSolverConstraintPool.reserve(minReservation);
598                 //m_tmpSolverFrictionConstraintPool.reserve(minReservation);
599
600                 {
601                         int i;
602
603                         for (i=0;i<numManifolds;i++)
604                         {
605                                 manifold = manifoldPtr[i];
606                                 colObj0 = (btCollisionObject*)manifold->getBody0();
607                                 colObj1 = (btCollisionObject*)manifold->getBody1();
608                         
609                                 int solverBodyIdA=-1;
610                                 int solverBodyIdB=-1;
611
612                                 if (manifold->getNumContacts())
613                                 {
614
615                                         
616
617                                         if (colObj0->getIslandTag() >= 0)
618                                         {
619                                                 if (colObj0->getCompanionId() >= 0)
620                                                 {
621                                                         //body has already been converted
622                                                         solverBodyIdA = colObj0->getCompanionId();
623                                                 } else
624                                                 {
625                                                         solverBodyIdA = m_tmpSolverBodyPool.size();
626                                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
627                                                         initSolverBody(&solverBody,colObj0);
628                                                         colObj0->setCompanionId(solverBodyIdA);
629                                                 }
630                                         } else
631                                         {
632                                                 //create a static body
633                                                 solverBodyIdA = m_tmpSolverBodyPool.size();
634                                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
635                                                 initSolverBody(&solverBody,colObj0);
636                                         }
637
638                                         if (colObj1->getIslandTag() >= 0)
639                                         {
640                                                 if (colObj1->getCompanionId() >= 0)
641                                                 {
642                                                         solverBodyIdB = colObj1->getCompanionId();
643                                                 } else
644                                                 {
645                                                         solverBodyIdB = m_tmpSolverBodyPool.size();
646                                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
647                                                         initSolverBody(&solverBody,colObj1);
648                                                         colObj1->setCompanionId(solverBodyIdB);
649                                                 }
650                                         } else
651                                         {
652                                                 //create a static body
653                                                 solverBodyIdB = m_tmpSolverBodyPool.size();
654                                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
655                                                 initSolverBody(&solverBody,colObj1);
656                                         }
657                                 }
658
659                                 btVector3 rel_pos1;
660                                 btVector3 rel_pos2;
661                                 btScalar relaxation;
662
663                                 for (int j=0;j<manifold->getNumContacts();j++)
664                                 {
665                                         
666                                         btManifoldPoint& cp = manifold->getContactPoint(j);
667                                         
668                                         if (cp.getDistance() <= btScalar(0.))
669                                         {
670                                                 
671                                                 const btVector3& pos1 = cp.getPositionWorldOnA();
672                                                 const btVector3& pos2 = cp.getPositionWorldOnB();
673
674                                                  rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
675                                                  rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
676
677                                                 
678                                                 relaxation = 1.f;
679                                                 btScalar rel_vel;
680                                                 btVector3 vel;
681
682                                                 int frictionIndex = m_tmpSolverConstraintPool.size();
683
684                                                 {
685                                                         btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
686                                                         btRigidBody* rb0 = btRigidBody::upcast(colObj0);
687                                                         btRigidBody* rb1 = btRigidBody::upcast(colObj1);
688
689                                                         solverConstraint.m_solverBodyIdA = solverBodyIdA;
690                                                         solverConstraint.m_solverBodyIdB = solverBodyIdB;
691                                                         solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
692
693                                                         solverConstraint.m_originalContactPoint = &cp;
694
695                                                         btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
696                                                         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
697                                                         btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
698                                                         solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
699                                                         {
700 #ifdef COMPUTE_IMPULSE_DENOM
701                                                                 btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
702                                                                 btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
703 #else                                                   
704                                                                 btVector3 vec;
705                                                                 btScalar denom0 = 0.f;
706                                                                 btScalar denom1 = 0.f;
707                                                                 if (rb0)
708                                                                 {
709                                                                         vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
710                                                                         denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
711                                                                 }
712                                                                 if (rb1)
713                                                                 {
714                                                                         vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
715                                                                         denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
716                                                                 }
717 #endif //COMPUTE_IMPULSE_DENOM          
718                                                                 
719                                                                 btScalar denom = relaxation/(denom0+denom1);
720                                                                 solverConstraint.m_jacDiagABInv = denom;
721                                                         }
722
723                                                         solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
724                                                         solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
725                                                         solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
726
727
728                                                         btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
729                                                         btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
730                                 
731                                                         vel  = vel1 - vel2;
732                                                         
733                                                         rel_vel = cp.m_normalWorldOnB.dot(vel);
734                                                         
735                                                         solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
736                                                         //solverConstraint.m_penetration = cp.getDistance();
737
738                                                         solverConstraint.m_friction = cp.m_combinedFriction;
739                                                         solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
740                                                         if (solverConstraint.m_restitution <= btScalar(0.))
741                                                         {
742                                                                 solverConstraint.m_restitution = 0.f;
743                                                         };
744
745                                                         
746                                                         btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
747
748                                                         
749
750                                                         if (solverConstraint.m_restitution > penVel)
751                                                         {
752                                                                 solverConstraint.m_penetration = btScalar(0.);
753                                                         }
754                                                          
755                                                         
756                                                         
757                                                         ///warm starting (or zero if disabled)
758                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
759                                                         {
760                                                                 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
761                                                                 if (rb0)
762                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
763                                                                 if (rb1)
764                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
765                                                         } else
766                                                         {
767                                                                 solverConstraint.m_appliedImpulse = 0.f;
768                                                         }
769
770                                                         solverConstraint.m_appliedPushImpulse = 0.f;
771                                                         
772                                                         solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
773                                                         if (!cp.m_lateralFrictionInitialized)
774                                                         {
775                                                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
776                                                                 
777                                                                 //scale anisotropic friction
778                                                                 
779                                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
780                                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
781
782                                                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
783
784
785                                                                 if (lat_rel_vel > SIMD_EPSILON)//0.0f)
786                                                                 {
787                                                                         cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
788                                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
789                                                                         cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
790                                                                         cp.m_lateralFrictionDir2.normalize();
791                                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
792                                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
793
794                                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
795                                                                 } else
796                                                                 {
797                                                                         //re-calculate friction direction every frame, todo: check if this is really needed
798                                                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
799                                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
800                                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
801                                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
802                                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
803                                                                 }
804                                                                 cp.m_lateralFrictionInitialized = true;
805                                                                 
806                                                         } else
807                                                         {
808                                                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
809                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
810                                                         }
811
812                                                         {
813                                                                 btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
814                                                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
815                                                                 {
816                                                                         frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
817                                                                         if (rb0)
818                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
819                                                                         if (rb1)
820                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
821                                                                 } else
822                                                                 {
823                                                                         frictionConstraint1.m_appliedImpulse = 0.f;
824                                                                 }
825                                                         }
826                                                         {
827                                                                 btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
828                                                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
829                                                                 {
830                                                                         frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
831                                                                         if (rb0)
832                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
833                                                                         if (rb1)
834                                                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
835                                                                 } else
836                                                                 {
837                                                                         frictionConstraint2.m_appliedImpulse = 0.f;
838                                                                 }
839                                                         }
840                                                 }
841
842
843                                         }
844                                 }
845                         }
846                 }
847         }
848         
849         btContactSolverInfo info = infoGlobal;
850
851         {
852                 int j;
853                 for (j=0;j<numConstraints;j++)
854                 {
855                         btTypedConstraint* constraint = constraints[j];
856                         constraint->buildJacobian();
857                 }
858         }
859         
860         
861
862         int numConstraintPool = m_tmpSolverConstraintPool.size();
863         int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
864
865         ///todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
866         m_orderTmpConstraintPool.resize(numConstraintPool);
867         m_orderFrictionConstraintPool.resize(numFrictionPool);
868         {
869                 int i;
870                 for (i=0;i<numConstraintPool;i++)
871                 {
872                         m_orderTmpConstraintPool[i] = i;
873                 }
874                 for (i=0;i<numFrictionPool;i++)
875                 {
876                         m_orderFrictionConstraintPool[i] = i;
877                 }
878         }
879
880
881
882         return 0.f;
883
884 }
885
886 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
887 {
888         BT_PROFILE("solveGroupCacheFriendlyIterations");
889         int numConstraintPool = m_tmpSolverConstraintPool.size();
890         int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
891
892         //should traverse the contacts random order...
893         int iteration;
894         {
895                 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
896                 {                       
897
898                         int j;
899                         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
900                         {
901                                 if ((iteration & 7) == 0) {
902                                         for (j=0; j<numConstraintPool; ++j) {
903                                                 int tmp = m_orderTmpConstraintPool[j];
904                                                 int swapi = btRandInt2(j+1);
905                                                 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
906                                                 m_orderTmpConstraintPool[swapi] = tmp;
907                                         }
908
909                                         for (j=0; j<numFrictionPool; ++j) {
910                                                 int tmp = m_orderFrictionConstraintPool[j];
911                                                 int swapi = btRandInt2(j+1);
912                                                 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
913                                                 m_orderFrictionConstraintPool[swapi] = tmp;
914                                         }
915                                 }
916                         }
917
918                         for (j=0;j<numConstraints;j++)
919                         {
920                                 btTypedConstraint* constraint = constraints[j];
921                                 ///todo: use solver bodies, so we don't need to copy from/to btRigidBody
922
923                                 if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
924                                 {
925                                         m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
926                                 }
927                                 if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
928                                 {
929                                         m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
930                                 }
931
932                                 constraint->solveConstraint(infoGlobal.m_timeStep);
933
934                                 if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
935                                 {
936                                         m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
937                                 }
938                                 if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
939                                 {
940                                         m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
941                                 }
942
943                         }
944
945                         {
946                                 int numPoolConstraints = m_tmpSolverConstraintPool.size();
947                                 for (j=0;j<numPoolConstraints;j++)
948                                 {
949                                         
950                                         const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
951                                         resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
952                                                 m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
953                                 }
954                         }
955
956                         {
957                                  int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
958                                 
959                                  for (j=0;j<numFrictionPoolConstraints;j++)
960                                 {
961                                         const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
962                                         btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
963                                                                 m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;                  
964
965                                                 resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
966                                                         m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
967                                                         totalImpulse);
968                                 }
969                         }
970                         
971
972
973                 }
974         
975                 if (infoGlobal.m_splitImpulse)
976                 {
977                         
978                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
979                         {
980                                 {
981                                         int numPoolConstraints = m_tmpSolverConstraintPool.size();
982                                         int j;
983                                         for (j=0;j<numPoolConstraints;j++)
984                                         {
985                                                 const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
986
987                                                 resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
988                                                         m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
989                                         }
990                                 }
991                         }
992
993                 }
994
995         }
996
997         return 0.f;
998 }
999
1000
1001 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1002 {
1003         int i;
1004
1005         solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1006         solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1007
1008         int numPoolConstraints = m_tmpSolverConstraintPool.size();
1009         int j;
1010         for (j=0;j<numPoolConstraints;j++)
1011         {
1012                 
1013                 const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
1014                 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1015                 btAssert(pt);
1016                 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1017                 pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1018                 pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1019
1020                 //do a callback here?
1021
1022         }
1023
1024         if (infoGlobal.m_splitImpulse)
1025         {               
1026                 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1027                 {
1028                         m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
1029                 }
1030         } else
1031         {
1032                 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1033         {
1034                 m_tmpSolverBodyPool[i].writebackVelocity();
1035         }
1036         }
1037
1038 //      printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
1039
1040 /*
1041         printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
1042         printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
1043         printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
1044
1045         
1046         printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
1047         printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
1048         printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
1049 */
1050
1051         m_tmpSolverBodyPool.resize(0);
1052         m_tmpSolverConstraintPool.resize(0);
1053         m_tmpSolverFrictionConstraintPool.resize(0);
1054
1055
1056         return 0.f;
1057 }
1058
1059 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
1060 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
1061 {
1062         BT_PROFILE("solveGroup");
1063         if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
1064         {
1065                 //you need to provide at least some bodies
1066                 //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
1067                 btAssert(bodies);
1068                 btAssert(numBodies);
1069                 return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1070         }
1071
1072         
1073
1074         btContactSolverInfo info = infoGlobal;
1075
1076         int numiter = infoGlobal.m_numIterations;
1077
1078         int totalPoints = 0;
1079
1080
1081         {
1082                 short j;
1083                 for (j=0;j<numManifolds;j++)
1084                 {
1085                         btPersistentManifold* manifold = manifoldPtr[j];
1086                         prepareConstraints(manifold,info,debugDrawer);
1087
1088                         for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
1089                         {
1090                                 gOrder[totalPoints].m_manifoldIndex = j;
1091                                 gOrder[totalPoints].m_pointIndex = p;
1092                                 totalPoints++;
1093                         }
1094                 }
1095         }
1096
1097         {
1098                 int j;
1099                 for (j=0;j<numConstraints;j++)
1100                 {
1101                         btTypedConstraint* constraint = constraints[j];
1102                         constraint->buildJacobian();
1103                 }
1104         }
1105         
1106
1107         //should traverse the contacts random order...
1108         int iteration;
1109
1110         {
1111                 for ( iteration = 0;iteration<numiter;iteration++)
1112                 {
1113                         int j;
1114                         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1115                         {
1116                                 if ((iteration & 7) == 0) {
1117                                         for (j=0; j<totalPoints; ++j) {
1118                                                 btOrderIndex tmp = gOrder[j];
1119                                                 int swapi = btRandInt2(j+1);
1120                                                 gOrder[j] = gOrder[swapi];
1121                                                 gOrder[swapi] = tmp;
1122                                         }
1123                                 }
1124                         }
1125
1126                         for (j=0;j<numConstraints;j++)
1127                         {
1128                                 btTypedConstraint* constraint = constraints[j];
1129                                 constraint->solveConstraint(info.m_timeStep);
1130                         }
1131
1132                         for (j=0;j<totalPoints;j++)
1133                         {
1134                                 btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
1135                                 solve( (btRigidBody*)manifold->getBody0(),
1136                                                                         (btRigidBody*)manifold->getBody1()
1137                                 ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
1138                         }
1139                 
1140                         for (j=0;j<totalPoints;j++)
1141                         {
1142                                 btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
1143                                 solveFriction((btRigidBody*)manifold->getBody0(),
1144                                         (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
1145                         }
1146                         
1147                 }
1148         }
1149                 
1150
1151
1152
1153         return btScalar(0.);
1154 }
1155
1156
1157
1158
1159
1160
1161
1162 void    btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
1163 {
1164
1165         (void)debugDrawer;
1166
1167         btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
1168         btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
1169
1170
1171         //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
1172         {
1173 #ifdef FORCE_REFESH_CONTACT_MANIFOLDS
1174                 manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
1175 #endif //FORCE_REFESH_CONTACT_MANIFOLDS         
1176                 int numpoints = manifoldPtr->getNumContacts();
1177
1178                 gTotalContactPoints += numpoints;
1179
1180                 
1181                 for (int i=0;i<numpoints ;i++)
1182                 {
1183                         btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
1184                         if (cp.getDistance() <= btScalar(0.))
1185                         {
1186                                 const btVector3& pos1 = cp.getPositionWorldOnA();
1187                                 const btVector3& pos2 = cp.getPositionWorldOnB();
1188
1189                                 btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
1190                                 btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
1191                                 
1192
1193                                 //this jacobian entry is re-used for all iterations
1194                                 btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
1195                                         body1->getCenterOfMassTransform().getBasis().transpose(),
1196                                         rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
1197                                         body1->getInvInertiaDiagLocal(),body1->getInvMass());
1198
1199                                 
1200                                 btScalar jacDiagAB = jac.getDiagonal();
1201
1202                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1203                                 if (cpd)
1204                                 {
1205                                         //might be invalid
1206                                         cpd->m_persistentLifeTime++;
1207                                         if (cpd->m_persistentLifeTime != cp.getLifeTime())
1208                                         {
1209                                                 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
1210                                                 new (cpd) btConstraintPersistentData;
1211                                                 cpd->m_persistentLifeTime = cp.getLifeTime();
1212
1213                                         } else
1214                                         {
1215                                                 //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
1216                                                 
1217                                         }
1218                                 } else
1219                                 {
1220                                                 
1221                                         //todo: should this be in a pool?
1222                                         void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
1223                                         cpd = new (mem)btConstraintPersistentData;
1224                                         assert(cpd);
1225
1226                                         totalCpd ++;
1227                                         //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
1228                                         cp.m_userPersistentData = cpd;
1229                                         cpd->m_persistentLifeTime = cp.getLifeTime();
1230                                         //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
1231                                         
1232                                 }
1233                                 assert(cpd);
1234
1235                                 cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
1236
1237                                 //Dependent on Rigidbody A and B types, fetch the contact/friction response func
1238                                 //perhaps do a similar thing for friction/restutution combiner funcs...
1239                                 
1240                                 cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
1241                                 cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
1242                                 
1243                                 btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
1244                                 btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
1245                                 btVector3 vel = vel1 - vel2;
1246                                 btScalar rel_vel;
1247                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
1248                                 
1249                                 btScalar combinedRestitution = cp.m_combinedRestitution;
1250                                 
1251                                 cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
1252                                 cpd->m_friction = cp.m_combinedFriction;
1253                                 cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
1254                                 if (cpd->m_restitution <= btScalar(0.))
1255                                 {
1256                                         cpd->m_restitution = btScalar(0.0);
1257
1258                                 };
1259                                 
1260                                 //restitution and penetration work in same direction so
1261                                 //rel_vel 
1262                                 
1263                                 btScalar penVel = -cpd->m_penetration/info.m_timeStep;
1264
1265                                 if (cpd->m_restitution > penVel)
1266                                 {
1267                                         cpd->m_penetration = btScalar(0.);
1268                                 }                               
1269                                 
1270
1271                                 btScalar relaxation = info.m_damping;
1272                                 if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
1273                                 {
1274                                         cpd->m_appliedImpulse *= relaxation;
1275                                 } else
1276                                 {
1277                                         cpd->m_appliedImpulse =btScalar(0.);
1278                                 }
1279         
1280                                 //for friction
1281                                 cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
1282                                 
1283                                 //re-calculate friction direction every frame, todo: check if this is really needed
1284                                 btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
1285
1286
1287 #define NO_FRICTION_WARMSTART 1
1288
1289         #ifdef NO_FRICTION_WARMSTART
1290                                 cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
1291                                 cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
1292         #endif //NO_FRICTION_WARMSTART
1293                                 btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
1294                                 btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
1295                                 btScalar denom = relaxation/(denom0+denom1);
1296                                 cpd->m_jacDiagABInvTangent0 = denom;
1297
1298
1299                                 denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
1300                                 denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
1301                                 denom = relaxation/(denom0+denom1);
1302                                 cpd->m_jacDiagABInvTangent1 = denom;
1303
1304                                 btVector3 totalImpulse = 
1305         #ifndef NO_FRICTION_WARMSTART
1306                                         cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
1307                                         cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
1308         #endif //NO_FRICTION_WARMSTART
1309                                         cp.m_normalWorldOnB*cpd->m_appliedImpulse;
1310
1311
1312
1313                                 ///
1314                                 {
1315                                 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
1316                                 cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
1317                                 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
1318                                 cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
1319                                 }
1320                                 {
1321                                         btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
1322                                         cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
1323                                 }
1324                                 {
1325                                         btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
1326                                         cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
1327                                 }
1328                                 {
1329                                         btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
1330                                         cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
1331                                 }
1332                                 {
1333                                         btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
1334                                         cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
1335                                 }
1336                                 
1337                                 ///
1338
1339
1340
1341                                 //apply previous frames impulse on both bodies
1342                                 body0->applyImpulse(totalImpulse, rel_pos1);
1343                                 body1->applyImpulse(-totalImpulse, rel_pos2);
1344                         }
1345                         
1346                 }
1347         }
1348 }
1349
1350
1351 btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1352 {
1353         btScalar maxImpulse = btScalar(0.);
1354
1355         {
1356
1357                 
1358                 {
1359                         if (cp.getDistance() <= btScalar(0.))
1360                         {
1361
1362                                 
1363
1364                                 {
1365
1366                                         //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1367                                         btScalar impulse = resolveSingleCollisionCombined(
1368                                                 *body0,*body1,
1369                                                 cp,
1370                                                 info);
1371
1372                                         if (maxImpulse < impulse)
1373                                                 maxImpulse  = impulse;
1374
1375                                 }
1376                         }
1377                 }
1378         }
1379         return maxImpulse;
1380 }
1381
1382
1383
1384 btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1385 {
1386
1387         btScalar maxImpulse = btScalar(0.);
1388
1389         {
1390
1391                 
1392                 {
1393                         if (cp.getDistance() <= btScalar(0.))
1394                         {
1395
1396                                 
1397
1398                                 {
1399
1400                                         btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1401                                         btScalar impulse = cpd->m_contactSolverFunc(
1402                                                 *body0,*body1,
1403                                                 cp,
1404                                                 info);
1405
1406                                         if (maxImpulse < impulse)
1407                                                 maxImpulse  = impulse;
1408
1409                                 }
1410                         }
1411                 }
1412         }
1413         return maxImpulse;
1414 }
1415
1416 btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1417 {
1418
1419         (void)debugDrawer;
1420         (void)iter;
1421
1422
1423         {
1424
1425                 
1426                 {
1427                         
1428                         if (cp.getDistance() <= btScalar(0.))
1429                         {
1430
1431                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1432                                 cpd->m_frictionSolverFunc(
1433                                         *body0,*body1,
1434                                         cp,
1435                                         info);
1436
1437                                 
1438                         }
1439                 }
1440
1441         
1442         }
1443         return btScalar(0.);
1444 }
1445
1446
1447 void    btSequentialImpulseConstraintSolver::reset()
1448 {
1449         m_btSeed2 = 0;
1450 }
1451
1452