more work towards GPU bvh traversal

opencl/gpu_sat/host/ConvexHullContact.cpp

@@ -329,35 +329,9 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
 				btOpenCLArray<btBvhSubtreeInfo>	subTreesGPU(this->m_context,this->m_queue,numSubTrees);
 				subTreesGPU.copyFromHost(bvhData[0]->getSubtreeInfoArray());
 
-
+				btVector3 bvhAabbMin = bvhData[0]->m_bvhAabbMin;
-				/*
+				btVector3 bvhAabbMax = bvhData[0]->m_bvhAabbMax;
-				__kernel void   bvhTraversalKernel( __global const int2* pairs, 
+				btVector3 bvhQuantization = bvhData[0]->m_bvhQuantization;
-													__global const BodyData* rigidBodies, 
-													__global const btCollidableGpu* collidables,
-													__global btAabbCL* aabbs,
-													__global int4* concavePairsOut,
-													__global volatile int* numConcavePairsOut,
-													int numPairs,
-													int maxNumConcavePairsCapacity
-													)
-
-																					btBufferInfoCL( pairs->getBufferCL(), true ), 
-				btBufferInfoCL( bodyBuf->getBufferCL(),true), 
-				btBufferInfoCL( gpuCollidables.getBufferCL(),true), 
-				btBufferInfoCL( convexData.getBufferCL(),true),
-				btBufferInfoCL( gpuVertices.getBufferCL(),true),
-				btBufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
-				btBufferInfoCL( gpuFaces.getBufferCL(),true),
-				btBufferInfoCL( gpuIndices.getBufferCL(),true),
-				btBufferInfoCL( clAabbsWS.getBufferCL(),true),
-				btBufferInfoCL( sepNormals.getBufferCL()),
-				btBufferInfoCL( hasSeparatingNormals.getBufferCL()),
-				btBufferInfoCL( triangleConvexPairsOut.getBufferCL()),
-				btBufferInfoCL( concaveSepNormals.getBufferCL()),
-				btBufferInfoCL( numConcavePairsOut.getBufferCL())
-
-				*/
-
 				{
 					int np = numConcavePairsOut.at(0);
 					printf("np=%d\n", np);
@@ -368,12 +342,23 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( const btOpenCLArray<btI
 					launcher.setBuffer( clAabbsWS.getBufferCL());
 					launcher.setBuffer( triangleConvexPairsOut.getBufferCL());
 					launcher.setBuffer( numConcavePairsOut.getBufferCL());
+					launcher.setBuffer( subTreesGPU.getBufferCL());
+					launcher.setBuffer( treeNodesGPU.getBufferCL());
+					launcher.setConst( bvhAabbMin);
+					launcher.setConst( bvhAabbMax);
+					launcher.setConst( bvhQuantization);
+					launcher.setConst(numSubTrees);
 					launcher.setConst( nPairs  );
 					launcher.setConst( maxTriConvexPairCapacity);
 					int num = nPairs;
 					launcher.launch1D( num);
 					clFinish(m_queue);
 					np = numConcavePairsOut.at(0);
+					triangleConvexPairsOut.resize(np);
+					btAlignedObjectArray<btInt4> pairsOutCPU;
+					triangleConvexPairsOut.copyToHost(pairsOutCPU);
+
+
 					printf("np=%d\n", np);
 
 				}

opencl/gpu_sat/host/btQuantizedBvh.h

@@ -181,13 +181,14 @@ public:
 		TRAVERSAL_RECURSIVE
 	};
 
-protected:
+
 
 
 	btVector3			m_bvhAabbMin;
 	btVector3			m_bvhAabbMax;
 	btVector3			m_bvhQuantization;
 
+protected:
 	int					m_bulletVersion;	//for serialization versioning. It could also be used to detect endianess.
 
 	int					m_curNodeIndex;

opencl/gpu_sat/kernels/bvhTraversal.cl

@@ -8,6 +8,76 @@
 
 typedef unsigned int u32;
 
