work-in-progress Bullet 3.x GPU raytest

work-in-progress P2P constraint for b3GpuDynamicsWorld

src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h

@@ -325,14 +325,18 @@ class b3LauncherCL
 		{
 			int sz=sizeof(T);
 			b3Assert(sz<=B3_CL_MAX_ARG_SIZE);
-            b3KernelArgData kernelArg;
-            kernelArg.m_argIndex = m_idx;
-            kernelArg.m_isBuffer = 0;
-            T* destArg = (T*)kernelArg.m_argData;
-            *destArg = consts;
-            kernelArg.m_argSizeInBytes = sizeof(T);
-            m_kernelArguments.push_back(kernelArg);
-            m_serializationSizeInBytes+=sizeof(b3KernelArgData);
+
+			if (m_enableSerialization)
+			{
+				b3KernelArgData kernelArg;
+				kernelArg.m_argIndex = m_idx;
+				kernelArg.m_isBuffer = 0;
+				T* destArg = (T*)kernelArg.m_argData;
+				*destArg = consts;
+				kernelArg.m_argSizeInBytes = sizeof(T);
+				m_kernelArguments.push_back(kernelArg);
+				m_serializationSizeInBytes+=sizeof(b3KernelArgData);
+			}
             
 			cl_int status = clSetKernelArg( m_kernel, m_idx++, sz, &consts );
 			b3Assert( status == CL_SUCCESS );

src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp

@@ -49,23 +49,25 @@ b3GpuRaycast::~b3GpuRaycast()
 	delete m_data;
 }
 
-bool sphere_intersect(const b3Vector3& spherePos,  b3Scalar radius, const b3Vector3& rayFrom, const b3Vector3& rayTo)
+bool sphere_intersect(const b3Vector3& spherePos,  b3Scalar radius, const b3Vector3& rayFrom, const b3Vector3& rayTo, float& hitFraction)
 {
-    // rs = ray.org - sphere.center
-    const b3Vector3& rs = rayFrom - spherePos;
-	b3Vector3 rayDir = rayTo-rayFrom;//rayFrom-rayTo;
-	rayDir.normalize();
-
+    b3Vector3 rs = rayFrom - spherePos;
+	b3Vector3 rayDir = rayTo-rayFrom;
+	
+	float A = b3Dot(rayDir,rayDir);
     float B = b3Dot(rs, rayDir);
     float C = b3Dot(rs, rs) - (radius * radius);
-    float D = B * B - C;
+    
+	float D = B * B - A*C;
 
     if (D > 0.0)
     {
-        float t = -B - sqrt(D);
-        if ( (t > 0.0))// && (t < isect.t) )
+        float t = (-B - sqrt(D))/A;
+
+        if ( (t >= 0.0f) && (t < hitFraction) )
         {
-            return true;//isect.t = t;
+			hitFraction = t;
+            return true;
 		}
 	}
 	return false;
@@ -79,16 +81,15 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays,	b3A
 //	return castRays(rays,hitResults,numBodies,bodies,numCollidables,collidables);
 
 	B3_PROFILE("castRaysHost");
-
 	for (int r=0;r<rays.size();r++)
 	{
 		b3Vector3 rayFrom = rays[r].m_from;
 		b3Vector3 rayTo = rays[r].m_to;
+		float hitFraction = hitResults[r].m_hitFraction;
 
-		//if there is a hit, color the pixels
-		bool hits  = false;
+		int sphereHit = -1;
 
-		for (int b=0;b<numBodies && !hits;b++)
+		for (int b=0;b<numBodies;b++)
 		{
 				
 			const b3Vector3& pos = bodies[b].m_pos;
@@ -96,11 +97,19 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays,	b3A
 			
 			b3Scalar radius = 1;
 
-			if (sphere_intersect(pos,  radius, rayFrom, rayTo))
-				hits = true;
+			if (sphere_intersect(pos,  radius, rayFrom, rayTo,hitFraction))
+			{
+				sphereHit = b;
+			}
+		}
+		if (sphereHit>=0)
+		{
+
+			hitResults[r].m_hitFraction = hitFraction;
+			hitResults[r].m_hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to,hitFraction);
+			hitResults[r].m_hitNormal = (hitResults[r].m_hitPoint-bodies[sphereHit].m_pos).normalize();
+			hitResults[r].m_hitResult0 = sphereHit;
 		}
-		if (hits)
-			hitResults[r].m_hitFraction = 0.f;
 
 	}
 }
@@ -115,6 +124,7 @@ void b3GpuRaycast::castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3Align
 
 	b3OpenCLArray<b3RayHit> gpuHitResults(m_data->m_context,m_data->m_q);
 	gpuHitResults.resize(hitResults.size());
