dali-physics/third-party/bullet3/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl

   1 //keep this enum in sync with the CPU version (in btCollidable.h)
   2 //written by Erwin Coumans
   3
   4 #define SHAPE_CONVEX_HULL 3
   5 #define SHAPE_CONCAVE_TRIMESH 5
   6 #define TRIANGLE_NUM_CONVEX_FACES 5
   7 #define SHAPE_COMPOUND_OF_CONVEX_HULLS 6
   8 #define SHAPE_SPHERE 7
   9
  10 typedef unsigned int u32;
  11
  12 #define MAX_NUM_PARTS_IN_BITS 10
  13
  14 ///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
  15 ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
  16 typedef struct
  17 {
  18         //12 bytes
  19         unsigned short int      m_quantizedAabbMin[3];
  20         unsigned short int      m_quantizedAabbMax[3];
  21         //4 bytes
  22         int     m_escapeIndexOrTriangleIndex;
  23 } btQuantizedBvhNode;
  24
  25 typedef struct
  26 {
  27         float4          m_aabbMin;
  28         float4          m_aabbMax;
  29         float4          m_quantization;
  30         int                     m_numNodes;
  31         int                     m_numSubTrees;
  32         int                     m_nodeOffset;
  33         int                     m_subTreeOffset;
  34
  35 } b3BvhInfo;
  36
  37 int     getTriangleIndex(const btQuantizedBvhNode* rootNode)
  38 {
  39         unsigned int x=0;
  40         unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
  41         // Get only the lower bits where the triangle index is stored
  42         return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
  43 }
  44
  45 int isLeaf(const btQuantizedBvhNode* rootNode)
  46 {
  47         //skipindex is negative (internal node), triangleindex >=0 (leafnode)
  48         return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
  49 }
  50
  51 int getEscapeIndex(const btQuantizedBvhNode* rootNode)
  52 {
  53         return -rootNode->m_escapeIndexOrTriangleIndex;
  54 }
  55
  56 typedef struct
  57 {
  58         //12 bytes
  59         unsigned short int      m_quantizedAabbMin[3];
  60         unsigned short int      m_quantizedAabbMax[3];
  61         //4 bytes, points to the root of the subtree
  62         int                     m_rootNodeIndex;
  63         //4 bytes
  64         int                     m_subtreeSize;
  65         int                     m_padding[3];
  66 } btBvhSubtreeInfo;
  67
  68 ///keep this in sync with btCollidable.h
  69 typedef struct
  70 {
  71         int m_numChildShapes;
  72         int blaat2;
  73         int m_shapeType;
  74         int m_shapeIndex;
  75
  76 } btCollidableGpu;
  77
  78 typedef struct
  79 {
  80         float4  m_childPosition;
  81         float4  m_childOrientation;
  82         int m_shapeIndex;
  83         int m_unused0;
  84         int m_unused1;
  85         int m_unused2;
  86 } btGpuChildShape;
  87
  88
  89 typedef struct
  90 {
  91         float4 m_pos;
  92         float4 m_quat;
  93         float4 m_linVel;
  94         float4 m_angVel;
  95
  96         u32 m_collidableIdx;
  97         float m_invMass;
  98         float m_restituitionCoeff;
  99         float m_frictionCoeff;
 100 } BodyData;
 101
 102 typedef struct
 103 {
 104         union
 105         {
 106                 float4  m_min;
 107                 float   m_minElems[4];
 108                 int                     m_minIndices[4];
 109         };
 110         union
 111         {
 112                 float4  m_max;
 113                 float   m_maxElems[4];
 114                 int                     m_maxIndices[4];
 115         };
 116 } btAabbCL;
 117
 118
 119 int testQuantizedAabbAgainstQuantizedAabb(
 120                                                                 const unsigned short int* aabbMin1,
 121                                                                 const unsigned short int* aabbMax1,
 122                                                                 const unsigned short int* aabbMin2,
 123                                                                 const unsigned short int* aabbMax2)
 124 {
 125         //int overlap = 1;
 126         if (aabbMin1[0] > aabbMax2[0])
 127                 return 0;
 128         if (aabbMax1[0] < aabbMin2[0])
 129                 return 0;
 130         if (aabbMin1[1] > aabbMax2[1])
 131                 return 0;
 132         if (aabbMax1[1] < aabbMin2[1])
 133                 return 0;
 134         if (aabbMin1[2] > aabbMax2[2])
 135                 return 0;
 136         if (aabbMax1[2] < aabbMin2[2])
 137                 return 0;
 138         return 1;
 139         //overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
 140         //overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
 141         //overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
 142         //return overlap;
 143 }
 144
 145
 146 void quantizeWithClamp(unsigned short* out, float4 point2,int isMax, float4 bvhAabbMin, float4 bvhAabbMax, float4 bvhQuantization)
 147 {
 148         float4 clampedPoint = max(point2,bvhAabbMin);
 149         clampedPoint = min (clampedPoint, bvhAabbMax);
 150
 151         float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
 152         if (isMax)
 153         {
 154                 out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
 155                 out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
 156                 out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
 157         } else
 158         {
 159                 out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
 160                 out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
 161                 out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
 162         }
 163
 164 }
 165
 166
 167 // work-in-progress
 168 __kernel void   bvhTraversalKernel( __global const int4* pairs,
 169                                                                         __global const BodyData* rigidBodies,
 170                                                                         __global const btCollidableGpu* collidables,
 171                                                                         __global btAabbCL* aabbs,
 172                                                                         __global int4* concavePairsOut,
 173                                                                         __global volatile int* numConcavePairsOut,
 174                                                                         __global const btBvhSubtreeInfo* subtreeHeadersRoot,
 175                                                                         __global const btQuantizedBvhNode* quantizedNodesRoot,
 176                                                                         __global const b3BvhInfo* bvhInfos,
 177                                                                         int numPairs,
 178                                                                         int maxNumConcavePairsCapacity)
 179 {
 180         int id = get_global_id(0);
 181         if (id>=numPairs)
 182                 return;
 183
 184         int bodyIndexA = pairs[id].x;
 185         int bodyIndexB = pairs[id].y;
 186         int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
 187         int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
 188
 189         //once the broadphase avoids static-static pairs, we can remove this test
 190         if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
 191         {
 192                 return;
 193         }
 194
 195         if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)
 196                 return;
 197
 198         int shapeTypeB = collidables[collidableIndexB].m_shapeType;
 199
 200         if (shapeTypeB!=SHAPE_CONVEX_HULL &&
 201                 shapeTypeB!=SHAPE_SPHERE        &&
 202                 shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
 203                 )
 204                 return;
 205
 206         b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
 207
 208         float4 bvhAabbMin = bvhInfo.m_aabbMin;
 209         float4 bvhAabbMax = bvhInfo.m_aabbMax;
 210         float4 bvhQuantization = bvhInfo.m_quantization;
 211         int numSubtreeHeaders = bvhInfo.m_numSubTrees;
 212         __global const btBvhSubtreeInfo* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
 213         __global const btQuantizedBvhNode* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
 214
 215
 216         unsigned short int quantizedQueryAabbMin[3];
 217         unsigned short int quantizedQueryAabbMax[3];
 218         quantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_min,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
 219         quantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_max,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
 220
 221         for (int i=0;i<numSubtreeHeaders;i++)
 222         {
 223                 btBvhSubtreeInfo subtree = subtreeHeaders[i];
 224
 225                 int overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
 226                 if (overlap != 0)
 227                 {
 228                         int startNodeIndex = subtree.m_rootNodeIndex;
 229                         int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize;
 230                         int curIndex = startNodeIndex;
 231                         int escapeIndex;
 232                         int isLeafNode;
 233                         int aabbOverlap;
 234                         while (curIndex < endNodeIndex)
 235                         {
 236                                 btQuantizedBvhNode rootNode = quantizedNodes[curIndex];
 237                                 aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax);
 238                                 isLeafNode = isLeaf(&rootNode);
 239                                 if (aabbOverlap)
 240                                 {
 241                                         if (isLeafNode)
 242                                         {
 243                                                 int triangleIndex = getTriangleIndex(&rootNode);
 244                                                 if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS)
 245                                                 {
 246                                                                 int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
 247                                                                 int pairIdx = atomic_add(numConcavePairsOut,numChildrenB);
 248                                                                 for (int b=0;b<numChildrenB;b++)
 249                                                                 {
 250                                                                         if ((pairIdx+b)<maxNumConcavePairsCapacity)
 251                                                                         {
 252                                                                                 int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b;
 253                                                                                 int4 newPair = (int4)(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB);
 254                                                                                 concavePairsOut[pairIdx+b] = newPair;
 255                                                                         }
 256                                                                 }
 257                                                 } else
 258                                                 {
 259                                                         int pairIdx = atomic_inc(numConcavePairsOut);
 260                                                         if (pairIdx<maxNumConcavePairsCapacity)
 261                                                         {
 262                                                                 int4 newPair = (int4)(bodyIndexA,bodyIndexB,triangleIndex,0);
 263                                                                 concavePairsOut[pairIdx] = newPair;
 264                                                         }
 265                                                 }
 266                                         }
 267                                         curIndex++;
 268                                 } else
 269                                 {
 270                                         if (isLeafNode)
 271                                         {
 272                                                 curIndex++;
 273                                         } else
 274                                         {
 275                                                 escapeIndex = getEscapeIndex(&rootNode);
 276                                                 curIndex += escapeIndex;
 277                                         }
 278                                 }
 279                         }
 280                 }
 281         }
 282
 283 }