diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h | 126 |
1 files changed, 126 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h new file mode 100644 index 0000000000..2618da24bc --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h @@ -0,0 +1,126 @@ + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" + + + +// work-in-progress +void 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) +{ + + int bodyIndexA = pairs[id].x; + int bodyIndexB = pairs[id].y; + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + //once the broadphase avoids static-static pairs, we can remove this test + if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0)) + { + return; + } + + if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH) + return; + + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + + if (shapeTypeB!=SHAPE_CONVEX_HULL && + shapeTypeB!=SHAPE_SPHERE && + shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS + ) + return; + + b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes]; + + b3Float4 bvhAabbMin = bvhInfo.m_aabbMin; + b3Float4 bvhAabbMax = bvhInfo.m_aabbMax; + b3Float4 bvhQuantization = bvhInfo.m_quantization; + int numSubtreeHeaders = bvhInfo.m_numSubTrees; + __global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset]; + __global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset]; + + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization); + b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization); + + for (int i=0;i<numSubtreeHeaders;i++) + { + b3BvhSubtreeInfoData subtree = subtreeHeaders[i]; + + int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap != 0) + { + int startNodeIndex = subtree.m_rootNodeIndex; + int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize; + int curIndex = startNodeIndex; + int escapeIndex; + int isLeafNode; + int aabbOverlap; + while (curIndex < endNodeIndex) + { + b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex]; + aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax); + isLeafNode = b3IsLeaf(&rootNode); + if (aabbOverlap) + { + if (isLeafNode) + { + int triangleIndex = b3GetTriangleIndex(&rootNode); + if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + int numChildrenB = collidables[collidableIndexB].m_numChildShapes; + int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB); + for (int b=0;b<numChildrenB;b++) + { + if ((pairIdx+b)<maxNumConcavePairsCapacity) + { + int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b; + b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB); + concavePairsOut[pairIdx+b] = newPair; + } + } + } else + { + int pairIdx = b3AtomicInc(numConcavePairsOut); + if (pairIdx<maxNumConcavePairsCapacity) + { + b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0); + concavePairsOut[pairIdx] = newPair; + } + } + } + curIndex++; + } else + { + if (isLeafNode) + { + curIndex++; + } else + { + escapeIndex = b3GetEscapeIndex(&rootNode); + curIndex += escapeIndex; + } + } + } + } + } + +}
\ No newline at end of file |