+	gpuHitResults.copyFromHost(hitResults);
 
 	b3OpenCLArray<b3RigidBodyCL> gpuBodies(m_data->m_context,m_data->m_q);
 	gpuBodies.resize(numBodies);

src/Bullet3OpenCL/Raycast/b3GpuRaycast.h

@@ -5,22 +5,9 @@
 #include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
 
 #include "Bullet3Common/b3AlignedObjectArray.h"
+#include "b3RaycastInfo.h"
 
-struct b3RayInfo
-{
-	b3Vector3 m_from;
-	b3Vector3 m_to;
-};
 
-struct b3RayHit
-{
-		b3Scalar	m_hitFraction;
-		int	m_hitResult0;
-		int	m_hitResult1;
-		int	m_hitResult2;
-		b3Vector3 m_hitPoint;
-		b3Vector3 m_hitNormal;
-};
 
 class b3GpuRaycast
 {

src/Bullet3OpenCL/Raycast/b3RaycastInfo.h

@@ -0,0 +1,24 @@
+
+#ifndef B3_RAYCAST_INFO_H
+#define B3_RAYCAST_INFO_H
+
+#include "Bullet3Common/b3Vector3.h"
+
+B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo
+{
+	b3Vector3 m_from;
+	b3Vector3 m_to;
+};
+
+B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit
+{
+		b3Scalar	m_hitFraction;
+		int	m_hitResult0;
+		int	m_hitResult1;
+		int	m_hitResult2;
+		b3Vector3 m_hitPoint;
+		b3Vector3 m_hitNormal;
+};
+
+#endif //B3_RAYCAST_INFO_H
+

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp

@@ -33,7 +33,7 @@ bool dumpContactStats = false;
 
 #include "Bullet3Common/b3Quickprof.h"
 #include "b3Config.h"
-
+#include "Bullet3OpenCL/Raycast/b3GpuRaycast.h"
 
 
 
@@ -55,6 +55,8 @@ b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id devic
 	
 	m_data->m_solver2 = new b3GpuBatchingPgsSolver(ctx,device,q,config.m_maxBroadphasePairs);
 
+	m_data->m_raycaster = new b3GpuRaycast(ctx,device,q);
+
 	
 	m_data->m_broadphaseDbvt = broadphaseDbvt;
 	m_data->m_broadphaseSap = broadphaseSap;
@@ -85,6 +87,7 @@ b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline()
 {
 	clReleaseKernel(m_data->m_integrateTransformsKernel);
 
+	delete m_data->m_raycaster;
 	delete m_data->m_solver;
 	delete m_data->m_allAabbsGPU;
 	delete m_data->m_overlappingPairsGPU;
@@ -110,6 +113,14 @@ void	b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint)
 {
 	m_data->m_joints.push_back(constraint);
 }
+
+void	b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint)
+{
+	m_data->m_joints.remove(constraint);
+}
+
+
+
 void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 {
 
@@ -220,7 +231,7 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 		{
 			b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 0;
 			b3Contact4* contacts = numContacts? &hostContacts[0]: 0;
-//			m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,contacts,numJoints, joints);
+			//m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,contacts,numJoints, joints);
 			m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(),&hostBodies[0],&hostInertias[0],0,0,numJoints, joints);
 
 		}
@@ -438,3 +449,9 @@ int		b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
 
 	return bodyIndex;
 }
+
+void	b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3AlignedObjectArray<b3RayHit>& hitResults)
+{
+	this->m_data->m_raycaster->castRaysHost(rays,hitResults,getNumBodies(),this->m_data->m_narrowphase->getBodiesCpu(),m_data->m_narrowphase->getNumCollidablesGpu(), m_data->m_narrowphase->getCollidablesCpu());
+}
+

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h

@@ -4,6 +4,9 @@
 #include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
 #include "b3Config.h"
 
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3OpenCL/Raycast/b3RaycastInfo.h"
+
 class b3GpuRigidBodyPipeline
 {
 protected:
@@ -38,6 +41,9 @@ public:
 	void reset();
 	
 	void	addConstraint(class b3TypedConstraint* constraint);
+	void	removeConstraint(b3TypedConstraint* constraint);
+
+	void	castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3AlignedObjectArray<b3RayHit>& hitResults);
 
 	cl_mem	getBodyBuffer();
 

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h

@@ -28,6 +28,7 @@ struct b3GpuRigidBodyPipelineInternalData
 	class b3PgsJacobiSolver* m_solver;
 	class b3GpuBatchingPgsSolver* m_solver2;
 	class b3GpuJacobiSolver* m_solver3;
+	class b3GpuRaycast* m_raycaster;
 	
 	class b3GpuSapBroadphase* m_broadphaseSap;