Engine/lib/bullet/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16
  17 #include "btConvexConcaveCollisionAlgorithm.h"
  18 #include "LinearMath/btQuickprof.h"
  19 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  20 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
  21 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
  22 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
  23 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
  24 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
  25 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
  26 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  27 #include "LinearMath/btIDebugDraw.h"
  28 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  29 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  30
  31 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm( const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped)
  32 : btActivatingCollisionAlgorithm(ci,body0Wrap,body1Wrap),
  33 m_isSwapped(isSwapped),
  34 m_btConvexTriangleCallback(ci.m_dispatcher1,body0Wrap,body1Wrap,isSwapped)
  35 {
  36 }
  37
  38 btConvexConcaveCollisionAlgorithm::~btConvexConcaveCollisionAlgorithm()
  39 {
  40 }
  41
  42 void    btConvexConcaveCollisionAlgorithm::getAllContactManifolds(btManifoldArray&      manifoldArray)
  43 {
  44         if (m_btConvexTriangleCallback.m_manifoldPtr)
  45         {
  46                 manifoldArray.push_back(m_btConvexTriangleCallback.m_manifoldPtr);
  47         }
  48 }
  49
  50
  51 btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher*  dispatcher,const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,bool isSwapped):
  52           m_dispatcher(dispatcher),
  53         m_dispatchInfoPtr(0)
  54 {
  55         m_convexBodyWrap = isSwapped? body1Wrap:body0Wrap;
  56         m_triBodyWrap = isSwapped? body0Wrap:body1Wrap;
  57
  58           //
  59           // create the manifold from the dispatcher 'manifold pool'
  60           //
  61           m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(),m_triBodyWrap->getCollisionObject());
  62
  63           clearCache();
  64 }
  65
  66 btConvexTriangleCallback::~btConvexTriangleCallback()
  67 {
  68         clearCache();
  69         m_dispatcher->releaseManifold( m_manifoldPtr );
  70
  71 }
  72
  73
  74 void    btConvexTriangleCallback::clearCache()
  75 {
  76         m_dispatcher->clearManifold(m_manifoldPtr);
  77 }
  78
  79
  80 void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
  81 partId, int triangleIndex)
  82 {
  83         BT_PROFILE("btConvexTriangleCallback::processTriangle");
  84
  85         if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
  86         {
  87                 return;
  88         }
  89
  90         //just for debugging purposes
  91         //printf("triangle %d",m_triangleCount++);
  92
  93
  94
  95         btCollisionAlgorithmConstructionInfo ci;
  96         ci.m_dispatcher1 = m_dispatcher;
  97
  98
  99
 100 #if 0
 101
 102         ///debug drawing of the overlapping triangles
 103         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
 104         {
 105                 const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
 106                 btVector3 color(1,1,0);
 107                 btTransform& tr = ob->getWorldTransform();
 108                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
 109                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
 110                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
 111         }
 112 #endif
 113
 114         if (m_convexBodyWrap->getCollisionShape()->isConvex())
 115         {
 116                 btTriangleShape tm(triangle[0],triangle[1],triangle[2]);
 117                 tm.setMargin(m_collisionMarginTriangle);
 118
 119
 120                 btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform?
 121                 btCollisionAlgorithm* colAlgo = 0;
 122
 123                 if (m_resultOut->m_closestPointDistanceThreshold > 0)
 124                 {
 125                         colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
 126                 }
 127                 else
 128                 {
 129                         colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS);
 130                 }
 131                 const btCollisionObjectWrapper* tmpWrap = 0;
 132
 133                 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
 134                 {
 135                         tmpWrap = m_resultOut->getBody0Wrap();
 136                         m_resultOut->setBody0Wrap(&triObWrap);
 137                         m_resultOut->setShapeIdentifiersA(partId,triangleIndex);
 138                 }
 139                 else
 140                 {
 141                         tmpWrap = m_resultOut->getBody1Wrap();
 142                         m_resultOut->setBody1Wrap(&triObWrap);
 143                         m_resultOut->setShapeIdentifiersB(partId,triangleIndex);
 144                 }
 145
 146                 colAlgo->processCollision(m_convexBodyWrap,&triObWrap,*m_dispatchInfoPtr,m_resultOut);
 147
 148                 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
 149                 {
 150                         m_resultOut->setBody0Wrap(tmpWrap);
 151                 } else
 152                 {
 153                         m_resultOut->setBody1Wrap(tmpWrap);
 154                 }
 155
 156
 157
 158                 colAlgo->~btCollisionAlgorithm();
 159                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 160         }
 161
 162 }
 163
 164
 165
 166 void    btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle,const btDispatcherInfo& dispatchInfo,const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
 167 {
 168         m_convexBodyWrap = convexBodyWrap;
 169         m_triBodyWrap = triBodyWrap;
 170
 171         m_dispatchInfoPtr = &dispatchInfo;
 172         m_collisionMarginTriangle = collisionMarginTriangle;
 173         m_resultOut = resultOut;
 174
 175         //recalc aabbs
 176         btTransform convexInTriangleSpace;
 177         convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
 178         const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
 179         //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
 180         convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);
 181         btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold;
 182
 183         btVector3 extra(extraMargin,extraMargin,extraMargin);
 184
 185         m_aabbMax += extra;
 186         m_aabbMin -= extra;
 187
 188 }
 189
 190 void btConvexConcaveCollisionAlgorithm::clearCache()
 191 {
 192         m_btConvexTriangleCallback.clearCache();
 193
 194 }
 195
 196 void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 197 {
 198         BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
 199
 200         const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
 201         const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
 202
 203         if (triBodyWrap->getCollisionShape()->isConcave())
 204         {
 205
 206
 207
 208                 const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>( triBodyWrap->getCollisionShape());
 209
 210                 if (convexBodyWrap->getCollisionShape()->isConvex())
 211                 {
 212                         btScalar collisionMarginTriangle = concaveShape->getMargin();
 213
 214                         resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
 215                         m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,convexBodyWrap,triBodyWrap,resultOut);
 216
 217                         m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(),triBodyWrap->getCollisionObject());
 218
 219                         concaveShape->processAllTriangles( &m_btConvexTriangleCallback,m_btConvexTriangleCallback.getAabbMin(),m_btConvexTriangleCallback.getAabbMax());
 220
 221                         resultOut->refreshContactPoints();
 222
 223                         m_btConvexTriangleCallback.clearWrapperData();
 224
 225                 }
 226
 227         }
 228
 229 }
 230
 231
 232 btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 233 {
 234         (void)resultOut;
 235         (void)dispatchInfo;
 236         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
 237         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 238
 239
 240         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
 241
 242         //only perform CCD above a certain threshold, this prevents blocking on the long run
 243         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
 244         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
 245         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
 246         {
 247                 return btScalar(1.);
 248         }
 249
 250         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
 251         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
 252         //todo: only do if the motion exceeds the 'radius'
 253
 254         btTransform triInv = triBody->getWorldTransform().inverse();
 255         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
 256         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
 257
 258         struct LocalTriangleSphereCastCallback  : public btTriangleCallback
 259         {
 260                 btTransform m_ccdSphereFromTrans;
 261                 btTransform m_ccdSphereToTrans;
 262                 btTransform     m_meshTransform;
 263
 264                 btScalar        m_ccdSphereRadius;
 265                 btScalar        m_hitFraction;
 266
 267
 268                 LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction)
 269                         :m_ccdSphereFromTrans(from),
 270                         m_ccdSphereToTrans(to),
 271                         m_ccdSphereRadius(ccdSphereRadius),
 272                         m_hitFraction(hitFraction)
 273                 {
 274                 }
 275
 276
 277                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 278                 {
 279                         BT_PROFILE("processTriangle");
 280                         (void)partId;
 281                         (void)triangleIndex;
 282                         //do a swept sphere for now
 283                         btTransform ident;
 284                         ident.setIdentity();
 285                         btConvexCast::CastResult castResult;
 286                         castResult.m_fraction = m_hitFraction;
 287                         btSphereShape   pointShape(m_ccdSphereRadius);
 288                         btTriangleShape triShape(triangle[0],triangle[1],triangle[2]);
 289                         btVoronoiSimplexSolver  simplexSolver;
 290                         btSubsimplexConvexCast convexCaster(&pointShape,&triShape,&simplexSolver);
 291                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
 292                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
 293                         //local space?
 294
 295                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans,m_ccdSphereToTrans,
 296                                 ident,ident,castResult))
 297                         {
 298                                 if (m_hitFraction > castResult.m_fraction)
 299                                         m_hitFraction = castResult.m_fraction;
 300                         }
 301
 302                 }
 303
 304         };
 305
 306
 307
 308
 309
 310         if (triBody->getCollisionShape()->isConcave())
 311         {
 312                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
 313                 rayAabbMin.setMin(convexToLocal.getOrigin());
 314                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
 315                 rayAabbMax.setMax(convexToLocal.getOrigin());
 316                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
 317                 rayAabbMin -= btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
 318                 rayAabbMax += btVector3(ccdRadius0,ccdRadius0,ccdRadius0);
 319
 320                 btScalar curHitFraction = btScalar(1.); //is this available?
 321                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal,convexToLocal,
 322                         convexbody->getCcdSweptSphereRadius(),curHitFraction);
 323
 324                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
 325
 326                 btCollisionObject* concavebody = triBody;
 327
 328                 btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape();
 329
 330                 if (triangleMesh)
 331                 {
 332                         triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax);
 333                 }
 334
 335
 336
 337                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
 338                 {
 339                         convexbody->setHitFraction( raycastCallback.m_hitFraction);
 340                         return raycastCallback.m_hitFraction;
 341                 }
 342         }
 343
 344         return btScalar(1.);
 345
 346 }