+#define MAX_NUM_PARTS_IN_BITS 10
+
+///btQuantizedBvhNode is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+typedef struct
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes
+	int	m_escapeIndexOrTriangleIndex;
+} btQuantizedBvhNode;
+/*
+	bool isLeafNode() const
+	{
+		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (m_escapeIndexOrTriangleIndex >= 0);
+	}
+	int getEscapeIndex() const
+	{
+		btAssert(!isLeafNode());
+		return -m_escapeIndexOrTriangleIndex;
+	}
+	int	getTriangleIndex() const
+	{
+		btAssert(isLeafNode());
+		unsigned int x=0;
+		unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
+		// Get only the lower bits where the triangle index is stored
+		return (m_escapeIndexOrTriangleIndex&~(y));
+	}
+	int	getPartId() const
+	{
+		btAssert(isLeafNode());
+		// Get only the highest bits where the part index is stored
+		return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
+	}
+*/
+
+int	getTriangleIndex(__global const btQuantizedBvhNode* rootNode)
+{
+	unsigned int x=0;
+	unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
+	// Get only the lower bits where the triangle index is stored
+	return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
+}
+
+bool isLeaf(__global const btQuantizedBvhNode* rootNode)
+{
+	//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+	return (rootNode->m_escapeIndexOrTriangleIndex >= 0);
+}
+	
+int getEscapeIndex(__global const btQuantizedBvhNode* rootNode)
+{
+	return -rootNode->m_escapeIndexOrTriangleIndex;
+}
+
+typedef struct
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes, points to the root of the subtree
+	int			m_rootNodeIndex;
+	//4 bytes
+	int			m_subtreeSize;
+	int			m_padding[3];
+} btBvhSubtreeInfo;
+
 ///keep this in sync with btCollidable.h
 typedef struct
 {
@@ -58,16 +128,53 @@ typedef struct
 	};
 } btAabbCL;
 
