update Bullet 2.x with latest changes, notice that the integration is not finished...
[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 bool  MyContactDestroyedCallback(void* userPersistentData)
36 {
37         assert (userPersistentData);
38         btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
39         delete cpd;
40         totalCpd--;
41         //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
42         return true;
43 }
44
45
46 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
47 {
48         gContactDestroyedCallback = &MyContactDestroyedCallback;
49
50         //initialize default friction/contact funcs
51         int i,j;
52         for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
53                 for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
54                 {
55
56                         m_contactDispatch[i][j] = resolveSingleCollision;
57                         m_frictionDispatch[i][j] = resolveSingleFriction;
58                 }
59 }
60
61 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
62 float btSequentialImpulseConstraintSolver::solveGroup(btPersistentManifold** manifoldPtr, int numManifolds,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
63 {
64         
65         btContactSolverInfo info = infoGlobal;
66
67         int numiter = infoGlobal.m_numIterations;
68 #ifdef USE_PROFILE
69         btProfiler::beginBlock("solve");
70 #endif //USE_PROFILE
71
72         {
73                 int j;
74                 for (j=0;j<numManifolds;j++)
75                 {
76                         int k=j;
77                         prepareConstraints(manifoldPtr[k],info,debugDrawer);
78                         solve(manifoldPtr[k],info,0,debugDrawer);
79                 }
80         }
81         
82         
83         //should traverse the contacts random order...
84         int i;
85         for ( i = 0;i<numiter-1;i++)
86         {
87                 int j;
88                 for (j=0;j<numManifolds;j++)
89                 {
90                         int k=j;
91                         if (i&1)
92                                 k=numManifolds-j-1;
93
94                         solve(manifoldPtr[k],info,i,debugDrawer);
95                 }
96                 
97         }
98 #ifdef USE_PROFILE
99         btProfiler::endBlock("solve");
100
101         btProfiler::beginBlock("solveFriction");
102 #endif //USE_PROFILE
103
104         //now solve the friction                
105         for (i = 0;i<numiter-1;i++)
106         {
107                 int j;
108         for (j=0;j<numManifolds;j++)
109                 {
110                         int k = j;
111                         if (i&1)
112                                 k=numManifolds-j-1;
113                         solveFriction(manifoldPtr[k],info,i,debugDrawer);
114                 }
115         }
116 #ifdef USE_PROFILE
117         btProfiler::endBlock("solveFriction");
118 #endif //USE_PROFILE
119
120         return 0.f;
121 }
122
123
124 float penetrationResolveFactor = 0.9f;
125 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
126 {
127         btScalar rest = restitution * -rel_vel;
128         return rest;
129 }
130
131
132 void    btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
133 {
134
135         btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
136         btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
137
138
139         //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
140         {
141                 manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
142                 
143                 int numpoints = manifoldPtr->getNumContacts();
144
145                 gTotalContactPoints += numpoints;
146
147                 btVector3 color(0,1,0);
148                 for (int i=0;i<numpoints ;i++)
149                 {
150                         btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
151                         if (cp.getDistance() <= 0.f)
152                         {
153                                 const btVector3& pos1 = cp.getPositionWorldOnA();
154                                 const btVector3& pos2 = cp.getPositionWorldOnB();
155
156                                 btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
157                                 btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
158                                 
159
160                                 //this jacobian entry is re-used for all iterations
161                                 btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
162                                         body1->getCenterOfMassTransform().getBasis().transpose(),
163                                         rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
164                                         body1->getInvInertiaDiagLocal(),body1->getInvMass());
165
166                                 
167                                 btScalar jacDiagAB = jac.getDiagonal();
168
169                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
170                                 if (cpd)
171                                 {
172                                         //might be invalid
173                                         cpd->m_persistentLifeTime++;
174                                         if (cpd->m_persistentLifeTime != cp.getLifeTime())
175                                         {
176                                                 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
177                                                 new (cpd) btConstraintPersistentData;
178                                                 cpd->m_persistentLifeTime = cp.getLifeTime();
179
180                                         } else
181                                         {
182                                                 //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
183                                                 
184                                         }
185                                 } else
186                                 {
187                                                 
188                                         cpd = new btConstraintPersistentData();
189                                         totalCpd ++;
190                                         //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
191                                         cp.m_userPersistentData = cpd;
192                                         cpd->m_persistentLifeTime = cp.getLifeTime();
193                                         //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
194                                         
195                                 }
196                                 assert(cpd);
197
198                                 cpd->m_jacDiagABInv = 1.f / jacDiagAB;
199
200                                 //Dependent on Rigidbody A and B types, fetch the contact/friction response func
201                                 //perhaps do a similar thing for friction/restutution combiner funcs...
202                                 
203                                 cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
204                                 cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
205                                 
206                                 btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
207                                 btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
208                                 btVector3 vel = vel1 - vel2;
209                                 btScalar rel_vel;
210                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
211                                 
212                                 float combinedRestitution = cp.m_combinedRestitution;
213                                 
214                                 cpd->m_penetration = cp.getDistance();
215                                 cpd->m_friction = cp.m_combinedFriction;
216                                 cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
217                                 if (cpd->m_restitution <= 0.) //0.f)
218                                 {
219                                         cpd->m_restitution = 0.0f;
220
221                                 };
222                                 
223                                 //restitution and penetration work in same direction so
224                                 //rel_vel 
225                                 
226                                 btScalar penVel = -cpd->m_penetration/info.m_timeStep;
227
228                                 if (cpd->m_restitution >= penVel)
229                                 {
230                                         cpd->m_penetration = 0.f;
231                                 }                               
232                                 
233                                 
234
235                                 float relaxation = info.m_damping;
236                                 cpd->m_appliedImpulse *= relaxation;
237                                 //for friction
238                                 cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
239                                 
240                                 //re-calculate friction direction every frame, todo: check if this is really needed
241                                 btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
242
243
244 #define NO_FRICTION_WARMSTART 1
245
246         #ifdef NO_FRICTION_WARMSTART
247                                 cpd->m_accumulatedTangentImpulse0 = 0.f;
248                                 cpd->m_accumulatedTangentImpulse1 = 0.f;
249         #endif //NO_FRICTION_WARMSTART
250                                 float denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
251                                 float denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
252                                 float denom = relaxation/(denom0+denom1);
253                                 cpd->m_jacDiagABInvTangent0 = denom;
254
255
256                                 denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
257                                 denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
258                                 denom = relaxation/(denom0+denom1);
259                                 cpd->m_jacDiagABInvTangent1 = denom;
260
261                                 btVector3 totalImpulse = 
262         #ifndef NO_FRICTION_WARMSTART
263                                         cp.m_frictionWorldTangential0*cp.m_accumulatedTangentImpulse0+
264                                         cp.m_frictionWorldTangential1*cp.m_accumulatedTangentImpulse1+
265         #endif //NO_FRICTION_WARMSTART
266                                         cp.m_normalWorldOnB*cpd->m_appliedImpulse;
267
268
269
270                                 ///
271                                 {
272                                 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
273                                 cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
274                                 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);            
275                                 cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
276                                 }
277                                 {
278                                         btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
279                                         cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
280                                 }
281                                 {
282                                         btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
283                                         cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
284                                 }
285                                 {
286                                         btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
287                                         cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
288                                 }
289                                 {
290                                         btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
291                                         cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
292                                 }
293                                 
294                                 ///
295
296
297
298                                 //apply previous frames impulse on both bodies
299                                 body0->applyImpulse(totalImpulse, rel_pos1);
300                                 body1->applyImpulse(-totalImpulse, rel_pos2);
301                         }
302                         
303                 }
304         }
305 }
306
307 float btSequentialImpulseConstraintSolver::solve(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
308 {
309
310         btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
311         btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
312
313         float maxImpulse = 0.f;
314
315         {
316                 const int numpoints = manifoldPtr->getNumContacts();
317
318                 btVector3 color(0,1,0);
319                 for (int i=0;i<numpoints ;i++)
320                 {
321
322                         int j=i;
323                         if (iter % 2)
324                                 j = numpoints-1-i;
325                         else
326                                 j=i;
327
328                         btManifoldPoint& cp = manifoldPtr->getContactPoint(j);
329                                 if (cp.getDistance() <= 0.f)
330                         {
331
332                                 if (iter == 0)
333                                 {
334                                         if (debugDrawer)
335                                                 debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
336                                 }
337
338                                 {
339
340                                         btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
341                                         float impulse = cpd->m_contactSolverFunc(
342                                                 *body0,*body1,
343                                                 cp,
344                                                 info);
345
346                                         if (maxImpulse < impulse)
347                                                 maxImpulse  = impulse;
348
349                                 }
350                         }
351                 }
352         }
353         return maxImpulse;
354 }
355
356 float btSequentialImpulseConstraintSolver::solveFriction(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
357 {
358         btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
359         btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
360
361
362         {
363                 const int numpoints = manifoldPtr->getNumContacts();
364
365                 btVector3 color(0,1,0);
366                 for (int i=0;i<numpoints ;i++)
367                 {
368
369                         int j=i;
370                         //if (iter % 2)
371                         //      j = numpoints-1-i;
372
373                         btManifoldPoint& cp = manifoldPtr->getContactPoint(j);
374                         if (cp.getDistance() <= 0.f)
375                         {
376
377                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
378                                 cpd->m_frictionSolverFunc(
379                                         *body0,*body1,
380                                         cp,
381                                         info);
382
383                                 
384                         }
385                 }
386
387         
388         }
389         return 0.f;
390 }