Merge pull request #24740 from OBKF/update-bullet-physics

Update Bullet physics to commit 126b676

1 files changed, 160 insertions, 191 deletions

diff --git a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
index 783e443060..fef33ad1cd 100644
--- a/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
+++ b/thirdparty/bullet/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
@@ -47,7 +47,7 @@ bool gClearPairsOnGpu = true;
 #define TEST_OTHER_GPU_SOLVER 1
 #ifdef TEST_OTHER_GPU_SOLVER
 #include "b3GpuJacobiContactSolver.h"
-#endif //TEST_OTHER_GPU_SOLVER
+#endif  //TEST_OTHER_GPU_SOLVER
 
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
 #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
@@ -59,73 +59,68 @@ bool gClearPairsOnGpu = true;
 #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "Bullet3OpenCL/Raycast/b3GpuRaycast.h"
 
-	
 #include "Bullet3Dynamics/shared/b3IntegrateTransforms.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h"
 
-b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue  q,class b3GpuNarrowPhase* narrowphase, class b3GpuBroadphaseInterface* broadphaseSap , struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
+b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx, cl_device_id device, cl_command_queue q, class b3GpuNarrowPhase* narrowphase, class b3GpuBroadphaseInterface* broadphaseSap, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
 {
 	m_data = new b3GpuRigidBodyPipelineInternalData;
-	m_data->m_constraintUid=0;
+	m_data->m_constraintUid = 0;
 	m_data->m_config = config;
 	m_data->m_context = ctx;
 	m_data->m_device = device;
 	m_data->m_queue = q;
 
-	m_data->m_solver = new b3PgsJacobiSolver(true);//new b3PgsJacobiSolver(true);
-	m_data->m_gpuSolver = new b3GpuPgsConstraintSolver(ctx,device,q,true);//new b3PgsJacobiSolver(true);
-	
-	m_data->m_allAabbsGPU = new b3OpenCLArray<b3SapAabb>(ctx,q,config.m_maxConvexBodies);
-	m_data->m_overlappingPairsGPU = new b3OpenCLArray<b3BroadphasePair>(ctx,q,config.m_maxBroadphasePairs);
+	m_data->m_solver = new b3PgsJacobiSolver(true);                            //new b3PgsJacobiSolver(true);
+	m_data->m_gpuSolver = new b3GpuPgsConstraintSolver(ctx, device, q, true);  //new b3PgsJacobiSolver(true);
 
-	m_data->m_gpuConstraints = new b3OpenCLArray<b3GpuGenericConstraint>(ctx,q);
+	m_data->m_allAabbsGPU = new b3OpenCLArray<b3SapAabb>(ctx, q, config.m_maxConvexBodies);
+	m_data->m_overlappingPairsGPU = new b3OpenCLArray<b3BroadphasePair>(ctx, q, config.m_maxBroadphasePairs);
+
+	m_data->m_gpuConstraints = new b3OpenCLArray<b3GpuGenericConstraint>(ctx, q);
 #ifdef TEST_OTHER_GPU_SOLVER
-	m_data->m_solver3 = new b3GpuJacobiContactSolver(ctx,device,q,config.m_maxBroadphasePairs);	
-#endif //	TEST_OTHER_GPU_SOLVER
-	
-	m_data->m_solver2 = new b3GpuPgsContactSolver(ctx,device,q,config.m_maxBroadphasePairs);
+	m_data->m_solver3 = new b3GpuJacobiContactSolver(ctx, device, q, config.m_maxBroadphasePairs);
+#endif  //	TEST_OTHER_GPU_SOLVER
+
+	m_data->m_solver2 = new b3GpuPgsContactSolver(ctx, device, q, config.m_maxBroadphasePairs);
 
-	m_data->m_raycaster = new b3GpuRaycast(ctx,device,q);
+	m_data->m_raycaster = new b3GpuRaycast(ctx, device, q);
 
-	
 	m_data->m_broadphaseDbvt = broadphaseDbvt;
 	m_data->m_broadphaseSap = broadphaseSap;
 	m_data->m_narrowphase = narrowphase;
-	m_data->m_gravity.setValue(0.f,-9.8f,0.f);
+	m_data->m_gravity.setValue(0.f, -9.8f, 0.f);
 
-	cl_int errNum=0;
+	cl_int errNum = 0;
 
 	{
-		cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,integrateKernelCL,&errNum,"",B3_RIGIDBODY_INTEGRATE_PATH);
-		b3Assert(errNum==CL_SUCCESS);
-		m_data->m_integrateTransformsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,integrateKernelCL, "integrateTransformsKernel",&errNum,prog);
-		b3Assert(errNum==CL_SUCCESS);
+		cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context, m_data->m_device, integrateKernelCL, &errNum, "", B3_RIGIDBODY_INTEGRATE_PATH);
+		b3Assert(errNum == CL_SUCCESS);
+		m_data->m_integrateTransformsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device, integrateKernelCL, "integrateTransformsKernel", &errNum, prog);
+		b3Assert(errNum == CL_SUCCESS);
 		clReleaseProgram(prog);
 	}
 	{
-		cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context,m_data->m_device,updateAabbsKernelCL,&errNum,"",B3_RIGIDBODY_UPDATEAABB_PATH);
-		b3Assert(errNum==CL_SUCCESS);
-		m_data->m_updateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,updateAabbsKernelCL, "initializeGpuAabbsFull",&errNum,prog);
-		b3Assert(errNum==CL_SUCCESS);
+		cl_program prog = b3OpenCLUtils::compileCLProgramFromString(m_data->m_context, m_data->m_device, updateAabbsKernelCL, &errNum, "", B3_RIGIDBODY_UPDATEAABB_PATH);
+		b3Assert(errNum == CL_SUCCESS);
+		m_data->m_updateAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device, updateAabbsKernelCL, "initializeGpuAabbsFull", &errNum, prog);
+		b3Assert(errNum == CL_SUCCESS);
 