+
+bool testQuantizedAabbAgainstQuantizedAabb(__private const unsigned short int* aabbMin1,__private const unsigned short int* aabbMax1,__global const unsigned short int* aabbMin2,__global const unsigned short int* aabbMax2)
+{
+	bool overlap = true;
+	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
+	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
+	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
+	return overlap;
+}
+
+
+void quantizeWithClamp(unsigned short* out, float4 point2,int isMax, float4 bvhAabbMin, float4 bvhAabbMax, float4 bvhQuantization)
+{
+	float4 clampedPoint = max(point2,bvhAabbMin);
+	clampedPoint = min (clampedPoint, bvhAabbMax);
+
+	float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
+	if (isMax)
+	{
+		out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
+		out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
+		out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
+	} else
+	{
+		out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
+		out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
+		out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
+	}
+
+}
+
+
 // work-in-progress
 __kernel void   bvhTraversalKernel( __global const int2* pairs, 
-																					__global const BodyData* rigidBodies, 
+									__global const BodyData* rigidBodies, 
-																					__global const btCollidableGpu* collidables,
+									__global const btCollidableGpu* collidables,
-																					__global btAabbCL* aabbs,
+									__global btAabbCL* aabbs,
-																					__global int4* concavePairsOut,
+									__global int4* concavePairsOut,
-																					__global volatile int* numConcavePairsOut,
+									__global volatile int* numConcavePairsOut,
-																					int numPairs,
+									__global const btBvhSubtreeInfo* subtreeHeaders,
-																					int maxNumConcavePairsCapacity
+									__global const btQuantizedBvhNode* quantizedNodes,
-																					)
+									float4 bvhAabbMin,
+									float4 bvhAabbMax,
+									float4 bvhQuantization,
+									int numSubtreeHeaders,
+									int numPairs,
+									int maxNumConcavePairsCapacity)
 {
 
 	int i = get_global_id(0);
@@ -94,16 +201,62 @@ __kernel void   bvhTraversalKernel( __global const int2* pairs,
 		
 		if ((collidables[collidableIndexA].m_shapeType==SHAPE_CONCAVE_TRIMESH))// && (collidables[collidableIndexB].m_shapeType==SHAPE_CONVEX_HULL))
 		{
-			int pairIdx = atomic_inc(numConcavePairsOut);
+
-			if (pairIdx<maxNumConcavePairsCapacity)
+			
+			unsigned short int quantizedQueryAabbMin[3];
+			unsigned short int quantizedQueryAabbMax[3];
+			quantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_min,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
+			quantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_max,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
+
+
+			int i;
+			for (i=0;i<numSubtreeHeaders;i++)
 			{
-				//int4 newPair;
+				const __global btBvhSubtreeInfo* subtree = &subtreeHeaders[i];
-				concavePairsOut[pairIdx].x = bodyIndexA;
+				//PCK: unsigned instead of bool
-				concavePairsOut[pairIdx].y = bodyIndexB;
+				unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree->m_quantizedAabbMin,subtree->m_quantizedAabbMax);
-				concavePairsOut[pairIdx].z = 5;
+				if (overlap != 0)
-				concavePairsOut[pairIdx].w = 3;
+				{
+					int startNodeIndex = subtree->m_rootNodeIndex;
+					int endNodeIndex = subtree->m_rootNodeIndex+subtree->m_subtreeSize;
+
+					int curIndex = startNodeIndex;
+					int subTreeSize = endNodeIndex - startNodeIndex;
+					__global const btQuantizedBvhNode* rootNode = &quantizedNodes[startNodeIndex];
+					int escapeIndex;
+					bool isLeafNode;
+					unsigned aabbOverlap;
+					while (curIndex < endNodeIndex)
+					{
+						aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);
+						isLeafNode = isLeaf(rootNode);
+						if (isLeafNode && aabbOverlap)
+						{
+							//do your thing! nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());
+							int triangleIndex = getTriangleIndex(rootNode);
+							int pairIdx = atomic_inc(numConcavePairsOut);
+							if (pairIdx<maxNumConcavePairsCapacity)
+							{
+								//int4 newPair;
+								concavePairsOut[pairIdx].x = bodyIndexA;
+								concavePairsOut[pairIdx].y = bodyIndexB;
+								concavePairsOut[pairIdx].z = triangleIndex;
+								concavePairsOut[pairIdx].w = 3;
+							}
+						} 
+						if ((aabbOverlap != 0) || isLeafNode)
+						{
+							rootNode++;
+							curIndex++;
+						} else
+						{
+							escapeIndex = getEscapeIndex(rootNode);
+							rootNode += escapeIndex;
+							curIndex += escapeIndex;
+						}
+					}
+				}
 			}
 		}//SHAPE_CONCAVE_TRIMESH
-		
 	}//i<numpairs
 }

opencl/gpu_sat/kernels/bvhTraversal.h

@@ -10,6 +10,76 @@ static const char* bvhTraversalKernelCL= \
 "\n"
 "typedef unsigned int u32;\n"
 "\n"
