angular-only hinge updated
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btSequentialImpulseConstraintSolver.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17 #include "btSequentialImpulseConstraintSolver.h"
18 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
19 #include "BulletDynamics/Dynamics/btRigidBody.h"
20 #include "btContactConstraint.h"
21 #include "btSolve2LinearConstraint.h"
22 #include "btContactSolverInfo.h"
23 #include "LinearMath/btIDebugDraw.h"
24 #include "btJacobianEntry.h"
25 #include "LinearMath/btMinMax.h"
26
27 #ifdef USE_PROFILE
28 #include "LinearMath/btQuickprof.h"
29 #endif //USE_PROFILE
30
31 int totalCpd = 0;
32
33 int     gTotalContactPoints = 0;
34
35 struct  btOrderIndex
36 {
37         short int       m_manifoldIndex;
38         short int       m_pointIndex;
39 };
40
41 #define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
42 static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
43 static unsigned long btSeed2 = 0;
44 unsigned long btRand2()
45 {
46   btSeed2 = (1664525L*btSeed2 + 1013904223L) & 0xffffffff;
47   return btSeed2;
48 }
49
50 int btRandInt2 (int n)
51 {
52   float a = float(n) / 4294967296.0f;
53   return (int) (float(btRand2()) * a);
54 }
55
56 bool  MyContactDestroyedCallback(void* userPersistentData)
57 {
58         assert (userPersistentData);
59         btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
60         delete cpd;
61         totalCpd--;
62         //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
63         return true;
64 }
65
66 btSequentialImpulseConstraintSolver2::btSequentialImpulseConstraintSolver2()
67 {
68         setSolverMode(SOLVER_USE_WARMSTARTING);
69 }
70
71
72 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
73 :m_solverMode(SOLVER_RANDMIZE_ORDER)
74 {
75         gContactDestroyedCallback = &MyContactDestroyedCallback;
76
77         //initialize default friction/contact funcs
78         int i,j;
79         for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
80                 for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
81                 {
82
83                         m_contactDispatch[i][j] = resolveSingleCollision;
84                         m_frictionDispatch[i][j] = resolveSingleFriction;
85                 }
86 }
87
88 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
89 float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
90 {
91         
92         btContactSolverInfo info = infoGlobal;
93
94         int numiter = infoGlobal.m_numIterations;
95 #ifdef USE_PROFILE
96         btProfiler::beginBlock("solve");
97 #endif //USE_PROFILE
98
99         int totalPoints = 0;
100
101
102         {
103                 int j;
104                 for (j=0;j<numManifolds;j++)
105                 {
106                         btPersistentManifold* manifold = manifoldPtr[j];
107                         prepareConstraints(manifold,info,debugDrawer);
108                         for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
109                         {
110                                 gOrder[totalPoints].m_manifoldIndex = j;
111                                 gOrder[totalPoints].m_pointIndex = p;
112                                 totalPoints++;
113                         }
114                 }
115         }
116         
117         //should traverse the contacts random order...
118         int iteration;
119
120         {
121                 for ( iteration = 0;iteration<numiter-1;iteration++)
122                 {
123                         int j;
124                         if (m_solverMode & SOLVER_RANDMIZE_ORDER)
125                         {
126                                 if ((iteration & 7) == 0) {
127                                         for (j=0; j<totalPoints; ++j) {
128                                                 btOrderIndex tmp = gOrder[j];
129                                                 int swapi = btRandInt2(j+1);
130                                                 gOrder[j] = gOrder[swapi];
131                                                 gOrder[swapi] = tmp;
132                                         }
133                                 }
134                         }
135
136                         for (j=0;j<totalPoints;j++)
137                         {
138                                 btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
139                                 solve( (btRigidBody*)manifold->getBody0(),
140                                                                         (btRigidBody*)manifold->getBody1()
141                                 ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
142                         }
143                 
144                         for (j=0;j<totalPoints;j++)
145                         {
146                                 btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
147                                 solveFriction((btRigidBody*)manifold->getBody0(),
148                                         (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
149                         }
150                 }
151         }
152                 
153 #ifdef USE_PROFILE
154         btProfiler::endBlock("solve");
155 #endif //USE_PROFILE
156
157         return 0.f;
158 }
159
160
161 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
162 float btSequentialImpulseConstraintSolver2::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
163 {
164         
165         btContactSolverInfo info = infoGlobal;
166
167         int numiter = infoGlobal.m_numIterations;
168 #ifdef USE_PROFILE
169         btProfiler::beginBlock("solve");
170 #endif //USE_PROFILE
171
172         {
173                 int j;
174                 for (j=0;j<numManifolds;j++)
175                 {
176                         btPersistentManifold* manifold = manifoldPtr[j];
177                         prepareConstraints(manifold,info,debugDrawer);
178                         for (int p=0;p<manifoldPtr[j]->getNumContacts();p++)
179                         {
180                                 //interleaving here gives better results
181                                 solve( (btRigidBody*)manifold->getBody0(),
182                                                                         (btRigidBody*)manifold->getBody1()
183                                 ,manifoldPtr[j]->getContactPoint(p),info,0,debugDrawer);
184                         }
185                 }
186         }
187         
188         //should traverse the contacts random order...
189         int iteration;
190
191         for ( iteration = 0;iteration<numiter-1;iteration++)
192         {
193                 int j;
194
195                 for (j=0;j<numManifolds;j++)
196                 {
197                         btPersistentManifold* manifold = manifoldPtr[j];
198                         for (int p=0;p<manifold->getNumContacts();p++)
199                         {
200                                 solve( (btRigidBody*)manifold->getBody0(),
201                                                                         (btRigidBody*)manifold->getBody1()
202                                 ,manifold->getContactPoint(p),info,iteration,debugDrawer);
203                         }
204                 }
205         
206         }
207
208         for ( iteration = 0;iteration<numiter-1;iteration++)
209         {
210                 int j;
211                 for (j=0;j<numManifolds;j++)
212                 {
213                         btPersistentManifold* manifold = manifoldPtr[j];
214                         for (int p=0;p<manifold->getNumContacts();p++)
215                         {
216                                 solveFriction((btRigidBody*)manifold->getBody0(),
217                                         (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(p),info,iteration,debugDrawer);
218                         }
219                 }
220         }
221
222                 
223 #ifdef USE_PROFILE
224         btProfiler::endBlock("solve");
225 #endif //USE_PROFILE
226
227         return 0.f;
228 }
229
230
231 float penetrationResolveFactor = 0.9f;
232 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
233 {
234         btScalar rest = restitution * -rel_vel;
235         return rest;
236 }
237
238
239 void    btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
240 {
241
242         btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
243         btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
244
245
246         //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
247         {
248                 manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
249                 
250                 int numpoints = manifoldPtr->getNumContacts();
251
252                 gTotalContactPoints += numpoints;
253
254                 btVector3 color(0,1,0);
255                 for (int i=0;i<numpoints ;i++)
256                 {
257                         btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
258                         if (cp.getDistance() <= 0.f)
259                         {
260                                 const btVector3& pos1 = cp.getPositionWorldOnA();
261                                 const btVector3& pos2 = cp.getPositionWorldOnB();
262
263                                 btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
264                                 btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
265                                 
266
267                                 //this jacobian entry is re-used for all iterations
268                                 btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
269                                         body1->getCenterOfMassTransform().getBasis().transpose(),
270                                         rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
271                                         body1->getInvInertiaDiagLocal(),body1->getInvMass());
272
273                                 
274                                 btScalar jacDiagAB = jac.getDiagonal();
275
276                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
277                                 if (cpd)
278                                 {
279                                         //might be invalid
280                                         cpd->m_persistentLifeTime++;
281                                         if (cpd->m_persistentLifeTime != cp.getLifeTime())
282                                         {
283                                                 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
284                                                 new (cpd) btConstraintPersistentData;
285                                                 cpd->m_persistentLifeTime = cp.getLifeTime();
286
287                                         } else
288                                         {
289                                                 //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
290                                                 
291                                         }
292                                 } else
293                                 {
294                                                 
295                                         cpd = new btConstraintPersistentData();
296                                         totalCpd ++;
297                                         //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
298                                         cp.m_userPersistentData = cpd;
299                                         cpd->m_persistentLifeTime = cp.getLifeTime();
300                                         //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
301                                         
302                                 }
303                                 assert(cpd);
304
305                                 cpd->m_jacDiagABInv = 1.f / jacDiagAB;
306
307                                 //Dependent on Rigidbody A and B types, fetch the contact/friction response func
308                                 //perhaps do a similar thing for friction/restutution combiner funcs...
309                                 
310                                 cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
311                                 cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
312                                 
313                                 btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
314                                 btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
315                                 btVector3 vel = vel1 - vel2;
316                                 btScalar rel_vel;
317                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
318                                 
319                                 float combinedRestitution = cp.m_combinedRestitution;
320                                 
321                                 cpd->m_penetration = cp.getDistance();
322                                 cpd->m_friction = cp.m_combinedFriction;
323                                 cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
324                                 if (cpd->m_restitution <= 0.) //0.f)
325                                 {
326                                         cpd->m_restitution = 0.0f;
327
328                                 };
329                                 
330                                 //restitution and penetration work in same direction so
331                                 //rel_vel 
332                                 
333                                 btScalar penVel = -cpd->m_penetration/info.m_timeStep;
334
335                                 if (cpd->m_restitution > penVel)
336                                 {
337                                         cpd->m_penetration = 0.f;
338                                 }                               
339                                 
340                                 
341
342                                 float relaxation = info.m_damping;
343                                 if (m_solverMode & SOLVER_USE_WARMSTARTING)
344                                 {
345                                         cpd->m_appliedImpulse *= relaxation;
346                                 } else
347                                 {
348                                         cpd->m_appliedImpulse =0.f;
349                                 }
350         
351                                 //for friction
352                                 cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
353                                 
354                                 //re-calculate friction direction every frame, todo: check if this is really needed
355                                 btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
356
357
358 #define NO_FRICTION_WARMSTART 1
359
360         #ifdef NO_FRICTION_WARMSTART
361                                 cpd->m_accumulatedTangentImpulse0 = 0.f;
362                                 cpd->m_accumulatedTangentImpulse1 = 0.f;
363         #endif //NO_FRICTION_WARMSTART
364                                 float denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
365                                 float denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
366                                 float denom = relaxation/(denom0+denom1);
367                                 cpd->m_jacDiagABInvTangent0 = denom;
368
369
370                                 denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
371                                 denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
372                                 denom = relaxation/(denom0+denom1);
373                                 cpd->m_jacDiagABInvTangent1 = denom;
374
375                                 btVector3 totalImpulse = 
376         #ifndef NO_FRICTION_WARMSTART
377                                         cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
378                                         cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
379         #endif //NO_FRICTION_WARMSTART
380                                         cp.m_normalWorldOnB*cpd->m_appliedImpulse;
381
382
383
384                                 ///
385                                 {
386                                 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
387                                 cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
388                                 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
389                                 cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
390                                 }
391                                 {
392                                         btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
393                                         cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
394                                 }
395                                 {
396                                         btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
397                                         cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
398                                 }
399                                 {
400                                         btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
401                                         cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
402                                 }
403                                 {
404                                         btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
405                                         cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
406                                 }
407                                 
408                                 ///
409
410
411
412                                 //apply previous frames impulse on both bodies
413                                 body0->applyImpulse(totalImpulse, rel_pos1);
414                                 body1->applyImpulse(-totalImpulse, rel_pos2);
415                         }
416                         
417                 }
418         }
419 }
420
421 float btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
422 {
423
424         float maxImpulse = 0.f;
425
426         {
427
428                 btVector3 color(0,1,0);
429                 {
430                         if (cp.getDistance() <= 0.f)
431                         {
432
433                                 if (iter == 0)
434                                 {
435                                         if (debugDrawer)
436                                                 debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
437                                 }
438
439                                 {
440
441                                         btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
442                                         float impulse = cpd->m_contactSolverFunc(
443                                                 *body0,*body1,
444                                                 cp,
445                                                 info);
446
447                                         if (maxImpulse < impulse)
448                                                 maxImpulse  = impulse;
449
450                                 }
451                         }
452                 }
453         }
454         return maxImpulse;
455 }
456
457 float btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
458 {
459
460
461         {
462
463                 btVector3 color(0,1,0);
464                 {
465                         
466                         if (cp.getDistance() <= 0.f)
467                         {
468
469                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
470                                 cpd->m_frictionSolverFunc(
471                                         *body0,*body1,
472                                         cp,
473                                         info);
474
475                                 
476                         }
477                 }
478
479         
480         }
481         return 0.f;
482 }