← Back to branch summary

~ubuntu-branches/ubuntu/precise/supertuxkart/precise

« back to all changes in this revision

Viewing changes to src/bullet/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Christoph Egger
  • Date: 2011-02-24 22:36:25 UTC
  • mfrom: (1.1.9 upstream) (6.1.4 sid)
  • Revision ID: james.westby@ubuntu.com-20110224223625-ygrjfpg92obovuch
Tags: 0.7+dfsg1-1
New upstream release

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/bullet/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 ,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
89
 
btScalar 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
 
        assert(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
 
 
153
 
btScalar 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
 
        assert(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
 
 
236
 
btScalar resolveSingleFrictionOriginal(
237
 
        btRigidBody& body1,
238
 
        btRigidBody& body2,
239
 
        btManifoldPoint& contactPoint,
240
 
        const btContactSolverInfo& solverInfo)
241
 
{
242
 
 
243
 
        (void)solverInfo;
244
 
 
245
 
        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
246
 
        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
247
 
 
248
 
        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
249
 
        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
250
 
        
251
 
        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
252
 
        assert(cpd);
253
 
 
254
 
        btScalar combinedFriction = cpd->m_friction;
255
 
        
256
 
        btScalar limit = cpd->m_appliedImpulse * combinedFriction;
257
 
        //if (contactPoint.m_appliedImpulse>btScalar(0.))
258
 
        //friction
259
 
        {
260
 
                //apply friction in the 2 tangential directions
261
 
                
262
 
                {
263
 
                        // 1st tangent
264
 
                        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
265
 
                        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
266
 
                        btVector3 vel = vel1 - vel2;
267
 
                        
268
 
                        btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
269
 
 
270
 
                        // calculate j that moves us to zero relative velocity
271
 
                        btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
272
 
                        btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
273
 
                        btSetMin(total, limit);
274
 
                        btSetMax(total, -limit);
275
 
                        j = total - cpd->m_accumulatedTangentImpulse0;
276
 
                        cpd->m_accumulatedTangentImpulse0 = total;
277
 
                        body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
278
 
                        body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
279
 
                }
280
 
 
281
 
                                
282
 
                {
283
 
                        // 2nd tangent
284
 
                        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
285
 
                        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
286
 
                        btVector3 vel = vel1 - vel2;
287
 
 
288
 
                        btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
289
 
                        
290
 
                        // calculate j that moves us to zero relative velocity
291
 
                        btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
292
 
                        btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
293
 
                        btSetMin(total, limit);
294
 
                        btSetMax(total, -limit);
295
 
                        j = total - cpd->m_accumulatedTangentImpulse1;
296
 
                        cpd->m_accumulatedTangentImpulse1 = total;
297
 
                        body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
298
 
                        body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
299
 
                }
300
 
        } 
301
 
        return cpd->m_appliedImpulse;
302
 
}
303
 
 
304
 
 
305
 
//velocity + friction
306
 
//response  between two dynamic objects with friction
307
 
btScalar resolveSingleCollisionCombined(
308
 
        btRigidBody& body1,
309
 
        btRigidBody& body2,
310
 
        btManifoldPoint& contactPoint,
311
 
        const btContactSolverInfo& solverInfo)
312
 
{
313
 
 
314
 
        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
315
 
        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
316
 
        const btVector3& normal = contactPoint.m_normalWorldOnB;
317
 
 
318
 
        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
319
 
        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
320
 
        
321
 
        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
322
 
        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
323
 
        btVector3 vel = vel1 - vel2;
324
 
        btScalar rel_vel;
325
 
        rel_vel = normal.dot(vel);
326
 
        
327
 
        btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
328
 
 
329
 
        //btScalar damping = solverInfo.m_damping ;
330
 
        btScalar Kerp = solverInfo.m_erp;
331
 
        btScalar Kcor = Kerp *Kfps;
332
 
 
333
 
        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
334
 
        assert(cpd);
335
 
        btScalar distance = cpd->m_penetration;
336
 
        btScalar positionalError = Kcor *-distance;
337
 
        btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
338
 
 
339
 
        btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
340
 
 
341
 
        btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
342
 
 
343
 
        btScalar normalImpulse = penetrationImpulse+velocityImpulse;
344
 
        
345
 
        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
346
 
        btScalar oldNormalImpulse = cpd->m_appliedImpulse;
347
 
        btScalar sum = oldNormalImpulse + normalImpulse;
348
 
        cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
349
 
 
350
 
        normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
351
 
 
352
 
 
353
 
#ifdef USE_INTERNAL_APPLY_IMPULSE
354
 
        if (body1.getInvMass())
355
 
        {
356
 
                body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
357
 
        }
358
 
        if (body2.getInvMass())
359
 
        {
360
 
                body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
361
 
        }
362
 
#else //USE_INTERNAL_APPLY_IMPULSE
363
 
        body1.applyImpulse(normal*(normalImpulse), rel_pos1);
364
 
        body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
365
 
#endif //USE_INTERNAL_APPLY_IMPULSE
366
 
 
367
 
        {
368
 
                //friction
369
 
                btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
370
 
                btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
371
 
                btVector3 vel = vel1 - vel2;
372
 
          
373
 
                rel_vel = normal.dot(vel);
374
 
 
375
 
 
376
 
                btVector3 lat_vel = vel - normal * rel_vel;
377
 
                btScalar lat_rel_vel = lat_vel.length();
378
 
 
379
 
                btScalar combinedFriction = cpd->m_friction;
380
 
 
381
 
                if (cpd->m_appliedImpulse > 0)
382
 
                if (lat_rel_vel > SIMD_EPSILON)
383
 
                {
384
 
                        lat_vel /= lat_rel_vel;
385
 
                        btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
386
 
                        btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
387
 
                        btScalar friction_impulse = lat_rel_vel /
388
 
                                (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
389
 
                        btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
390
 
 
391
 
                        btSetMin(friction_impulse, normal_impulse);
392
 
                        btSetMax(friction_impulse, -normal_impulse);
393
 
                        body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
394
 
                        body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
395
 
                }
396
 
        }
397
 
 
398
 
 
399
 
 
400
 
        return normalImpulse;
401
 
}
402
 
 
403
 
btScalar resolveSingleFrictionEmpty(
404
 
        btRigidBody& body1,
405
 
        btRigidBody& body2,
406
 
        btManifoldPoint& contactPoint,
407
 
        const btContactSolverInfo& solverInfo)
408
 
{
409
 
        (void)contactPoint;
410
 
        (void)body1;
411
 
        (void)body2;
412
 
        (void)solverInfo;
413
 
 
414
 
 
415
 
        return btScalar(0.);
416
 
};
417
 
 
 
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 ,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
 
89
btScalar 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
        assert(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
 
 
153
btScalar 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
        assert(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
 
 
236
btScalar resolveSingleFrictionOriginal(
 
237
        btRigidBody& body1,
 
238
        btRigidBody& body2,
 
239
        btManifoldPoint& contactPoint,
 
240
        const btContactSolverInfo& solverInfo)
 
241
{
 
242
 
 
243
        (void)solverInfo;
 
244
 
 
245
        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
 
246
        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
 
247
 
 
248
        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
 
249
        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
 
250
        
 
251
        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
 
252
        assert(cpd);
 
253
 
 
254
        btScalar combinedFriction = cpd->m_friction;
 
255
        
 
256
        btScalar limit = cpd->m_appliedImpulse * combinedFriction;
 
257
        //if (contactPoint.m_appliedImpulse>btScalar(0.))
 
258
        //friction
 
259
        {
 
260
                //apply friction in the 2 tangential directions
 
261
                
 
262
                {
 
263
                        // 1st tangent
 
264
                        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
 
265
                        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 
266
                        btVector3 vel = vel1 - vel2;
 
267
                        
 
268
                        btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);
 
269
 
 
270
                        // calculate j that moves us to zero relative velocity
 
271
                        btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
 
272
                        btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
 
273
                        btSetMin(total, limit);
 
274
                        btSetMax(total, -limit);
 
275
                        j = total - cpd->m_accumulatedTangentImpulse0;
 
276
                        cpd->m_accumulatedTangentImpulse0 = total;
 
277
                        body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
 
278
                        body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
 
279
                }
 
280
 
 
281
                                
 
282
                {
 
283
                        // 2nd tangent
 
284
                        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
 
285
                        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 
286
                        btVector3 vel = vel1 - vel2;
 
287
 
 
288
                        btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
 
289
                        
 
290
                        // calculate j that moves us to zero relative velocity
 
291
                        btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
 
292
                        btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
 
293
                        btSetMin(total, limit);
 
294
                        btSetMax(total, -limit);
 
295
                        j = total - cpd->m_accumulatedTangentImpulse1;
 
296
                        cpd->m_accumulatedTangentImpulse1 = total;
 
297
                        body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
 
298
                        body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
 
299
                }
 
300
        } 
 
301
        return cpd->m_appliedImpulse;
 
302
}
 
303
 
 
304
 
 
305
//velocity + friction
 
306
//response  between two dynamic objects with friction
 
307
btScalar resolveSingleCollisionCombined(
 
308
        btRigidBody& body1,
 
309
        btRigidBody& body2,
 
310
        btManifoldPoint& contactPoint,
 
311
        const btContactSolverInfo& solverInfo)
 
312
{
 
313
 
 
314
        const btVector3& pos1 = contactPoint.getPositionWorldOnA();
 
315
        const btVector3& pos2 = contactPoint.getPositionWorldOnB();
 
316
        const btVector3& normal = contactPoint.m_normalWorldOnB;
 
317
 
 
318
        btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
 
319
        btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
 
320
        
 
321
        btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
 
322
        btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 
323
        btVector3 vel = vel1 - vel2;
 
324
        btScalar rel_vel;
 
325
        rel_vel = normal.dot(vel);
 
326
        
 
327
        btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;
 
328
 
 
329
        //btScalar damping = solverInfo.m_damping ;
 
330
        btScalar Kerp = solverInfo.m_erp;
 
331
        btScalar Kcor = Kerp *Kfps;
 
332
 
 
333
        btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
 
334
        assert(cpd);
 
335
        btScalar distance = cpd->m_penetration;
 
336
        btScalar positionalError = Kcor *-distance;
 
337
        btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;
 
338
 
 
339
        btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;
 
340
 
 
341
        btScalar        velocityImpulse = velocityError * cpd->m_jacDiagABInv;
 
342
 
 
343
        btScalar normalImpulse = penetrationImpulse+velocityImpulse;
 
344
        
 
345
        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
 
346
        btScalar oldNormalImpulse = cpd->m_appliedImpulse;
 
347
        btScalar sum = oldNormalImpulse + normalImpulse;
 
348
        cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
 
349
 
 
350
        normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;
 
351
 
 
352
 
 
353
#ifdef USE_INTERNAL_APPLY_IMPULSE
 
354
        if (body1.getInvMass())
 
355
        {
 
356
                body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
 
357
        }
 
358
        if (body2.getInvMass())
 
359
        {
 
360
                body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
 
361
        }
 
362
#else //USE_INTERNAL_APPLY_IMPULSE
 
363
        body1.applyImpulse(normal*(normalImpulse), rel_pos1);
 
364
        body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
 
365
#endif //USE_INTERNAL_APPLY_IMPULSE
 
366
 
 
367
        {
 
368
                //friction
 
369
                btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
 
370
                btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
 
371
                btVector3 vel = vel1 - vel2;
 
372
          
 
373
                rel_vel = normal.dot(vel);
 
374
 
 
375
 
 
376
                btVector3 lat_vel = vel - normal * rel_vel;
 
377
                btScalar lat_rel_vel = lat_vel.length();
 
378
 
 
379
                btScalar combinedFriction = cpd->m_friction;
 
380
 
 
381
                if (cpd->m_appliedImpulse > 0)
 
382
                if (lat_rel_vel > SIMD_EPSILON)
 
383
                {
 
384
                        lat_vel /= lat_rel_vel;
 
385
                        btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
 
386
                        btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
 
387
                        btScalar friction_impulse = lat_rel_vel /
 
388
                                (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
 
389
                        btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;
 
390
 
 
391
                        btSetMin(friction_impulse, normal_impulse);
 
392
                        btSetMax(friction_impulse, -normal_impulse);
 
393
                        body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
 
394
                        body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
 
395
                }
 
396
        }
 
397
 
 
398
 
 
399
 
 
400
        return normalImpulse;
 
401
}
 
402
 
 
403
btScalar resolveSingleFrictionEmpty(
 
404
        btRigidBody& body1,
 
405
        btRigidBody& body2,
 
406
        btManifoldPoint& contactPoint,
 
407
        const btContactSolverInfo& solverInfo)
 
408
{
 
409
        (void)contactPoint;
 
410
        (void)body1;
 
411
        (void)body2;
 
412
        (void)solverInfo;
 
413
 
 
414
 
 
415
        return btScalar(0.);
 
416
};
 
417