← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.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
///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
 
17
///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
 
18
///with reproduction case
 
19
//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
 
20
 
 
21
#include "btConvexConvexAlgorithm.h"
 
22
 
 
23
//#include <stdio.h>
 
24
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
 
25
#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
 
26
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
27
#include "BulletCollision/CollisionShapes/btConvexShape.h"
 
28
#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
 
29
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
 
30
 
 
31
 
 
32
 
 
33
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
 
34
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
 
35
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 
36
#include "BulletCollision/CollisionShapes/btBoxShape.h"
 
37
#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
 
38
 
 
39
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
 
40
#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
 
41
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
 
42
#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
 
43
 
 
44
 
 
45
 
 
46
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
 
47
#include "BulletCollision/CollisionShapes/btSphereShape.h"
 
48
 
 
49
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
 
50
 
 
51
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 
52
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
 
53
#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
 
54
 
 
55
 
 
56
///////////
 
57
 
 
58
 
 
59
 
 
60
static SIMD_FORCE_INLINE void segmentsClosestPoints(
 
61
        btVector3& ptsVector,
 
62
        btVector3& offsetA,
 
63
        btVector3& offsetB,
 
64
        btScalar& tA, btScalar& tB,
 
65
        const btVector3& translation,
 
66
        const btVector3& dirA, btScalar hlenA,
 
67
        const btVector3& dirB, btScalar hlenB )
 
68
{
 
69
        // compute the parameters of the closest points on each line segment
 
70
 
 
71
        btScalar dirA_dot_dirB = btDot(dirA,dirB);
 
72
        btScalar dirA_dot_trans = btDot(dirA,translation);
 
73
        btScalar dirB_dot_trans = btDot(dirB,translation);
 
74
 
 
75
        btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
 
76
 
 
77
        if ( denom == 0.0f ) {
 
78
                tA = 0.0f;
 
79
        } else {
 
80
                tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
 
81
                if ( tA < -hlenA )
 
82
                        tA = -hlenA;
 
83
                else if ( tA > hlenA )
 
84
                        tA = hlenA;
 
85
        }
 
86
 
 
87
        tB = tA * dirA_dot_dirB - dirB_dot_trans;
 
88
 
 
89
        if ( tB < -hlenB ) {
 
90
                tB = -hlenB;
 
91
                tA = tB * dirA_dot_dirB + dirA_dot_trans;
 
92
 
 
93
                if ( tA < -hlenA )
 
94
                        tA = -hlenA;
 
95
                else if ( tA > hlenA )
 
96
                        tA = hlenA;
 
97
        } else if ( tB > hlenB ) {
 
98
                tB = hlenB;
 
99
                tA = tB * dirA_dot_dirB + dirA_dot_trans;
 
100
 
 
101
                if ( tA < -hlenA )
 
102
                        tA = -hlenA;
 
103
                else if ( tA > hlenA )
 
104
                        tA = hlenA;
 
105
        }
 
106
 
 
107
        // compute the closest points relative to segment centers.
 
108
 
 
109
        offsetA = dirA * tA;
 
110
        offsetB = dirB * tB;
 
111
 
 
112
        ptsVector = translation - offsetA + offsetB;
 
113
}
 
114
 
 
115
 
 
116
static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
 
117
        btVector3& normalOnB,
 
118
        btVector3& pointOnB,
 
119
        btScalar capsuleLengthA,
 
120
        btScalar        capsuleRadiusA,
 
121
        btScalar capsuleLengthB,
 
122
        btScalar        capsuleRadiusB,
 
123
        int capsuleAxisA,
 
124
        int capsuleAxisB,
 
125
        const btTransform& transformA,
 
126
        const btTransform& transformB,
 
127
        btScalar distanceThreshold )
 
128
{
 
129
        btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
 
130
        btVector3 translationA = transformA.getOrigin();
 
131
        btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
 
132
        btVector3 translationB = transformB.getOrigin();
 
133
 
 
134
        // translation between centers
 
135
 
 
136
        btVector3 translation = translationB - translationA;
 
137
 
 
138
        // compute the closest points of the capsule line segments
 
139
 
 
140
        btVector3 ptsVector;           // the vector between the closest points
 
141
        
 
142
        btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
 
143
        btScalar tA, tB;              // parameters on line segment
 
144
 
 
145
        segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
 
146
                                                   directionA, capsuleLengthA, directionB, capsuleLengthB );
 
