start implementing ray-convex on GPU (work-in-progress)

src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp

@@ -76,14 +76,14 @@ bool sphere_intersect(const b3Vector3& spherePos,  b3Scalar radius, const b3Vect
 }
 
 bool rayConvex(const b3Vector3& rayFromLocal, const b3Vector3& rayToLocal, const b3ConvexPolyhedronCL& poly,
-	const struct b3GpuNarrowPhaseInternalData* narrowphaseData, float& hitFraction, b3Vector3& hitNormal)
+	const b3AlignedObjectArray<b3GpuFace>& faces,  float& hitFraction, b3Vector3& hitNormal)
 {
 	float exitFraction = hitFraction;
 	float enterFraction = -0.1f;
 	b3Vector3 curHitNormal(0,0,0);
 	for (int i=0;i<poly.m_numFaces;i++)
 	{
-		const b3GpuFace& face = narrowphaseData->m_convexFaces[poly.m_faceOffset+i];
+		const b3GpuFace& face = faces[poly.m_faceOffset+i];
 		float fromPlaneDist = b3Dot(rayFromLocal,face.m_plane)+face.m_plane.w;
 		float toPlaneDist = b3Dot(rayToLocal,face.m_plane)+face.m_plane.w;
 		if (fromPlaneDist<0.f)
@@ -174,7 +174,7 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays,	b3A
 					
 					int shapeIndex = collidables[bodies[b].m_collidableIdx].m_shapeIndex;
 					const b3ConvexPolyhedronCL& poly = narrowphaseData->m_convexPolyhedra[shapeIndex];
-					if (rayConvex(rayFromLocal, rayToLocal,poly,narrowphaseData, hitFraction, hitNormal))
+					if (rayConvex(rayFromLocal, rayToLocal,poly,narrowphaseData->m_convexFaces, hitFraction, hitNormal))
 					{
 						hitBodyIndex = b;
 					}
@@ -218,14 +218,6 @@ void b3GpuRaycast::castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3Align
 	gpuHitResults.resize(hitResults.size());
 	gpuHitResults.copyFromHost(hitResults);
 
-	b3OpenCLArray<b3RigidBodyCL> gpuBodies(m_data->m_context,m_data->m_q);
-	gpuBodies.resize(numBodies);
-	gpuBodies.copyFromHostPointer(bodies,numBodies);
-
-	b3OpenCLArray<b3Collidable> gpuCollidables(m_data->m_context,m_data->m_q);
-	gpuCollidables.resize(numCollidables);
-	gpuCollidables.copyFromHostPointer(collidables,numCollidables);
-
 
 	//run kernel
 	{
@@ -239,9 +231,11 @@ void b3GpuRaycast::castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3Align
 		launcher.setBuffer(gpuHitResults.getBufferCL());
 
 		launcher.setConst(numBodies);
-		launcher.setBuffer(gpuBodies.getBufferCL());
+		launcher.setBuffer(narrowphaseData->m_bodyBufferGPU->getBufferCL());
-		launcher.setBuffer(gpuCollidables.getBufferCL());
+		launcher.setBuffer(narrowphaseData->m_collidablesGPU->getBufferCL());
-
+		launcher.setBuffer(narrowphaseData->m_convexFacesGPU->getBufferCL());
+		launcher.setBuffer(narrowphaseData->m_convexPolyhedraGPU->getBufferCL());
+		
 		launcher.launch1D(numRays);
 		clFinish(m_data->m_q);
 	}

src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.cl

@@ -47,6 +47,181 @@ typedef struct Collidable
 } Collidable;
 
 
+typedef struct  
+{
+	float4		m_localCenter;
+	float4		m_extents;
+	float4		mC;
+	float4		mE;
+	
+	float			m_radius;
+	int	m_faceOffset;
+	int m_numFaces;
+	int	m_numVertices;
+	
+	int m_vertexOffset;
+	int	m_uniqueEdgesOffset;
+	int	m_numUniqueEdges;
+	int m_unused;
+
+} ConvexPolyhedronCL;
+
+typedef struct
+{
+	float4 m_plane;
+	int m_indexOffset;
+	int m_numIndices;
+} b3GpuFace;
+
+
+
+///////////////////////////////////////
+//	Quaternion
+///////////////////////////////////////
+
+typedef float4 Quaternion;
+
+__inline
+Quaternion qtMul(Quaternion a, Quaternion b);
+
+__inline
+Quaternion qtNormalize(Quaternion in);
+
+__inline
+float4 qtRotate(Quaternion q, float4 vec);
+
+__inline
+Quaternion qtInvert(Quaternion q);
+
+
+__inline
+float dot3F4(float4 a, float4 b)
+{
+	float4 a1 = (float4)(a.xyz,0.f);
+	float4 b1 = (float4)(b.xyz,0.f);
+	return dot(a1, b1);
+}
+
+
+__inline
+Quaternion qtMul(Quaternion a, Quaternion b)
+{
+	Quaternion ans;
+	ans = cross( a, b );
+	ans += a.w*b+b.w*a;
+//	ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
+	ans.w = a.w*b.w - dot3F4(a, b);
+	return ans;
+}
+
+__inline
+Quaternion qtNormalize(Quaternion in)
+{
+	return fast_normalize(in);
+//	in /= length( in );
+//	return in;
+}
+__inline
+float4 qtRotate(Quaternion q, float4 vec)
+{
+	Quaternion qInv = qtInvert( q );
+	float4 vcpy = vec;
+	vcpy.w = 0.f;
+	float4 out = qtMul(qtMul(q,vcpy),qInv);
+	return out;
+}
+
+__inline
+Quaternion qtInvert(Quaternion q)
+{
+	return (Quaternion)(-q.xyz, q.w);
+}
+
+__inline
+float4 qtInvRotate(const Quaternion q, float4 vec)
+{
+	return qtRotate( qtInvert( q ), vec );
+}
+
+__inline
+float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)
+{
+	return qtRotate( *orientation, *p ) + (*translation);
+}
+
+void	trInverse(float4 translationIn, Quaternion orientationIn,
+		float4* translationOut, Quaternion* orientationOut)
+{
+	*orientationOut = qtInvert(orientationIn);
+	*translationOut = qtRotate(*orientationOut, -translationIn);
+}
+
+void	trMul(float4 translationA, Quaternion orientationA,
+						float4 translationB, Quaternion orientationB,
+		float4* translationOut, Quaternion* orientationOut)
+{
+	*orientationOut = qtMul(orientationA,orientationB);
+	*translationOut = transform(&translationB,&translationA,&orientationA);
+}
+
+
+
+bool rayConvex(float4 rayFromLocal, float4 rayToLocal, int numFaces, int faceOffset,
+	__global const b3GpuFace* faces, float* hitFraction, float4* hitNormal)
+{
+	rayFromLocal.w = 0.f;
+	rayToLocal.w = 0.f;
+
+	float exitFraction = *hitFraction;
+	float enterFraction = -0.1f;
+	float4 curHitNormal = (float4)(0,0,0,0);
+	for (int i=0;i<numFaces;i++)
+	{
+		b3GpuFace face = faces[faceOffset+i];
+		float fromPlaneDist = dot(rayFromLocal,face.m_plane)+face.m_plane.w;
+		float toPlaneDist = dot(rayToLocal,face.m_plane)+face.m_plane.w;
+		if (fromPlaneDist<0.f)
+		{
+			if (toPlaneDist >= 0.f)
+			{
+				float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);
+				if (exitFraction>fraction)
+				{
+					exitFraction = fraction;
+				}
+			} 			
+		} else
+		{
+			if (toPlaneDist<0.f)
+			{
+				float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);
+				if (enterFraction <= fraction)
+				{
+					enterFraction = fraction;
+					curHitNormal = face.m_plane;
+					curHitNormal.w = 0.f;
+				}
+			} else
+			{
+				return false;
+			}
+		}
+		if (exitFraction <= enterFraction)
+			return false;
+	}
+
+	if (enterFraction < 0.f)
+		return false;
+
+	*hitFraction = enterFraction;
+	*hitNormal = curHitNormal;
+	return true;
+}
+
+
+
+
+
 
 bool sphere_intersect(float4 spherePos,  float radius, float4 rayFrom, float4 rayTo, float* hitFraction)
 {
@@ -88,10 +263,11 @@ __kernel void rayCastKernel(
 	__global b3RayHit* hitResults, 
 	const int numBodies, 
 	__global Body* bodies,
-	__global Collidable* collidables)
+	__global Collidable* collidables,
+	__global const b3GpuFace* faces,
+	__global const ConvexPolyhedronCL* convexShapes	)
 {
 
-
 	int i = get_global_id(0);
 	if (i<numRays)
 	{
@@ -100,6 +276,8 @@ __kernel void rayCastKernel(
 		float4 rayFrom = rays[i].m_from;
 		float4 rayTo = rays[i].m_to;
 		float hitFraction = 1.f;
+		float4 hitPoint;
+		float4 hitNormal;
 		int hitBodyIndex= -1;
 		
 		int cachedCollidableIndex = -1;		
@@ -109,7 +287,7 @@ __kernel void rayCastKernel(
 		{
 					
 				float4 pos = bodies[b].m_pos;
-	//		float4 orn = bodies[b].m_quat;
+				float4 orn = bodies[b].m_quat;
 				if (cachedCollidableIndex !=bodies[b].m_collidableIdx)
 				{
 						cachedCollidableIndex = bodies[b].m_collidableIdx;
@@ -123,15 +301,38 @@ __kernel void rayCastKernel(
 					if (sphere_intersect(pos,  radius, rayFrom, rayTo, &hitFraction))
 					{
 						hitBodyIndex = b;
+						hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction);
+						hitNormal = (float4) (hitPoint-bodies[b].m_pos);
 					}
 				}
+				
+				if (cachedCollidable.m_shapeType == SHAPE_CONVEX_HULL)
+				{
+				
+					float4 invPos = (float4)(0,0,0,0);
+					float4 invOrn = (float4)(0,0,0,0);
+					float4 rayFromLocal = (float4)(0,0,0,0);
+					float4 rayToLocal = (float4)(0,0,0,0);
+					
+					trInverse(pos,orn, &invPos, &invOrn);
+					rayFromLocal = transform(&rayFrom, &invPos, &invOrn);
+					rayToLocal = transform(&rayTo, &invPos, &invOrn);
+					
+					int numFaces = convexShapes[cachedCollidable.m_shapeIndex].m_numFaces;
+					int faceOffset = convexShapes[cachedCollidable.m_shapeIndex].m_faceOffset;
+					
+					if (rayConvex(rayFromLocal, rayToLocal, numFaces, faceOffset,faces, &hitFraction, &hitNormal))
+					{
+						hitBodyIndex = b;
+					}
+				}
+			
 		}
 		
 		if (hitBodyIndex>=0)
 		{
 			hitResults[i].m_hitFraction = hitFraction;
-			hitResults[i].m_hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction);
+			hitResults[i].m_hitPoint = hitPoint;
-			float4 hitNormal = (float4) (hitResults[i].m_hitPoint-bodies[hitBodyIndex].m_pos);
 			hitResults[i].m_hitNormal = normalize(hitNormal);
 			hitResults[i].m_hitResult0 = hitBodyIndex;
 		}

src/Bullet3OpenCL/Raycast/kernels/rayCastKernels.h

@@ -49,6 +49,181 @@ static const char* rayCastKernelCL= \
 "} Collidable;\n"
 "\n"
 "\n"
+"typedef struct  \n"
+"{\n"
+"	float4		m_localCenter;\n"
+"	float4		m_extents;\n"
+"	float4		mC;\n"
+"	float4		mE;\n"
+"	\n"
+"	float			m_radius;\n"
+"	int	m_faceOffset;\n"
+"	int m_numFaces;\n"
+"	int	m_numVertices;\n"
+"	\n"
+"	int m_vertexOffset;\n"
+"	int	m_uniqueEdgesOffset;\n"
+"	int	m_numUniqueEdges;\n"
+"	int m_unused;\n"
+"\n"
+"} ConvexPolyhedronCL;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_plane;\n"
+"	int m_indexOffset;\n"
+"	int m_numIndices;\n"
+"} b3GpuFace;\n"
+"\n"
+"\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Quaternion\n"
+"///////////////////////////////////////\n"
+"\n"
+"typedef float4 Quaternion;\n"
+"\n"
+"__inline\n"
+"Quaternion qtMul(Quaternion a, Quaternion b);\n"
+"\n"
+"__inline\n"
+"Quaternion qtNormalize(Quaternion in);\n"
+"\n"
+"__inline\n"
+"float4 qtRotate(Quaternion q, float4 vec);\n"
+"\n"
+"__inline\n"
+"Quaternion qtInvert(Quaternion q);\n"
+"\n"
+"\n"
+"__inline\n"
+"float dot3F4(float4 a, float4 b)\n"
+"{\n"
+"	float4 a1 = (float4)(a.xyz,0.f);\n"
+"	float4 b1 = (float4)(b.xyz,0.f);\n"
+"	return dot(a1, b1);\n"
+"}\n"
+"\n"
+"\n"
+"__inline\n"
+"Quaternion qtMul(Quaternion a, Quaternion b)\n"
+"{\n"
+"	Quaternion ans;\n"
+"	ans = cross( a, b );\n"
+"	ans += a.w*b+b.w*a;\n"
+"//	ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);\n"
+"	ans.w = a.w*b.w - dot3F4(a, b);\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Quaternion qtNormalize(Quaternion in)\n"
+"{\n"
+"	return fast_normalize(in);\n"
+"//	in /= length( in );\n"
+"//	return in;\n"
+"}\n"
+"__inline\n"
+"float4 qtRotate(Quaternion q, float4 vec)\n"
+"{\n"
+"	Quaternion qInv = qtInvert( q );\n"
+"	float4 vcpy = vec;\n"
+"	vcpy.w = 0.f;\n"
+"	float4 out = qtMul(qtMul(q,vcpy),qInv);\n"
+"	return out;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Quaternion qtInvert(Quaternion q)\n"
+"{\n"
+"	return (Quaternion)(-q.xyz, q.w);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 qtInvRotate(const Quaternion q, float4 vec)\n"
+"{\n"
+"	return qtRotate( qtInvert( q ), vec );\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n"
+"{\n"
+"	return qtRotate( *orientation, *p ) + (*translation);\n"
+"}\n"
+"\n"
+"void	trInverse(float4 translationIn, Quaternion orientationIn,\n"
+"		float4* translationOut, Quaternion* orientationOut)\n"
+"{\n"
+"	*orientationOut = qtInvert(orientationIn);\n"
+"	*translationOut = qtRotate(*orientationOut, -translationIn);\n"
+"}\n"
+"\n"
+"void	trMul(float4 translationA, Quaternion orientationA,\n"
+"						float4 translationB, Quaternion orientationB,\n"
+"		float4* translationOut, Quaternion* orientationOut)\n"
+"{\n"
+"	*orientationOut = qtMul(orientationA,orientationB);\n"
+"	*translationOut = transform(&translationB,&translationA,&orientationA);\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"bool rayConvex(float4 rayFromLocal, float4 rayToLocal, int numFaces, int faceOffset,\n"
+"	__global const b3GpuFace* faces, float* hitFraction, float4* hitNormal)\n"
+"{\n"
+"	rayFromLocal.w = 0.f;\n"
+"	rayToLocal.w = 0.f;\n"
+"\n"
+"	float exitFraction = *hitFraction;\n"
+"	float enterFraction = -0.1f;\n"
+"	float4 curHitNormal = (float4)(0,0,0,0);\n"
+"	for (int i=0;i<numFaces;i++)\n"
+"	{\n"
+"		b3GpuFace face = faces[faceOffset+i];\n"
+"		float fromPlaneDist = dot(rayFromLocal,face.m_plane)+face.m_plane.w;\n"
+"		float toPlaneDist = dot(rayToLocal,face.m_plane)+face.m_plane.w;\n"
+"		if (fromPlaneDist<0.f)\n"
+"		{\n"
+"			if (toPlaneDist >= 0.f)\n"
+"			{\n"
+"				float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);\n"
+"				if (exitFraction>fraction)\n"
+"				{\n"
+"					exitFraction = fraction;\n"
+"				}\n"
+"			} 			\n"
+"		} else\n"
+"		{\n"
+"			if (toPlaneDist<0.f)\n"
+"			{\n"
+"				float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);\n"
+"				if (enterFraction <= fraction)\n"
+"				{\n"
+"					enterFraction = fraction;\n"
+"					curHitNormal = face.m_plane;\n"
+"					curHitNormal.w = 0.f;\n"
+"				}\n"
+"			} else\n"
+"			{\n"
+"				return false;\n"
+"			}\n"
+"		}\n"
+"		if (exitFraction <= enterFraction)\n"
+"			return false;\n"
+"	}\n"
+"\n"
+"	if (enterFraction < 0.f)\n"
+"		return false;\n"
+"\n"
+"	*hitFraction = enterFraction;\n"
+"	*hitNormal = curHitNormal;\n"
+"	return true;\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
 "\n"
 "bool sphere_intersect(float4 spherePos,  float radius, float4 rayFrom, float4 rayTo, float* hitFraction)\n"
 "{\n"
@@ -90,10 +265,11 @@ static const char* rayCastKernelCL= \
 "	__global b3RayHit* hitResults, \n"
 "	const int numBodies, \n"
 "	__global Body* bodies,\n"
-"	__global Collidable* collidables)\n"
+"	__global Collidable* collidables,\n"
+"	__global const b3GpuFace* faces,\n"
+"	__global const ConvexPolyhedronCL* convexShapes	)\n"
 "{\n"
 "\n"
-"\n"
 "	int i = get_global_id(0);\n"
 "	if (i<numRays)\n"
 "	{\n"
@@ -102,6 +278,8 @@ static const char* rayCastKernelCL= \
 "		float4 rayFrom = rays[i].m_from;\n"
 "		float4 rayTo = rays[i].m_to;\n"
 "		float hitFraction = 1.f;\n"
+"		float4 hitPoint;\n"
+"		float4 hitNormal;\n"
 "		int hitBodyIndex= -1;\n"
 "		\n"
 "		int cachedCollidableIndex = -1;		\n"
@@ -111,7 +289,7 @@ static const char* rayCastKernelCL= \
 "		{\n"
 "					\n"
 "				float4 pos = bodies[b].m_pos;\n"
-"	//		float4 orn = bodies[b].m_quat;\n"
+"				float4 orn = bodies[b].m_quat;\n"
 "				if (cachedCollidableIndex !=bodies[b].m_collidableIdx)\n"
 "				{\n"
 "						cachedCollidableIndex = bodies[b].m_collidableIdx;\n"
@@ -125,15 +303,38 @@ static const char* rayCastKernelCL= \
 "					if (sphere_intersect(pos,  radius, rayFrom, rayTo, &hitFraction))\n"
 "					{\n"
 "						hitBodyIndex = b;\n"
+"						hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction);\n"
+"						hitNormal = (float4) (hitPoint-bodies[b].m_pos);\n"
 "					}\n"
 "				}\n"
+"				\n"
+"				if (cachedCollidable.m_shapeType == SHAPE_CONVEX_HULL)\n"
+"				{\n"
+"				\n"
+"					float4 invPos = (float4)(0,0,0,0);\n"
+"					float4 invOrn = (float4)(0,0,0,0);\n"
+"					float4 rayFromLocal = (float4)(0,0,0,0);\n"
+"					float4 rayToLocal = (float4)(0,0,0,0);\n"
+"					\n"
+"					trInverse(pos,orn, &invPos, &invOrn);\n"
+"					rayFromLocal = transform(&rayFrom, &invPos, &invOrn);\n"
+"					rayToLocal = transform(&rayTo, &invPos, &invOrn);\n"
+"					\n"
+"					int numFaces = convexShapes[cachedCollidable.m_shapeIndex].m_numFaces;\n"
+"					int faceOffset = convexShapes[cachedCollidable.m_shapeIndex].m_faceOffset;\n"
+"					\n"
+"					if (rayConvex(rayFromLocal, rayToLocal, numFaces, faceOffset,faces, &hitFraction, &hitNormal))\n"
+"					{\n"
+"						hitBodyIndex = b;\n"
+"					}\n"
+"				}\n"
+"			\n"
 "		}\n"
 "		\n"
 "		if (hitBodyIndex>=0)\n"
 "		{\n"
 "			hitResults[i].m_hitFraction = hitFraction;\n"
-"			hitResults[i].m_hitPoint = setInterpolate3(rayFrom, rayTo,hitFraction);\n"
+"			hitResults[i].m_hitPoint = hitPoint;\n"
-"			float4 hitNormal = (float4) (hitResults[i].m_hitPoint-bodies[hitBodyIndex].m_pos);\n"
 "			hitResults[i].m_hitNormal = normalize(hitNormal);\n"
 "			hitResults[i].m_hitResult0 = hitBodyIndex;\n"
 "		}\n"

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp

@@ -457,7 +457,7 @@ int		b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
 
 void	b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3AlignedObjectArray<b3RayHit>& hitResults)
 {
-	this->m_data->m_raycaster->castRaysHost(rays,hitResults,
+	this->m_data->m_raycaster->castRays(rays,hitResults,
 		getNumBodies(),this->m_data->m_narrowphase->getBodiesCpu(),
 		m_data->m_narrowphase->getNumCollidablesGpu(), m_data->m_narrowphase->getCollidablesCpu(),	m_data->m_narrowphase->getInternalData()
 		);