-
-		m_data->m_clearOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device,updateAabbsKernelCL, "clearOverlappingPairsKernel",&errNum,prog);
-		b3Assert(errNum==CL_SUCCESS);
+		m_data->m_clearOverlappingPairsKernel = b3OpenCLUtils::compileCLKernelFromString(m_data->m_context, m_data->m_device, updateAabbsKernelCL, "clearOverlappingPairsKernel", &errNum, prog);
+		b3Assert(errNum == CL_SUCCESS);
 
 		clReleaseProgram(prog);
 	}
-
-
 }
 
 b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline()
 {
 	if (m_data->m_integrateTransformsKernel)
 		clReleaseKernel(m_data->m_integrateTransformsKernel);
-	
+
 	if (m_data->m_updateAabbsKernel)
 		clReleaseKernel(m_data->m_updateAabbsKernel);
-	
+
 	if (m_data->m_clearOverlappingPairsKernel)
 		clReleaseKernel(m_data->m_clearOverlappingPairsKernel);
 	delete m_data->m_raycaster;
@@ -136,15 +131,14 @@ b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline()
 
 #ifdef TEST_OTHER_GPU_SOLVER
 	delete m_data->m_solver3;
-#endif //TEST_OTHER_GPU_SOLVER
-	
+#endif  //TEST_OTHER_GPU_SOLVER
+
 	delete m_data->m_solver2;
-	
-	
+
 	delete m_data;
 }
 
-void	b3GpuRigidBodyPipeline::reset()
+void b3GpuRigidBodyPipeline::reset()
 {
 	m_data->m_gpuConstraints->resize(0);
 	m_data->m_cpuConstraints.resize(0);
@@ -152,30 +146,28 @@ void	b3GpuRigidBodyPipeline::reset()
 	m_data->m_allAabbsCPU.resize(0);
 }
 
-void	b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint)
+void b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint)
 {
 	m_data->m_joints.push_back(constraint);
 }
 
-void	b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint)
+void b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint)
 {
 	m_data->m_joints.remove(constraint);
 }
 
-
-
-void  b3GpuRigidBodyPipeline::removeConstraintByUid(int uid)
+void b3GpuRigidBodyPipeline::removeConstraintByUid(int uid)
 {
 	m_data->m_gpuSolver->recomputeBatches();
 	//slow linear search
 	m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints);
 	//remove
-	for (int i=0;i<m_data->m_cpuConstraints.size();i++)
+	for (int i = 0; i < m_data->m_cpuConstraints.size(); i++)
 	{
 		if (m_data->m_cpuConstraints[i].m_uid == uid)
 		{
 			//m_data->m_cpuConstraints.remove(m_data->m_cpuConstraints[i]);
-			m_data->m_cpuConstraints.swap(i,m_data->m_cpuConstraints.size()-1);
+			m_data->m_cpuConstraints.swap(i, m_data->m_cpuConstraints.size() - 1);
 			m_data->m_cpuConstraints.pop_back();
 
 			break;
@@ -185,13 +177,13 @@ void  b3GpuRigidBodyPipeline::removeConstraintByUid(int uid)
 	if (m_data->m_cpuConstraints.size())
 	{
 		m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints);
-	} else
+	}
+	else
 	{
 		m_data->m_gpuConstraints->resize(0);
 	}