147
 
 
148
        btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
 
149
 
 
150
        if ( distance > distanceThreshold )
 
151
                return distance;
 
152
 
 
153
        btScalar lenSqr = ptsVector.length2();
 
154
        if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
 
155
        {
 
156
                //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
 
157
                btVector3 q;
 
158
                btPlaneSpace1(directionA,normalOnB,q);
 
159
        } else
 
160
        {
 
161
                // compute the contact normal
 
162
                normalOnB = ptsVector*-btRecipSqrt(lenSqr);
 
163
        }
 
164
        pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
 
165
 
 
166
        return distance;
 
167
}
 
168
 
 
169
 
 
170
 
 
171
 
 
172
 
 
173
 
 
174
 
 
175
//////////
 
176
 
 
177
 
 
178
 
 
179
 
 
180
 
 
181
btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                       simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
 
182
{
 
183
        m_numPerturbationIterations = 0;
 
184
        m_minimumPointsPerturbationThreshold = 3;
 
185
        m_simplexSolver = simplexSolver;
 
186
        m_pdSolver = pdSolver;
 
187
}
 
188
 
 
189
btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
 
190
{ 
 
191
}
 
192
 
 
193
btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
 
194
: btActivatingCollisionAlgorithm(ci,body0,body1),
 
195
m_simplexSolver(simplexSolver),
 
196
m_pdSolver(pdSolver),
 
197
m_ownManifold (false),
 
198
m_manifoldPtr(mf),
 
199
m_lowLevelOfDetail(false),
 
200
#ifdef USE_SEPDISTANCE_UTIL2
 
201
m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
 
