dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   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 #include "btConvexConcaveCollisionAlgorithm.h"
  17 #include "LinearMath/btQuickprof.h"
  18 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  19 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
  20 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
  21 #include "BulletCollision/CollisionShapes/btConcaveShape.h"
  22 #include "BulletCollision/CollisionDispatch/btManifoldResult.h"
  23 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
  24 #include "BulletCollision/CollisionShapes/btTriangleShape.h"
  25 #include "BulletCollision/CollisionShapes/btSphereShape.h"
  26 #include "LinearMath/btIDebugDraw.h"
  27 #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
  28 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  29 #include "BulletCollision/CollisionShapes/btSdfCollisionShape.h"
  30
  31 btConvexConcaveCollisionAlgorithm::btConvexConcaveCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped)
  32         : btActivatingCollisionAlgorithm(ci, body0Wrap, body1Wrap),
  33           m_btConvexTriangleCallback(ci.m_dispatcher1, body0Wrap, body1Wrap, isSwapped),
  34           m_isSwapped(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 btConvexTriangleCallback::btConvexTriangleCallback(btDispatcher* dispatcher, const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, bool isSwapped) : m_dispatcher(dispatcher),
  51                                                                                                                                                                                                                                                                                                                                                                          m_dispatchInfoPtr(0)
  52 {
  53         m_convexBodyWrap = isSwapped ? body1Wrap : body0Wrap;
  54         m_triBodyWrap = isSwapped ? body0Wrap : body1Wrap;
  55
  56         //
  57         // create the manifold from the dispatcher 'manifold pool'
  58         //
  59         m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBodyWrap->getCollisionObject(), m_triBodyWrap->getCollisionObject());
  60
  61         clearCache();
  62 }
  63
  64 btConvexTriangleCallback::~btConvexTriangleCallback()
  65 {
  66         clearCache();
  67         m_dispatcher->releaseManifold(m_manifoldPtr);
  68 }
  69
  70 void btConvexTriangleCallback::clearCache()
  71 {
  72         m_dispatcher->clearManifold(m_manifoldPtr);
  73 }
  74
  75 void btConvexTriangleCallback::processTriangle(btVector3* triangle, int partId, int triangleIndex)
  76 {
  77         BT_PROFILE("btConvexTriangleCallback::processTriangle");
  78
  79         if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
  80         {
  81                 return;
  82         }
  83
  84         //just for debugging purposes
  85         //printf("triangle %d",m_triangleCount++);
  86
  87         btCollisionAlgorithmConstructionInfo ci;
  88         ci.m_dispatcher1 = m_dispatcher;
  89
  90 #if 0
  91
  92         ///debug drawing of the overlapping triangles
  93         if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe ))
  94         {
  95                 const btCollisionObject* ob = const_cast<btCollisionObject*>(m_triBodyWrap->getCollisionObject());
  96                 btVector3 color(1,1,0);
  97                 btTransform& tr = ob->getWorldTransform();
  98                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color);
  99                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color);
 100                 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color);
 101         }
 102 #endif
 103
 104         if (m_convexBodyWrap->getCollisionShape()->isConvex())
 105         {
 106 #ifdef BT_ENABLE_CONVEX_CONCAVE_EARLY_OUT
 107     //todo: check this issue https://github.com/bulletphysics/bullet3/issues/4263
 108                 //an early out optimisation if the object is separated from the triangle
 109                 //projected on the triangle normal)
 110                 {
 111                         const btVector3 v0 = m_triBodyWrap->getWorldTransform()*triangle[0];
 112                         const btVector3 v1 = m_triBodyWrap->getWorldTransform()*triangle[1];
 113                         const btVector3 v2 = m_triBodyWrap->getWorldTransform()*triangle[2];
 114
 115                         btVector3 triangle_normal_world = ( v1 - v0).cross(v2 - v0);
 116                         triangle_normal_world.normalize();
 117
 118                     btConvexShape* convex = (btConvexShape*)m_convexBodyWrap->getCollisionShape();
 119
 120                         btVector3 localPt = convex->localGetSupportingVertex(m_convexBodyWrap->getWorldTransform().getBasis().inverse()*triangle_normal_world);
 121                         btVector3 worldPt = m_convexBodyWrap->getWorldTransform()*localPt;
 122                         //now check if this is fully on one side of the triangle
 123                         btScalar proj_distPt = triangle_normal_world.dot(worldPt);
 124                         btScalar proj_distTr = triangle_normal_world.dot(v0);
 125                         btScalar contact_threshold = m_manifoldPtr->getContactBreakingThreshold()+ m_resultOut->m_closestPointDistanceThreshold;
 126                         btScalar dist = proj_distTr - proj_distPt;
 127                         if (dist > contact_threshold)
 128                                 return;
 129
 130                         //also check the other side of the triangle
 131                         triangle_normal_world*=-1;
 132
 133                         localPt = convex->localGetSupportingVertex(m_convexBodyWrap->getWorldTransform().getBasis().inverse()*triangle_normal_world);
 134                         worldPt = m_convexBodyWrap->getWorldTransform()*localPt;
 135                         //now check if this is fully on one side of the triangle
 136                         proj_distPt = triangle_normal_world.dot(worldPt);
 137                         proj_distTr = triangle_normal_world.dot(v0);
 138
 139                         dist = proj_distTr - proj_distPt;
 140                         if (dist > contact_threshold)
 141                                 return;
 142         }
 143 #endif //BT_ENABLE_CONVEX_CONCAVE_EARLY_OUT
 144
 145                 btTriangleShape tm(triangle[0], triangle[1], triangle[2]);
 146                 tm.setMargin(m_collisionMarginTriangle);
 147
 148                 btCollisionObjectWrapper triObWrap(m_triBodyWrap, &tm, m_triBodyWrap->getCollisionObject(), m_triBodyWrap->getWorldTransform(), partId, triangleIndex);  //correct transform?
 149                 btCollisionAlgorithm* colAlgo = 0;
 150
 151                 if (m_resultOut->m_closestPointDistanceThreshold > 0)
 152                 {
 153                         colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS);
 154                 }
 155                 else
 156                 {
 157                         colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS);
 158                 }
 159                 const btCollisionObjectWrapper* tmpWrap = 0;
 160
 161                 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
 162                 {
 163                         tmpWrap = m_resultOut->getBody0Wrap();
 164                         m_resultOut->setBody0Wrap(&triObWrap);
 165                         m_resultOut->setShapeIdentifiersA(partId, triangleIndex);
 166                 }
 167                 else
 168                 {
 169                         tmpWrap = m_resultOut->getBody1Wrap();
 170                         m_resultOut->setBody1Wrap(&triObWrap);
 171                         m_resultOut->setShapeIdentifiersB(partId, triangleIndex);
 172                 }
 173
 174                 {
 175                         BT_PROFILE("processCollision (GJK?)");
 176                         colAlgo->processCollision(m_convexBodyWrap, &triObWrap, *m_dispatchInfoPtr, m_resultOut);
 177                 }
 178
 179                 if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject())
 180                 {
 181                         m_resultOut->setBody0Wrap(tmpWrap);
 182                 }
 183                 else
 184                 {
 185                         m_resultOut->setBody1Wrap(tmpWrap);
 186                 }
 187
 188                 colAlgo->~btCollisionAlgorithm();
 189                 ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo);
 190         }
 191 }
 192
 193 void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTriangle, const btDispatcherInfo& dispatchInfo, const btCollisionObjectWrapper* convexBodyWrap, const btCollisionObjectWrapper* triBodyWrap, btManifoldResult* resultOut)
 194 {
 195         m_convexBodyWrap = convexBodyWrap;
 196         m_triBodyWrap = triBodyWrap;
 197
 198         m_dispatchInfoPtr = &dispatchInfo;
 199         m_collisionMarginTriangle = collisionMarginTriangle;
 200         m_resultOut = resultOut;
 201
 202         //recalc aabbs
 203         btTransform convexInTriangleSpace;
 204         convexInTriangleSpace = m_triBodyWrap->getWorldTransform().inverse() * m_convexBodyWrap->getWorldTransform();
 205         const btCollisionShape* convexShape = static_cast<const btCollisionShape*>(m_convexBodyWrap->getCollisionShape());
 206         //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
 207         convexShape->getAabb(convexInTriangleSpace, m_aabbMin, m_aabbMax);
 208         btScalar extraMargin = collisionMarginTriangle + resultOut->m_closestPointDistanceThreshold;
 209
 210         btVector3 extra(extraMargin, extraMargin, extraMargin);
 211
 212         m_aabbMax += extra;
 213         m_aabbMin -= extra;
 214 }
 215
 216 void btConvexConcaveCollisionAlgorithm::clearCache()
 217 {
 218         m_btConvexTriangleCallback.clearCache();
 219 }
 220
 221 void btConvexConcaveCollisionAlgorithm::processCollision(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 222 {
 223         BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
 224
 225         const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
 226         const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
 227
 228         if (triBodyWrap->getCollisionShape()->isConcave())
 229         {
 230                 if (triBodyWrap->getCollisionShape()->getShapeType() == SDF_SHAPE_PROXYTYPE)
 231                 {
 232                         btSdfCollisionShape* sdfShape = (btSdfCollisionShape*)triBodyWrap->getCollisionShape();
 233                         if (convexBodyWrap->getCollisionShape()->isConvex())
 234                         {
 235                                 btConvexShape* convex = (btConvexShape*)convexBodyWrap->getCollisionShape();
 236                                 btAlignedObjectArray<btVector3> queryVertices;
 237
 238                                 if (convex->isPolyhedral())
 239                                 {
 240                                         btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
 241                                         for (int v = 0; v < poly->getNumVertices(); v++)
 242                                         {
 243                                                 btVector3 vtx;
 244                                                 poly->getVertex(v, vtx);
 245                                                 queryVertices.push_back(vtx);
 246                                         }
 247                                 }
 248                                 btScalar maxDist = SIMD_EPSILON;
 249
 250                                 if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
 251                                 {
 252                                         queryVertices.push_back(btVector3(0, 0, 0));
 253                                         btSphereShape* sphere = (btSphereShape*)convex;
 254                                         maxDist = sphere->getRadius() + SIMD_EPSILON;
 255                                 }
 256                                 if (queryVertices.size())
 257                                 {
 258                                         resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
 259                                         //m_btConvexTriangleCallback.m_manifoldPtr->clearManifold();
 260
 261                                         btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex;
 262                                         for (int v = 0; v < queryVertices.size(); v++)
 263                                         {
 264                                                 const btVector3& vtx = queryVertices[v];
 265                                                 btVector3 vtxWorldSpace = convexBodyWrap->getWorldTransform() * vtx;
 266                                                 btVector3 vtxInSdf = triBodyWrap->getWorldTransform().invXform(vtxWorldSpace);
 267
 268                                                 btVector3 normalLocal;
 269                                                 btScalar dist;
 270                                                 if (sdfShape->queryPoint(vtxInSdf, dist, normalLocal))
 271                                                 {
 272                                                         if (dist <= maxDist)
 273                                                         {
 274                                                                 normalLocal.safeNormalize();
 275                                                                 btVector3 normal = triBodyWrap->getWorldTransform().getBasis() * normalLocal;
 276
 277                                                                 if (convex->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
 278                                                                 {
 279                                                                         btSphereShape* sphere = (btSphereShape*)convex;
 280                                                                         dist -= sphere->getRadius();
 281                                                                         vtxWorldSpace -= sphere->getRadius() * normal;
 282                                                                 }
 283                                                                 resultOut->addContactPoint(normal, vtxWorldSpace - normal * dist, dist);
 284                                                         }
 285                                                 }
 286                                         }
 287                                         resultOut->refreshContactPoints();
 288                                 }
 289                         }
 290                 }
 291                 else
 292                 {
 293                         const btConcaveShape* concaveShape = static_cast<const btConcaveShape*>(triBodyWrap->getCollisionShape());
 294
 295                         if (convexBodyWrap->getCollisionShape()->isConvex())
 296                         {
 297                                 btScalar collisionMarginTriangle = concaveShape->getMargin();
 298
 299                                 resultOut->setPersistentManifold(m_btConvexTriangleCallback.m_manifoldPtr);
 300                                 m_btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, convexBodyWrap, triBodyWrap, resultOut);
 301
 302                                 m_btConvexTriangleCallback.m_manifoldPtr->setBodies(convexBodyWrap->getCollisionObject(), triBodyWrap->getCollisionObject());
 303
 304                                 concaveShape->processAllTriangles(&m_btConvexTriangleCallback, m_btConvexTriangleCallback.getAabbMin(), m_btConvexTriangleCallback.getAabbMax());
 305
 306                                 resultOut->refreshContactPoints();
 307
 308                                 m_btConvexTriangleCallback.clearWrapperData();
 309                         }
 310                 }
 311         }
 312 }
 313
 314 btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObject* body0, btCollisionObject* body1, const btDispatcherInfo& dispatchInfo, btManifoldResult* resultOut)
 315 {
 316         (void)resultOut;
 317         (void)dispatchInfo;
 318         btCollisionObject* convexbody = m_isSwapped ? body1 : body0;
 319         btCollisionObject* triBody = m_isSwapped ? body0 : body1;
 320
 321         //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
 322
 323         //only perform CCD above a certain threshold, this prevents blocking on the long run
 324         //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
 325         btScalar squareMot0 = (convexbody->getInterpolationWorldTransform().getOrigin() - convexbody->getWorldTransform().getOrigin()).length2();
 326         if (squareMot0 < convexbody->getCcdSquareMotionThreshold())
 327         {
 328                 return btScalar(1.);
 329         }
 330
 331         //const btVector3& from = convexbody->m_worldTransform.getOrigin();
 332         //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
 333         //todo: only do if the motion exceeds the 'radius'
 334
 335         btTransform triInv = triBody->getWorldTransform().inverse();
 336         btTransform convexFromLocal = triInv * convexbody->getWorldTransform();
 337         btTransform convexToLocal = triInv * convexbody->getInterpolationWorldTransform();
 338
 339         struct LocalTriangleSphereCastCallback : public btTriangleCallback
 340         {
 341                 btTransform m_ccdSphereFromTrans;
 342                 btTransform m_ccdSphereToTrans;
 343                 btTransform m_meshTransform;
 344
 345                 btScalar m_ccdSphereRadius;
 346                 btScalar m_hitFraction;
 347
 348                 LocalTriangleSphereCastCallback(const btTransform& from, const btTransform& to, btScalar ccdSphereRadius, btScalar hitFraction)
 349                         : m_ccdSphereFromTrans(from),
 350                           m_ccdSphereToTrans(to),
 351                           m_ccdSphereRadius(ccdSphereRadius),
 352                           m_hitFraction(hitFraction)
 353                 {
 354                 }
 355
 356                 virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 357                 {
 358                         BT_PROFILE("processTriangle");
 359                         (void)partId;
 360                         (void)triangleIndex;
 361                         //do a swept sphere for now
 362                         btTransform ident;
 363                         ident.setIdentity();
 364                         btConvexCast::CastResult castResult;
 365                         castResult.m_fraction = m_hitFraction;
 366                         btSphereShape pointShape(m_ccdSphereRadius);
 367                         btTriangleShape triShape(triangle[0], triangle[1], triangle[2]);
 368                         btVoronoiSimplexSolver simplexSolver;
 369                         btSubsimplexConvexCast convexCaster(&pointShape, &triShape, &simplexSolver);
 370                         //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
 371                         //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
 372                         //local space?
 373
 374                         if (convexCaster.calcTimeOfImpact(m_ccdSphereFromTrans, m_ccdSphereToTrans,
 375                                                                                           ident, ident, castResult))
 376                         {
 377                                 if (m_hitFraction > castResult.m_fraction)
 378                                         m_hitFraction = castResult.m_fraction;
 379                         }
 380                 }
 381         };
 382
 383         if (triBody->getCollisionShape()->isConcave())
 384         {
 385                 btVector3 rayAabbMin = convexFromLocal.getOrigin();
 386                 rayAabbMin.setMin(convexToLocal.getOrigin());
 387                 btVector3 rayAabbMax = convexFromLocal.getOrigin();
 388                 rayAabbMax.setMax(convexToLocal.getOrigin());
 389                 btScalar ccdRadius0 = convexbody->getCcdSweptSphereRadius();
 390                 rayAabbMin -= btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
 391                 rayAabbMax += btVector3(ccdRadius0, ccdRadius0, ccdRadius0);
 392
 393                 btScalar curHitFraction = btScalar(1.);  //is this available?
 394                 LocalTriangleSphereCastCallback raycastCallback(convexFromLocal, convexToLocal,
 395                                                                                                                 convexbody->getCcdSweptSphereRadius(), curHitFraction);
 396
 397                 raycastCallback.m_hitFraction = convexbody->getHitFraction();
 398
 399                 btCollisionObject* concavebody = triBody;
 400
 401                 btConcaveShape* triangleMesh = (btConcaveShape*)concavebody->getCollisionShape();
 402
 403                 if (triangleMesh)
 404                 {
 405                         triangleMesh->processAllTriangles(&raycastCallback, rayAabbMin, rayAabbMax);
 406                 }
 407
 408                 if (raycastCallback.m_hitFraction < convexbody->getHitFraction())
 409                 {
 410                         convexbody->setHitFraction(raycastCallback.m_hitFraction);
 411                         return raycastCallback.m_hitFraction;
 412                 }
 413         }
 414
 415         return btScalar(1.);
 416 }