-
 }
-int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold)
+int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, float breakingThreshold)
 {
 	m_data->m_gpuSolver->recomputeBatches();
 	b3GpuGenericConstraint c;
@@ -200,14 +192,14 @@ int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, co
 	c.m_flags = B3_CONSTRAINT_FLAG_ENABLED;
 	c.m_rbA = bodyA;
 	c.m_rbB = bodyB;
-	c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]);
-	c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]);
+	c.m_pivotInA.setValue(pivotInA[0], pivotInA[1], pivotInA[2]);
+	c.m_pivotInB.setValue(pivotInB[0], pivotInB[1], pivotInB[2]);
 	c.m_breakingImpulseThreshold = breakingThreshold;
 	c.m_constraintType = B3_GPU_POINT2POINT_CONSTRAINT_TYPE;
 	m_data->m_cpuConstraints.push_back(c);
 	return c.m_uid;
 }
-int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB,float breakingThreshold)
+int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold)
 {
 	m_data->m_gpuSolver->recomputeBatches();
 	b3GpuGenericConstraint c;
@@ -216,9 +208,9 @@ int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const fl
 	c.m_flags = B3_CONSTRAINT_FLAG_ENABLED;
 	c.m_rbA = bodyA;
 	c.m_rbB = bodyB;
-	c.m_pivotInA.setValue(pivotInA[0],pivotInA[1],pivotInA[2]);
-	c.m_pivotInB.setValue(pivotInB[0],pivotInB[1],pivotInB[2]);
-	c.m_relTargetAB.setValue(relTargetAB[0],relTargetAB[1],relTargetAB[2],relTargetAB[3]);
+	c.m_pivotInA.setValue(pivotInA[0], pivotInA[1], pivotInA[2]);
+	c.m_pivotInB.setValue(pivotInB[0], pivotInB[1], pivotInB[2]);
+	c.m_relTargetAB.setValue(relTargetAB[0], relTargetAB[1], relTargetAB[2], relTargetAB[3]);
 	c.m_breakingImpulseThreshold = breakingThreshold;
 	c.m_constraintType = B3_GPU_FIXED_CONSTRAINT_TYPE;
 
@@ -226,31 +218,28 @@ int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const fl
 	return c.m_uid;
 }
 
-
-void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
+void b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 {
-
 	//update worldspace AABBs from local AABB/worldtransform
 	{
 		B3_PROFILE("setupGpuAabbs");
 		setupGpuAabbsFull();
 	}
 
-	int numPairs =0;
+	int numPairs = 0;
 
 	//compute overlapping pairs
 	{
-
 		if (gUseDbvt)
 		{
 			{
 				B3_PROFILE("setAabb");
 				m_data->m_allAabbsGPU->copyToHost(m_data->m_allAabbsCPU);
-				for (int i=0;i<m_data->m_allAabbsCPU.size();i++)
+				for (int i = 0; i < m_data->m_allAabbsCPU.size(); i++)
 				{
-					b3Vector3 aabbMin=b3MakeVector3(m_data->m_allAabbsCPU[i].m_min[0],m_data->m_allAabbsCPU[i].m_min[1],m_data->m_allAabbsCPU[i].m_min[2]);
-					b3Vector3 aabbMax=b3MakeVector3(m_data->m_allAabbsCPU[i].m_max[0],m_data->m_allAabbsCPU[i].m_max[1],m_data->m_allAabbsCPU[i].m_max[2]);
-					m_data->m_broadphaseDbvt->setAabb(i,aabbMin,aabbMax,0);
+					b3Vector3 aabbMin = b3MakeVector3(m_data->m_allAabbsCPU[i].m_min[0], m_data->m_allAabbsCPU[i].m_min[1], m_data->m_allAabbsCPU[i].m_min[2]);
+					b3Vector3 aabbMax = b3MakeVector3(m_data->m_allAabbsCPU[i].m_max[0], m_data->m_allAabbsCPU[i].m_max[1], m_data->m_allAabbsCPU[i].m_max[2]);
+					m_data->m_broadphaseDbvt->setAabb(i, aabbMin, aabbMax, 0);
 				}
 			}
 
@@ -259,13 +248,14 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 				m_data->m_broadphaseDbvt->calculateOverlappingPairs();
 			}
 			numPairs = m_data->m_broadphaseDbvt->getOverlappingPairCache()->getNumOverlappingPairs();
-
-		} else
+		}
+		else
 		{
 			if (gUseCalculateOverlappingPairsHost)
 			{
 				m_data->m_broadphaseSap->calculateOverlappingPairsHost(m_data->m_config.m_maxBroadphasePairs);
-			} else
+			}
+			else
 			{
 				m_data->m_broadphaseSap->calculateOverlappingPairs(m_data->m_config.m_maxBroadphasePairs);
 			}
