angular-only hinge updated
[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         
168         if (cpd->m_appliedImpulse>0.f)
169         //friction
170         {
171                 //apply friction in the 2 tangential directions
172                 
173                 // 1st tangent
174                 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
175                 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
176                 btVector3 vel = vel1 - vel2;
177         
178                 btScalar j1,j2;
179
180                 {
181                                 
182                         btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
183
184                         // calculate j that moves us to zero relative velocity
185                         j1 = -vrel * cpd->m_jacDiagABInvTangent0;
186                         float oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
187                         cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
188                         GEN_set_min(cpd->m_accumulatedTangentImpulse0, limit);
189                         GEN_set_max(cpd->m_accumulatedTangentImpulse0, -limit);
190                         j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;
191
192                 }
193                 {
194                         // 2nd tangent
195
196                         btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
197                         
198                         // calculate j that moves us to zero relative velocity
199                         j2 = -vrel * cpd->m_jacDiagABInvTangent1;
200                         float oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
201                         cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
202                         GEN_set_min(cpd->m_accumulatedTangentImpulse1, limit);
203                         GEN_set_max(cpd->m_accumulatedTangentImpulse1, -limit);
204                         j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
205                 }
206
207 #ifdef USE_INTERNAL_APPLY_IMPULSE
208         if (body1.getInvMass())
209         {
210                 body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
211                 body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
212         }
213         if (body2.getInvMass())
214         {
215                 body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
216                 body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);   
217         }
218 #else //USE_INTERNAL_APPLY_IMPULSE
219         body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
220         body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
221 #endif //USE_INTERNAL_APPLY_IMPULSE
222
223
224         } 
225         return cpd->m_appliedImpulse;
226 }
227
228
229 float resolveSingleFrictionOriginal(
230         btRigidBody& body1,
231         btRigidBody& body2,
232         btManifoldPoint& contactPoint,
233         const btContactSolverInfo& solverInfo)
234 {
235
236         const btVector3& pos1 = contactPoint.getPositionWorldOnA();
237         const btVector3& pos2 = contactPoint.getPositionWorldOnB();
238
239         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
240         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
241         
242         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
243         assert(cpd);
244
245         float combinedFriction = cpd->m_friction;
246         
247         btScalar limit = cpd->m_appliedImpulse * combinedFriction;
248         //if (contactPoint.m_appliedImpulse>0.f)
249         //friction
250         {
251                 //apply friction in the 2 tangential directions
252                 
253                 {
254                         // 1st tangent
255                         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
256                         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
257                         btVector3 vel = vel1 - vel2;
258                         
259                         btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
260
261                         // calculate j that moves us to zero relative velocity
262                         btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
263                         float total = cpd->m_accumulatedTangentImpulse0 + j;
264                         GEN_set_min(total, limit);
265                         GEN_set_max(total, -limit);
266                         j = total - cpd->m_accumulatedTangentImpulse0;
267                         cpd->m_accumulatedTangentImpulse0 = total;
268                         body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
269                         body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
270                 }
271
272                                 
273                 {
274                         // 2nd tangent
275                         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
276                         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
277                         btVector3 vel = vel1 - vel2;
278
279                         btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
280                         
281                         // calculate j that moves us to zero relative velocity
282                         btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
283                         float total = cpd->m_accumulatedTangentImpulse1 + j;
284                         GEN_set_min(total, limit);
285                         GEN_set_max(total, -limit);
286                         j = total - cpd->m_accumulatedTangentImpulse1;
287                         cpd->m_accumulatedTangentImpulse1 = total;
288                         body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
289                         body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
290                 }
291         } 
292         return cpd->m_appliedImpulse;
293 }
294
295
296 //velocity + friction
297 //response  between two dynamic objects with friction
298 float resolveSingleCollisionCombined(
299         btRigidBody& body1,
300         btRigidBody& body2,
301         btManifoldPoint& contactPoint,
302         const btContactSolverInfo& solverInfo)
303 {
304
305         const btVector3& pos1 = contactPoint.getPositionWorldOnA();
306         const btVector3& pos2 = contactPoint.getPositionWorldOnB();
307         const btVector3& normal = contactPoint.m_normalWorldOnB;
308
309         btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
310         btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
311         
312         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
313         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
314         btVector3 vel = vel1 - vel2;
315         btScalar rel_vel;
316         rel_vel = normal.dot(vel);
317         
318         btScalar Kfps = 1.f / solverInfo.m_timeStep ;
319
320         //float damping = solverInfo.m_damping ;
321         float Kerp = solverInfo.m_erp;
322         float Kcor = Kerp *Kfps;
323
324         btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
325         assert(cpd);
326         btScalar distance = cpd->m_penetration;
327         btScalar positionalError = Kcor *-distance;
328         btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
329
330         btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
331
332         btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
333
334         btScalar normalImpulse = penetrationImpulse+velocityImpulse;
335         
336         // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
337         float oldNormalImpulse = cpd->m_appliedImpulse;
338         float sum = oldNormalImpulse + normalImpulse;
339         cpd->m_appliedImpulse = 0.f > sum ? 0.f: sum;
340
341         normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
342
343
344 #ifdef USE_INTERNAL_APPLY_IMPULSE
345         if (body1.getInvMass())
346         {
347                 body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
348         }
349         if (body2.getInvMass())
350         {
351                 body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
352         }
353 #else //USE_INTERNAL_APPLY_IMPULSE
354         body1.applyImpulse(normal*(normalImpulse), rel_pos1);
355         body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
356 #endif //USE_INTERNAL_APPLY_IMPULSE
357
358         {
359                 //friction
360                 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
361                 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
362                 btVector3 vel = vel1 - vel2;
363           
364                 rel_vel = normal.dot(vel);
365
366
367                 btVector3 lat_vel = vel - normal * rel_vel;
368                 btScalar lat_rel_vel = lat_vel.length();
369
370                 float combinedFriction = cpd->m_friction;
371
372                 if (cpd->m_appliedImpulse > 0)
373                 if (lat_rel_vel > SIMD_EPSILON)
374                 {
375                         lat_vel /= lat_rel_vel;
376                         btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
377                         btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
378                         btScalar friction_impulse = lat_rel_vel /
379                                 (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
380                         btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
381
382                         GEN_set_min(friction_impulse, normal_impulse);
383                         GEN_set_max(friction_impulse, -normal_impulse);
384                         body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
385                         body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
386                 }
387         }
388
389
390
391         return normalImpulse;
392 }
393 float resolveSingleFrictionEmpty(
394         btRigidBody& body1,
395         btRigidBody& body2,
396         btManifoldPoint& contactPoint,
397         const btContactSolverInfo& solverInfo)
398 {
399         return 0.f;
400 };
401