Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @ 2777

Last change on this file since 2777 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.
  • Property svn:eol-style set to native
File size: 45.8 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
40int totalCpd = 0;
41
42int     gTotalContactPoints = 0;
43
44struct  btOrderIndex
45{
46        int     m_manifoldIndex;
47        int     m_pointIndex;
48};
49
50
51
52#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
53static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
54
55
56unsigned 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)
65int 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
95bool  MyContactDestroyedCallback(void* userPersistentData);
96bool  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
108btSequentialImpulseConstraintSolver::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
124btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
125{
126
127}
128
129void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
130void    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
157int             gNumSplitImpulseRecoveries = 0;
158
159btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
160btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
161{
162        btScalar rest = restitution * -rel_vel;
163        return rest;
164}
165
166
167void    resolveSplitPenetrationImpulseCacheFriendly(
168        btSolverBody& body1,
169        btSolverBody& body2,
170        const btSolverConstraint& contactConstraint,
171        const btContactSolverInfo& solverInfo);
172
173//SIMD_FORCE_INLINE
174void    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
231btScalar resolveSingleCollisionCombinedCacheFriendly(
232        btSolverBody& body1,
233        btSolverBody& body2,
234        const btSolverConstraint& contactConstraint,
235        const btContactSolverInfo& solverInfo);
236
237//SIMD_FORCE_INLINE
238btScalar 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
298btScalar resolveSingleFrictionCacheFriendly(
299        btSolverBody& body1,
300        btSolverBody& body2,
301        const btSolverConstraint& contactConstraint,
302        const btContactSolverInfo& solverInfo,
303        btScalar appliedNormalImpulse);
304
305//SIMD_FORCE_INLINE
306btScalar 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
382btScalar 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
440btSolverConstraint&     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        return solverConstraint;
495}
496
497
498
499btScalar 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
722                                                       
723                                                        if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
724                                                        {
725                                                                solverConstraint.m_restitution = 0.f;
726                                                        } else
727                                                        {
728                                                                solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
729                                                                if (solverConstraint.m_restitution <= btScalar(0.))
730                                                                {
731                                                                        solverConstraint.m_restitution = 0.f;
732                                                                };
733                                                        }
734
735                                                       
736                                                        btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
737
738                                                       
739
740                                                        if (solverConstraint.m_restitution > penVel)
741                                                        {
742                                                                solverConstraint.m_penetration = btScalar(0.);
743                                                        }
744                                                         
745                                                       
746                                                       
747                                                        ///warm starting (or zero if disabled)
748                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
749                                                        {
750                                                                solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
751                                                                if (rb0)
752                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
753                                                                if (rb1)
754                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
755                                                        } else
756                                                        {
757                                                                solverConstraint.m_appliedImpulse = 0.f;
758                                                        }
759
760                                                        solverConstraint.m_appliedPushImpulse = 0.f;
761                                                       
762                                                        solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
763                                                        if (!cp.m_lateralFrictionInitialized)
764                                                        {
765                                                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
766                                                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
767                                                                if (lat_rel_vel > SIMD_EPSILON)//0.0f)
768                                                                {
769                                                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
770                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
771                                                                        if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
772                                                                        {
773                                                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
774                                                                                cp.m_lateralFrictionDir2.normalize();//??
775                                                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
776                                                                                cp.m_lateralFrictionInitialized = true;
777                                                                        }
778                                                                } else
779                                                                {
780                                                                        //re-calculate friction direction every frame, todo: check if this is really needed
781                                                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
782                                                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
783                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
784                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
785                                                                        {
786                                                                                cp.m_lateralFrictionInitialized = true;
787                                                                        }
788                                                                }
789                                                               
790                                                        } else
791                                                        {
792                                                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
793                                                                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
794                                                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
795                                                        }
796
797                                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
798                                                        {
799                                                                {
800                                                                        btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
801                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
802                                                                        {
803                                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
804                                                                                if (rb0)
805                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
806                                                                                if (rb1)
807                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
808                                                                        } else
809                                                                        {
810                                                                                frictionConstraint1.m_appliedImpulse = 0.f;
811                                                                        }
812                                                                }
813                                                                {
814                                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
815                                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
816                                                                        {
817                                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
818                                                                                if (rb0)
819                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
820                                                                                if (rb1)
821                                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
822                                                                        } else
823                                                                        {
824                                                                                frictionConstraint2.m_appliedImpulse = 0.f;
825                                                                        }
826                                                                }
827                                                        }
828                                                }
829
830
831                                        }
832                                }
833                        }
834                }
835        }
836       
837        btContactSolverInfo info = infoGlobal;
838
839        {
840                int j;
841                for (j=0;j<numConstraints;j++)
842                {
843                        btTypedConstraint* constraint = constraints[j];
844                        constraint->buildJacobian();
845                }
846        }
847       
848       
849
850        int numConstraintPool = m_tmpSolverConstraintPool.size();
851        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
852
853        ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
854        m_orderTmpConstraintPool.resize(numConstraintPool);
855        m_orderFrictionConstraintPool.resize(numFrictionPool);
856        {
857                int i;
858                for (i=0;i<numConstraintPool;i++)
859                {
860                        m_orderTmpConstraintPool[i] = i;
861                }
862                for (i=0;i<numFrictionPool;i++)
863                {
864                        m_orderFrictionConstraintPool[i] = i;
865                }
866        }
867
868
869
870        return 0.f;
871
872}
873
874btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
875{
876        BT_PROFILE("solveGroupCacheFriendlyIterations");
877        int numConstraintPool = m_tmpSolverConstraintPool.size();
878        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
879
880        //should traverse the contacts random order...
881        int iteration;
882        {
883                for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
884                {                       
885
886                        int j;
887                        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
888                        {
889                                if ((iteration & 7) == 0) {
890                                        for (j=0; j<numConstraintPool; ++j) {
891                                                int tmp = m_orderTmpConstraintPool[j];
892                                                int swapi = btRandInt2(j+1);
893                                                m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
894                                                m_orderTmpConstraintPool[swapi] = tmp;
895                                        }
896
897                                        for (j=0; j<numFrictionPool; ++j) {
898                                                int tmp = m_orderFrictionConstraintPool[j];
899                                                int swapi = btRandInt2(j+1);
900                                                m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
901                                                m_orderFrictionConstraintPool[swapi] = tmp;
902                                        }
903                                }
904                        }
905
906                        for (j=0;j<numConstraints;j++)
907                        {
908                                btTypedConstraint* constraint = constraints[j];
909                                ///todo: use solver bodies, so we don't need to copy from/to btRigidBody
910
911                                if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
912                                {
913                                        m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
914                                }
915                                if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
916                                {
917                                        m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
918                                }
919
920                                constraint->solveConstraint(infoGlobal.m_timeStep);
921
922                                if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
923                                {
924                                        m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
925                                }
926                                if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
927                                {
928                                        m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
929                                }
930
931                        }
932
933                        {
934                                int numPoolConstraints = m_tmpSolverConstraintPool.size();
935                                for (j=0;j<numPoolConstraints;j++)
936                                {
937                                       
938                                        const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
939                                        resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
940                                                m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
941                                }
942                        }
943
944                        {
945                                 int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
946                               
947                                 for (j=0;j<numFrictionPoolConstraints;j++)
948                                {
949                                        const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
950                                        btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
951                                                                m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;                 
952
953                                                resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
954                                                        m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
955                                                        totalImpulse);
956                                }
957                        }
958                       
959
960
961                }
962       
963                if (infoGlobal.m_splitImpulse)
964                {
965                       
966                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
967                        {
968                                {
969                                        int numPoolConstraints = m_tmpSolverConstraintPool.size();
970                                        int j;
971                                        for (j=0;j<numPoolConstraints;j++)
972                                        {
973                                                const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
974
975                                                resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
976                                                        m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
977                                        }
978                                }
979                        }
980
981                }
982
983        }
984
985        return 0.f;
986}
987
988
989btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
990{
991        int i;
992
993        solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
994        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
995
996        int numPoolConstraints = m_tmpSolverConstraintPool.size();
997        int j;
998        for (j=0;j<numPoolConstraints;j++)
999        {
1000               
1001                const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
1002                btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1003                btAssert(pt);
1004                pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1005                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
1006                {
1007                        pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1008                        pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1009                }
1010
1011                //do a callback here?
1012
1013        }
1014
1015        if (infoGlobal.m_splitImpulse)
1016        {               
1017                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1018                {
1019                        m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
1020                }
1021        } else
1022        {
1023                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1024        {
1025                m_tmpSolverBodyPool[i].writebackVelocity();
1026        }
1027        }
1028
1029//      printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
1030
1031/*
1032        printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
1033        printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
1034        printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
1035
1036       
1037        printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
1038        printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
1039        printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
1040*/
1041
1042        m_tmpSolverBodyPool.resize(0);
1043        m_tmpSolverConstraintPool.resize(0);
1044        m_tmpSolverFrictionConstraintPool.resize(0);
1045
1046
1047        return 0.f;
1048}
1049
1050/// btSequentialImpulseConstraintSolver Sequentially applies impulses
1051btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
1052{
1053        BT_PROFILE("solveGroup");
1054        if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
1055        {
1056                //you need to provide at least some bodies
1057                //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
1058                btAssert(bodies);
1059                btAssert(numBodies);
1060                return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1061        }
1062
1063       
1064
1065        btContactSolverInfo info = infoGlobal;
1066
1067        int numiter = infoGlobal.m_numIterations;
1068
1069        int totalPoints = 0;
1070
1071
1072        {
1073                short j;
1074                for (j=0;j<numManifolds;j++)
1075                {
1076                        btPersistentManifold* manifold = manifoldPtr[j];
1077                        prepareConstraints(manifold,info,debugDrawer);
1078
1079                        for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
1080                        {
1081                                gOrder[totalPoints].m_manifoldIndex = j;
1082                                gOrder[totalPoints].m_pointIndex = p;
1083                                totalPoints++;
1084                        }
1085                }
1086        }
1087
1088        {
1089                int j;
1090                for (j=0;j<numConstraints;j++)
1091                {
1092                        btTypedConstraint* constraint = constraints[j];
1093                        constraint->buildJacobian();
1094                }
1095        }
1096       
1097
1098        //should traverse the contacts random order...
1099        int iteration;
1100
1101        {
1102                for ( iteration = 0;iteration<numiter;iteration++)
1103                {
1104                        int j;
1105                        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1106                        {
1107                                if ((iteration & 7) == 0) {
1108                                        for (j=0; j<totalPoints; ++j) {
1109                                                btOrderIndex tmp = gOrder[j];
1110                                                int swapi = btRandInt2(j+1);
1111                                                gOrder[j] = gOrder[swapi];
1112                                                gOrder[swapi] = tmp;
1113                                        }
1114                                }
1115                        }
1116
1117                        for (j=0;j<numConstraints;j++)
1118                        {
1119                                btTypedConstraint* constraint = constraints[j];
1120                                constraint->solveConstraint(info.m_timeStep);
1121                        }
1122
1123                        for (j=0;j<totalPoints;j++)
1124                        {
1125                                btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
1126                                solve( (btRigidBody*)manifold->getBody0(),
1127                                                                        (btRigidBody*)manifold->getBody1()
1128                                ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
1129                        }
1130               
1131                        for (j=0;j<totalPoints;j++)
1132                        {
1133                                btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
1134                                solveFriction((btRigidBody*)manifold->getBody0(),
1135                                        (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
1136                        }
1137                       
1138                }
1139        }
1140               
1141
1142
1143
1144        return btScalar(0.);
1145}
1146
1147
1148
1149
1150
1151
1152
1153void    btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
1154{
1155
1156        (void)debugDrawer;
1157
1158        btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
1159        btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
1160
1161
1162        //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
1163        {
1164#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
1165                manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
1166#endif //FORCE_REFESH_CONTACT_MANIFOLDS         
1167                int numpoints = manifoldPtr->getNumContacts();
1168
1169                gTotalContactPoints += numpoints;
1170
1171               
1172                for (int i=0;i<numpoints ;i++)
1173                {
1174                        btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
1175                        if (cp.getDistance() <= btScalar(0.))
1176                        {
1177                                const btVector3& pos1 = cp.getPositionWorldOnA();
1178                                const btVector3& pos2 = cp.getPositionWorldOnB();
1179
1180                                btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
1181                                btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
1182                               
1183
1184                                //this jacobian entry is re-used for all iterations
1185                                btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
1186                                        body1->getCenterOfMassTransform().getBasis().transpose(),
1187                                        rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
1188                                        body1->getInvInertiaDiagLocal(),body1->getInvMass());
1189
1190                               
1191                                btScalar jacDiagAB = jac.getDiagonal();
1192
1193                                btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1194                                if (cpd)
1195                                {
1196                                        //might be invalid
1197                                        cpd->m_persistentLifeTime++;
1198                                        if (cpd->m_persistentLifeTime != cp.getLifeTime())
1199                                        {
1200                                                //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
1201                                                new (cpd) btConstraintPersistentData;
1202                                                cpd->m_persistentLifeTime = cp.getLifeTime();
1203
1204                                        } else
1205                                        {
1206                                                //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
1207                                               
1208                                        }
1209                                } else
1210                                {
1211                                               
1212                                        //todo: should this be in a pool?
1213                                        void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
1214                                        cpd = new (mem)btConstraintPersistentData;
1215                                        assert(cpd);
1216
1217                                        totalCpd ++;
1218                                        //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
1219                                        cp.m_userPersistentData = cpd;
1220                                        cpd->m_persistentLifeTime = cp.getLifeTime();
1221                                        //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
1222                                       
1223                                }
1224                                assert(cpd);
1225
1226                                cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
1227
1228                                //Dependent on Rigidbody A and B types, fetch the contact/friction response func
1229                                //perhaps do a similar thing for friction/restutution combiner funcs...
1230                               
1231                                cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
1232                                cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
1233                               
1234                                btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
1235                                btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
1236                                btVector3 vel = vel1 - vel2;
1237                                btScalar rel_vel;
1238                                rel_vel = cp.m_normalWorldOnB.dot(vel);
1239                               
1240                                btScalar combinedRestitution = cp.m_combinedRestitution;
1241                               
1242                                cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
1243                                cpd->m_friction = cp.m_combinedFriction;
1244                                if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
1245                                {
1246                                        cpd->m_restitution = 0.f;
1247                                } else
1248                                {
1249                                        cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
1250                                        if (cpd->m_restitution <= btScalar(0.))
1251                                        {
1252                                                cpd->m_restitution = btScalar(0.0);
1253                                        };
1254                                }
1255                               
1256                                //restitution and penetration work in same direction so
1257                                //rel_vel
1258                               
1259                                btScalar penVel = -cpd->m_penetration/info.m_timeStep;
1260
1261                                if (cpd->m_restitution > penVel)
1262                                {
1263                                        cpd->m_penetration = btScalar(0.);
1264                                }                               
1265                               
1266
1267                                btScalar relaxation = info.m_damping;
1268                                if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
1269                                {
1270                                        cpd->m_appliedImpulse *= relaxation;
1271                                } else
1272                                {
1273                                        cpd->m_appliedImpulse =btScalar(0.);
1274                                }
1275       
1276                                //for friction
1277                                cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
1278                               
1279                                //re-calculate friction direction every frame, todo: check if this is really needed
1280                                btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
1281
1282
1283#define NO_FRICTION_WARMSTART 1
1284
1285        #ifdef NO_FRICTION_WARMSTART
1286                                cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
1287                                cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
1288        #endif //NO_FRICTION_WARMSTART
1289                                btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
1290                                btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
1291                                btScalar denom = relaxation/(denom0+denom1);
1292                                cpd->m_jacDiagABInvTangent0 = denom;
1293
1294
1295                                denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
1296                                denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
1297                                denom = relaxation/(denom0+denom1);
1298                                cpd->m_jacDiagABInvTangent1 = denom;
1299
1300                                btVector3 totalImpulse = 
1301        #ifndef NO_FRICTION_WARMSTART
1302                                        cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
1303                                        cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
1304        #endif //NO_FRICTION_WARMSTART
1305                                        cp.m_normalWorldOnB*cpd->m_appliedImpulse;
1306
1307
1308
1309                                ///
1310                                {
1311                                btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
1312                                cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
1313                                btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
1314                                cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
1315                                }
1316                                {
1317                                        btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
1318                                        cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
1319                                }
1320                                {
1321                                        btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
1322                                        cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
1323                                }
1324                                {
1325                                        btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
1326                                        cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
1327                                }
1328                                {
1329                                        btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
1330                                        cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
1331                                }
1332                               
1333                                ///
1334
1335
1336
1337                                //apply previous frames impulse on both bodies
1338                                body0->applyImpulse(totalImpulse, rel_pos1);
1339                                body1->applyImpulse(-totalImpulse, rel_pos2);
1340                        }
1341                       
1342                }
1343        }
1344}
1345
1346
1347btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1348{
1349        btScalar maxImpulse = btScalar(0.);
1350
1351        {
1352
1353               
1354                {
1355                        if (cp.getDistance() <= btScalar(0.))
1356                        {
1357
1358                               
1359
1360                                {
1361
1362                                        //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1363                                        btScalar impulse = resolveSingleCollisionCombined(
1364                                                *body0,*body1,
1365                                                cp,
1366                                                info);
1367
1368                                        if (maxImpulse < impulse)
1369                                                maxImpulse  = impulse;
1370
1371                                }
1372                        }
1373                }
1374        }
1375        return maxImpulse;
1376}
1377
1378
1379
1380btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1381{
1382
1383        btScalar maxImpulse = btScalar(0.);
1384
1385        {
1386
1387               
1388                {
1389                        if (cp.getDistance() <= btScalar(0.))
1390                        {
1391
1392                               
1393
1394                                {
1395
1396                                        btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1397                                        btScalar impulse = cpd->m_contactSolverFunc(
1398                                                *body0,*body1,
1399                                                cp,
1400                                                info);
1401
1402                                        if (maxImpulse < impulse)
1403                                                maxImpulse  = impulse;
1404
1405                                }
1406                        }
1407                }
1408        }
1409        return maxImpulse;
1410}
1411
1412btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
1413{
1414
1415        (void)debugDrawer;
1416        (void)iter;
1417
1418
1419        {
1420
1421               
1422                {
1423                       
1424                        if (cp.getDistance() <= btScalar(0.))
1425                        {
1426
1427                                btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
1428                                cpd->m_frictionSolverFunc(
1429                                        *body0,*body1,
1430                                        cp,
1431                                        info);
1432
1433                               
1434                        }
1435                }
1436
1437       
1438        }
1439        return btScalar(0.);
1440}
1441
1442
1443void    btSequentialImpulseConstraintSolver::reset()
1444{
1445        m_btSeed2 = 0;
1446}
1447
1448
Note: See TracBrowser for help on using the repository browser.