@@ -274,24 +264,24 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 	}
 
 	//compute contact points
-//	printf("numPairs=%d\n",numPairs);
-	
-	int numContacts  = 0;
+	//	printf("numPairs=%d\n",numPairs);
 
+	int numContacts = 0;
 
 	int numBodies = m_data->m_narrowphase->getNumRigidBodies();
 
 	if (numPairs)
 	{
-		cl_mem pairs =0;
-		cl_mem aabbsWS =0;
+		cl_mem pairs = 0;
+		cl_mem aabbsWS = 0;
 		if (gUseDbvt)
 		{
 			B3_PROFILE("m_overlappingPairsGPU->copyFromHost");
 			m_data->m_overlappingPairsGPU->copyFromHost(m_data->m_broadphaseDbvt->getOverlappingPairCache()->getOverlappingPairArray());
 			pairs = m_data->m_overlappingPairsGPU->getBufferCL();
 			aabbsWS = m_data->m_allAabbsGPU->getBufferCL();
-		} else
+		}
+		else
 		{
 			pairs = m_data->m_broadphaseSap->getOverlappingPairBuffer();
 			aabbsWS = m_data->m_broadphaseSap->getAabbBufferWS();
@@ -302,31 +292,27 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 		//mark the contacts for each pair as 'unused'
 		if (numPairs)
 		{
-			b3OpenCLArray<b3BroadphasePair> gpuPairs(this->m_data->m_context,m_data->m_queue);
-			gpuPairs.setFromOpenCLBuffer(pairs,numPairs);
+			b3OpenCLArray<b3BroadphasePair> gpuPairs(this->m_data->m_context, m_data->m_queue);
+			gpuPairs.setFromOpenCLBuffer(pairs, numPairs);
 
 			if (gClearPairsOnGpu)
 			{
-				
-
 				//b3AlignedObjectArray<b3BroadphasePair> hostPairs;//just for debugging
 				//gpuPairs.copyToHost(hostPairs);
 
-				b3LauncherCL launcher(m_data->m_queue,m_data->m_clearOverlappingPairsKernel,"clearOverlappingPairsKernel");
+				b3LauncherCL launcher(m_data->m_queue, m_data->m_clearOverlappingPairsKernel, "clearOverlappingPairsKernel");
 				launcher.setBuffer(pairs);
 				launcher.setConst(numPairs);
 				launcher.launch1D(numPairs);
 
-
 				//gpuPairs.copyToHost(hostPairs);
-			
-
-			} else
+			}
+			else
 			{
 				b3AlignedObjectArray<b3BroadphasePair> hostPairs;
 				gpuPairs.copyToHost(hostPairs);
 
-				for (int i=0;i<hostPairs.size();i++)
+				for (int i = 0; i < hostPairs.size(); i++)
 				{
 					hostPairs[i].z = 0xffffffff;
 				}
@@ -335,7 +321,7 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 			}
 		}
 
-		m_data->m_narrowphase->computeContacts(pairs,numPairs,aabbsWS,numBodies);
+		m_data->m_narrowphase->computeContacts(pairs, numPairs, aabbsWS, numBodies);
 		numContacts = m_data->m_narrowphase->getNumContactsGpu();
 
 		if (gUseDbvt)
@@ -347,56 +333,54 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 		if (gDumpContactStats && numContacts)
 		{
 			m_data->m_narrowphase->getContactsGpu();
-			
+
 			printf("numContacts = %d\n", numContacts);
 
-			int totalPoints  = 0;
+			int totalPoints = 0;
 			const b3Contact4* contacts = m_data->m_narrowphase->getContactsCPU();
 
-			for (int i=0;i<numContacts;i++)
+			for (int i = 0; i < numContacts; i++)
 			{
 				totalPoints += contacts->getNPoints();
 			}
-			printf("totalPoints=%d\n",totalPoints);
-
+			printf("totalPoints=%d\n", totalPoints);
 		}
 	}
