Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @ 2424

Last change on this file since 2424 was 2192, checked in by rgrieder, 17 years ago

Reverted all changes of attempt to update physics branch.
  • Property svn:eol-style set to native
File size: 45.0 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
440void    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
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                                                        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
856btScalar 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
971btScalar 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
1030btScalar 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
1132void    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
1321btScalar 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
1354btScalar 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
1386btScalar 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
1417void    btSequentialImpulseConstraintSolver::reset()
1418{
1419        m_btSeed2 = 0;
1420}
1421
1422
Note: See TracBrowser for help on using the repository browser.