add support for picking, using point 2 point constraint

allow to remove constraints by unique id
added tiny wavefront loader, plan to use this instead of existing slow wavefront loader

src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp

@@ -199,12 +199,12 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays,	b3A
 			hitResults[r].m_hitFraction = hitFraction;
 			hitResults[r].m_hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to,hitFraction);
 			hitResults[r].m_hitNormal = hitNormal;
-			hitResults[r].m_hitResult0 = hitBodyIndex;
+			hitResults[r].m_hitBody = hitBodyIndex;
 		}
 
 	}
 }
-
+///todo: add some acceleration structure (AABBs, tree etc)
 void b3GpuRaycast::castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3AlignedObjectArray<b3RayHit>& hitResults,
 		int numBodies,const struct b3RigidBodyCL* bodies, int numCollidables, const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData)
 {

src/Bullet3OpenCL/Raycast/b3RaycastInfo.h

@@ -13,7 +13,7 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3RayInfo
 B3_ATTRIBUTE_ALIGNED16(struct) b3RayHit
 {
 		b3Scalar	m_hitFraction;
-		int	m_hitResult0;
+		int	m_hitBody;
 		int	m_hitResult1;
 		int	m_hitResult2;
 		b3Vector3 m_hitPoint;

src/Bullet3OpenCL/RigidBody/b3GpuGenericConstraint.h

@@ -87,7 +87,8 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3GpuGenericConstraint
 	b3Quaternion m_relTargetAB;
 
 	int	m_flags;
-	int m_padding[3];
+	int m_uid;
+	int m_padding[2];
 
 	int	getRigidBodyA() const
 	{

src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.cpp

@@ -189,7 +189,10 @@ struct b3BatchConstraint
 static b3AlignedObjectArray<b3BatchConstraint> batchConstraints;
 static b3AlignedObjectArray<int> batches;
 
-
+void	b3GpuPgsJacobiSolver::recomputeBatches()
+{
+	batches.clear();
+}
 
 
 
@@ -695,7 +698,7 @@ b3Scalar b3GpuPgsJacobiSolver::solveGroupCacheFriendlyIterations(b3OpenCLArray<b
 
 				B3_PROFILE("batch joints");
 				b3Assert(batchConstraints.size()==numConstraints);
-				int simdWidth =numConstraints;
+				int simdWidth =numConstraints+1;
 				int numBodies = m_tmpSolverBodyPool.size();
 				sortConstraintByBatch3( &batchConstraints[0], numConstraints, simdWidth , m_staticIdx,  numBodies);
 

src/Bullet3OpenCL/RigidBody/b3GpuPgsJacobiSolver.h

@@ -72,6 +72,7 @@ public:
 				int numConstraints, b3OpenCLArray<b3GpuGenericConstraint>* gpuConstraints);
 
 	int sortConstraintByBatch3( struct b3BatchConstraint* cs, int numConstraints, int simdWidth , int staticIdx, int numBodies);
+	void	recomputeBatches();
 };
 
 #endif //B3_GPU_PGS_JACOBI_SOLVER_H

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp

@@ -56,6 +56,7 @@ bool dumpContactStats = false;
 b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue  q,class b3GpuNarrowPhase* narrowphase, class b3GpuSapBroadphase* broadphaseSap , struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
 {
 	m_data = new b3GpuRigidBodyPipelineInternalData;
+	m_data->m_constraintUid=0;
 	m_data->m_config = config;
 	m_data->m_context = ctx;
 	m_data->m_device = device;
@@ -140,9 +141,41 @@ void	b3GpuRigidBodyPipeline::removeConstraint(b3TypedConstraint* constraint)
 	m_data->m_joints.remove(constraint);
 }
 
+
+
+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++)
+	{
+		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.pop_back();
+
+			break;
+		}
+	}
+
+	if (m_data->m_cpuConstraints.size())
+	{
+		m_data->m_gpuConstraints->copyFromHost(m_data->m_cpuConstraints);
+	} else
+	{
+		m_data->m_gpuConstraints->resize(0);
+	}
+
+}
 int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB)
 {
+	m_data->m_gpuSolver->recomputeBatches();
 	b3GpuGenericConstraint c;
+	c.m_uid = m_data->m_constraintUid;
+	m_data->m_constraintUid++;
 	c.m_flags = B3_CONSTRAINT_FLAG_ENABLED;
 	c.m_rbA = bodyA;
 	c.m_rbB = bodyB;
@@ -151,10 +184,11 @@ int b3GpuRigidBodyPipeline::createPoint2PointConstraint(int bodyA, int bodyB, co
 	c.m_breakingImpulseThreshold = 1e30f;
 	c.m_constraintType = B3_GPU_POINT2POINT_CONSTRAINT_TYPE;
 	m_data->m_cpuConstraints.push_back(c);
-	return 0;
+	return c.m_uid;
 }
 int b3GpuRigidBodyPipeline::createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* frameOrnA, const float* frameOrnB)
 {
+	m_data->m_gpuSolver->recomputeBatches();
 	return 0;
 }
 

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h

@@ -57,6 +57,7 @@ public:
 	
 	int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB);
 	int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* frameOrnA, const float* frameOrnB);
+	void removeConstraintByUid(int uid);
 
 	void	addConstraint(class b3TypedConstraint* constraint);
 	void	removeConstraint(b3TypedConstraint* constraint);

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h

@@ -60,7 +60,7 @@ struct b3GpuRigidBodyPipelineInternalData
 	b3AlignedObjectArray<b3GpuGenericConstraint> m_cpuConstraints;
 
 	b3AlignedObjectArray<b3TypedConstraint*> m_joints;
-
+	int	m_constraintUid;
 	class b3GpuNarrowPhase*	m_narrowphase;
 	b3Vector3	m_gravity;