-	
 
 	//convert contact points to contact constraints
-	
+
 	//solve constraints
 
-	b3OpenCLArray<b3RigidBodyData> gpuBodies(m_data->m_context,m_data->m_queue,0,true);
-	gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumRigidBodies());
-	b3OpenCLArray<b3InertiaData> gpuInertias(m_data->m_context,m_data->m_queue,0,true);
-	gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumRigidBodies());
-	b3OpenCLArray<b3Contact4> gpuContacts(m_data->m_context,m_data->m_queue,0,true);
-	gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu());
+	b3OpenCLArray<b3RigidBodyData> gpuBodies(m_data->m_context, m_data->m_queue, 0, true);
+	gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(), m_data->m_narrowphase->getNumRigidBodies());
+	b3OpenCLArray<b3InertiaData> gpuInertias(m_data->m_context, m_data->m_queue, 0, true);
+	gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(), m_data->m_narrowphase->getNumRigidBodies());
+	b3OpenCLArray<b3Contact4> gpuContacts(m_data->m_context, m_data->m_queue, 0, true);
+	gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(), m_data->m_narrowphase->getNumContactsGpu());
 
-	int numJoints =  m_data->m_joints.size() ?  m_data->m_joints.size() : m_data->m_cpuConstraints.size();
+	int numJoints = m_data->m_joints.size() ? m_data->m_joints.size() : m_data->m_cpuConstraints.size();
 	if (useBullet2CpuSolver && numJoints)
 	{
-
-	//	b3AlignedObjectArray<b3Contact4> hostContacts;
+		//	b3AlignedObjectArray<b3Contact4> hostContacts;
 		//gpuContacts.copyToHost(hostContacts);
 		{
-			bool useGpu = m_data->m_joints.size()==0;
+			bool useGpu = m_data->m_joints.size() == 0;
 
-//			b3Contact4* contacts = numContacts? &hostContacts[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);
 			if (useGpu)
 			{
-				m_data->m_gpuSolver->solveJoints(m_data->m_narrowphase->getNumRigidBodies(),&gpuBodies,&gpuInertias,numJoints, m_data->m_gpuConstraints);
-			} else
+				m_data->m_gpuSolver->solveJoints(m_data->m_narrowphase->getNumRigidBodies(), &gpuBodies, &gpuInertias, numJoints, m_data->m_gpuConstraints);
+			}
+			else
 			{
 				b3AlignedObjectArray<b3RigidBodyData> hostBodies;
 				gpuBodies.copyToHost(hostBodies);
 				b3AlignedObjectArray<b3InertiaData> hostInertias;
 				gpuInertias.copyToHost(hostInertias);
 
-				b3TypedConstraint** joints = numJoints? &m_data->m_joints[0] : 0;
-				m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(),&hostBodies[0],&hostInertias[0],0,0,numJoints, joints);
+				b3TypedConstraint** joints = numJoints ? &m_data->m_joints[0] : 0;
+				m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumRigidBodies(), &hostBodies[0], &hostInertias[0], 0, 0, numJoints, joints);
 				gpuBodies.copyFromHost(hostBodies);
 			}
 		}
@@ -404,22 +388,20 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 
 	if (numContacts)
 	{
-
 #ifdef TEST_OTHER_GPU_SOLVER
-		
+
 		if (gUseJacobi)
 		{
 			bool useGpu = true;
 			if (useGpu)
 			{
-
 				bool forceHost = false;
 				if (forceHost)
 				{
 					b3AlignedObjectArray<b3RigidBodyData> hostBodies;
 					b3AlignedObjectArray<b3InertiaData> hostInertias;
 					b3AlignedObjectArray<b3Contact4> hostContacts;
-				
+
 					{
 						B3_PROFILE("copyToHost");
 						gpuBodies.copyToHost(hostBodies);
@@ -429,25 +411,24 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 
 					{
 						b3JacobiSolverInfo solverInfo;
-						m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),solverInfo);
-
-						
+						m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(), &hostContacts[0], hostContacts.size(), solverInfo);
 					}
 					{
 						B3_PROFILE("copyFromHost");
 						gpuBodies.copyFromHost(hostBodies);
 					}
-				} else
-
+				}
+				else
 
 				{
 					int static0Index = m_data->m_narrowphase->getStatic0Index();
 					b3JacobiSolverInfo solverInfo;
 					//m_data->m_solver3->solveContacts(    >solveGroup(&gpuBodies, &gpuInertias, &gpuContacts,solverInfo);
 					//m_data->m_solver3->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]);
-					m_data->m_solver3->solveContacts(numBodies, gpuBodies.getBufferCL(),gpuInertias.getBufferCL(),numContacts, gpuContacts.getBufferCL(),m_data->m_config, static0Index);
+					m_data->m_solver3->solveContacts(numBodies, gpuBodies.getBufferCL(), gpuInertias.getBufferCL(), numContacts, gpuContacts.getBufferCL(), m_data->m_config, static0Index);
 				}
