Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/tutorial/src/external/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @ 10256

Last change on this file since 10256 was 5781, checked in by rgrieder, 15 years ago

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