+"#define MAX_NUM_PARTS_IN_BITS 10\n"
+"\n"
+"///btQuantizedBvhNode is a compressed aabb node, 16 bytes.\n"
+"///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).\n"
+"typedef struct\n"
+"{\n"
+"	//12 bytes\n"
+"	unsigned short int	m_quantizedAabbMin[3];\n"
+"	unsigned short int	m_quantizedAabbMax[3];\n"
+"	//4 bytes\n"
+"	int	m_escapeIndexOrTriangleIndex;\n"
+"} btQuantizedBvhNode;\n"
+"/*\n"
+"	bool isLeafNode() const\n"
+"	{\n"
+"		//skipindex is negative (internal node), triangleindex >=0 (leafnode)\n"
+"		return (m_escapeIndexOrTriangleIndex >= 0);\n"
+"	}\n"
+"	int getEscapeIndex() const\n"
+"	{\n"
+"		btAssert(!isLeafNode());\n"
+"		return -m_escapeIndexOrTriangleIndex;\n"
+"	}\n"
+"	int	getTriangleIndex() const\n"
+"	{\n"
+"		btAssert(isLeafNode());\n"
+"		unsigned int x=0;\n"
+"		unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);\n"
+"		// Get only the lower bits where the triangle index is stored\n"
+"		return (m_escapeIndexOrTriangleIndex&~(y));\n"
+"	}\n"
+"	int	getPartId() const\n"
+"	{\n"
+"		btAssert(isLeafNode());\n"
+"		// Get only the highest bits where the part index is stored\n"
+"		return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));\n"
+"	}\n"
+"*/\n"
+"\n"
+"int	getTriangleIndex(__global const btQuantizedBvhNode* rootNode)\n"
+"{\n"
+"	unsigned int x=0;\n"
+"	unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);\n"
+"	// Get only the lower bits where the triangle index is stored\n"
+"	return (rootNode->m_escapeIndexOrTriangleIndex&~(y));\n"
+"}\n"
+"\n"
+"bool isLeaf(__global const btQuantizedBvhNode* rootNode)\n"
+"{\n"
+"	//skipindex is negative (internal node), triangleindex >=0 (leafnode)\n"
+"	return (rootNode->m_escapeIndexOrTriangleIndex >= 0);\n"
+"}\n"
+"	\n"
+"int getEscapeIndex(__global const btQuantizedBvhNode* rootNode)\n"
+"{\n"
+"	return -rootNode->m_escapeIndexOrTriangleIndex;\n"
+"}\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	//12 bytes\n"
+"	unsigned short int	m_quantizedAabbMin[3];\n"
+"	unsigned short int	m_quantizedAabbMax[3];\n"
+"	//4 bytes, points to the root of the subtree\n"
+"	int			m_rootNodeIndex;\n"
+"	//4 bytes\n"
+"	int			m_subtreeSize;\n"
+"	int			m_padding[3];\n"
+"} btBvhSubtreeInfo;\n"
+"\n"
 "///keep this in sync with btCollidable.h\n"
 "typedef struct\n"
 "{\n"
@@ -60,16 +130,53 @@ static const char* bvhTraversalKernelCL= \
 "	};\n"
 "} btAabbCL;\n"
 "\n"
+"\n"
+"bool testQuantizedAabbAgainstQuantizedAabb(__private const unsigned short int* aabbMin1,__private const unsigned short int* aabbMax1,__global const unsigned short int* aabbMin2,__global const unsigned short int* aabbMax2)\n"
+"{\n"
+"	bool overlap = true;\n"
+"	overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;\n"
+"	overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;\n"
+"	overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;\n"
+"	return overlap;\n"
+"}\n"
+"\n"
+"\n"
+"void quantizeWithClamp(unsigned short* out, float4 point2,int isMax, float4 bvhAabbMin, float4 bvhAabbMax, float4 bvhQuantization)\n"
+"{\n"
+"	float4 clampedPoint = max(point2,bvhAabbMin);\n"
+"	clampedPoint = min (clampedPoint, bvhAabbMax);\n"
+"\n"
+"	float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;\n"
+"	if (isMax)\n"
+"	{\n"
+"		out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));\n"
+"		out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));\n"
+"		out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));\n"
+"	} else\n"
+"	{\n"
+"		out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));\n"
+"		out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));\n"
+"		out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));\n"
+"	}\n"
+"\n"
+"}\n"
+"\n"
+"\n"
 "// work-in-progress\n"
 "__kernel void   bvhTraversalKernel( __global const int2* pairs, \n"
-"																					__global const BodyData* rigidBodies, \n"
+"									__global const BodyData* rigidBodies, \n"
-"																					__global const btCollidableGpu* collidables,\n"
+"									__global const btCollidableGpu* collidables,\n"
-"																					__global btAabbCL* aabbs,\n"
+"									__global btAabbCL* aabbs,\n"
-"																					__global int4* concavePairsOut,\n"
+"									__global int4* concavePairsOut,\n"
-"																					__global volatile int* numConcavePairsOut,\n"
+"									__global volatile int* numConcavePairsOut,\n"
-"																					int numPairs,\n"
+"									__global const btBvhSubtreeInfo* subtreeHeaders,\n"
-"																					int maxNumConcavePairsCapacity\n"
+"									__global const btQuantizedBvhNode* quantizedNodes,\n"
-"																					)\n"
+"									float4 bvhAabbMin,\n"
+"									float4 bvhAabbMax,\n"
+"									float4 bvhQuantization,\n"
+"									int numSubtreeHeaders,\n"
+"									int numPairs,\n"
+"									int maxNumConcavePairsCapacity)\n"
 "{\n"
 "\n"
 "	int i = get_global_id(0);\n"