-			} else
+			}
+			else
 			{
 				b3AlignedObjectArray<b3RigidBodyData> hostBodies;
 				gpuBodies.copyToHost(hostBodies);
@@ -460,17 +441,15 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 				}
 				gpuBodies.copyFromHost(hostBodies);
 			}
-		
-		} else
-#endif //TEST_OTHER_GPU_SOLVER
+		}
+		else
+#endif  //TEST_OTHER_GPU_SOLVER
 		{
-			
 			int static0Index = m_data->m_narrowphase->getStatic0Index();
-			m_data->m_solver2->solveContacts(numBodies, gpuBodies.getBufferCL(),gpuInertias.getBufferCL(),numContacts, gpuContacts.getBufferCL(),m_data->m_config, static0Index);
-			
+			m_data->m_solver2->solveContacts(numBodies, gpuBodies.getBufferCL(), gpuInertias.getBufferCL(), numContacts, gpuContacts.getBufferCL(), m_data->m_config, static0Index);
+
 			//m_data->m_solver4->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(), gpuBodies.getBufferCL(), gpuInertias.getBufferCL(), numContacts, gpuContacts.getBufferCL());
-			
-			
+
 			/*m_data->m_solver3->solveContactConstraintHost(
 			(b3OpenCLArray<RigidBodyBase::Body>*)&gpuBodies,
 			(b3OpenCLArray<RigidBodyBase::Inertia>*)&gpuInertias,
@@ -481,11 +460,9 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 	}
 
 	integrate(deltaTime);
-
 }
 
-
-void	b3GpuRigidBodyPipeline::integrate(float timeStep)
+void b3GpuRigidBodyPipeline::integrate(float timeStep)
 {
 	//integrate
 	int numBodies = m_data->m_narrowphase->getNumRigidBodies();
@@ -493,24 +470,25 @@ void	b3GpuRigidBodyPipeline::integrate(float timeStep)
 
 	if (gIntegrateOnCpu)
 	{
-		if(numBodies)
+		if (numBodies)
 		{
-			b3GpuNarrowPhaseInternalData*	npData = m_data->m_narrowphase->getInternalData();
+			b3GpuNarrowPhaseInternalData* npData = m_data->m_narrowphase->getInternalData();
 			npData->m_bodyBufferGPU->copyToHost(*npData->m_bodyBufferCPU);
 
 			b3RigidBodyData_t* bodies = &npData->m_bodyBufferCPU->at(0);
 
-			for (int nodeID=0;nodeID<numBodies;nodeID++)
+			for (int nodeID = 0; nodeID < numBodies; nodeID++)
 			{
-				integrateSingleTransform( bodies,nodeID, timeStep, angularDamp, m_data->m_gravity);
+				integrateSingleTransform(bodies, nodeID, timeStep, angularDamp, m_data->m_gravity);
 			}
 			npData->m_bodyBufferGPU->copyFromHost(*npData->m_bodyBufferCPU);
 		}
-	} else
+	}
+	else
 	{
-		b3LauncherCL launcher(m_data->m_queue,m_data->m_integrateTransformsKernel,"m_integrateTransformsKernel");
+		b3LauncherCL launcher(m_data->m_queue, m_data->m_integrateTransformsKernel, "m_integrateTransformsKernel");
 		launcher.setBuffer(m_data->m_narrowphase->getBodiesGpu());
-		
+
 		launcher.setConst(numBodies);
 		launcher.setConst(timeStep);
 		launcher.setConst(angularDamp);
@@ -519,12 +497,9 @@ void	b3GpuRigidBodyPipeline::integrate(float timeStep)
 	}
 }
 
-
-
-
-void	b3GpuRigidBodyPipeline::setupGpuAabbsFull()
+void b3GpuRigidBodyPipeline::setupGpuAabbsFull()
 {
-	cl_int ciErrNum=0;
+	cl_int ciErrNum = 0;
 
 	int numBodies = m_data->m_narrowphase->getNumRigidBodies();
 	if (!numBodies)
@@ -532,34 +507,35 @@ void	b3GpuRigidBodyPipeline::setupGpuAabbsFull()
 
 	if (gCalcWorldSpaceAabbOnCpu)
 	{
-		
 		if (numBodies)
 		{
 			if (gUseDbvt)
 			{
 				m_data->m_allAabbsCPU.resize(numBodies);
 				m_data->m_narrowphase->readbackAllBodiesToCpu();
-				for (int i=0;i<numBodies;i++)
+				for (int i = 0; i < numBodies; i++)
 				{
-					b3ComputeWorldAabb(  i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(),&m_data->m_allAabbsCPU[0]);
+					b3ComputeWorldAabb(i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(), &m_data->m_allAabbsCPU[0]);
 				}
 				m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU);
-			} else
+			}
+			else
 			{
 				m_data->m_broadphaseSap->getAllAabbsCPU().resize(numBodies);
 				m_data->m_narrowphase->readbackAllBodiesToCpu();
-				for (int i=0;i<numBodies;i++)
+				for (int i = 0; i < numBodies; i++)
 				{
-					b3ComputeWorldAabb(  i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(),&m_data->m_broadphaseSap->getAllAabbsCPU()[0]);
+					b3ComputeWorldAabb(i, m_data->m_narrowphase->getBodiesCpu(), m_data->m_narrowphase->getCollidablesCpu(), m_data->m_narrowphase->getLocalSpaceAabbsCpu(), &m_data->m_broadphaseSap->getAllAabbsCPU()[0]);
 				}
 				m_data->m_broadphaseSap->getAllAabbsGPU().copyFromHost(m_data->m_broadphaseSap->getAllAabbsCPU());
 				//m_data->m_broadphaseSap->writeAabbsToGpu();
 			}
 		}
-	} else
+	}
+	else
 	{
 		//__kernel void initializeGpuAabbsFull(  const int numNodes, __global Body* gBodies,__global Collidable* collidables, __global b3AABBCL* plocalShapeAABB, __global b3AABBCL* pAABB)
-		b3LauncherCL launcher(m_data->m_queue,m_data->m_updateAabbsKernel,"m_updateAabbsKernel");
+		b3LauncherCL launcher(m_data->m_queue, m_data->m_updateAabbsKernel, "m_updateAabbsKernel");
 		launcher.setConst(numBodies);
 		cl_mem bodies = m_data->m_narrowphase->getBodiesGpu();
 		launcher.setBuffer(bodies);
@@ -568,17 +544,18 @@ void	b3GpuRigidBodyPipeline::setupGpuAabbsFull()
 		cl_mem localAabbs = m_data->m_narrowphase->getAabbLocalSpaceBufferGpu();
 		launcher.setBuffer(localAabbs);
 
-		cl_mem worldAabbs =0;
+		cl_mem worldAabbs = 0;
 		if (gUseDbvt)
 		{
 			worldAabbs = m_data->m_allAabbsGPU->getBufferCL();
-		} else
+		}
+		else
 		{
 			worldAabbs = m_data->m_broadphaseSap->getAabbBufferWS();
 		}
 		launcher.setBuffer(worldAabbs);
 		launcher.launch1D(numBodies);
-	
+
 		oclCHECKERROR(ciErrNum, CL_SUCCESS);
 	}
 
@@ -595,78 +572,68 @@ void	b3GpuRigidBodyPipeline::setupGpuAabbsFull()
 
 	};
 	*/
-
-	
-
-
-
 }
 
