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