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
b3BvhTraversal(__global const b3Int4 * pairs,__global const b3RigidBodyData * rigidBodies,__global const b3Collidable * collidables,__global b3Aabb * aabbs,__global b3Int4 * concavePairsOut,__global volatile int * numConcavePairsOut,__global const b3BvhSubtreeInfo * subtreeHeadersRoot,__global const b3QuantizedBvhNode * quantizedNodesRoot,__global const b3BvhInfo * bvhInfos,int numPairs,int maxNumConcavePairsCapacity,int id)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 }