-
-
-cl_mem	b3GpuRigidBodyPipeline::getBodyBuffer()
+cl_mem b3GpuRigidBodyPipeline::getBodyBuffer()
 {
 	return m_data->m_narrowphase->getBodiesGpu();
 }
 
-int	b3GpuRigidBodyPipeline::getNumBodies() const
+int b3GpuRigidBodyPipeline::getNumBodies() const
 {
 	return m_data->m_narrowphase->getNumRigidBodies();
 }
 
-void	b3GpuRigidBodyPipeline::setGravity(const float* grav)
+void b3GpuRigidBodyPipeline::setGravity(const float* grav)
 {
-	m_data->m_gravity.setValue(grav[0],grav[1],grav[2]);
+	m_data->m_gravity.setValue(grav[0], grav[1], grav[2]);
 }
 
-void 		b3GpuRigidBodyPipeline::copyConstraintsToHost()
+void b3GpuRigidBodyPipeline::copyConstraintsToHost()
 {
 	m_data->m_gpuConstraints->copyToHost(m_data->m_cpuConstraints);
 }
 
-void 		b3GpuRigidBodyPipeline::writeAllInstancesToGpu()
+void b3GpuRigidBodyPipeline::writeAllInstancesToGpu()
 {
 	m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU);
 	m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints);
 }
 
