dali-physics/third-party/bullet3/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h

   1
   2
   3 #include "Bullet3Common/shared/b3Int4.h"
   4 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
   5 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
   6 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
   7 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
   8 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
   9
  10 // work-in-progress
  11 void b3BvhTraversal(__global const b3Int4* pairs,
  12                                         __global const b3RigidBodyData* rigidBodies,
  13                                         __global const b3Collidable* collidables,
  14                                         __global b3Aabb* aabbs,
  15                                         __global b3Int4* concavePairsOut,
  16                                         __global volatile int* numConcavePairsOut,
  17                                         __global const b3BvhSubtreeInfo* subtreeHeadersRoot,
  18                                         __global const b3QuantizedBvhNode* quantizedNodesRoot,
  19                                         __global const b3BvhInfo* bvhInfos,
  20                                         int numPairs,
  21                                         int maxNumConcavePairsCapacity,
  22                                         int id)
  23 {
  24         int bodyIndexA = pairs[id].x;
  25         int bodyIndexB = pairs[id].y;
  26         int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
  27         int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
  28
  29         //once the broadphase avoids static-static pairs, we can remove this test
  30         if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
  31         {
  32                 return;
  33         }
  34
  35         if (collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH)
  36                 return;
  37
  38         int shapeTypeB = collidables[collidableIndexB].m_shapeType;
  39
  40         if (shapeTypeB != SHAPE_CONVEX_HULL &&
  41                 shapeTypeB != SHAPE_SPHERE &&
  42                 shapeTypeB != SHAPE_COMPOUND_OF_CONVEX_HULLS)
  43                 return;
  44
  45         b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
  46
  47         b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
  48         b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
  49         b3Float4 bvhQuantization = bvhInfo.m_quantization;
  50         int numSubtreeHeaders = bvhInfo.m_numSubTrees;
  51         __global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
  52         __global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
  53
  54         unsigned short int quantizedQueryAabbMin[3];
  55         unsigned short int quantizedQueryAabbMax[3];
  56         b3QuantizeWithClamp(quantizedQueryAabbMin, aabbs[bodyIndexB].m_minVec, false, bvhAabbMin, bvhAabbMax, bvhQuantization);
  57         b3QuantizeWithClamp(quantizedQueryAabbMax, aabbs[bodyIndexB].m_maxVec, true, bvhAabbMin, bvhAabbMax, bvhQuantization);
  58
  59         for (int i = 0; i < numSubtreeHeaders; i++)
  60         {
  61                 b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
  62
  63                 int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
  64                 if (overlap != 0)
  65                 {
  66                         int startNodeIndex = subtree.m_rootNodeIndex;
  67                         int endNodeIndex = subtree.m_rootNodeIndex + subtree.m_subtreeSize;
  68                         int curIndex = startNodeIndex;
  69                         int escapeIndex;
  70                         int isLeafNode;
  71                         int aabbOverlap;
  72                         while (curIndex < endNodeIndex)
  73                         {
  74                                 b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
  75                                 aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.m_quantizedAabbMin, rootNode.m_quantizedAabbMax);
  76                                 isLeafNode = b3IsLeaf(&rootNode);
  77                                 if (aabbOverlap)
  78                                 {
  79                                         if (isLeafNode)
  80                                         {
  81                                                 int triangleIndex = b3GetTriangleIndex(&rootNode);
  82                                                 if (shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS)
  83                                                 {
  84                                                         int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
  85                                                         int pairIdx = b3AtomicAdd(numConcavePairsOut, numChildrenB);
  86                                                         for (int b = 0; b < numChildrenB; b++)
  87                                                         {
  88                                                                 if ((pairIdx + b) < maxNumConcavePairsCapacity)
  89                                                                 {
  90                                                                         int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
  91                                                                         b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, childShapeIndexB);
  92                                                                         concavePairsOut[pairIdx + b] = newPair;
  93                                                                 }
  94                                                         }
  95                                                 }
  96                                                 else
  97                                                 {
  98                                                         int pairIdx = b3AtomicInc(numConcavePairsOut);
  99                                                         if (pairIdx < maxNumConcavePairsCapacity)
 100                                                         {
 101                                                                 b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, 0);
 102                                                                 concavePairsOut[pairIdx] = newPair;
 103                                                         }
 104                                                 }
 105                                         }
 106                                         curIndex++;
 107                                 }
 108                                 else
 109                                 {
 110                                         if (isLeafNode)
 111                                         {
 112                                                 curIndex++;
 113                                         }
 114                                         else
 115                                         {
 116                                                 escapeIndex = b3GetEscapeIndex(&rootNode);
 117                                                 curIndex += escapeIndex;
 118                                         }
 119                                 }
 120                         }
 121                 }
 122         }
 123 }