202
                          (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
 
203
#endif
 
204
m_numPerturbationIterations(numPerturbationIterations),
 
205
m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
 
206
{
 
207
        (void)body0;
 
208
        (void)body1;
 
209
}
 
210
 
 
211
 
 
212
 
 
213
 
 
214
btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
 
215
{
 
216
        if (m_ownManifold)
 
217
        {
 
218
                if (m_manifoldPtr)
 
219
                        m_dispatcher->releaseManifold(m_manifoldPtr);
 
220
        }
 
221
}
 
222
 
 
223
void    btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
 
224
{
 
225
        m_lowLevelOfDetail = useLowLevel;
 
226
}
 
227
 
 
228
 
 
229
struct btPerturbedContactResult : public btManifoldResult
 
230
{
 
231
        btManifoldResult* m_originalManifoldResult;
 
232
        btTransform m_transformA;
 
233
        btTransform m_transformB;
 
234
        btTransform     m_unPerturbedTransform;
 
235
        bool    m_perturbA;
 
236
        btIDebugDraw*   m_debugDrawer;
 
237
 
 
238
 
 
239
        btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
 
240
                :m_originalManifoldResult(originalResult),
 
241
                m_transformA(transformA),
 
242
                m_transformB(transformB),
 
243
                m_unPerturbedTransform(unPerturbedTransform),
 
244
                m_perturbA(perturbA),
 
245
                m_debugDrawer(debugDrawer)
 
246
        {
 
247
        }
 
248
        virtual ~ btPerturbedContactResult()
 
249
        {
 
250
        }
 
251
 
 
252
        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
 
253
        {
 
254
                btVector3 endPt,startPt;
 
255
                btScalar newDepth;
 
256
                btVector3 newNormal;
 
257
 
 
258
                if (m_perturbA)
 
259
                {
 
260
                        btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
 
261
                        endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
 
262
                        newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
 
263
                        startPt = endPt+normalOnBInWorld*newDepth;
 
264
                } else
 
265
                {
 
266
                        endPt = pointInWorld + normalOnBInWorld*orgDepth;
 
267
                        startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
 
268
                        newDepth = (endPt -  startPt).dot(normalOnBInWorld);
 
269
                        
 
270
                }
 
271
 
 
272
//#define DEBUG_CONTACTS 1
 
273
#ifdef DEBUG_CONTACTS
 
274
                m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
 
275
                m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
 
276
                m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
 
277
#endif //DEBUG_CONTACTS
 
278
 
 
279
                
 
280
                m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
 
281
        }
 
282
 
 
283
};
 
284
 
 
285
extern btScalar gContactBreakingThreshold;
 
286
 
 
287
 
 
288
//
 
289
// Convex-Convex collision algorithm
 
290
//
 
291
void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 
292
{
 
293
 
 
294
        if (!m_manifoldPtr)
 
295
        {
 
296
                //swapped?
 
297
                m_manifoldPtr = m_dispatcher->getNewManifold(body0,body1);
 
298
                m_ownManifold = true;
 
299
        }
 
300
        resultOut->setPersistentManifold(m_manifoldPtr);
 
301
 
 
302
        //comment-out next line to test multi-contact generation
 
303
        //resultOut->getPersistentManifold()->clearManifold();
 
304
        
 
305
 
 
306
        btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
 
307
        btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
 
308
 
 
309
        btVector3  normalOnB;
 
310
                btVector3  pointOnBWorld;
 
311
#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
 
312
        if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
 
313
        {
 
314
                btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
 
315
                btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
 
316
                btVector3 localScalingA = capsuleA->getLocalScaling();
 
317
                btVector3 localScalingB = capsuleB->getLocalScaling();
 
318
                
 
319
                btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
 
320
 
 
321
                btScalar dist = capsuleCapsuleDistance(normalOnB,       pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
 
322
                        capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
 
323
                        body0->getWorldTransform(),body1->getWorldTransform(),threshold);
 
324
 
 
325
                if (dist<threshold)
 
326
                {
 
327
                        btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
 
328
                        resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);       
 
329
                }
 
330
                resultOut->refreshContactPoints();
 
331
                return;
 
332
        }
 
333
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
 
334
 
 
335
 
 
336
 
 
337
 
 
338
#ifdef USE_SEPDISTANCE_UTIL2
 
339
        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
 
340
        {
 
341
                m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
 
342
        }
 
343
 
 
344
        if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
 
345
#endif //USE_SEPDISTANCE_UTIL2
 
346
 
 
347
        {
 
348
 
 
349
        
 
350
        btGjkPairDetector::ClosestPointInput input;
 
351
 
 
352
        btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
 
353
        //TODO: if (dispatchInfo.m_useContinuous)
 
354
        gjkPairDetector.setMinkowskiA(min0);
 
355
        gjkPairDetector.setMinkowskiB(min1);
 
356
 
 
357
#ifdef USE_SEPDISTANCE_UTIL2
 
358
        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
 
359
        {
 
360
                input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
 
361
        } else
 
362
#endif //USE_SEPDISTANCE_UTIL2
 
363
        {
 
364
                //if (dispatchInfo.m_convexMaxDistanceUseCPT)
 
365
                //{
 
366
                //      input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
 
367
                //} else
 
368
                //{
 
369
                input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
 
370
//              }
 
371
 
 
372
                input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
 
373
        }
 
374
 
 
375
        input.m_stackAlloc = dispatchInfo.m_stackAllocator;
 
376
        input.m_transformA = body0->getWorldTransform();
 
377
        input.m_transformB = body1->getWorldTransform();
 
378
 
 
379
 
 
380
 
 
381
        
 
382
 
 
383
#ifdef USE_SEPDISTANCE_UTIL2
 
384
        btScalar sepDist = 0.f;
 
385
        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
 
386
        {
 
387
                sepDist = gjkPairDetector.getCachedSeparatingDistance();
 
388
                if (sepDist>SIMD_EPSILON)
 
389
                {
 
390
                        sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
 
391
                        //now perturbe directions to get multiple contact points
 
392
                        
 
393
                }
 
394
        }
 
395
#endif //USE_SEPDISTANCE_UTIL2
 
396
 
 
397
        if (min0->isPolyhedral() && min1->isPolyhedral())
 
398
        {
 
399
 
 
400
 
 
401
                struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
 
402
                {
 
403
                        virtual void setShapeIdentifiersA(int partId0,int index0){}
 
404
                        virtual void setShapeIdentifiersB(int partId1,int index1){}
 
405
                        virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) 
 
406
                        {
 
407
                        }
 
408
                };
 
409
                
 
410
                btDummyResult dummy;
 
411
 
 
412
 
 
413
                btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
 
414
                btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
 
415
                if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
 
416
                {
 
417
 
 
418
 
 
419
                        gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
 
420
                        
 
421
 
 
422
                        btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
 
423
 
 
424
                        btScalar minDist = 0.f;
 
425
                        btVector3 sepNormalWorldSpace;
 
426
                        bool foundSepAxis  = true;
 
427
 
 
428
                        if (dispatchInfo.m_enableSatConvex)
 
429
                        {
 
430
                                foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
 
431
                                        *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
 
432
                                        body0->getWorldTransform(), 
 
433
                                        body1->getWorldTransform(),
 
434
                                        sepNormalWorldSpace);
 
435
                        } else
 
436
                        {
 
437
                                sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
 
438
                                minDist = gjkPairDetector.getCachedSeparatingDistance();
 
439
                        }
 
440
                        if (foundSepAxis)
 
441
                        {
 
442
//                              printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
 
443
 
 
444
                                btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
 
445
                                        body0->getWorldTransform(), 
 
446
                                        body1->getWorldTransform(), minDist-threshold, threshold, *resultOut);
 
447
                                
 
448
                        }
 
449
                        if (m_ownManifold)
 
450
                        {
 
451
                                resultOut->refreshContactPoints();
 
452
                        }
 
453
                        return;
 
454
 
 
455
                } else
 