-
-int		b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex, bool writeInstanceToGpu)
+int b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex, bool writeInstanceToGpu)
 {
-	
-	b3Vector3 aabbMin=b3MakeVector3(0,0,0),aabbMax=b3MakeVector3(0,0,0);
+	b3Vector3 aabbMin = b3MakeVector3(0, 0, 0), aabbMax = b3MakeVector3(0, 0, 0);
 
-	
-	if (collidableIndex>=0)
+	if (collidableIndex >= 0)
 	{
 		b3SapAabb localAabb = m_data->m_narrowphase->getLocalSpaceAabb(collidableIndex);
-		b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
-		b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
-		
+		b3Vector3 localAabbMin = b3MakeVector3(localAabb.m_min[0], localAabb.m_min[1], localAabb.m_min[2]);
+		b3Vector3 localAabbMax = b3MakeVector3(localAabb.m_max[0], localAabb.m_max[1], localAabb.m_max[2]);
+
 		b3Scalar margin = 0.01f;
 		b3Transform t;
 		t.setIdentity();
-		t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
-		t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
-		b3TransformAabb(localAabbMin,localAabbMax, margin,t,aabbMin,aabbMax);
-	} else
+		t.setOrigin(b3MakeVector3(position[0], position[1], position[2]));
+		t.setRotation(b3Quaternion(orientation[0], orientation[1], orientation[2], orientation[3]));
+		b3TransformAabb(localAabbMin, localAabbMax, margin, t, aabbMin, aabbMax);
+	}
+	else
 	{
 		b3Error("registerPhysicsInstance using invalid collidableIndex\n");
 		return -1;
 	}
-			
-	
+
 	bool writeToGpu = false;
 	int bodyIndex = m_data->m_narrowphase->getNumRigidBodies();
-	bodyIndex = m_data->m_narrowphase->registerRigidBody(collidableIndex,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu);
+	bodyIndex = m_data->m_narrowphase->registerRigidBody(collidableIndex, mass, position, orientation, &aabbMin.getX(), &aabbMax.getX(), writeToGpu);
 
-	if (bodyIndex>=0)
+	if (bodyIndex >= 0)
 	{
 		if (gUseDbvt)
 		{
-			m_data->m_broadphaseDbvt->createProxy(aabbMin,aabbMax,bodyIndex,0,1,1);
+			m_data->m_broadphaseDbvt->createProxy(aabbMin, aabbMax, bodyIndex, 0, 1, 1);
 			b3SapAabb aabb;
-			for (int i=0;i<3;i++)
+			for (int i = 0; i < 3; i++)
 			{
 				aabb.m_min[i] = aabbMin[i];
 				aabb.m_max[i] = aabbMax[i];
@@ -677,14 +644,16 @@ int		b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
 			{
 				m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU);
 			}
-		} else
+		}
+		else
 		{
 			if (mass)
 			{
-				m_data->m_broadphaseSap->createProxy(aabbMin,aabbMax,bodyIndex,1,1);//m_dispatcher);
-			} else
+				m_data->m_broadphaseSap->createProxy(aabbMin, aabbMax, bodyIndex, 1, 1);  //m_dispatcher);
+			}
+			else
 			{
-				m_data->m_broadphaseSap->createLargeProxy(aabbMin,aabbMax,bodyIndex,1,1);//m_dispatcher);	
+				m_data->m_broadphaseSap->createLargeProxy(aabbMin, aabbMax, bodyIndex, 1, 1);  //m_dispatcher);
 			}
 		}
 	}
@@ -699,10 +668,10 @@ int		b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* po
 	return bodyIndex;
 }
 
-void	b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3AlignedObjectArray<b3RayHit>& hitResults)
+void b3GpuRigidBodyPipeline::castRays(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& 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(), m_data->m_broadphaseSap);
+	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(), m_data->m_broadphaseSap);
 }