update Bullet 2.x with latest changes, notice that the integration is not finished...
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btContactConstraint.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 "btContactConstraint.h"
18 #include "BulletDynamics/Dynamics/btRigidBody.h"
19 #include "LinearMath/btVector3.h"
20 #include "btJacobianEntry.h"
21 #include "btContactSolverInfo.h"
22 #include "LinearMath/btMinMax.h"
23 #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
24
25 #define ASSERT2 assert
26
27 #define USE_INTERNAL_APPLY_IMPULSE 1
28
29
30 //bilateral constraint between two dynamic objects
31 void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
32                       btRigidBody& body2, const btVector3& pos2,
33                       btScalar distance, const btVector3& normal,btScalar& impulse ,float timeStep)
34 {
35         float normalLenSqr = normal.length2();
36         ASSERT2(fabs(normalLenSqr) < 1.1f);
37         if (normalLenSqr > 1.1f)
38         {
39                 impulse = 0.f;
40                 return;
41         }
42         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
43         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
44         //this jacobian entry could be re-used for all iterations
45         
46         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
47         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
48         btVector3 vel = vel1 - vel2;
49         
50
51           btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
52                 body2.getCenterOfMassTransform().getBasis().transpose(),
53                 rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
54                 body2.getInvInertiaDiagLocal(),body2.getInvMass());
55
56         btScalar jacDiagAB = jac.getDiagonal();
57         btScalar jacDiagABInv = 1.f / jacDiagAB;
58         
59           btScalar rel_vel = jac.getRelativeVelocity(
60                 body1.getLinearVelocity(),
61                 body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
62                 body2.getLinearVelocity(),
63                 body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
64         float a;
65         a=jacDiagABInv;
66
67
68         rel_vel = normal.dot(vel);
69         
70         //todo: move this into proper structure
71         btScalar contactDamping = 0.2f;
72
73 #ifdef ONLY_USE_LINEAR_MASS
74         btScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
75         impulse = - contactDamping * rel_vel * massTerm;
76 #else   
77         btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
78         impulse = velocityImpulse;
79 #endif
80 }
81
82
83
84 //response  between two dynamic objects with friction
85 float resolveSingleCollision(
86         btRigidBody& body1,
87         btRigidBody& body2,
88         btManifoldPoint& contactPoint,
89         const btContactSolverInfo& solverInfo)
90 {
91
92         const btVector3& pos1 = contactPoint.getPositionWorldOnA();
93         const btVector3& pos2 = contactPoint.getPositionWorldOnB();
94         const btVector3& normal = contactPoint.m_normalWorldOnB;
95
96         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
97         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
98         
99         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
100         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
101         btVector3 vel = vel1 - vel2;
102         btScalar rel_vel;
103         rel_vel = normal.dot(vel);
104         
105         btScalar Kfps = 1.f / solverInfo.m_timeStep ;
106
107         float damping = solverInfo.m_damping ;
108         float Kerp = solverInfo.m_erp;
109         float Kcor = Kerp *Kfps;
110
111         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
112         assert(cpd);
113         btScalar distance = cpd->m_penetration;
114         btScalar positionalError = Kcor *-distance;
115         btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
116
117         btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
118
119         btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
120
121         btScalar normalImpulse = penetrationImpulse+velocityImpulse;
122         
123         // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
124         float oldNormalImpulse = cpd->m_appliedImpulse;
125         float sum = oldNormalImpulse + normalImpulse;
126         cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum;
127
128         normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
129
130 #ifdef USE_INTERNAL_APPLY_IMPULSE
131         if (body1.getInvMass())
132         {
133                 body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
134         }
135         if (body2.getInvMass())
136         {
137                 body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
138         }
139 #else USE_INTERNAL_APPLY_IMPULSE
140         body1.applyImpulse(normal*(normalImpulse), rel_pos1);
141         body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
142 #endif //USE_INTERNAL_APPLY_IMPULSE
143
144         return normalImpulse;
145 }
146
147
148 float resolveSingleFriction(
149         btRigidBody& body1,
150         btRigidBody& body2,
151         btManifoldPoint& contactPoint,
152         const btContactSolverInfo& solverInfo)
153 {
154
155         const btVector3& pos1 = contactPoint.getPositionWorldOnA();
156         const btVector3& pos2 = contactPoint.getPositionWorldOnB();
157
158         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
159         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
160         
161         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
162         assert(cpd);
163
164         float combinedFriction = cpd->m_friction;
165         
166         btScalar limit = cpd->m_appliedImpulse * combinedFriction;
167         //if (contactPoint.m_appliedImpulse>0.f)
168         //friction
169         {
170                 //apply friction in the 2 tangential directions
171                 
172                 // 1st tangent
173                 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
174                 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
175                 btVector3 vel = vel1 - vel2;
176         
177                 btScalar j1,j2;
178
179                 {
180                                 
181                         btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
182
183                         // calculate j that moves us to zero relative velocity
184                         j1 = -vrel * cpd->m_jacDiagABInvTangent0;
185                         float total = cpd->m_accumulatedTangentImpulse0 + j1;
186                         GEN_set_min(total, limit);
187                         GEN_set_max(total, -limit);
188                         j1 = total - cpd->m_accumulatedTangentImpulse0;
189                         cpd->m_accumulatedTangentImpulse0 = total;
190                 }
191                 {
192                         // 2nd tangent
193
194                         btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
195                         
196                         // calculate j that moves us to zero relative velocity
197                         j2 = -vrel * cpd->m_jacDiagABInvTangent1;
198                         float total = cpd->m_accumulatedTangentImpulse1 + j2;
199                         GEN_set_min(total, limit);
200                         GEN_set_max(total, -limit);
201                         j2 = total - cpd->m_accumulatedTangentImpulse1;
202                         cpd->m_accumulatedTangentImpulse1 = total;
203                 }
204
205 #ifdef USE_INTERNAL_APPLY_IMPULSE
206         if (body1.getInvMass())
207         {
208                 body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
209                 body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
210         }
211         if (body2.getInvMass())
212         {
213                 body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
214                 body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);   
215         }
216 #else USE_INTERNAL_APPLY_IMPULSE
217         body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
218         body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
219 #endif //USE_INTERNAL_APPLY_IMPULSE
220
221
222         } 
223         return cpd->m_appliedImpulse;
224 }
225
226
227 float resolveSingleFrictionOriginal(
228         btRigidBody& body1,
229         btRigidBody& body2,
230         btManifoldPoint& contactPoint,
231         const btContactSolverInfo& solverInfo)
232 {
233
234         const btVector3& pos1 = contactPoint.getPositionWorldOnA();
235         const btVector3& pos2 = contactPoint.getPositionWorldOnB();
236
237         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
238         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
239         
240         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
241         assert(cpd);
242
243         float combinedFriction = cpd->m_friction;
244         
245         btScalar limit = cpd->m_appliedImpulse * combinedFriction;
246         //if (contactPoint.m_appliedImpulse>0.f)
247         //friction
248         {
249                 //apply friction in the 2 tangential directions
250                 
251                 {
252                         // 1st tangent
253                         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
254                         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
255                         btVector3 vel = vel1 - vel2;
256                         
257                         btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
258
259                         // calculate j that moves us to zero relative velocity
260                         btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
261                         float total = cpd->m_accumulatedTangentImpulse0 + j;
262                         GEN_set_min(total, limit);
263                         GEN_set_max(total, -limit);
264                         j = total - cpd->m_accumulatedTangentImpulse0;
265                         cpd->m_accumulatedTangentImpulse0 = total;
266                         body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
267                         body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
268                 }
269
270                                 
271                 {
272                         // 2nd tangent
273                         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
274                         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
275                         btVector3 vel = vel1 - vel2;
276
277                         btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
278                         
279                         // calculate j that moves us to zero relative velocity
280                         btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
281                         float total = cpd->m_accumulatedTangentImpulse1 + j;
282                         GEN_set_min(total, limit);
283                         GEN_set_max(total, -limit);
284                         j = total - cpd->m_accumulatedTangentImpulse1;
285                         cpd->m_accumulatedTangentImpulse1 = total;
286                         body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
287                         body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
288                 }
289         } 
290         return cpd->m_appliedImpulse;
291 }
292
293
294 //velocity + friction
295 //response  between two dynamic objects with friction
296 float resolveSingleCollisionCombined(
297         btRigidBody& body1,
298         btRigidBody& body2,
299         btManifoldPoint& contactPoint,
300         const btContactSolverInfo& solverInfo)
301 {
302
303         const btVector3& pos1 = contactPoint.getPositionWorldOnA();
304         const btVector3& pos2 = contactPoint.getPositionWorldOnB();
305         const btVector3& normal = contactPoint.m_normalWorldOnB;
306
307         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
308         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
309         
310         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
311         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
312         btVector3 vel = vel1 - vel2;
313         btScalar rel_vel;
314         rel_vel = normal.dot(vel);
315         
316         btScalar Kfps = 1.f / solverInfo.m_timeStep ;
317
318         float damping = solverInfo.m_damping ;
319         float Kerp = solverInfo.m_erp;
320         float Kcor = Kerp *Kfps;
321
322         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
323         assert(cpd);
324         btScalar distance = cpd->m_penetration;
325         btScalar positionalError = Kcor *-distance;
326         btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
327
328         btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
329
330         btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
331
332         btScalar normalImpulse = penetrationImpulse+velocityImpulse;
333         
334         // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
335         float oldNormalImpulse = cpd->m_appliedImpulse;
336         float sum = oldNormalImpulse + normalImpulse;
337         cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum;
338
339         normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
340
341
342 #ifdef USE_INTERNAL_APPLY_IMPULSE
343         if (body1.getInvMass())
344         {
345                 body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
346         }
347         if (body2.getInvMass())
348         {
349                 body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
350         }
351 #else USE_INTERNAL_APPLY_IMPULSE
352         body1.applyImpulse(normal*(normalImpulse), rel_pos1);
353         body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
354 #endif //USE_INTERNAL_APPLY_IMPULSE
355
356         {
357                 //friction
358                 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
359                 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
360                 btVector3 vel = vel1 - vel2;
361           
362                 rel_vel = normal.dot(vel);
363
364
365                 btVector3 lat_vel = vel - normal * rel_vel;
366                 btScalar lat_rel_vel = lat_vel.length();
367
368                 float combinedFriction = cpd->m_friction;
369
370                 if (cpd->m_appliedImpulse > 0)
371                 if (lat_rel_vel > SIMD_EPSILON)
372                 {
373                         lat_vel /= lat_rel_vel;
374                         btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
375                         btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
376                         btScalar friction_impulse = lat_rel_vel /
377                                 (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
378                         btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
379
380                         GEN_set_min(friction_impulse, normal_impulse);
381                         GEN_set_max(friction_impulse, -normal_impulse);
382                         body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
383                         body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
384                 }
385         }
386
387
388
389         return normalImpulse;
390 }
391 float resolveSingleFrictionEmpty(
392         btRigidBody& body1,
393         btRigidBody& body2,
394         btManifoldPoint& contactPoint,
395         const btContactSolverInfo& solverInfo)
396 {
397         return 0.f;
398 };