456
                {
 
457
                        //we can also deal with convex versus triangle (without connectivity data)
 
458
                        if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
 
459
                        {
 
460
                                gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
 
461
                
 
462
                                btVector3 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
 
463
 
 
464
                                btVertexArray vertices;
 
465
                                btTriangleShape* tri = (btTriangleShape*)polyhedronB;
 
466
                                vertices.push_back(     body1->getWorldTransform()*tri->m_vertices1[0]);
 
467
                                vertices.push_back(     body1->getWorldTransform()*tri->m_vertices1[1]);
 
468
                                vertices.push_back(     body1->getWorldTransform()*tri->m_vertices1[2]);
 
469
 
 
470
                                btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
 
471
                                btScalar minDist = gjkPairDetector.getCachedSeparatingDistance();
 
472
                                btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
 
473
                                        body0->getWorldTransform(), vertices, minDist-threshold, threshold, *resultOut);
 
474
                                
 
475
                                
 
476
                                if (m_ownManifold)
 
477
                                {
 
478
                                        resultOut->refreshContactPoints();
 
479
                                }
 
480
                                
 
481
                                return;
 
482
                        }
 
483
                        
 
484
                }
 
485
 
 
486
 
 
487
        }
 
488
        
 
489
        gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
 
490
 
 
491
        //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
 
492
        
 
493
        //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
 
494
        if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
 
495
        {
 
496
                
 
497
                int i;
 
498
                btVector3 v0,v1;
 
499
                btVector3 sepNormalWorldSpace;
 
500
        
 
501
                sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
 
502
                btPlaneSpace1(sepNormalWorldSpace,v0,v1);
 
503
 
 
504
 
 
505
                bool perturbeA = true;
 
506
                const btScalar angleLimit = 0.125f * SIMD_PI;
 
507
                btScalar perturbeAngle;
 
508
                btScalar radiusA = min0->getAngularMotionDisc();
 
509
                btScalar radiusB = min1->getAngularMotionDisc();
 
510
                if (radiusA < radiusB)
 
511
                {
 
512
                        perturbeAngle = gContactBreakingThreshold /radiusA;
 
513
                        perturbeA = true;
 
514
                } else
 
515
                {
 
516
                        perturbeAngle = gContactBreakingThreshold / radiusB;
 
517
                        perturbeA = false;
 
518
                }
 
519
                if ( perturbeAngle > angleLimit ) 
 
520
                                perturbeAngle = angleLimit;
 
521
 
 
522
                btTransform unPerturbedTransform;
 
523
                if (perturbeA)
 
524
                {
 
525
                        unPerturbedTransform = input.m_transformA;
 
526
                } else
 
527
                {
 
528
                        unPerturbedTransform = input.m_transformB;
 
529
                }
 
530
                
 
531
                for ( i=0;i<m_numPerturbationIterations;i++)
 
532
                {
 
533
                        if (v0.length2()>SIMD_EPSILON)
 
534
                        {
 
535
                        btQuaternion perturbeRot(v0,perturbeAngle);
 
536
                        btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
 
537
                        btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
 
538
                        
 
539
                        
 
540
                        if (perturbeA)
 
541
                        {
 
542
                                input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis());
 
543
                                input.m_transformB = body1->getWorldTransform();
 
544
#ifdef DEBUG_CONTACTS
 
545
                                dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
 
546
#endif //DEBUG_CONTACTS
 
547
                        } else
 
548
                        {
 
549
                                input.m_transformA = body0->getWorldTransform();
 
550
                                input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis());
 
551
#ifdef DEBUG_CONTACTS
 
552
                                dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
 
553
#endif
 
554
                        }
 
555
                        
 
556
                        btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
 
557
                        gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
 
558
                        }
 