@@ -96,17 +203,63 @@ static const char* bvhTraversalKernelCL= \
 "		\n"
 "		if ((collidables[collidableIndexA].m_shapeType==SHAPE_CONCAVE_TRIMESH))// && (collidables[collidableIndexB].m_shapeType==SHAPE_CONVEX_HULL))\n"
 "		{\n"
-"			int pairIdx = atomic_inc(numConcavePairsOut);\n"
+"\n"
-"			if (pairIdx<maxNumConcavePairsCapacity)\n"
+"			\n"
+"			unsigned short int quantizedQueryAabbMin[3];\n"
+"			unsigned short int quantizedQueryAabbMax[3];\n"
+"			quantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_min,false,bvhAabbMin, bvhAabbMax,bvhQuantization);\n"
+"			quantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_max,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);\n"
+"\n"
+"\n"
+"			int i;\n"
+"			for (i=0;i<numSubtreeHeaders;i++)\n"
 "			{\n"
-"				//int4 newPair;\n"
+"				const __global btBvhSubtreeInfo* subtree = &subtreeHeaders[i];\n"
-"				concavePairsOut[pairIdx].x = bodyIndexA;\n"
+"				//PCK: unsigned instead of bool\n"
-"				concavePairsOut[pairIdx].y = bodyIndexB;\n"
+"				unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree->m_quantizedAabbMin,subtree->m_quantizedAabbMax);\n"
-"				concavePairsOut[pairIdx].z = 5;\n"
+"				if (overlap != 0)\n"
-"				concavePairsOut[pairIdx].w = 3;\n"
+"				{\n"
+"					int startNodeIndex = subtree->m_rootNodeIndex;\n"
+"					int endNodeIndex = subtree->m_rootNodeIndex+subtree->m_subtreeSize;\n"
+"\n"
+"					int curIndex = startNodeIndex;\n"
+"					int subTreeSize = endNodeIndex - startNodeIndex;\n"
+"					__global const btQuantizedBvhNode* rootNode = &quantizedNodes[startNodeIndex];\n"
+"					int escapeIndex;\n"
+"					bool isLeafNode;\n"
+"					unsigned aabbOverlap;\n"
+"					while (curIndex < endNodeIndex)\n"
+"					{\n"
+"						aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax);\n"
+"						isLeafNode = isLeaf(rootNode);\n"
+"						if (isLeafNode && aabbOverlap)\n"
+"						{\n"
+"							//do your thing! nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex());\n"
+"							int triangleIndex = getTriangleIndex(rootNode);\n"
+"							int pairIdx = atomic_inc(numConcavePairsOut);\n"
+"							if (pairIdx<maxNumConcavePairsCapacity)\n"
+"							{\n"
+"								//int4 newPair;\n"
+"								concavePairsOut[pairIdx].x = bodyIndexA;\n"
+"								concavePairsOut[pairIdx].y = bodyIndexB;\n"
+"								concavePairsOut[pairIdx].z = triangleIndex;\n"
+"								concavePairsOut[pairIdx].w = 3;\n"
+"							}\n"
+"						} \n"
+"						if ((aabbOverlap != 0) || isLeafNode)\n"
+"						{\n"
+"							rootNode++;\n"
+"							curIndex++;\n"
+"						} else\n"
+"						{\n"
+"							escapeIndex = getEscapeIndex(rootNode);\n"
+"							rootNode += escapeIndex;\n"
+"							curIndex += escapeIndex;\n"
+"						}\n"
+"					}\n"
+"				}\n"
 "			}\n"
 "		}//SHAPE_CONCAVE_TRIMESH\n"
-"		\n"
 "	}//i<numpairs\n"
 "}\n"
 ;