btConvexConvexAlgorithm.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00019 //define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
00020 //#define ZERO_MARGIN
00021 
00022 #include "btConvexConvexAlgorithm.h"
00023 
00024 //#include <stdio.h>
00025 #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
00026 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
00027 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
00028 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00029 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
00030 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
00031 
00032 
00033 
00034 #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
00035 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
00036 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
00037 #include "BulletCollision/CollisionShapes/btBoxShape.h"
00038 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
00039 
00040 #include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
00041 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
00042 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
00043 #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
00044 
00045 
00046 
00047 #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
00048 #include "BulletCollision/CollisionShapes/btSphereShape.h"
00049 
00050 #include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
00051 
00052 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
00053 #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
00054 #include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h"
00055 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
00056 
00058 
00059 
00060 
00061 static SIMD_FORCE_INLINE void segmentsClosestPoints(
00062         btVector3& ptsVector,
00063         btVector3& offsetA,
00064         btVector3& offsetB,
00065         btScalar& tA, btScalar& tB,
00066         const btVector3& translation,
00067         const btVector3& dirA, btScalar hlenA,
00068         const btVector3& dirB, btScalar hlenB )
00069 {
00070         // compute the parameters of the closest points on each line segment
00071 
00072         btScalar dirA_dot_dirB = btDot(dirA,dirB);
00073         btScalar dirA_dot_trans = btDot(dirA,translation);
00074         btScalar dirB_dot_trans = btDot(dirB,translation);
00075 
00076         btScalar denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
00077 
00078         if ( denom == 0.0f ) {
00079                 tA = 0.0f;
00080         } else {
00081                 tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
00082                 if ( tA < -hlenA )
00083                         tA = -hlenA;
00084                 else if ( tA > hlenA )
00085                         tA = hlenA;
00086         }
00087 
00088         tB = tA * dirA_dot_dirB - dirB_dot_trans;
00089 
00090         if ( tB < -hlenB ) {
00091                 tB = -hlenB;
00092                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
00093 
00094                 if ( tA < -hlenA )
00095                         tA = -hlenA;
00096                 else if ( tA > hlenA )
00097                         tA = hlenA;
00098         } else if ( tB > hlenB ) {
00099                 tB = hlenB;
00100                 tA = tB * dirA_dot_dirB + dirA_dot_trans;
00101 
00102                 if ( tA < -hlenA )
00103                         tA = -hlenA;
00104                 else if ( tA > hlenA )
00105                         tA = hlenA;
00106         }
00107 
00108         // compute the closest points relative to segment centers.
00109 
00110         offsetA = dirA * tA;
00111         offsetB = dirB * tB;
00112 
00113         ptsVector = translation - offsetA + offsetB;
00114 }
00115 
00116 
00117 static SIMD_FORCE_INLINE btScalar capsuleCapsuleDistance(
00118         btVector3& normalOnB,
00119         btVector3& pointOnB,
00120         btScalar capsuleLengthA,
00121         btScalar        capsuleRadiusA,
00122         btScalar capsuleLengthB,
00123         btScalar        capsuleRadiusB,
00124         int capsuleAxisA,
00125         int capsuleAxisB,
00126         const btTransform& transformA,
00127         const btTransform& transformB,
00128         btScalar distanceThreshold )
00129 {
00130         btVector3 directionA = transformA.getBasis().getColumn(capsuleAxisA);
00131         btVector3 translationA = transformA.getOrigin();
00132         btVector3 directionB = transformB.getBasis().getColumn(capsuleAxisB);
00133         btVector3 translationB = transformB.getOrigin();
00134 
00135         // translation between centers
00136 
00137         btVector3 translation = translationB - translationA;
00138 
00139         // compute the closest points of the capsule line segments
00140 
00141         btVector3 ptsVector;           // the vector between the closest points
00142         
00143         btVector3 offsetA, offsetB;    // offsets from segment centers to their closest points
00144         btScalar tA, tB;              // parameters on line segment
00145 
00146         segmentsClosestPoints( ptsVector, offsetA, offsetB, tA, tB, translation,
00147                                                    directionA, capsuleLengthA, directionB, capsuleLengthB );
00148 
00149         btScalar distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
00150 
00151         if ( distance > distanceThreshold )
00152                 return distance;
00153 
00154         btScalar lenSqr = ptsVector.length2();
00155         if (lenSqr<= (SIMD_EPSILON*SIMD_EPSILON))
00156         {
00157                 //degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
00158                 btVector3 q;
00159                 btPlaneSpace1(directionA,normalOnB,q);
00160         } else
00161         {
00162                 // compute the contact normal
00163                 normalOnB = ptsVector*-btRecipSqrt(lenSqr);
00164         }
00165         pointOnB = transformB.getOrigin()+offsetB + normalOnB * capsuleRadiusB;
00166 
00167         return distance;
00168 }
00169 
00170 
00171 
00172 
00173 
00174 
00175 
00177 
00178 
00179 
00180 
00181 
00182 btConvexConvexAlgorithm::CreateFunc::CreateFunc(btSimplexSolverInterface*                       simplexSolver, btConvexPenetrationDepthSolver* pdSolver)
00183 {
00184         m_numPerturbationIterations = 0;
00185         m_minimumPointsPerturbationThreshold = 3;
00186         m_simplexSolver = simplexSolver;
00187         m_pdSolver = pdSolver;
00188 }
00189 
00190 btConvexConvexAlgorithm::CreateFunc::~CreateFunc() 
00191 { 
00192 }
00193 
00194 btConvexConvexAlgorithm::btConvexConvexAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* pdSolver,int numPerturbationIterations, int minimumPointsPerturbationThreshold)
00195 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
00196 m_simplexSolver(simplexSolver),
00197 m_pdSolver(pdSolver),
00198 m_ownManifold (false),
00199 m_manifoldPtr(mf),
00200 m_lowLevelOfDetail(false),
00201 #ifdef USE_SEPDISTANCE_UTIL2
00202 m_sepDistance((static_cast<btConvexShape*>(body0->getCollisionShape()))->getAngularMotionDisc(),
00203                           (static_cast<btConvexShape*>(body1->getCollisionShape()))->getAngularMotionDisc()),
00204 #endif
00205 m_numPerturbationIterations(numPerturbationIterations),
00206 m_minimumPointsPerturbationThreshold(minimumPointsPerturbationThreshold)
00207 {
00208         (void)body0Wrap;
00209         (void)body1Wrap;
00210 }
00211 
00212 
00213 
00214 
00215 btConvexConvexAlgorithm::~btConvexConvexAlgorithm()
00216 {
00217         if (m_ownManifold)
00218         {
00219                 if (m_manifoldPtr)
00220                         m_dispatcher->releaseManifold(m_manifoldPtr);
00221         }
00222 }
00223 
00224 void    btConvexConvexAlgorithm ::setLowLevelOfDetail(bool useLowLevel)
00225 {
00226         m_lowLevelOfDetail = useLowLevel;
00227 }
00228 
00229 
00230 struct btPerturbedContactResult : public btManifoldResult
00231 {
00232         btManifoldResult* m_originalManifoldResult;
00233         btTransform m_transformA;
00234         btTransform m_transformB;
00235         btTransform     m_unPerturbedTransform;
00236         bool    m_perturbA;
00237         btIDebugDraw*   m_debugDrawer;
00238 
00239 
00240         btPerturbedContactResult(btManifoldResult* originalResult,const btTransform& transformA,const btTransform& transformB,const btTransform& unPerturbedTransform,bool perturbA,btIDebugDraw* debugDrawer)
00241                 :m_originalManifoldResult(originalResult),
00242                 m_transformA(transformA),
00243                 m_transformB(transformB),
00244                 m_unPerturbedTransform(unPerturbedTransform),
00245                 m_perturbA(perturbA),
00246                 m_debugDrawer(debugDrawer)
00247         {
00248         }
00249         virtual ~ btPerturbedContactResult()
00250         {
00251         }
00252 
00253         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar orgDepth)
00254         {
00255                 btVector3 endPt,startPt;
00256                 btScalar newDepth;
00257                 btVector3 newNormal;
00258 
00259                 if (m_perturbA)
00260                 {
00261                         btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
00262                         endPt = (m_unPerturbedTransform*m_transformA.inverse())(endPtOrg);
00263                         newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
00264                         startPt = endPt+normalOnBInWorld*newDepth;
00265                 } else
00266                 {
00267                         endPt = pointInWorld + normalOnBInWorld*orgDepth;
00268                         startPt = (m_unPerturbedTransform*m_transformB.inverse())(pointInWorld);
00269                         newDepth = (endPt -  startPt).dot(normalOnBInWorld);
00270                         
00271                 }
00272 
00273 //#define DEBUG_CONTACTS 1
00274 #ifdef DEBUG_CONTACTS
00275                 m_debugDrawer->drawLine(startPt,endPt,btVector3(1,0,0));
00276                 m_debugDrawer->drawSphere(startPt,0.05,btVector3(0,1,0));
00277                 m_debugDrawer->drawSphere(endPt,0.05,btVector3(0,0,1));
00278 #endif //DEBUG_CONTACTS
00279 
00280                 
00281                 m_originalManifoldResult->addContactPoint(normalOnBInWorld,startPt,newDepth);
00282         }
00283 
00284 };
00285 
00286 extern btScalar gContactBreakingThreshold;
00287 
00288 
00289 //
00290 // Convex-Convex collision algorithm
00291 //
00292 void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00293 {
00294 
00295         if (!m_manifoldPtr)
00296         {
00297                 //swapped?
00298                 m_manifoldPtr = m_dispatcher->getNewManifold(body0Wrap->getCollisionObject(),body1Wrap->getCollisionObject());
00299                 m_ownManifold = true;
00300         }
00301         resultOut->setPersistentManifold(m_manifoldPtr);
00302 
00303         //comment-out next line to test multi-contact generation
00304         //resultOut->getPersistentManifold()->clearManifold();
00305         
00306 
00307         const btConvexShape* min0 = static_cast<const btConvexShape*>(body0Wrap->getCollisionShape());
00308         const btConvexShape* min1 = static_cast<const btConvexShape*>(body1Wrap->getCollisionShape());
00309 
00310         btVector3  normalOnB;
00311                 btVector3  pointOnBWorld;
00312 #ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
00313         if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
00314         {
00315                 btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
00316                 btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
00317         //      btVector3 localScalingA = capsuleA->getLocalScaling();
00318         //      btVector3 localScalingB = capsuleB->getLocalScaling();
00319                 
00320                 btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
00321 
00322                 btScalar dist = capsuleCapsuleDistance(normalOnB,       pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
00323                         capsuleB->getHalfHeight(),capsuleB->getRadius(),capsuleA->getUpAxis(),capsuleB->getUpAxis(),
00324                         body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
00325 
00326                 if (dist<threshold)
00327                 {
00328                         btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
00329                         resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);       
00330                 }
00331                 resultOut->refreshContactPoints();
00332                 return;
00333         }
00334 #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
00335 
00336 
00337 
00338 
00339 #ifdef USE_SEPDISTANCE_UTIL2
00340         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
00341         {
00342                 m_sepDistance.updateSeparatingDistance(body0->getWorldTransform(),body1->getWorldTransform());
00343         }
00344 
00345         if (!dispatchInfo.m_useConvexConservativeDistanceUtil || m_sepDistance.getConservativeSeparatingDistance()<=0.f)
00346 #endif //USE_SEPDISTANCE_UTIL2
00347 
00348         {
00349 
00350         
00351         btGjkPairDetector::ClosestPointInput input;
00352 
00353         btGjkPairDetector       gjkPairDetector(min0,min1,m_simplexSolver,m_pdSolver);
00354         //TODO: if (dispatchInfo.m_useContinuous)
00355         gjkPairDetector.setMinkowskiA(min0);
00356         gjkPairDetector.setMinkowskiB(min1);
00357 
00358 #ifdef USE_SEPDISTANCE_UTIL2
00359         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
00360         {
00361                 input.m_maximumDistanceSquared = BT_LARGE_FLOAT;
00362         } else
00363 #endif //USE_SEPDISTANCE_UTIL2
00364         {
00365                 //if (dispatchInfo.m_convexMaxDistanceUseCPT)
00366                 //{
00367                 //      input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold();
00368                 //} else
00369                 //{
00370                 input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold();
00371 //              }
00372 
00373                 input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared;
00374         }
00375 
00376         input.m_stackAlloc = dispatchInfo.m_stackAllocator;
00377         input.m_transformA = body0Wrap->getWorldTransform();
00378         input.m_transformB = body1Wrap->getWorldTransform();
00379 
00380 
00381 
00382         
00383 
00384 #ifdef USE_SEPDISTANCE_UTIL2
00385         btScalar sepDist = 0.f;
00386         if (dispatchInfo.m_useConvexConservativeDistanceUtil)
00387         {
00388                 sepDist = gjkPairDetector.getCachedSeparatingDistance();
00389                 if (sepDist>SIMD_EPSILON)
00390                 {
00391                         sepDist += dispatchInfo.m_convexConservativeDistanceThreshold;
00392                         //now perturbe directions to get multiple contact points
00393                         
00394                 }
00395         }
00396 #endif //USE_SEPDISTANCE_UTIL2
00397 
00398         if (min0->isPolyhedral() && min1->isPolyhedral())
00399         {
00400 
00401 
00402                 struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result
00403                 {
00404                         virtual void setShapeIdentifiersA(int partId0,int index0){}
00405                         virtual void setShapeIdentifiersB(int partId1,int index1){}
00406                         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) 
00407                         {
00408                         }
00409                 };
00410 
00411                 
00412                 struct btWithoutMarginResult : public btDiscreteCollisionDetectorInterface::Result
00413                 {
00414                         btDiscreteCollisionDetectorInterface::Result* m_originalResult;
00415                         btVector3       m_reportedNormalOnWorld;
00416                         btScalar m_marginOnA;
00417                         btScalar m_marginOnB;
00418                         btScalar        m_reportedDistance;
00419                         
00420                         bool            m_foundResult;
00421                         btWithoutMarginResult(btDiscreteCollisionDetectorInterface::Result* result, btScalar marginOnA, btScalar marginOnB)
00422                         :m_originalResult(result),
00423                         m_marginOnA(marginOnA),
00424                         m_marginOnB(marginOnB),
00425                         m_foundResult(false)
00426                         {
00427                         }
00428                         
00429                         virtual void setShapeIdentifiersA(int partId0,int index0){}
00430                         virtual void setShapeIdentifiersB(int partId1,int index1){}
00431                         virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorldOrg,btScalar depthOrg) 
00432                         {
00433                                 m_reportedDistance = depthOrg;
00434                                 m_reportedNormalOnWorld = normalOnBInWorld;
00435                                 
00436                                 btVector3 adjustedPointB = pointInWorldOrg - normalOnBInWorld*m_marginOnB;
00437                                 m_reportedDistance = depthOrg+(m_marginOnA+m_marginOnB);
00438                                 if (m_reportedDistance<0.f)
00439                                 {
00440                                         m_foundResult = true;                                   
00441                                 }
00442                                 m_originalResult->addContactPoint(normalOnBInWorld,adjustedPointB,m_reportedDistance);
00443                         }
00444                 };
00445 
00446                 
00447                 btDummyResult dummy;
00448 
00450 
00451                 btScalar min0Margin = min0->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min0->getMargin();
00452                 btScalar min1Margin = min1->getShapeType()==BOX_SHAPE_PROXYTYPE? 0.f : min1->getMargin();
00453 
00454                 btWithoutMarginResult   withoutMargin(resultOut, min0Margin,min1Margin);
00455 
00456                 btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0;
00457                 btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1;
00458                 if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron())
00459                 {
00460 
00461 
00462                         
00463 
00464                         btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
00465 
00466                         btScalar minDist = -1e30f;
00467                         btVector3 sepNormalWorldSpace;
00468                         bool foundSepAxis  = true;
00469 
00470                         if (dispatchInfo.m_enableSatConvex)
00471                         {
00472                                 foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
00473                                         *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
00474                                         body0Wrap->getWorldTransform(), 
00475                                         body1Wrap->getWorldTransform(),
00476                                         sepNormalWorldSpace,*resultOut);
00477                         } else
00478                         {
00479 #ifdef ZERO_MARGIN
00480                                 gjkPairDetector.setIgnoreMargin(true);
00481                                 gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
00482 #else
00483 
00484 
00485                                 gjkPairDetector.getClosestPoints(input,withoutMargin,dispatchInfo.m_debugDraw);
00486                                 //gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
00487 #endif //ZERO_MARGIN
00488                                 //btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
00489                                 //if (l2>SIMD_EPSILON)
00490                                 {
00491                                         sepNormalWorldSpace = withoutMargin.m_reportedNormalOnWorld;//gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
00492                                         //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance();
00493                                         minDist = withoutMargin.m_reportedDistance;//gjkPairDetector.getCachedSeparatingDistance()+min0->getMargin()+min1->getMargin();
00494         
00495 #ifdef ZERO_MARGIN
00496                                         foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f;
00497 #else
00498                                         foundSepAxis = withoutMargin.m_foundResult && minDist<0;//-(min0->getMargin()+min1->getMargin());
00499 #endif
00500                                 }
00501                         }
00502                         if (foundSepAxis)
00503                         {
00504                                 
00505 //                              printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
00506 
00507                                 btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
00508                                         body0Wrap->getWorldTransform(), 
00509                                         body1Wrap->getWorldTransform(), minDist-threshold, threshold, *resultOut);
00510                                 
00511                         }
00512                         if (m_ownManifold)
00513                         {
00514                                 resultOut->refreshContactPoints();
00515                         }
00516                         return;
00517 
00518                 } else
00519                 {
00520                         //we can also deal with convex versus triangle (without connectivity data)
00521                         if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE)
00522                         {
00523 
00524                                 btVertexArray vertices;
00525                                 btTriangleShape* tri = (btTriangleShape*)polyhedronB;
00526                                 vertices.push_back(     body1Wrap->getWorldTransform()*tri->m_vertices1[0]);
00527                                 vertices.push_back(     body1Wrap->getWorldTransform()*tri->m_vertices1[1]);
00528                                 vertices.push_back(     body1Wrap->getWorldTransform()*tri->m_vertices1[2]);
00529                                 
00530                                 //tri->initializePolyhedralFeatures();
00531 
00532                                 btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
00533 
00534                                 btVector3 sepNormalWorldSpace;
00535                                 btScalar minDist =-1e30f;
00536                                 btScalar maxDist = threshold;
00537                                 
00538                                 bool foundSepAxis = false;
00539                                 if (0)
00540                                 {
00541                                         polyhedronB->initializePolyhedralFeatures();
00542                                          foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis(
00543                                         *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(),
00544                                         body0Wrap->getWorldTransform(), 
00545                                         body1Wrap->getWorldTransform(),
00546                                         sepNormalWorldSpace,*resultOut);
00547                                 //       printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ());
00548 
00549                                 } else
00550                                 {
00551 #ifdef ZERO_MARGIN
00552                                         gjkPairDetector.setIgnoreMargin(true);
00553                                         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
00554 #else
00555                                         gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw);
00556 #endif//ZERO_MARGIN
00557                                         
00558                                         btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
00559                                         if (l2>SIMD_EPSILON)
00560                                         {
00561                                                 sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
00562                                                 //minDist = gjkPairDetector.getCachedSeparatingDistance();
00563                                                 //maxDist = threshold;
00564                                                 minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin();
00565                                                 foundSepAxis = true;
00566                                         }
00567                                 }
00568 
00569                                 
00570                         if (foundSepAxis)
00571                         {
00572                                 btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), 
00573                                         body0Wrap->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut);
00574                         }
00575                                 
00576                                 
00577                                 if (m_ownManifold)
00578                                 {
00579                                         resultOut->refreshContactPoints();
00580                                 }
00581                                 
00582                                 return;
00583                         }
00584                         
00585                 }
00586 
00587 
00588         }
00589         
00590         gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw);
00591 
00592         //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
00593         
00594         //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points
00595         if (m_numPerturbationIterations && resultOut->getPersistentManifold()->getNumContacts() < m_minimumPointsPerturbationThreshold)
00596         {
00597                 
00598                 int i;
00599                 btVector3 v0,v1;
00600                 btVector3 sepNormalWorldSpace;
00601                 btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2();
00602         
00603                 if (l2>SIMD_EPSILON)
00604                 {
00605                         sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2);
00606                         
00607                         btPlaneSpace1(sepNormalWorldSpace,v0,v1);
00608 
00609 
00610                         bool perturbeA = true;
00611                         const btScalar angleLimit = 0.125f * SIMD_PI;
00612                         btScalar perturbeAngle;
00613                         btScalar radiusA = min0->getAngularMotionDisc();
00614                         btScalar radiusB = min1->getAngularMotionDisc();
00615                         if (radiusA < radiusB)
00616                         {
00617                                 perturbeAngle = gContactBreakingThreshold /radiusA;
00618                                 perturbeA = true;
00619                         } else
00620                         {
00621                                 perturbeAngle = gContactBreakingThreshold / radiusB;
00622                                 perturbeA = false;
00623                         }
00624                         if ( perturbeAngle > angleLimit ) 
00625                                         perturbeAngle = angleLimit;
00626 
00627                         btTransform unPerturbedTransform;
00628                         if (perturbeA)
00629                         {
00630                                 unPerturbedTransform = input.m_transformA;
00631                         } else
00632                         {
00633                                 unPerturbedTransform = input.m_transformB;
00634                         }
00635                         
00636                         for ( i=0;i<m_numPerturbationIterations;i++)
00637                         {
00638                                 if (v0.length2()>SIMD_EPSILON)
00639                                 {
00640                                 btQuaternion perturbeRot(v0,perturbeAngle);
00641                                 btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations));
00642                                 btQuaternion rotq(sepNormalWorldSpace,iterationAngle);
00643                                 
00644                                 
00645                                 if (perturbeA)
00646                                 {
00647                                         input.m_transformA.setBasis(  btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0Wrap->getWorldTransform().getBasis());
00648                                         input.m_transformB = body1Wrap->getWorldTransform();
00649         #ifdef DEBUG_CONTACTS
00650                                         dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0);
00651         #endif //DEBUG_CONTACTS
00652                                 } else
00653                                 {
00654                                         input.m_transformA = body0Wrap->getWorldTransform();
00655                                         input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1Wrap->getWorldTransform().getBasis());
00656         #ifdef DEBUG_CONTACTS
00657                                         dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0);
00658         #endif
00659                                 }
00660                                 
00661                                 btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw);
00662                                 gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw);
00663                                 }
00664                         }
00665                 }
00666         }
00667 
00668         
00669 
00670 #ifdef USE_SEPDISTANCE_UTIL2
00671         if (dispatchInfo.m_useConvexConservativeDistanceUtil && (sepDist>SIMD_EPSILON))
00672         {
00673                 m_sepDistance.initSeparatingDistance(gjkPairDetector.getCachedSeparatingAxis(),sepDist,body0->getWorldTransform(),body1->getWorldTransform());
00674         }
00675 #endif //USE_SEPDISTANCE_UTIL2
00676 
00677 
00678         }
00679 
00680         if (m_ownManifold)
00681         {
00682                 resultOut->refreshContactPoints();
00683         }
00684 
00685 }
00686 
00687 
00688 
00689 bool disableCcd = false;
00690 btScalar        btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0,btCollisionObject* col1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
00691 {
00692         (void)resultOut;
00693         (void)dispatchInfo;
00695     
00698         btScalar resultFraction = btScalar(1.);
00699 
00700 
00701         btScalar squareMot0 = (col0->getInterpolationWorldTransform().getOrigin() - col0->getWorldTransform().getOrigin()).length2();
00702         btScalar squareMot1 = (col1->getInterpolationWorldTransform().getOrigin() - col1->getWorldTransform().getOrigin()).length2();
00703     
00704         if (squareMot0 < col0->getCcdSquareMotionThreshold() &&
00705                 squareMot1 < col1->getCcdSquareMotionThreshold())
00706                 return resultFraction;
00707 
00708         if (disableCcd)
00709                 return btScalar(1.);
00710 
00711 
00712         //An adhoc way of testing the Continuous Collision Detection algorithms
00713         //One object is approximated as a sphere, to simplify things
00714         //Starting in penetration should report no time of impact
00715         //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
00716         //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
00717 
00718                 
00720         {
00721                 btConvexShape* convex0 = static_cast<btConvexShape*>(col0->getCollisionShape());
00722 
00723                 btSphereShape   sphere1(col1->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
00724                 btConvexCast::CastResult result;
00725                 btVoronoiSimplexSolver voronoiSimplex;
00726                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
00728                 btGjkConvexCast ccd1( convex0 ,&sphere1,&voronoiSimplex);
00729                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
00730                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
00731                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
00732                 {
00733                 
00734                         //store result.m_fraction in both bodies
00735                 
00736                         if (col0->getHitFraction()> result.m_fraction)
00737                                 col0->setHitFraction( result.m_fraction );
00738 
00739                         if (col1->getHitFraction() > result.m_fraction)
00740                                 col1->setHitFraction( result.m_fraction);
00741 
00742                         if (resultFraction > result.m_fraction)
00743                                 resultFraction = result.m_fraction;
00744 
00745                 }
00746                 
00747                 
00748 
00749 
00750         }
00751 
00753         {
00754                 btConvexShape* convex1 = static_cast<btConvexShape*>(col1->getCollisionShape());
00755 
00756                 btSphereShape   sphere0(col0->getCcdSweptSphereRadius()); //todo: allow non-zero sphere sizes, for better approximation
00757                 btConvexCast::CastResult result;
00758                 btVoronoiSimplexSolver voronoiSimplex;
00759                 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
00761                 btGjkConvexCast ccd1(&sphere0,convex1,&voronoiSimplex);
00762                 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
00763                 if (ccd1.calcTimeOfImpact(col0->getWorldTransform(),col0->getInterpolationWorldTransform(),
00764                         col1->getWorldTransform(),col1->getInterpolationWorldTransform(),result))
00765                 {
00766                 
00767                         //store result.m_fraction in both bodies
00768                 
00769                         if (col0->getHitFraction()      > result.m_fraction)
00770                                 col0->setHitFraction( result.m_fraction);
00771 
00772                         if (col1->getHitFraction() > result.m_fraction)
00773                                 col1->setHitFraction( result.m_fraction);
00774 
00775                         if (resultFraction > result.m_fraction)
00776                                 resultFraction = result.m_fraction;
00777 
00778                 }
00779         }
00780         
00781         return resultFraction;
00782 
00783 }
00784