559
                        
 
560
                }
 
561
        }
 
562
 
 
563
        
 
564
 
 
565
#ifdef USE_SEPDISTANCE_UTIL2
 
566
        if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
 
567
        {
 
568
                m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
 
569
        }
 
570
#endif //USE_SEPDISTANCE_UTIL2
 
571
 
 
572
 
 
573
        }
 
574
 
 
575
        if (m_ownManifold)
 
576
        {
 
577
                resultOut->refreshContactPoints();
 
578
        }
 
579
 
 
580
}
 
581
 
 
582
 
 
583
 
 
584
bool disableCcd = false;
 
585
btScalar        btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 
586
{
 
587
        (void)resultOut;
 
588
        (void)dispatchInfo;
 
589
        ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
 
590
    
 
591
        ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
 
592
        ///col0->m_worldTransform,
 
593
        btScalar resultFraction = btScalar(1.);
 
594
 
 
595
 
 
596
        btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
 
597
        btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
 
598
    
 
599
        if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
 
600
                squareMot1 < col1->getCcdSquareMotionThreshold())
 
601
                return resultFraction;
 
602
 
 
603
        if (disableCcd)
 
604
                return btScalar(1.);
 
605
 
 
606
 
 
607
        //An adhoc way of testing the Continuous Collision Detection algorithms
 
608
        //One object is approximated as a sphere, to simplify things
 
609
        //Starting in penetration should report no time of impact
 
610
        //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
 
611
        //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
 
612
 
 
613
                
 
614
        /// Convex0 against sphere for Convex1
 
615
        {
 
616
                btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
 
617
 
 
618
                btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
 
619
                btConvexCast::CastResult result;
 
620
                btVoronoiSimplexSolver voronoiSimplex;
 
621
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 
622
                ///Simplification, one object is simplified as a sphere
 
623
                btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
 
624
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
 
625
                if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
 
626
                        col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
 
627
                {
 
628
                
 
629
                        //store result.m_fraction in both bodies
 
630
                
 
631
                        if (col0->getHitFraction()> result.m_fraction)
 
632
                                col0->setHitFraction( result.m_fraction );
 
633
 
 
634
                        if (col1->getHitFraction() > result.m_fraction)
 
635
                                col1->setHitFraction( result.m_fraction);
 
636
 
 
637
                        if (resultFraction > result.m_fraction)
 
638
                                resultFraction = result.m_fraction;
 
639
 
 
640
                }
 
641
                
 
642
                
 
643
 
 
644
 
 
645
        }
 
646
 
 
647
        /// Sphere (for convex0) against Convex1
 
648
        {
 
649
                btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
 
650
 
 
651
                btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
 
652
                btConvexCast::CastResult result;
 
653
                btVoronoiSimplexSolver voronoiSimplex;
 
654
                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
 
655
                ///Simplification, one object is simplified as a sphere
 
656
                btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
 
657
                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
 
658
                if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
 
659
                        col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
 
660
                {
 
661
                
 
662
                        //store result.m_fraction in both bodies
 
663
                
 
664
                        if (col0->getHitFraction()      > result.m_fraction)
 
665
                                col0->setHitFraction( result.m_fraction);
 
666
 
 
667
                        if (col1->getHitFraction() > result.m_fraction)
 
668
                                col1->setHitFraction( result.m_fraction);
 
669
 
 
670
                        if (resultFraction > result.m_fraction)
 
671
                                resultFraction = result.m_fraction;
 
672
 
 
673
                }
 
674
        }
 
675
        
 
676
        return resultFraction;
 
677
 
 
678
}
 
679