Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @ 9137

Last change on this file since 9137 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 21.2 KB
RevLine 
[1963]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
[8351]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
[1963]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"
[8351]28#include "BulletCollision/CollisionShapes/btCapsuleShape.h"
[8393]29#include "BulletCollision/CollisionShapes/btTriangleShape.h"
[8351]30
31
[8393]32
[1963]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
[2430]51#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
[1963]52#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
[8393]53#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
[1963]54
55
[8351]56///////////
[1963]57
58
59
[8351]60static 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
[1963]70
[8351]71        btScalar dirA_dot_dirB = btDot(dirA,dirB);
72        btScalar dirA_dot_trans = btDot(dirA,translation);
73        btScalar dirB_dot_trans = btDot(dirB,translation);
[1963]74
[8351]75        btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
[1963]76
[8351]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
116static 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
[1963]181btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                       simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
182{
[2882]183        m_numPerturbationIterations = 0;
184        m_minimumPointsPerturbationThreshold = 3;
[1963]185        m_simplexSolver = simplexSolver;
186        m_pdSolver = pdSolver;
187}
188
189btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
190{ 
191}
192
[2882]193btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* body0,btCollisionObject* body1,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
[2430]194: btActivatingCollisionAlgorithm(ci,body0,body1),
195m_simplexSolver(simplexSolver),
196m_pdSolver(pdSolver),
[1963]197m_ownManifold (false),
198m_manifoldPtr(mf),
[2882]199m_lowLevelOfDetail(false),
[2430]200#ifdef USE_SEPDISTANCE_UTIL2
[8351]201m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
[2882]202                          (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
[2430]203#endif
[2882]204m_numPerturbationIterations(numPerturbationIterations),
205m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
[1963]206{
207        (void)body0;
208        (void)body1;
209}
210
211
212
213
214btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
215{
216        if (m_ownManifold)
217        {
218                if (m_manifoldPtr)
219                        m_dispatcher->releaseManifold(m_manifoldPtr);
220        }
221}
222
223void    btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
224{
225        m_lowLevelOfDetail = useLowLevel;
226}
227
228
[2882]229struct 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;
[1963]237
238
[2882]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),
[8351]243                m_unPerturbedTransform(unPerturbedTransform),
[2882]244                m_perturbA(perturbA),
245                m_debugDrawer(debugDrawer)
246        {
247        }
248        virtual ~ btPerturbedContactResult()
249        {
250        }
[1963]251
[2882]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
285extern btScalar gContactBreakingThreshold;
286
[8351]287
[1963]288//
289// Convex-Convex collision algorithm
290//
291void 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
[2882]302        //comment-out next line to test multi-contact generation
303        //resultOut->getPersistentManifold()->clearManifold();
[2430]304       
[1963]305
306        btConvexShape* min0 = static_cast<btConvexShape*>(body0->getCollisionShape());
307        btConvexShape* min1 = static_cast<btConvexShape*>(body1->getCollisionShape());
[2430]308
[8351]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
[8393]336
337
[2430]338#ifdef USE_SEPDISTANCE_UTIL2
[8351]339        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
340        {
341                m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
342        }
343
[2430]344        if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
345#endif //USE_SEPDISTANCE_UTIL2
346
347        {
348
[1963]349       
350        btGjkPairDetector::ClosestPointInput input;
351
[2430]352        btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
[1963]353        //TODO: if (dispatchInfo.m_useContinuous)
[2430]354        gjkPairDetector.setMinkowskiA(min0);
355        gjkPairDetector.setMinkowskiB(min1);
356
357#ifdef USE_SEPDISTANCE_UTIL2
358        if (dispatchInfo.m_useConvexConservativeDistanceUtil)
359        {
[8351]360                input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
[2430]361        } else
362#endif //USE_SEPDISTANCE_UTIL2
363        {
[8393]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
[2430]372                input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
373        }
374
[1963]375        input.m_stackAlloc = dispatchInfo.m_stackAllocator;
376        input.m_transformA = body0->getWorldTransform();
377        input.m_transformB = body1->getWorldTransform();
378
[2430]379
[8393]380
[8351]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
[8393]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
[2882]491        //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
492       
493        //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
[8351]494        if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
[2882]495        {
496               
497                int i;
[8351]498                btVector3 v0,v1;
499                btVector3 sepNormalWorldSpace;
500       
501                sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized();
502                btPlaneSpace1(sepNormalWorldSpace,v0,v1);
[2882]503
[8351]504
[2882]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                {
[8351]533                        if (v0.length2()>SIMD_EPSILON)
534                        {
[2882]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);
[8351]558                        }
[2882]559                       
560                }
561        }
562
563       
564
[2430]565#ifdef USE_SEPDISTANCE_UTIL2
[8351]566        if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
[2430]567        {
568                m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
569        }
570#endif //USE_SEPDISTANCE_UTIL2
571
572
573        }
574
[1963]575        if (m_ownManifold)
576        {
577                resultOut->refreshContactPoints();
578        }
579
580}
581
582
583
584bool disableCcd = false;
585btScalar        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
Note: See TracBrowser for help on using the repository browser.