reorder files, in preparation for Bullet 3 -> Bullet 2 merge

src/Bullet3OpenCL/RigidBody/b3Config.h

@@ -0,0 +1,40 @@
+#ifndef B3_CONFIG_H
+#define B3_CONFIG_H
+
+struct	b3Config
+{
+	int	m_maxConvexBodies;
+	int	m_maxConvexShapes;
+	int	m_maxBroadphasePairs;
+	int m_maxContactCapacity;
+
+	int m_maxVerticesPerFace;
+	int m_maxFacesPerShape;
+	int	m_maxConvexVertices;
+	int m_maxConvexIndices;
+	int m_maxConvexUniqueEdges;
+	
+	int	m_maxCompoundChildShapes;
+	
+	int m_maxTriConvexPairCapacity;
+
+	b3Config()
+		:m_maxConvexBodies(32*1024),
+		m_maxConvexShapes(8192),
+		m_maxVerticesPerFace(64),
+		m_maxFacesPerShape(64),
+		m_maxConvexVertices(8192),
+		m_maxConvexIndices(8192),
+		m_maxConvexUniqueEdges(8192),
+		m_maxCompoundChildShapes(8192),
+		//m_maxTriConvexPairCapacity(512*1024)
+		m_maxTriConvexPairCapacity(256*1024)
+	{
+		m_maxBroadphasePairs = 16*m_maxConvexBodies;
+		m_maxContactCapacity = m_maxBroadphasePairs;
+	}
+};
+
+
+#endif//B3_CONFIG_H
+

src/Bullet3OpenCL/RigidBody/b3GpuBatchingPgsSolver.h

@@ -0,0 +1,40 @@
+
+#ifndef B3_GPU_BATCHING_PGS_SOLVER_H
+#define B3_GPU_BATCHING_PGS_SOLVER_H
+
+#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
+#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
+#include "b3GpuConstraint4.h"
+
+class b3GpuBatchingPgsSolver
+{
+protected:
+
+	
+
+	struct b3GpuBatchingPgsSolverInternalData*		m_data;
+
+	void batchContacts( b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* n, b3OpenCLArray<unsigned int>* offsets, int staticIdx );
+	
+	inline int sortConstraintByBatch( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies);
+	inline int sortConstraintByBatch2( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies);
+	inline int sortConstraintByBatch3( b3Contact4* cs, int n, int simdWidth , int staticIdx, int numBodies);
+	
+
+
+	void solveContactConstraint(  const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, const b3OpenCLArray<b3InertiaCL>* shapeBuf, 
+			b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches, int numIterations);
+
+public:
+	
+	b3GpuBatchingPgsSolver(cl_context ctx,cl_device_id device, cl_command_queue  q,int pairCapacity);
+	virtual ~b3GpuBatchingPgsSolver();
+
+	void solveContacts(int numBodies, cl_mem bodyBuf, cl_mem inertiaBuf, int numContacts, cl_mem contactBuf, const struct b3Config& config);
+
+};
+
+#endif //B3_GPU_BATCHING_PGS_SOLVER_H
+

src/Bullet3OpenCL/RigidBody/b3GpuConstraint4.h

@@ -0,0 +1,29 @@
+
+#ifndef B3_CONSTRAINT4_h
+#define B3_CONSTRAINT4_h
+#include "Bullet3Common/b3Vector3.h"
+
+B3_ATTRIBUTE_ALIGNED16(struct) b3GpuConstraint4
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	b3Vector3 m_linear;//normal?
+	b3Vector3 m_worldPos[4];
+	b3Vector3 m_center;	//	friction
+	float m_jacCoeffInv[4];
+	float m_b[4];
+	float m_appliedRambdaDt[4];
+	float m_fJacCoeffInv[2];	//	friction
+	float m_fAppliedRambdaDt[2];	//	friction
+
+	unsigned int m_bodyA;
+	unsigned int m_bodyB;
+	int m_batchIdx;
+	unsigned int m_paddings;
+
+	inline	void setFrictionCoeff(float value) { m_linear[3] = value; }
+	inline	float getFrictionCoeff() const { return m_linear[3]; }
+};
+
+#endif //B3_CONSTRAINT4_h
+

src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp

@@ -0,0 +1,901 @@
+#include "b3GpuNarrowPhase.h"
+
+
+#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
+#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h"
+#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h"
+#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h"
+#include <string.h>
+#include "b3Config.h"
+#include "Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h"
+#include "Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h"
+#include "Bullet3Geometry/b3AabbUtil.h"
+#include "Bullet3OpenCL/NarrowphaseCollision/b3BvhInfo.h"
+
+struct b3GpuNarrowPhaseInternalData
+{
+	b3AlignedObjectArray<b3ConvexUtility*>* m_convexData;
+    
+	b3AlignedObjectArray<b3ConvexPolyhedronCL> m_convexPolyhedra;
+	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
+	b3AlignedObjectArray<b3Vector3> m_convexVertices;
+	b3AlignedObjectArray<int> m_convexIndices;
+    
+	b3OpenCLArray<b3ConvexPolyhedronCL>* m_convexPolyhedraGPU;
+	b3OpenCLArray<b3Vector3>* m_uniqueEdgesGPU;
+	b3OpenCLArray<b3Vector3>* m_convexVerticesGPU;
+	b3OpenCLArray<int>* m_convexIndicesGPU;
+    
+    b3OpenCLArray<b3Vector3>* m_worldVertsB1GPU;
+    b3OpenCLArray<b3Int4>* m_clippingFacesOutGPU;
+    b3OpenCLArray<b3Vector3>* m_worldNormalsAGPU;
+    b3OpenCLArray<b3Vector3>* m_worldVertsA1GPU;
+    b3OpenCLArray<b3Vector3>* m_worldVertsB2GPU;
+    
+	b3AlignedObjectArray<b3GpuChildShape> m_cpuChildShapes;
+	b3OpenCLArray<b3GpuChildShape>*	m_gpuChildShapes;
+    
+	b3AlignedObjectArray<b3GpuFace> m_convexFaces;
+	b3OpenCLArray<b3GpuFace>* m_convexFacesGPU;
+    
+	GpuSatCollision*	m_gpuSatCollision;
+	    
+	b3AlignedObjectArray<b3Int2>* m_pBufPairsCPU;
+    
+	b3OpenCLArray<b3Int2>* m_convexPairsOutGPU;
+	b3OpenCLArray<b3Int2>* m_planePairs;
+    
+	b3OpenCLArray<b3Contact4>* m_pBufContactOutGPU;
+	b3AlignedObjectArray<b3Contact4>* m_pBufContactOutCPU;
+	
+    
+	b3AlignedObjectArray<b3RigidBodyCL>* m_bodyBufferCPU;
+	b3OpenCLArray<b3RigidBodyCL>* m_bodyBufferGPU;
+    
+	b3AlignedObjectArray<b3InertiaCL>*	m_inertiaBufferCPU;
+	b3OpenCLArray<b3InertiaCL>*	m_inertiaBufferGPU;
+    
+	int m_numAcceleratedShapes;
+	int m_numAcceleratedRigidBodies;
+    
+	b3AlignedObjectArray<b3Collidable>	m_collidablesCPU;
+	b3OpenCLArray<b3Collidable>*	m_collidablesGPU;
+
+	b3OpenCLArray<b3SapAabb>* m_localShapeAABBGPU;
+	b3AlignedObjectArray<b3SapAabb>* m_localShapeAABBCPU;
+
+	b3AlignedObjectArray<class b3OptimizedBvh*> m_bvhData;
+
+	b3AlignedObjectArray<b3QuantizedBvhNode>	m_treeNodesCPU;
+	b3AlignedObjectArray<b3BvhSubtreeInfo>	m_subTreesCPU;
+
+	b3AlignedObjectArray<b3BvhInfo>	m_bvhInfoCPU;
+	b3OpenCLArray<b3BvhInfo>*			m_bvhInfoGPU;
+	
+	b3OpenCLArray<b3QuantizedBvhNode>*	m_treeNodesGPU;
+	b3OpenCLArray<b3BvhSubtreeInfo>*	m_subTreesGPU;
+	
+
+	b3Config	m_config;
+    
+};
+
+
+
+
+
+b3GpuNarrowPhase::b3GpuNarrowPhase(cl_context ctx, cl_device_id device, cl_command_queue queue, const b3Config& config)
+:m_data(0) ,m_planeBodyIndex(-1),m_static0Index(-1),
+m_context(ctx),
+m_device(device),
+m_queue(queue)
+{
+    
+	m_data = new b3GpuNarrowPhaseInternalData();
+	memset(m_data,0,sizeof(b3GpuNarrowPhaseInternalData));
+    
+	m_data->m_config = config;
+	
+	m_data->m_gpuSatCollision = new GpuSatCollision(ctx,device,queue);
+	m_data->m_pBufPairsCPU = new b3AlignedObjectArray<b3Int2>;
+	m_data->m_pBufPairsCPU->resize(config.m_maxBroadphasePairs);
+	
+	m_data->m_convexPairsOutGPU = new b3OpenCLArray<b3Int2>(ctx,queue,config.m_maxBroadphasePairs,false);
+	m_data->m_planePairs = new b3OpenCLArray<b3Int2>(ctx,queue,config.m_maxBroadphasePairs,false);
+    
+	m_data->m_pBufContactOutCPU = new b3AlignedObjectArray<b3Contact4>();
+	m_data->m_pBufContactOutCPU->resize(config.m_maxBroadphasePairs);
+	m_data->m_bodyBufferCPU = new b3AlignedObjectArray<b3RigidBodyCL>();
+	m_data->m_bodyBufferCPU->resize(config.m_maxConvexBodies);
+    
+	m_data->m_inertiaBufferCPU = new b3AlignedObjectArray<b3InertiaCL>();
+	m_data->m_inertiaBufferCPU->resize(config.m_maxConvexBodies);
+	
+	m_data->m_pBufContactOutGPU = new b3OpenCLArray<b3Contact4>(ctx,queue, config.m_maxContactCapacity,true);
+	
+	m_data->m_inertiaBufferGPU = new b3OpenCLArray<b3InertiaCL>(ctx,queue,config.m_maxConvexBodies,false);
+	m_data->m_collidablesGPU = new b3OpenCLArray<b3Collidable>(ctx,queue,config.m_maxConvexShapes);
+
+	m_data->m_localShapeAABBCPU = new b3AlignedObjectArray<b3SapAabb>;
+	m_data->m_localShapeAABBGPU = new b3OpenCLArray<b3SapAabb>(ctx,queue,config.m_maxConvexShapes);
+    
+    
+	//m_data->m_solverDataGPU = adl::Solver<adl::TYPE_CL>::allocate(ctx,queue, config.m_maxBroadphasePairs,false);
+	m_data->m_bodyBufferGPU = new b3OpenCLArray<b3RigidBodyCL>(ctx,queue, config.m_maxConvexBodies,false);
+
+	m_data->m_convexFacesGPU = new b3OpenCLArray<b3GpuFace>(ctx,queue,config.m_maxConvexShapes*config.m_maxFacesPerShape,false);
+	m_data->m_gpuChildShapes = new b3OpenCLArray<b3GpuChildShape>(ctx,queue,config.m_maxCompoundChildShapes,false);
+	
+	m_data->m_convexPolyhedraGPU = new b3OpenCLArray<b3ConvexPolyhedronCL>(ctx,queue,config.m_maxConvexShapes,false);
+	m_data->m_uniqueEdgesGPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexUniqueEdges,true);
+	m_data->m_convexVerticesGPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexVertices,true);
+	m_data->m_convexIndicesGPU = new b3OpenCLArray<int>(ctx,queue,config.m_maxConvexIndices,true);
+    
+    
+	m_data->m_worldVertsB1GPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace);
+    m_data->m_clippingFacesOutGPU = new  b3OpenCLArray<b3Int4>(ctx,queue,config.m_maxConvexBodies);
+    m_data->m_worldNormalsAGPU = new  b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies);
+	m_data->m_worldVertsA1GPU = new b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace);
+    m_data->m_worldVertsB2GPU = new  b3OpenCLArray<b3Vector3>(ctx,queue,config.m_maxConvexBodies*config.m_maxVerticesPerFace);
+    
+    
+
+	m_data->m_convexData = new b3AlignedObjectArray<b3ConvexUtility* >();
+    
+
+	m_data->m_convexData->resize(config.m_maxConvexShapes);
+	m_data->m_convexPolyhedra.resize(config.m_maxConvexShapes);
+    
+	m_data->m_numAcceleratedShapes = 0;
+	m_data->m_numAcceleratedRigidBodies = 0;
+    
+		
+	m_data->m_subTreesGPU = new b3OpenCLArray<b3BvhSubtreeInfo>(this->m_context,this->m_queue);
+	m_data->m_treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context,this->m_queue);
+	m_data->m_bvhInfoGPU = new b3OpenCLArray<b3BvhInfo>(this->m_context,this->m_queue);
+
+	//m_data->m_contactCGPU = new b3OpenCLArray<Constraint4>(ctx,queue,config.m_maxBroadphasePairs,false);
+	//m_data->m_frictionCGPU = new b3OpenCLArray<adl::Solver<adl::TYPE_CL>::allocateFrictionConstraint( m_data->m_deviceCL, config.m_maxBroadphasePairs);
+    
+}
+
+
+b3GpuNarrowPhase::~b3GpuNarrowPhase()
+{
+	delete m_data->m_gpuSatCollision;
+	delete m_data->m_pBufPairsCPU;
+	delete m_data->m_convexPairsOutGPU;
+	delete m_data->m_planePairs;
+	delete m_data->m_pBufContactOutCPU;
+	delete m_data->m_bodyBufferCPU;
+	delete m_data->m_inertiaBufferCPU;
+	delete m_data->m_pBufContactOutGPU;
+	delete m_data->m_inertiaBufferGPU;
+	delete m_data->m_collidablesGPU;
+	delete m_data->m_localShapeAABBCPU;
+	delete m_data->m_localShapeAABBGPU;
+	delete m_data->m_bodyBufferGPU;
+	delete m_data->m_convexFacesGPU;
+	delete m_data->m_gpuChildShapes;
+	delete m_data->m_convexPolyhedraGPU;
+	delete m_data->m_uniqueEdgesGPU;
+	delete m_data->m_convexVerticesGPU;
+	delete m_data->m_convexIndicesGPU;
+	delete m_data->m_worldVertsB1GPU;
+    delete m_data->m_clippingFacesOutGPU;
+    delete m_data->m_worldNormalsAGPU;
+	delete m_data->m_worldVertsA1GPU;
+    delete m_data->m_worldVertsB2GPU;
+    
+	delete m_data->m_bvhInfoGPU;
+
+	delete m_data->m_treeNodesGPU;
+	delete m_data->m_subTreesGPU;
+
+    
+    delete m_data->m_convexData;
+	delete m_data;
+}
+
+
+int	b3GpuNarrowPhase::allocateCollidable()
+{
+	int curSize = m_data->m_collidablesCPU.size();
+	m_data->m_collidablesCPU.expand();
+	return curSize;
+}
+
+
+
+
+
+int		b3GpuNarrowPhase::registerSphereShape(float radius)
+{
+	int collidableIndex = allocateCollidable();
+
+	b3Collidable& col = getCollidableCpu(collidableIndex);
+	col.m_shapeType = SHAPE_SPHERE;
+	col.m_shapeIndex = 0;
+	col.m_radius = radius;
+	
+	if (col.m_shapeIndex>=0)
+	{
+		b3SapAabb aabb;
+		b3Vector3 myAabbMin(-radius,-radius,-radius);
+		b3Vector3 myAabbMax(radius,radius,radius);
+
+		aabb.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x;
+		aabb.m_min[1] = myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y;
+		aabb.m_min[2] = myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z;
+		aabb.m_minIndices[3] = 0;
+
+		aabb.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x;
+		aabb.m_max[1] = myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y;
+		aabb.m_max[2] = myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z;
+		aabb.m_signedMaxIndices[3] = 0;
+
+		m_data->m_localShapeAABBCPU->push_back(aabb);
+		m_data->m_localShapeAABBGPU->push_back(aabb);
+		clFinish(m_queue);
+	}
+	
+	return collidableIndex;
+}
+
+
+int b3GpuNarrowPhase::registerFace(const b3Vector3& faceNormal, float faceConstant)
+{
+	int faceOffset = m_data->m_convexFaces.size();
+	b3GpuFace& face = m_data->m_convexFaces.expand();
+	face.m_plane[0] = faceNormal.getX();
+	face.m_plane[1] = faceNormal.getY();
+	face.m_plane[2] = faceNormal.getZ();
+	face.m_plane[3] = faceConstant;
+	m_data->m_convexFacesGPU->copyFromHost(m_data->m_convexFaces);
+	return faceOffset;
+}
+
+int		b3GpuNarrowPhase::registerPlaneShape(const b3Vector3& planeNormal, float planeConstant)
+{
+	int collidableIndex = allocateCollidable();
+
+	b3Collidable& col = getCollidableCpu(collidableIndex);
+	col.m_shapeType = SHAPE_PLANE;
+	col.m_shapeIndex = registerFace(planeNormal,planeConstant);
+	col.m_radius = planeConstant;
+	
+	if (col.m_shapeIndex>=0)
+	{
+		b3SapAabb aabb;
+		aabb.m_min[0] = -1e30f;
+		aabb.m_min[1] = -1e30f;
+		aabb.m_min[2] = -1e30f;
+		aabb.m_minIndices[3] = 0;
+		
+		aabb.m_max[0] = 1e30f;
+		aabb.m_max[1] = 1e30f;
+		aabb.m_max[2] = 1e30f;
+		aabb.m_signedMaxIndices[3] = 0;
+
+		m_data->m_localShapeAABBCPU->push_back(aabb);
+		m_data->m_localShapeAABBGPU->push_back(aabb);
+		clFinish(m_queue);
+	}
+	
+	return collidableIndex;
+}
+
+
+int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* convexPtr,b3Collidable& col)
+{
+	m_data->m_convexData->resize(m_data->m_numAcceleratedShapes+1);
+	m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1);
+	
+    
+	b3ConvexPolyhedronCL& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1);
+	convex.mC = convexPtr->mC;
+	convex.mE = convexPtr->mE;
+	convex.m_extents= convexPtr->m_extents;
+	convex.m_localCenter = convexPtr->m_localCenter;
+	convex.m_radius = convexPtr->m_radius;
+	
+	convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
+	int edgeOffset = m_data->m_uniqueEdges.size();
+	convex.m_uniqueEdgesOffset = edgeOffset;
+	
+	m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges);
+    
+	//convex data here
+	int i;
+	for ( i=0;i<convexPtr->m_uniqueEdges.size();i++)
+	{
+		m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i];
+	}
+    
+	int faceOffset = m_data->m_convexFaces.size();
+	convex.m_faceOffset = faceOffset;
+	convex.m_numFaces = convexPtr->m_faces.size();
+	m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces);
+	for (i=0;i<convexPtr->m_faces.size();i++)
+	{
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[0] = convexPtr->m_faces[i].m_plane[0];
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[1] = convexPtr->m_faces[i].m_plane[1];
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[2] = convexPtr->m_faces[i].m_plane[2];
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[3] = convexPtr->m_faces[i].m_plane[3];
+		int indexOffset = m_data->m_convexIndices.size();
+		int numIndices = convexPtr->m_faces[i].m_indices.size();
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices;
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset;
+		m_data->m_convexIndices.resize(indexOffset+numIndices);
+		for (int p=0;p<numIndices;p++)
+		{
+			m_data->m_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p];
+		}
+	}
+    
+	convex.m_numVertices = convexPtr->m_vertices.size();
+	int vertexOffset = m_data->m_convexVertices.size();
+	convex.m_vertexOffset =vertexOffset;
+	m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices);
+	for (int i=0;i<convexPtr->m_vertices.size();i++)
+	{
+		m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i];
+	}
+
+	(*m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
+	
+	m_data->m_convexFacesGPU->copyFromHost(m_data->m_convexFaces);
+    
+	m_data->m_convexPolyhedraGPU->copyFromHost(m_data->m_convexPolyhedra);
+	m_data->m_uniqueEdgesGPU->copyFromHost(m_data->m_uniqueEdges);
+	m_data->m_convexVerticesGPU->copyFromHost(m_data->m_convexVertices);
+	m_data->m_convexIndicesGPU->copyFromHost(m_data->m_convexIndices);
+    
+    
+	return m_data->m_numAcceleratedShapes++;
+}
+
+
+int		b3GpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
+{
+	b3AlignedObjectArray<b3Vector3> verts;
+	
+	unsigned char* vts = (unsigned char*) vertices;
+	for (int i=0;i<numVertices;i++)
+	{
+		float* vertex = (float*) &vts[i*strideInBytes];
+		verts.push_back(b3Vector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2]));
+	}
+
+	b3ConvexUtility* utilPtr = new b3ConvexUtility();
+	bool merge = true;
+	if (numVertices)
+	{
+		utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge);
+	}
+
+	int collidableIndex = registerConvexHullShape(utilPtr);
+	return collidableIndex;
+}
+
+int		b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
+{
+	int collidableIndex = allocateCollidable();
+	b3Collidable& col = getCollidableCpu(collidableIndex);
+	col.m_shapeType = SHAPE_CONVEX_HULL;
+	col.m_shapeIndex = -1;
+	
+	
+	{
+		b3Vector3 localCenter(0,0,0);
+		for (int i=0;i<utilPtr->m_vertices.size();i++)
+			localCenter+=utilPtr->m_vertices[i];
+		localCenter*= (1.f/utilPtr->m_vertices.size());
+		utilPtr->m_localCenter = localCenter;
+
+		col.m_shapeIndex = registerConvexHullShape(utilPtr,col);
+	}
+
+	if (col.m_shapeIndex>=0)
+	{
+		b3SapAabb aabb;
+		
+		b3Vector3 myAabbMin(1e30f,1e30f,1e30f);
+		b3Vector3 myAabbMax(-1e30f,-1e30f,-1e30f);
+
+		for (int i=0;i<utilPtr->m_vertices.size();i++)
+		{
+			myAabbMin.setMin(utilPtr->m_vertices[i]);
+			myAabbMax.setMax(utilPtr->m_vertices[i]);
+		}
+		aabb.m_min[0] = myAabbMin[0];
+		aabb.m_min[1] = myAabbMin[1];
+		aabb.m_min[2] = myAabbMin[2];
+		aabb.m_minIndices[3] = 0;
+
+		aabb.m_max[0] = myAabbMax[0];
+		aabb.m_max[1] = myAabbMax[1];
+		aabb.m_max[2] = myAabbMax[2];
+		aabb.m_signedMaxIndices[3] = 0;
+
+		m_data->m_localShapeAABBCPU->push_back(aabb);
+		m_data->m_localShapeAABBGPU->push_back(aabb);
+	}
+	
+	return collidableIndex;
+
+}
+
+int		b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes)
+{
+	
+	int collidableIndex = allocateCollidable();
+	b3Collidable& col = getCollidableCpu(collidableIndex);
+	col.m_shapeType = SHAPE_COMPOUND_OF_CONVEX_HULLS;
+	
+	col.m_shapeIndex = m_data->m_cpuChildShapes.size();
+	{
+		b3Assert(col.m_shapeIndex+childShapes->size()<m_data->m_config.m_maxCompoundChildShapes);
+		for (int i=0;i<childShapes->size();i++)
+		{
+			m_data->m_cpuChildShapes.push_back(childShapes->at(i));
+		}
+		//if writing the data directly is too slow, we can delay it and do it all at once in
+		m_data->m_gpuChildShapes->copyFromHost(m_data->m_cpuChildShapes);
+	}
+
+
+
+	col.m_numChildShapes = childShapes->size();
+	
+	
+	b3SapAabb aabbWS;
+	b3Vector3 myAabbMin(1e30f,1e30f,1e30f);
+	b3Vector3 myAabbMax(-1e30f,-1e30f,-1e30f);
+	
+	//compute local AABB of the compound of all children
+	for (int i=0;i<childShapes->size();i++)
+	{
+		int childColIndex = childShapes->at(i).m_shapeIndex;
+		b3Collidable& childCol = getCollidableCpu(childColIndex);
+		b3SapAabb aabbLoc =m_data->m_localShapeAABBCPU->at(childColIndex);
+
+		b3Vector3 childLocalAabbMin(aabbLoc.m_min[0],aabbLoc.m_min[1],aabbLoc.m_min[2]);
+		b3Vector3 childLocalAabbMax(aabbLoc.m_max[0],aabbLoc.m_max[1],aabbLoc.m_max[2]);
+		b3Vector3 aMin,aMax;
+		b3Scalar margin(0.f);
+		b3Transform childTr;
+		childTr.setIdentity();
+
+		childTr.setOrigin(b3Vector3(childShapes->at(i).m_childPosition[0],
+									childShapes->at(i).m_childPosition[1],
+									childShapes->at(i).m_childPosition[2]));
+		childTr.setRotation(b3Quaternion(childShapes->at(i).m_childOrientation[0],
+										 childShapes->at(i).m_childOrientation[1],
+										 childShapes->at(i).m_childOrientation[2],
+										 childShapes->at(i).m_childOrientation[3]));
+		b3TransformAabb(childLocalAabbMin,childLocalAabbMax,margin,childTr,aMin,aMax);
+		myAabbMin.setMin(aMin);
+		myAabbMax.setMax(aMax);		
+	}
+	
+	aabbWS.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x;
+	aabbWS.m_min[1]= myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y;
+	aabbWS.m_min[2]= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z;
+	aabbWS.m_minIndices[3] = 0;
+	
+	aabbWS.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x;
+	aabbWS.m_max[1]= myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y;
+	aabbWS.m_max[2]= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z;
+	aabbWS.m_signedMaxIndices[3] = 0;
+	
+	m_data->m_localShapeAABBCPU->push_back(aabbWS);
+	m_data->m_localShapeAABBGPU->push_back(aabbWS);
+	clFinish(m_queue);
+	return collidableIndex;
+	
+}
+
+
+int		b3GpuNarrowPhase::registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling1)
+{
+	
+
+	b3Vector3 scaling(scaling1[0],scaling1[1],scaling1[2]);
+
+	int collidableIndex = allocateCollidable();
+	b3Collidable& col = getCollidableCpu(collidableIndex);
+	
+	col.m_shapeType = SHAPE_CONCAVE_TRIMESH;
+	col.m_shapeIndex = registerConcaveMeshShape(vertices,indices,col,scaling);
+	col.m_bvhIndex = m_data->m_bvhInfoCPU.size();
+		
+
+	b3SapAabb aabb;
+	b3Vector3 myAabbMin(1e30f,1e30f,1e30f);
+	b3Vector3 myAabbMax(-1e30f,-1e30f,-1e30f);
+
+	for (int i=0;i<vertices->size();i++)
+	{
+		b3Vector3 vtx(vertices->at(i)*scaling);
+		myAabbMin.setMin(vtx);
+		myAabbMax.setMax(vtx);
+	}
+	aabb.m_min[0] = myAabbMin[0];
+	aabb.m_min[1] = myAabbMin[1];
+	aabb.m_min[2] = myAabbMin[2];
+	aabb.m_minIndices[3] = 0;
+
+	aabb.m_max[0] = myAabbMax[0];
+	aabb.m_max[1]= myAabbMax[1];
+	aabb.m_max[2]= myAabbMax[2];
+	aabb.m_signedMaxIndices[3]= 0;
+
+	m_data->m_localShapeAABBCPU->push_back(aabb);
+	m_data->m_localShapeAABBGPU->push_back(aabb);
+
+	b3OptimizedBvh* bvh = new b3OptimizedBvh();
+	//void b3OptimizedBvh::build(b3StridingMeshInterface* triangles, bool useQuantizedAabbCompression, const b3Vector3& bvhAabbMin, const b3Vector3& bvhAabbMax)
+	
+	bool useQuantizedAabbCompression = true;
+	b3TriangleIndexVertexArray* meshInterface=new b3TriangleIndexVertexArray();
+	b3IndexedMesh mesh;
+	mesh.m_numTriangles = indices->size()/3;
+	mesh.m_numVertices = vertices->size();
+	mesh.m_vertexBase = (const unsigned char *)&vertices->at(0).getX();
+	mesh.m_vertexStride = sizeof(b3Vector3);
+	mesh.m_triangleIndexStride = 3 * sizeof(int);// or sizeof(int)
+	mesh.m_triangleIndexBase = (const unsigned char *)&indices->at(0);
+	
+	meshInterface->addIndexedMesh(mesh);
+	bvh->build(meshInterface, useQuantizedAabbCompression, (b3Vector3&)aabb.m_min, (b3Vector3&)aabb.m_max);
+	m_data->m_bvhData.push_back(bvh);
+	int numNodes = bvh->getQuantizedNodeArray().size();
+	//b3OpenCLArray<b3QuantizedBvhNode>*	treeNodesGPU = new b3OpenCLArray<b3QuantizedBvhNode>(this->m_context,this->m_queue,numNodes);
+	//treeNodesGPU->copyFromHost(bvh->getQuantizedNodeArray());
+	int numSubTrees = bvh->getSubtreeInfoArray().size();
+
+	b3BvhInfo bvhInfo;
+	
+	bvhInfo.m_aabbMin = bvh->m_bvhAabbMin;
+	bvhInfo.m_aabbMax = bvh->m_bvhAabbMax;
+	bvhInfo.m_quantization = bvh->m_bvhQuantization;
+	bvhInfo.m_numNodes = numNodes;
+	bvhInfo.m_numSubTrees = numSubTrees;
+	bvhInfo.m_nodeOffset = m_data->m_treeNodesCPU.size();
+	bvhInfo.m_subTreeOffset = m_data->m_subTreesCPU.size();
+
+	m_data->m_bvhInfoCPU.push_back(bvhInfo);
+	m_data->m_bvhInfoGPU->copyFromHost(m_data->m_bvhInfoCPU);
+
+
+	int numNewSubtrees = bvh->getSubtreeInfoArray().size();
+	m_data->m_subTreesCPU.reserve(m_data->m_subTreesCPU.size()+numNewSubtrees);
+	for (int i=0;i<numNewSubtrees;i++)
+	{
+		m_data->m_subTreesCPU.push_back(bvh->getSubtreeInfoArray()[i]);
+	}
+	int numNewTreeNodes = bvh->getQuantizedNodeArray().size();
+
+	for (int i=0;i<numNewTreeNodes;i++)
+	{
+		m_data->m_treeNodesCPU.push_back(bvh->getQuantizedNodeArray()[i]);
+	}
+
+	//b3OpenCLArray<b3BvhSubtreeInfo>* subTreesGPU = new b3OpenCLArray<b3BvhSubtreeInfo>(this->m_context,this->m_queue,numSubTrees);
+	//subTreesGPU->copyFromHost(bvh->getSubtreeInfoArray());
+
+	m_data->m_treeNodesGPU->copyFromHost(m_data->m_treeNodesCPU);
+	m_data->m_subTreesGPU->copyFromHost(m_data->m_subTreesCPU);
+
+
+	return collidableIndex;
+}
+
+int b3GpuNarrowPhase::registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,b3Collidable& col, const float* scaling1)
+{
+
+
+	b3Vector3 scaling(scaling1[0],scaling1[1],scaling1[2]);
+
+	m_data->m_convexData->resize(m_data->m_numAcceleratedShapes+1);
+	m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1);
+	
+    
+	b3ConvexPolyhedronCL& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1);
+	convex.mC = b3Vector3(0,0,0);
+	convex.mE = b3Vector3(0,0,0);
+	convex.m_extents= b3Vector3(0,0,0);
+	convex.m_localCenter = b3Vector3(0,0,0);
+	convex.m_radius = 0.f;
+	
+	convex.m_numUniqueEdges = 0;
+	int edgeOffset = m_data->m_uniqueEdges.size();
+	convex.m_uniqueEdgesOffset = edgeOffset;
+	
+	int faceOffset = m_data->m_convexFaces.size();
+	convex.m_faceOffset = faceOffset;
+	
+	convex.m_numFaces = indices->size()/3;
+	m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces);
+	m_data->m_convexIndices.reserve(convex.m_numFaces*3);
+	for (int i=0;i<convex.m_numFaces;i++)
+	{
+		if (i%256==0)
+		{
+			//printf("i=%d out of %d", i,convex.m_numFaces);
+		}
+		b3Vector3 vert0(vertices->at(indices->at(i*3))*scaling);
+		b3Vector3 vert1(vertices->at(indices->at(i*3+1))*scaling);
+		b3Vector3 vert2(vertices->at(indices->at(i*3+2))*scaling);
+
+		b3Vector3 normal = ((vert1-vert0).cross(vert2-vert0)).normalize();
+		b3Scalar c = -(normal.dot(vert0));
+
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[0] = normal.getX();
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[1] = normal.getY();
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[2] = normal.getZ();
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane[3] = c;
+		int indexOffset = m_data->m_convexIndices.size();
+		int numIndices = 3;
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices;
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset;
+		m_data->m_convexIndices.resize(indexOffset+numIndices);
+		for (int p=0;p<numIndices;p++)
+		{
+			int vi = indices->at(i*3+p);
+			m_data->m_convexIndices[indexOffset+p] = vi;//convexPtr->m_faces[i].m_indices[p];
+		}
+	}
+    
+	convex.m_numVertices = vertices->size();
+	int vertexOffset = m_data->m_convexVertices.size();
+	convex.m_vertexOffset =vertexOffset;
+	m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices);
+	for (int i=0;i<vertices->size();i++)
+	{
+		m_data->m_convexVertices[vertexOffset+i] = vertices->at(i)*scaling;
+	}
+
+	(*m_data->m_convexData)[m_data->m_numAcceleratedShapes] = 0;
+	
+	m_data->m_convexFacesGPU->copyFromHost(m_data->m_convexFaces);
+    
+	m_data->m_convexPolyhedraGPU->copyFromHost(m_data->m_convexPolyhedra);
+	m_data->m_uniqueEdgesGPU->copyFromHost(m_data->m_uniqueEdges);
+	m_data->m_convexVerticesGPU->copyFromHost(m_data->m_convexVertices);
+	m_data->m_convexIndicesGPU->copyFromHost(m_data->m_convexIndices);
+  
+	return m_data->m_numAcceleratedShapes++;
+}
+
+
+
+cl_mem	b3GpuNarrowPhase::getBodiesGpu()
+{
+	return (cl_mem)m_data->m_bodyBufferGPU->getBufferCL();
+}
+
+
+int	b3GpuNarrowPhase::getNumBodiesGpu() const
+{
+	return m_data->m_bodyBufferGPU->size();
+}
+
+cl_mem	b3GpuNarrowPhase::getBodyInertiasGpu()
+{
+	return (cl_mem)m_data->m_inertiaBufferGPU->getBufferCL();
+}
+
+int	b3GpuNarrowPhase::getNumBodyInertiasGpu() const
+{
+	return m_data->m_inertiaBufferGPU->size();
+}
+
+
+b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex)
+{
+	return m_data->m_collidablesCPU[collidableIndex];
+}
+
+const b3Collidable& b3GpuNarrowPhase::getCollidableCpu(int collidableIndex) const
+{
+	return m_data->m_collidablesCPU[collidableIndex];
+}
+
+cl_mem b3GpuNarrowPhase::getCollidablesGpu()
+{
+	return m_data->m_collidablesGPU->getBufferCL();
+}
+
+cl_mem	b3GpuNarrowPhase::getAabbBufferGpu()
+{
+	return m_data->m_localShapeAABBGPU->getBufferCL();
+}
+int	b3GpuNarrowPhase::getNumCollidablesGpu() const
+{
+	return m_data->m_collidablesGPU->size();
+}
+
+
+
+
+
+int	b3GpuNarrowPhase::getNumContactsGpu() const
+{
+	return m_data->m_pBufContactOutGPU->size();
+}
+cl_mem b3GpuNarrowPhase::getContactsGpu()
+{
+	return m_data->m_pBufContactOutGPU->getBufferCL();
+}
+
+const b3Contact4* b3GpuNarrowPhase::getContactsCPU() const
+{
+	m_data->m_pBufContactOutGPU->copyToHost(*m_data->m_pBufContactOutCPU);
+	return &m_data->m_pBufContactOutCPU->at(0);
+}
+
+void b3GpuNarrowPhase::computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWS, int numObjects)
+{
+	int nContactOut = 0;
+
+	int maxTriConvexPairCapacity = m_data->m_config.m_maxTriConvexPairCapacity;
+	b3OpenCLArray<b3Int4> triangleConvexPairs(m_context,m_queue, maxTriConvexPairCapacity);
+	int numTriConvexPairsOut=0;
+	
+	b3OpenCLArray<b3Int2> broadphasePairsGPU(m_context,m_queue);
+	broadphasePairsGPU.setFromOpenCLBuffer(broadphasePairs,numBroadphasePairs);
+	b3OpenCLArray<b3YetAnotherAabb> clAabbArray(this->m_context,this->m_queue);
+	clAabbArray.setFromOpenCLBuffer(aabbsWS,numObjects);
+
+	m_data->m_gpuSatCollision->computeConvexConvexContactsGPUSAT(
+		&broadphasePairsGPU, numBroadphasePairs,
+		m_data->m_bodyBufferGPU,
+		m_data->m_pBufContactOutGPU,
+		nContactOut,
+		m_data->m_config.m_maxContactCapacity,
+		*m_data->m_convexPolyhedraGPU,
+		*m_data->m_convexVerticesGPU,
+		*m_data->m_uniqueEdgesGPU,
+		*m_data->m_convexFacesGPU,
+		*m_data->m_convexIndicesGPU,
+		*m_data->m_collidablesGPU,
+		*m_data->m_gpuChildShapes,
+		clAabbArray,
+		*m_data->m_worldVertsB1GPU,
+		*m_data->m_clippingFacesOutGPU,
+		*m_data->m_worldNormalsAGPU,
+		*m_data->m_worldVertsA1GPU,
+		*m_data->m_worldVertsB2GPU,
+		m_data->m_bvhData,
+		m_data->m_treeNodesGPU,
+		m_data->m_subTreesGPU,
+		m_data->m_bvhInfoGPU,
+		numObjects,
+		maxTriConvexPairCapacity,
+		triangleConvexPairs,
+		numTriConvexPairsOut
+		);
+
+}
+
+const b3SapAabb& b3GpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
+{
+	return m_data->m_localShapeAABBCPU->at(collidableIndex);
+}
+
+
+
+
+
+int b3GpuNarrowPhase::registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation , const float* aabbMinPtr, const float* aabbMaxPtr,bool writeToGpu)
+{
+	b3Vector3 aabbMin(aabbMinPtr[0],aabbMinPtr[1],aabbMinPtr[2]);
+	b3Vector3 aabbMax (aabbMaxPtr[0],aabbMaxPtr[1],aabbMaxPtr[2]);
+	
+	b3Assert(m_data->m_numAcceleratedRigidBodies< (m_data->m_config.m_maxConvexBodies-1));
+    
+	m_data->m_bodyBufferGPU->resize(m_data->m_numAcceleratedRigidBodies+1);
+    
+	b3RigidBodyCL& body = m_data->m_bodyBufferCPU->at(m_data->m_numAcceleratedRigidBodies);
+    
+	float friction = 1.f;
+	float restitution = 0.f;
+    
+	body.m_frictionCoeff = friction;
+	body.m_restituitionCoeff = restitution;
+	body.m_angVel.setZero();
+	body.m_linVel.setValue(0,0,0);//.setZero();
+	body.m_pos.setValue(position[0],position[1],position[2]);
+	body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
+	body.m_collidableIdx = collidableIndex;
+	if (collidableIndex>=0)
+	{
+//		body.m_shapeType = m_data->m_collidablesCPU.at(collidableIndex).m_shapeType;
+	} else
+	{
+	//	body.m_shapeType = CollisionShape::SHAPE_PLANE;
+		m_planeBodyIndex = m_data->m_numAcceleratedRigidBodies;
+	}
+	//body.m_shapeType = shapeType;
+	
+	
+	body.m_invMass = mass? 1.f/mass : 0.f;
+    
+	if (writeToGpu)
+	{
+		m_data->m_bodyBufferGPU->copyFromHostPointer(&body,1,m_data->m_numAcceleratedRigidBodies);
+	}
+    
+	b3InertiaCL& shapeInfo = m_data->m_inertiaBufferCPU->at(m_data->m_numAcceleratedRigidBodies);
+    
+	if (mass==0.f)
+	{
+		if (m_data->m_numAcceleratedRigidBodies==0)
+			m_static0Index = 0;
+        
+		shapeInfo.m_initInvInertia.setValue(0,0,0,0,0,0,0,0,0);
+		shapeInfo.m_invInertiaWorld.setValue(0,0,0,0,0,0,0,0,0);
+	} else
+	{
+        
+		assert(body.m_collidableIdx>=0);
+        
+		//approximate using the aabb of the shape
+        
+		//Aabb aabb = (*m_data->m_shapePointers)[shapeIndex]->m_aabb;
+		b3Vector3 halfExtents = (aabbMax-aabbMin);//*0.5f;//fake larger inertia makes demos more stable ;-)
+        
+		b3Vector3 localInertia;
+        
+		float lx=2.f*halfExtents[0];
+		float ly=2.f*halfExtents[1];
+		float lz=2.f*halfExtents[2];
+        
+		localInertia.setValue( (mass/12.0f) * (ly*ly + lz*lz),
+                                   (mass/12.0f) * (lx*lx + lz*lz),
+                                   (mass/12.0f) * (lx*lx + ly*ly));
+        
+		b3Vector3 invLocalInertia;
+		invLocalInertia[0] = 1.f/localInertia[0];
+		invLocalInertia[1] = 1.f/localInertia[1];
+		invLocalInertia[2] = 1.f/localInertia[2];
+		invLocalInertia[3] = 0.f;
+        
+		shapeInfo.m_initInvInertia.setValue(
+			invLocalInertia[0],		0,						0,
+			0,						invLocalInertia[1],		0,
+			0,						0,						invLocalInertia[2]);
+
+		b3Matrix3x3 m (body.m_quat);
+
+		shapeInfo.m_invInertiaWorld = m.scaled(invLocalInertia) * m.transpose();
+        
+	}
+    
+	if (writeToGpu)
+		m_data->m_inertiaBufferGPU->copyFromHostPointer(&shapeInfo,1,m_data->m_numAcceleratedRigidBodies);
+    
+    
+    
+	return m_data->m_numAcceleratedRigidBodies++;
+}
+
+int b3GpuNarrowPhase::getNumRigidBodies() const
+{
+	return m_data->m_numAcceleratedRigidBodies;
+}
+
+void	b3GpuNarrowPhase::writeAllBodiesToGpu()
+{
+	m_data->m_bodyBufferGPU->resize(m_data->m_numAcceleratedRigidBodies);
+	m_data->m_inertiaBufferGPU->resize(m_data->m_numAcceleratedRigidBodies);
+    
+	m_data->m_bodyBufferGPU->copyFromHostPointer(&m_data->m_bodyBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies);
+	m_data->m_inertiaBufferGPU->copyFromHostPointer(&m_data->m_inertiaBufferCPU->at(0),m_data->m_numAcceleratedRigidBodies);
+    
+	m_data->m_collidablesGPU->copyFromHost(m_data->m_collidablesCPU);
+	
+    
+}

src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h

@@ -0,0 +1,86 @@
+#ifndef B3_GPU_NARROWPHASE_H
+#define B3_GPU_NARROWPHASE_H
+
+#include "Bullet3OpenCL/NarrowphaseCollision/b3Collidable.h"
+#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Vector3.h"
+
+class b3GpuNarrowPhase
+{
+protected:
+
+	struct b3GpuNarrowPhaseInternalData*	m_data;
+	int m_acceleratedCompanionShapeIndex;
+	int m_planeBodyIndex;
+	int	m_static0Index;
+
+	cl_context m_context;
+	cl_device_id m_device;
+	cl_command_queue m_queue;
+
+	int registerConvexHullShape(class b3ConvexUtility* convexPtr, b3Collidable& col);
+	int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
+
+public:
+
+	
+
+
+	b3GpuNarrowPhase(cl_context vtx, cl_device_id dev, cl_command_queue q, const struct b3Config& config);
+
+	virtual ~b3GpuNarrowPhase(void);
+
+	int		registerSphereShape(float radius);
+	int		registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
+
+	int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
+	int registerFace(const b3Vector3& faceNormal, float faceConstant);
+	
+	int	registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling);
+	
+	//do they need to be merged?
+	
+	int	registerConvexHullShape(b3ConvexUtility* utilPtr);
+	int	registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
+
+	int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
+	void setObjectTransform(const float* position, const float* orientation , int bodyIndex);
+
+	void	writeAllBodiesToGpu();
+
+	void	readbackAllBodiesToCpu();
+	void	getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const;
+
+	virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbs, int numObjects);
+	
+
+	cl_mem	getBodiesGpu();
+	int	getNumBodiesGpu() const;
+
+	cl_mem	getBodyInertiasGpu();
+	int	getNumBodyInertiasGpu() const;
+
+	cl_mem	getCollidablesGpu();
+	int		getNumCollidablesGpu() const;
+
+
+	const struct b3Contact4* getContactsCPU() const;
+
+	cl_mem	getContactsGpu();
+	int	getNumContactsGpu() const;
+
+	cl_mem	getAabbBufferGpu();
+	
+	int getNumRigidBodies() const;
+
+	int allocateCollidable();
+
+	b3Collidable& getCollidableCpu(int collidableIndex);
+	const b3Collidable& getCollidableCpu(int collidableIndex) const;
+
+	const struct b3SapAabb& getLocalSpaceAabb(int collidableIndex) const;
+};
+
+#endif //B3_GPU_NARROWPHASE_H
+

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp

@@ -0,0 +1,427 @@
+#include "b3GpuRigidBodyPipeline.h"
+#include "b3GpuRigidBodyPipelineInternalData.h"
+#include "kernels/integrateKernel.h"
+#include "kernels/updateAabbsKernel.h"
+
+#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
+#include "b3GpuNarrowPhase.h"
+#include "Bullet3Geometry/b3AabbUtil.h"
+#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h"
+#include "Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h"
+#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
+#include "Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h"
+
+#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
+
+//#define TEST_OTHER_GPU_SOLVER
+
+#define B3_RIGIDBODY_INTEGRATE_PATH "src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl"
+#define B3_RIGIDBODY_UPDATEAABB_PATH "src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl"
+
+bool useDbvt = false;
+bool useBullet2CpuSolver = false;//false;
+bool dumpContactStats = false;
+
+#ifdef TEST_OTHER_GPU_SOLVER
+#include "b3GpuJacobiSolver.h"
+#endif //TEST_OTHER_GPU_SOLVER
+
+#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
+#include "b3GpuBatchingPgsSolver.h"
+#include "b3Solver.h"
+
+#include "Bullet3Common/b3Quickprof.h"
+#include "b3Config.h"
+
+
+
+
+b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue  q,class b3GpuNarrowPhase* narrowphase, class b3GpuSapBroadphase* broadphaseSap , class b3DynamicBvhBroadphase* broadphaseDbvt)
+{
+	m_data = new b3GpuRigidBodyPipelineInternalData;
+	m_data->m_context = ctx;
+	m_data->m_device = device;
+	m_data->m_queue = q;
+
+	m_data->m_solver = new b3PgsJacobiSolver(true);
+	b3Config config;
+	m_data->m_allAabbsGPU = new b3OpenCLArray<b3SapAabb>(ctx,q,config.m_maxConvexBodies);
+	m_data->m_overlappingPairsGPU = new b3OpenCLArray<b3BroadphasePair>(ctx,q,config.m_maxBroadphasePairs);
+
+#ifdef TEST_OTHER_GPU_SOLVER
+	m_data->m_solver3 = new b3GpuJacobiSolver(ctx,device,q,config.m_maxBroadphasePairs);	
+#endif //	TEST_OTHER_GPU_SOLVER
+	
+	m_data->m_solver2 = new b3GpuBatchingPgsSolver(ctx,device,q,config.m_maxBroadphasePairs);
+
+	
+	m_data->m_broadphaseDbvt = broadphaseDbvt;
+	m_data->m_broadphaseSap = broadphaseSap;
+	m_data->m_narrowphase = narrowphase;
+
+	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);
+		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);
+		clReleaseProgram(prog);
+	}
+
+
+}
+
+b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline()
+{
+	clReleaseKernel(m_data->m_integrateTransformsKernel);
+
+	delete m_data->m_solver;
+	delete m_data->m_allAabbsGPU;
+	delete m_data->m_overlappingPairsGPU;
+
+#ifdef TEST_OTHER_GPU_SOLVER
+	delete m_data->m_solver3;
+#endif //TEST_OTHER_GPU_SOLVER
+	
+	delete m_data->m_solver2;
+	
+	
+	delete m_data;
+}
+
+
+void	b3GpuRigidBodyPipeline::addConstraint(b3TypedConstraint* constraint)
+{
+	m_data->m_joints.push_back(constraint);
+}
+void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
+{
+
+	//update worldspace AABBs from local AABB/worldtransform
+	{
+		setupGpuAabbsFull();
+	}
+
+	int numPairs =0;
+
+	//compute overlapping pairs
+	{
+
+		if (useDbvt)
+		{
+			{
+				B3_PROFILE("setAabb");
+				m_data->m_allAabbsGPU->copyToHost(m_data->m_allAabbsCPU);
+				for (int i=0;i<m_data->m_allAabbsCPU.size();i++)
+				{
+					b3BroadphaseProxy* proxy = &m_data->m_broadphaseDbvt->m_proxies[i];
+					b3Vector3 aabbMin(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(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(proxy,aabbMin,aabbMax,0);
+				}
+			}
+
+			{
+				B3_PROFILE("calculateOverlappingPairs");
+				m_data->m_broadphaseDbvt->calculateOverlappingPairs();
+			}
+			numPairs = m_data->m_broadphaseDbvt->getOverlappingPairCache()->getNumOverlappingPairs();
+		} else
+		{
+			m_data->m_broadphaseSap->calculateOverlappingPairs();
+			numPairs = m_data->m_broadphaseSap->getNumOverlap();
+		}
+	}
+
+	//compute contact points
+	
+	
+	int numContacts  = 0;
+
+
+	int numBodies = m_data->m_narrowphase->getNumBodiesGpu();
+
+	if (numPairs)
+	{
+		cl_mem pairs =0;
+		cl_mem aabbsWS =0;
+		if (useDbvt)
+		{
+			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
+		{
+			pairs = m_data->m_broadphaseSap->getOverlappingPairBuffer();
+			aabbsWS = m_data->m_broadphaseSap->getAabbBufferWS();
+		}
+		
+
+		m_data->m_narrowphase->computeContacts(pairs,numPairs,aabbsWS,numBodies);
+		numContacts = m_data->m_narrowphase->getNumContactsGpu();
+
+		if (dumpContactStats && numContacts)
+		{
+			m_data->m_narrowphase->getContactsGpu();
+			
+			printf("numContacts = %d\n", numContacts);
+
+			int totalPoints  = 0;
+			const b3Contact4* contacts = m_data->m_narrowphase->getContactsCPU();
+
+			for (int i=0;i<numContacts;i++)
+			{
+				totalPoints += contacts->getNPoints();
+			}
+			printf("totalPoints=%d\n",totalPoints);
+
+		}
+	}
+	
+
+	//convert contact points to contact constraints
+	
+	//solve constraints
+
+	b3OpenCLArray<b3RigidBodyCL> gpuBodies(m_data->m_context,m_data->m_queue,0,true);
+	gpuBodies.setFromOpenCLBuffer(m_data->m_narrowphase->getBodiesGpu(),m_data->m_narrowphase->getNumBodiesGpu());
+	b3OpenCLArray<b3InertiaCL> gpuInertias(m_data->m_context,m_data->m_queue,0,true);
+	gpuInertias.setFromOpenCLBuffer(m_data->m_narrowphase->getBodyInertiasGpu(),m_data->m_narrowphase->getNumBodiesGpu());
+	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());
+
+	if (useBullet2CpuSolver)
+	{
+
+		b3AlignedObjectArray<b3RigidBodyCL> hostBodies;
+		gpuBodies.copyToHost(hostBodies);
+		b3AlignedObjectArray<b3InertiaCL> hostInertias;
+		gpuInertias.copyToHost(hostInertias);
+		b3AlignedObjectArray<b3Contact4> hostContacts;
+		gpuContacts.copyToHost(hostContacts);
+		{
+			int numJoints = m_data->m_joints.size();
+			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],0,0,numJoints, joints);
+
+		}
+		gpuBodies.copyFromHost(hostBodies);
+	}
+
+	if (numContacts)
+	{
+
+#ifdef TEST_OTHER_GPU_SOLVER
+		if (useJacobi)
+		{
+			bool useGpu = true;
+			if (useGpu)
+			{
+				bool forceHost = false;
+				if (forceHost)
+				{
+					b3AlignedObjectArray<b3RigidBodyCL> hostBodies;
+					b3AlignedObjectArray<b3InertiaCL> hostInertias;
+					b3AlignedObjectArray<b3Contact4> hostContacts;
+				
+					{
+						B3_PROFILE("copyToHost");
+						gpuBodies.copyToHost(hostBodies);
+						gpuInertias.copyToHost(hostInertias);
+						gpuContacts.copyToHost(hostContacts);
+					}
+
+					{
+						b3JacobiSolverInfo solverInfo;
+						m_data->m_solver3->solveGroupHost(&hostBodies[0], &hostInertias[0], hostBodies.size(),&hostContacts[0],hostContacts.size(),0,0,solverInfo);
+
+						
+					}
+					{
+						B3_PROFILE("copyFromHost");
+						gpuBodies.copyFromHost(hostBodies);
+					}
+				} else
+				{
+					b3JacobiSolverInfo solverInfo;
+					m_data->m_solver3->solveGroup(&gpuBodies, &gpuInertias, &gpuContacts,solverInfo);
+				}
+			} else
+			{
+				b3AlignedObjectArray<b3RigidBodyCL> hostBodies;
+				gpuBodies.copyToHost(hostBodies);
+				b3AlignedObjectArray<b3InertiaCL> hostInertias;
+				gpuInertias.copyToHost(hostInertias);
+				b3AlignedObjectArray<b3Contact4> hostContacts;
+				gpuContacts.copyToHost(hostContacts);
+				{
+					m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]);
+				}
+				gpuBodies.copyFromHost(hostBodies);
+			}
+		
+		} else
+#endif //TEST_OTHER_GPU_SOLVER
+		{
+			b3Config config;
+			m_data->m_solver2->solveContacts(numBodies, gpuBodies.getBufferCL(),gpuInertias.getBufferCL(),numContacts, gpuContacts.getBufferCL(),config);
+			
+			//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,
+			(b3OpenCLArray<Constraint4>*) &gpuContacts,
+			0,numContacts,256);
+			*/
+		}
+	}
+
+	integrate(deltaTime);
+
+}
+
+void	b3GpuRigidBodyPipeline::integrate(float timeStep)
+{
+	//integrate
+
+	b3LauncherCL launcher(m_data->m_queue,m_data->m_integrateTransformsKernel);
+	launcher.setBuffer(m_data->m_narrowphase->getBodiesGpu());
+	int numBodies = m_data->m_narrowphase->getNumBodiesGpu();
+	launcher.setConst(numBodies);
+	launcher.setConst(timeStep);
+	float angularDamp = 0.99f;
+	launcher.setConst(angularDamp);
+	
+	b3Vector3 gravity(0.f,-9.8f,0.f);
+	launcher.setConst(gravity);
+
+	launcher.launch1D(numBodies);
+}
+
+
+
+void	b3GpuRigidBodyPipeline::setupGpuAabbsFull()
+{
+	cl_int ciErrNum=0;
+
+	int numBodies = m_data->m_narrowphase->getNumBodiesGpu();
+	if (!numBodies)
+		return;
+
+	//__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);
+	launcher.setConst(numBodies);
+	cl_mem bodies = m_data->m_narrowphase->getBodiesGpu();
+	launcher.setBuffer(bodies);
+	cl_mem collidables = m_data->m_narrowphase->getCollidablesGpu();
+	launcher.setBuffer(collidables);
+	cl_mem localAabbs = m_data->m_narrowphase->getAabbBufferGpu();
+	launcher.setBuffer(localAabbs);
+
+	cl_mem worldAabbs =0;
+	if (useDbvt)
+	{
+		worldAabbs = m_data->m_allAabbsGPU->getBufferCL();
+	} else
+	{
+		worldAabbs = m_data->m_broadphaseSap->getAabbBufferWS();
+	}
+	launcher.setBuffer(worldAabbs);
+	launcher.launch1D(numBodies);
+	oclCHECKERROR(ciErrNum, CL_SUCCESS);
+}
+
+
+
+cl_mem	b3GpuRigidBodyPipeline::getBodyBuffer()
+{
+	return m_data->m_narrowphase->getBodiesGpu();
+}
+
+int	b3GpuRigidBodyPipeline::getNumBodies() const
+{
+	return m_data->m_narrowphase->getNumBodiesGpu();
+}
+
+
+void 		b3GpuRigidBodyPipeline::writeAllInstancesToGpu()
+{
+	m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU);
+}
+
+
+int		b3GpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userIndex, bool writeInstanceToGpu)
+{
+	b3Vector3 aabbMin(0,0,0),aabbMax(0,0,0);
+
+	int bodyIndex = m_data->m_narrowphase->getNumRigidBodies();
+
+	
+	if (collidableIndex>=0)
+	{
+		b3SapAabb localAabb = m_data->m_narrowphase->getLocalSpaceAabb(collidableIndex);
+		b3Vector3 localAabbMin(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
+		b3Vector3 localAabbMax(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
+		
+		b3Scalar margin = 0.01f;
+		b3Transform t;
+		t.setIdentity();
+		t.setOrigin(b3Vector3(position[0],position[1],position[2]));
+		t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
+		b3TransformAabb(localAabbMin,localAabbMax, margin,t,aabbMin,aabbMax);
+		if (useDbvt)
+		{
+			m_data->m_broadphaseDbvt->createProxy(aabbMin,aabbMax,bodyIndex,0,1,1);
+			b3SapAabb aabb;
+			for (int i=0;i<3;i++)
+			{
+				aabb.m_min[i] = aabbMin[i];
+				aabb.m_max[i] = aabbMax[i];
+				aabb.m_minIndices[3] = bodyIndex;
+			}
+			m_data->m_allAabbsCPU.push_back(aabb);
+			if (writeInstanceToGpu)
+			{
+				m_data->m_allAabbsGPU->copyFromHost(m_data->m_allAabbsCPU);
+			}
+		} else
+		{
+			if (mass)
+			{
+				m_data->m_broadphaseSap->createProxy(aabbMin,aabbMax,userIndex,1,1);//m_dispatcher);
+			} else
+			{
+				m_data->m_broadphaseSap->createLargeProxy(aabbMin,aabbMax,userIndex,1,1);//m_dispatcher);	
+			}
+		}
+	}
+			
+	
+	bool writeToGpu = false;
+	
+	bodyIndex = m_data->m_narrowphase->registerRigidBody(collidableIndex,mass,position,orientation,&aabbMin.getX(),&aabbMax.getX(),writeToGpu);
+
+
+	/*
+	if (mass>0.f)
+		m_numDynamicPhysicsInstances++;
+
+	m_numPhysicsInstances++;
+	*/
+
+	return bodyIndex;
+}

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h

@@ -0,0 +1,45 @@
+#ifndef B3_GPU_RIGIDBODY_PIPELINE_H
+#define B3_GPU_RIGIDBODY_PIPELINE_H
+
+#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
+
+class b3GpuRigidBodyPipeline
+{
+protected:
+	struct b3GpuRigidBodyPipelineInternalData*	m_data;
+
+	int allocateCollidable();
+
+public:
+
+
+	b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id device, cl_command_queue  q , class b3GpuNarrowPhase* narrowphase, class b3GpuSapBroadphase* broadphaseSap, class b3DynamicBvhBroadphase* broadphaseDbvt);
+	virtual ~b3GpuRigidBodyPipeline();
+
+	void	stepSimulation(float deltaTime);
+	void	integrate(float timeStep);
+	void	setupGpuAabbsFull();
+
+	int		registerConvexPolyhedron(class b3ConvexUtility* convex);
+
+	//int		registerConvexPolyhedron(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
+	//int		registerSphereShape(float radius);
+	//int		registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
+	
+	//int		registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling);
+	//int		registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
+
+	
+	int		registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData, bool writeInstanceToGpu);
+	//if you passed "writeInstanceToGpu" false in the registerPhysicsInstance method (for performance) you need to call writeAllInstancesToGpu after all instances are registered
+	void	writeAllInstancesToGpu();
+
+	void	addConstraint(class b3TypedConstraint* constraint);
+
+	cl_mem	getBodyBuffer();
+
+	int	getNumBodies() const;
+
+};
+
+#endif //B3_GPU_RIGIDBODY_PIPELINE_H

src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h

@@ -0,0 +1,44 @@
+#ifndef B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H
+#define B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H
+
+#include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
+#include "Bullet3OpenCL/NarrowphaseCollision/b3Collidable.h"
+
+#include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h"
+#include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h"
+
+
+
+#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h"
+
+struct b3GpuRigidBodyPipelineInternalData
+{
+
+	cl_context			m_context;
+	cl_device_id		m_device;
+	cl_command_queue	m_queue;
+
+	cl_kernel	m_integrateTransformsKernel;
+	cl_kernel	m_updateAabbsKernel;
+	
+	class b3PgsJacobiSolver* m_solver;
+	class b3GpuBatchingPgsSolver* m_solver2;
+	class b3GpuJacobiSolver* m_solver3;
+	
+	class b3GpuSapBroadphase* m_broadphaseSap;
+	
+	class b3DynamicBvhBroadphase* m_broadphaseDbvt;
+	b3OpenCLArray<b3SapAabb>*	m_allAabbsGPU;
+	b3AlignedObjectArray<b3SapAabb>	m_allAabbsCPU;
+	b3OpenCLArray<b3BroadphasePair>*		m_overlappingPairsGPU;
+
+	b3AlignedObjectArray<b3TypedConstraint*> m_joints;
+	class b3GpuNarrowPhase*	m_narrowphase;
+	
+};
+
+#endif //B3_GPU_RIGIDBODY_PIPELINE_INTERNAL_DATA_H
+

src/Bullet3OpenCL/RigidBody/b3Solver.cpp

@@ -0,0 +1,935 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Takahiro Harada
+
+
+#include "b3Solver.h"
+
+///useNewBatchingKernel  is a rewritten kernel using just a single thread of the warp, for experiments
+bool useNewBatchingKernel = true;
+
+#define B3_SOLVER_SETUP_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl"
+#define B3_SOLVER_SETUP2_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl"
+#define B3_SOLVER_CONTACT_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl"
+#define B3_SOLVER_FRICTION_KERNEL_PATH "src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl"
+#define B3_BATCHING_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl"
+#define B3_BATCHING_NEW_PATH "src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl"
+
+
+#include "kernels/solverSetup.h"
+#include "kernels/solverSetup2.h"
+
+#include "kernels/solveContact.h"
+#include "kernels/solveFriction.h"
+
+#include "kernels/batchingKernels.h"
+#include "kernels/batchingKernelsNew.h"
+
+
+#include "Bullet3Common/b3Quickprof.h"
+#include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
+#include "Bullet3Common/b3Vector3.h"
+
+struct SolverDebugInfo
+{
+	int m_valInt0;
+	int m_valInt1;
+	int m_valInt2;
+	int m_valInt3;
+	
+	int m_valInt4;
+	int m_valInt5;
+	int m_valInt6;
+	int m_valInt7;
+
+	int m_valInt8;
+	int m_valInt9;
+	int m_valInt10;
+	int m_valInt11;
+
+	int	m_valInt12;
+	int	m_valInt13;
+	int	m_valInt14;
+	int	m_valInt15;
+
+
+	float m_val0;
+	float m_val1;
+	float m_val2;
+	float m_val3;
+};
+
+
+
+
+class SolverDeviceInl
+{
+public:
+	struct ParallelSolveData
+	{
+		b3OpenCLArray<unsigned int>* m_numConstraints;
+		b3OpenCLArray<unsigned int>* m_offsets;
+	};
+};
+
+
+
+b3Solver::b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity)
+			:m_nIterations(4),
+			m_context(ctx),
+			m_device(device),
+			m_queue(queue)
+{
+	m_sort32 = new b3RadixSort32CL(ctx,device,queue);
+	m_scan = new b3PrefixScanCL(ctx,device,queue,N_SPLIT*N_SPLIT);
+	m_search = new b3BoundSearchCL(ctx,device,queue,N_SPLIT*N_SPLIT);
+
+	const int sortSize = B3NEXTMULTIPLEOF( pairCapacity, 512 );
+
+	m_sortDataBuffer = new b3OpenCLArray<b3SortData>(ctx,queue,sortSize);
+	m_contactBuffer2 = new b3OpenCLArray<b3Contact4>(ctx,queue);
+
+	m_numConstraints = new b3OpenCLArray<unsigned int>(ctx,queue,N_SPLIT*N_SPLIT );
+	m_numConstraints->resize(N_SPLIT*N_SPLIT);
+
+	m_offsets = new b3OpenCLArray<unsigned int>( ctx,queue, N_SPLIT*N_SPLIT );
+	m_offsets->resize(N_SPLIT*N_SPLIT);
+	const char* additionalMacros = "";
+	const char* srcFileNameForCaching="";
+
+
+
+	cl_int pErrNum;
+	const char* batchKernelSource = batchingKernelsCL;
+	const char* batchKernelNewSource = batchingKernelsNewCL;
+	
+	const char* solverSetupSource = solverSetupCL;
+	const char* solverSetup2Source = solverSetup2CL;
+	const char* solveContactSource = solveContactCL;
+	const char* solveFrictionSource = solveFrictionCL;
+	
+	
+	
+	{
+		
+		cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveContactSource, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH);
+		b3Assert(solveContactProg);
+		
+		cl_program solveFrictionProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveFrictionSource, &pErrNum,additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH);
+		b3Assert(solveFrictionProg);
+
+		cl_program solverSetup2Prog= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetup2Source, &pErrNum,additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH);
+		b3Assert(solverSetup2Prog);
+
+		
+		cl_program solverSetupProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetupSource, &pErrNum,additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH);
+		b3Assert(solverSetupProg);
+		
+		
+		m_solveFrictionKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg,additionalMacros );
+		b3Assert(m_solveFrictionKernel);
+
+		m_solveContactKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg,additionalMacros );
+		b3Assert(m_solveContactKernel);
+		
+		m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg,additionalMacros );
+		b3Assert(m_contactToConstraintKernel);
+			
+		m_setSortDataKernel =  b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog,additionalMacros );
+		b3Assert(m_setSortDataKernel);
+				
+		m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog,additionalMacros );
+		b3Assert(m_reorderContactKernel);
+		
+
+		m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog,additionalMacros );
+		b3Assert(m_copyConstraintKernel);
+		
+	}
+
+	{
+		cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelSource, &pErrNum,additionalMacros, B3_BATCHING_PATH);
+		b3Assert(batchingProg);
+		
+		m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg,additionalMacros );
+		b3Assert(m_batchingKernel);
+	}
+	{
+		cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelNewSource, &pErrNum,additionalMacros, B3_BATCHING_NEW_PATH);
+		b3Assert(batchingNewProg);
+	
+		m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg,additionalMacros );
+		//m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesBruteForce", &pErrNum, batchingNewProg,additionalMacros );
+		b3Assert(m_batchingKernelNew);
+	}
+}
+		
+b3Solver::~b3Solver()
+{
+	delete m_sortDataBuffer;
+	delete m_contactBuffer2;
+
+	delete m_sort32;
+	delete m_scan;
+	delete m_search;
+
+
+	clReleaseKernel(m_batchingKernel);
+	clReleaseKernel(m_batchingKernelNew);
+	
+	clReleaseKernel( m_solveContactKernel);
+	clReleaseKernel( m_solveFrictionKernel);
+
+	clReleaseKernel( m_contactToConstraintKernel);
+	clReleaseKernel( m_setSortDataKernel);
+	clReleaseKernel( m_reorderContactKernel);
+	clReleaseKernel( m_copyConstraintKernel);
+			
+}
+
+
+ 
+
+
+/*void b3Solver::reorderConvertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, 
+	const b3OpenCLArray<b3InertiaCL>* shapeBuf,
+	b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData, 
+	int nContacts, const b3Solver::ConstraintCfg& cfg )
+{
+	if( m_contactBuffer )
+	{
+		m_contactBuffer->resize(nContacts);
+	}
+	if( m_contactBuffer == 0 )
+	{
+		B3_PROFILE("new m_contactBuffer;");
+		m_contactBuffer = new b3OpenCLArray<b3Contact4>(m_context,m_queue,nContacts );
+		m_contactBuffer->resize(nContacts);
+	}
+	
+
+	
+
+	//DeviceUtils::Config dhCfg;
+	//Device* deviceHost = DeviceUtils::allocate( TYPE_HOST, dhCfg );
+	if( cfg.m_enableParallelSolve )
+	{
+		
+
+		clFinish(m_queue);
+		
+		//	contactsIn -> m_contactBuffer
+		{
+			B3_PROFILE("sortContacts");
+			sortContacts( bodyBuf, contactsIn, additionalData, nContacts, cfg );
+			clFinish(m_queue);
+		}
+		
+		
+		{
+			B3_PROFILE("m_copyConstraintKernel");
+
+			
+
+			b3Int4 cdata; cdata.x = nContacts;
+			b3BufferInfoCL bInfo[] = { b3BufferInfoCL( m_contactBuffer->getBufferCL() ), b3BufferInfoCL( contactsIn->getBufferCL() ) };
+//			b3LauncherCL launcher( m_queue, data->m_device->getKernel( PATH, "CopyConstraintKernel",  "-I ..\\..\\ -Wf,--c++", 0 ) );
+			b3LauncherCL launcher( m_queue, m_copyConstraintKernel );
+			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+			launcher.setConst(  cdata );
+			launcher.launch1D( nContacts, 64 );
+			clFinish(m_queue);
+		}
+
+		{
+			B3_PROFILE("batchContacts");
+			b3Solver::batchContacts( contactsIn, nContacts, m_numConstraints, m_offsets, cfg.m_staticIdx );
+
+		}
+	}
+	{
+			B3_PROFILE("waitForCompletion (batchContacts)");
+			clFinish(m_queue);
+	}
+	
+	//================
+	
+	{
+		B3_PROFILE("convertToConstraints");
+		b3Solver::convertToConstraints(  bodyBuf, shapeBuf, contactsIn, contactCOut, additionalData, nContacts, cfg );
+	}
+
+	{
+		B3_PROFILE("convertToConstraints waitForCompletion");
+		clFinish(m_queue);
+	}
+	
+}
+*/
+
+	static
+	inline
+	float calcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, 
+					 const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
+	{
+		return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
+	}
+
+
+	static
+	inline
+	void setLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
+							 b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
+	{
+		linear = -n;
+		angular0 = -b3Cross(r0, n);
+		angular1 = b3Cross(r1, n);
+	}
+
+
+template<bool JACOBI>
+static
+__inline
+void solveContact(b3GpuConstraint4& cs, 
+	const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+	const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 
+	float maxRambdaDt[4], float minRambdaDt[4])
+{
+
+	b3Vector3 dLinVelA; dLinVelA.setZero();
+	b3Vector3 dAngVelA; dAngVelA.setZero();
+	b3Vector3 dLinVelB; dLinVelB.setZero();
+	b3Vector3 dAngVelB; dAngVelB.setZero();
+
+	for(int ic=0; ic<4; ic++)
+	{
+		//	dont necessary because this makes change to 0
+		if( cs.m_jacCoeffInv[ic] == 0.f ) continue;
+
+		{
+			b3Vector3 angular0, angular1, linear;
+			b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
+			b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
+			setLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 );
+
+			float rambdaDt = calcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1,
+				linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];
+			rambdaDt *= cs.m_jacCoeffInv[ic];
+
+			{
+				float prevSum = cs.m_appliedRambdaDt[ic];
+				float updated = prevSum;
+				updated += rambdaDt;
+				updated = b3Max( updated, minRambdaDt[ic] );
+				updated = b3Min( updated, maxRambdaDt[ic] );
+				rambdaDt = updated - prevSum;
+				cs.m_appliedRambdaDt[ic] = updated;
+			}
+
+			b3Vector3 linImp0 = invMassA*linear*rambdaDt;
+			b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
+			b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
+			b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
+#ifdef _WIN32
+            b3Assert(_finite(linImp0.getX()));
+			b3Assert(_finite(linImp1.getX()));
+#endif
+			if( JACOBI )
+			{
+				dLinVelA += linImp0;
+				dAngVelA += angImp0;
+				dLinVelB += linImp1;
+				dAngVelB += angImp1;
+			}
+			else
+			{
+				linVelA += linImp0;
+				angVelA += angImp0;
+				linVelB += linImp1;
+				angVelB += angImp1;
+			}
+		}
+	}
+
+	if( JACOBI )
+	{
+		linVelA += dLinVelA;
+		angVelA += dAngVelA;
+		linVelB += dLinVelB;
+		angVelB += dAngVelB;
+	}
+
+}
+
+
+
+
+
+	static
+	__inline
+	void solveFriction(b3GpuConstraint4& cs, 
+		const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+		const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 
+		float maxRambdaDt[4], float minRambdaDt[4])
+	{
+
+		if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
+		const b3Vector3& center = (const b3Vector3&)cs.m_center;
+
+		b3Vector3 n = -(const b3Vector3&)cs.m_linear;
+
+		b3Vector3 tangent[2];
+#if 1		
+		b3PlaneSpace1 (n, tangent[0],tangent[1]);
+#else
+		b3Vector3 r = cs.m_worldPos[0]-center;
+		tangent[0] = cross3( n, r );
+		tangent[1] = cross3( tangent[0], n );
+		tangent[0] = normalize3( tangent[0] );
+		tangent[1] = normalize3( tangent[1] );
+#endif
+
+		b3Vector3 angular0, angular1, linear;
+		b3Vector3 r0 = center - posA;
+		b3Vector3 r1 = center - posB;
+		for(int i=0; i<2; i++)
+		{
+			setLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
+			float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
+				linVelA, angVelA, linVelB, angVelB );
+			rambdaDt *= cs.m_fJacCoeffInv[i];
+
+				{
+					float prevSum = cs.m_fAppliedRambdaDt[i];
+					float updated = prevSum;
+					updated += rambdaDt;
+					updated = b3Max( updated, minRambdaDt[i] );
+					updated = b3Min( updated, maxRambdaDt[i] );
+					rambdaDt = updated - prevSum;
+					cs.m_fAppliedRambdaDt[i] = updated;
+				}
+
+			b3Vector3 linImp0 = invMassA*linear*rambdaDt;
+			b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
+			b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
+			b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
+#ifdef _WIN32
+			b3Assert(_finite(linImp0.getX()));
+			b3Assert(_finite(linImp1.getX()));
+#endif
+			linVelA += linImp0;
+			angVelA += angImp0;
+			linVelB += linImp1;
+			angVelB += angImp1;
+		}
+
+		{	//	angular damping for point constraint
+			b3Vector3 ab = ( posB - posA ).normalized();
+			b3Vector3 ac = ( center - posA ).normalized();
+			if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
+			{
+				float angNA = b3Dot( n, angVelA );
+				float angNB = b3Dot( n, angVelB );
+
+				angVelA -= (angNA*0.1f)*n;
+				angVelB -= (angNB*0.1f)*n;
+			}
+		}
+
+	}
+
+
+struct SolveTask// : public ThreadPool::Task
+{
+	SolveTask(b3AlignedObjectArray<b3RigidBodyCL>& bodies,  b3AlignedObjectArray<b3InertiaCL>& shapes, b3AlignedObjectArray<b3GpuConstraint4>& constraints,
+		int start, int nConstraints)
+		: m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ), m_start( start ), m_nConstraints( nConstraints ),
+		m_solveFriction( true ){}
+
+	unsigned short int getType(){ return 0; }
+
+	void run(int tIdx)
+	{
+		
+
+		for(int ic=0; ic<m_nConstraints; ic++)
+		{
+			int i = m_start + ic;
+
+			float frictionCoeff = m_constraints[i].getFrictionCoeff();
+			int aIdx = (int)m_constraints[i].m_bodyA;
+			int bIdx = (int)m_constraints[i].m_bodyB;
+			b3RigidBodyCL& bodyA = m_bodies[aIdx];
+			b3RigidBodyCL& bodyB = m_bodies[bIdx];
+
+			if( !m_solveFriction )
+			{
+				float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
+				float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
+
+				solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, 
+					 (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
+					maxRambdaDt, minRambdaDt );
+
+			}
+			else
+			{
+				float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
+				float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
+
+				float sum = 0;
+				for(int j=0; j<4; j++)
+				{
+					sum +=m_constraints[i].m_appliedRambdaDt[j];
+				}
+				frictionCoeff = 0.7f;
+				for(int j=0; j<4; j++)
+				{
+					maxRambdaDt[j] = frictionCoeff*sum;
+					minRambdaDt[j] = -maxRambdaDt[j];
+				}
+
+			solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, 
+					(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
+					maxRambdaDt, minRambdaDt );
+			
+			}
+		}
+
+		
+	}
+
+	b3AlignedObjectArray<b3RigidBodyCL>& m_bodies;
+	b3AlignedObjectArray<b3InertiaCL>& m_shapes;
+	b3AlignedObjectArray<b3GpuConstraint4>& m_constraints;
+	int m_start;
+	int m_nConstraints;
+	bool m_solveFriction;
+};
+
+
+void b3Solver::solveContactConstraintHost(  b3OpenCLArray<b3RigidBodyCL>* bodyBuf, b3OpenCLArray<b3InertiaCL>* shapeBuf, 
+			b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches)
+{
+
+	b3AlignedObjectArray<b3RigidBodyCL> bodyNative;
+	bodyBuf->copyToHost(bodyNative);
+	b3AlignedObjectArray<b3InertiaCL> shapeNative;
+	shapeBuf->copyToHost(shapeNative);
+	b3AlignedObjectArray<b3GpuConstraint4> constraintNative;
+	constraint->copyToHost(constraintNative);
+
+	for(int iter=0; iter<m_nIterations; iter++)
+	{
+		SolveTask task( bodyNative, shapeNative, constraintNative, 0, n );
+		task.m_solveFriction = false;
+		task.run(0);
+	}
+
+	for(int iter=0; iter<m_nIterations; iter++)
+	{
+		SolveTask task( bodyNative, shapeNative, constraintNative, 0, n );
+		task.m_solveFriction = true;
+		task.run(0);
+	}
+
+	bodyBuf->copyFromHost(bodyNative);
+	shapeBuf->copyFromHost(shapeNative);
+	constraint->copyFromHost(constraintNative);
+
+	
+}
+
+void b3Solver::solveContactConstraint(  const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, const b3OpenCLArray<b3InertiaCL>* shapeBuf, 
+			b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches)
+{
+	
+	
+	b3Int4 cdata = b3MakeInt4( n, 0, 0, 0 );
+	{
+		
+		const int nn = N_SPLIT*N_SPLIT;
+
+		cdata.x = 0;
+		cdata.y = maxNumBatches;//250;
+
+
+		int numWorkItems = 64*nn/N_BATCHES;
+#ifdef DEBUG_ME
+		SolverDebugInfo* debugInfo = new  SolverDebugInfo[numWorkItems];
+		adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device,numWorkItems);
+#endif
+
+
+
+		{
+
+			B3_PROFILE("m_batchSolveKernel iterations");
+			for(int iter=0; iter<m_nIterations; iter++)
+			{
+				for(int ib=0; ib<N_BATCHES; ib++)
+				{
+#ifdef DEBUG_ME
+					memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems);
+					gpuDebugInfo.write(debugInfo,numWorkItems);
+#endif
+
+
+					cdata.z = ib;
+					cdata.w = N_SPLIT;
+
+				b3LauncherCL launcher( m_queue, m_solveContactKernel );
+#if 1
+                    
+					b3BufferInfoCL bInfo[] = { 
+
+						b3BufferInfoCL( bodyBuf->getBufferCL() ), 
+						b3BufferInfoCL( shapeBuf->getBufferCL() ), 
+						b3BufferInfoCL( constraint->getBufferCL() ),
+						b3BufferInfoCL( m_numConstraints->getBufferCL() ), 
+						b3BufferInfoCL( m_offsets->getBufferCL() ) 
+#ifdef DEBUG_ME
+						,	b3BufferInfoCL(&gpuDebugInfo)
+#endif
+						};
+
+					
+
+                    launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+					//launcher.setConst(  cdata.x );
+                    launcher.setConst(  cdata.y );
+                    launcher.setConst(  cdata.z );
+                    launcher.setConst(  cdata.w );
+                    launcher.launch1D( numWorkItems, 64 );
+
+                    
+#else
+                    const char* fileName = "m_batchSolveKernel.bin";
+                    FILE* f = fopen(fileName,"rb");
+                    if (f)
+                    {
+                        int sizeInBytes=0;
+                        if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET))
+                        {
+                            printf("error, cannot get file size\n");
+                            exit(0);
+                        }
+                        
+                        unsigned char* buf = (unsigned char*) malloc(sizeInBytes);
+                        fread(buf,sizeInBytes,1,f);
+                        int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes,m_context);
+                        int num = *(int*)&buf[serializedBytes];
+                        
+                        launcher.launch1D( num);
+
+                        //this clFinish is for testing on errors
+                        clFinish(m_queue);
+                    }
+
+#endif
+					
+
+#ifdef DEBUG_ME
+					clFinish(m_queue);
+					gpuDebugInfo.read(debugInfo,numWorkItems);
+					clFinish(m_queue);
+					for (int i=0;i<numWorkItems;i++)
+					{
+						if (debugInfo[i].m_valInt2>0)
+						{
+							printf("debugInfo[i].m_valInt2 = %d\n",i,debugInfo[i].m_valInt2);
+						}
+
+						if (debugInfo[i].m_valInt3>0)
+						{
+							printf("debugInfo[i].m_valInt3 = %d\n",i,debugInfo[i].m_valInt3);
+						}
+					}
+#endif //DEBUG_ME
+
+
+				}
+			}
+		
+			clFinish(m_queue);
+
+
+		}
+
+		cdata.x = 1;
+		bool applyFriction=true;
+		if (applyFriction)
+    	{
+			B3_PROFILE("m_batchSolveKernel iterations2");
+			for(int iter=0; iter<m_nIterations; iter++)
+			{
+				for(int ib=0; ib<N_BATCHES; ib++)
+				{
+					cdata.z = ib;
+					cdata.w = N_SPLIT;
+
+					b3BufferInfoCL bInfo[] = { 
+						b3BufferInfoCL( bodyBuf->getBufferCL() ), 
+						b3BufferInfoCL( shapeBuf->getBufferCL() ), 
+						b3BufferInfoCL( constraint->getBufferCL() ),
+						b3BufferInfoCL( m_numConstraints->getBufferCL() ), 
+						b3BufferInfoCL( m_offsets->getBufferCL() )
+#ifdef DEBUG_ME
+						,b3BufferInfoCL(&gpuDebugInfo)
+#endif //DEBUG_ME
+					};
+					b3LauncherCL launcher( m_queue, m_solveFrictionKernel );
+					launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+					//launcher.setConst(  cdata.x );
+                    launcher.setConst(  cdata.y );
+                    launcher.setConst(  cdata.z );
+                    launcher.setConst(  cdata.w );
+                    
+					launcher.launch1D( 64*nn/N_BATCHES, 64 );
+				}
+			}
+			clFinish(m_queue);
+			
+		}
+#ifdef DEBUG_ME
+		delete[] debugInfo;
+#endif //DEBUG_ME
+	}
+
+	
+}
+
+void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, 
+	const b3OpenCLArray<b3InertiaCL>* shapeBuf, 
+	b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData, 
+	int nContacts, const ConstraintCfg& cfg )
+{
+	b3OpenCLArray<b3GpuConstraint4>* constraintNative =0;
+
+	struct CB
+	{
+		int m_nContacts;
+		float m_dt;
+		float m_positionDrift;
+		float m_positionConstraintCoeff;
+	};
+
+	{
+		B3_PROFILE("m_contactToConstraintKernel");
+		CB cdata;
+		cdata.m_nContacts = nContacts;
+		cdata.m_dt = cfg.m_dt;
+		cdata.m_positionDrift = cfg.m_positionDrift;
+		cdata.m_positionConstraintCoeff = cfg.m_positionConstraintCoeff;
+
+		
+		b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL() ), b3BufferInfoCL( shapeBuf->getBufferCL()),
+			b3BufferInfoCL( contactCOut->getBufferCL() )};
+		b3LauncherCL launcher( m_queue, m_contactToConstraintKernel );
+		launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+		//launcher.setConst(  cdata );
+        
+        launcher.setConst(cdata.m_nContacts);
+		launcher.setConst(cdata.m_dt);
+		launcher.setConst(cdata.m_positionDrift);
+		launcher.setConst(cdata.m_positionConstraintCoeff);
+        
+		launcher.launch1D( nContacts, 64 );	
+		clFinish(m_queue);
+
+	}
+
+	contactCOut->resize(nContacts);
+}
+
+/*
+void b3Solver::sortContacts(  const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, 
+			b3OpenCLArray<b3Contact4>* contactsIn, void* additionalData, 
+			int nContacts, const b3Solver::ConstraintCfg& cfg )
+{
+	
+	
+
+	const int sortAlignment = 512; // todo. get this out of sort
+	if( cfg.m_enableParallelSolve )
+	{
+		
+
+		int sortSize = NEXTMULTIPLEOF( nContacts, sortAlignment );
+
+		b3OpenCLArray<unsigned int>* countsNative = m_numConstraints;//BufferUtils::map<TYPE_CL, false>( data->m_device, &countsHost );
+		b3OpenCLArray<unsigned int>* offsetsNative = m_offsets;//BufferUtils::map<TYPE_CL, false>( data->m_device, &offsetsHost );
+
+		{	//	2. set cell idx
+			struct CB
+			{
+				int m_nContacts;
+				int m_staticIdx;
+				float m_scale;
+				int m_nSplit;
+			};
+
+			b3Assert( sortSize%64 == 0 );
+			CB cdata;
+			cdata.m_nContacts = nContacts;
+			cdata.m_staticIdx = cfg.m_staticIdx;
+			cdata.m_scale = 1.f/(N_OBJ_PER_SPLIT*cfg.m_averageExtent);
+			cdata.m_nSplit = N_SPLIT;
+
+			
+			b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( bodyBuf->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) };
+			b3LauncherCL launcher( m_queue, m_setSortDataKernel );
+			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+			launcher.setConst(  cdata );
+			launcher.launch1D( sortSize, 64 );
+		}
+
+		{	//	3. sort by cell idx
+			int n = N_SPLIT*N_SPLIT;
+			int sortBit = 32;
+			//if( n <= 0xffff ) sortBit = 16;
+			//if( n <= 0xff ) sortBit = 8;
+			m_sort32->execute(*m_sortDataBuffer,sortSize);
+		}
+		{	//	4. find entries
+			m_search->execute( *m_sortDataBuffer, nContacts, *countsNative, N_SPLIT*N_SPLIT, b3BoundSearchCL::COUNT);
+
+			m_scan->execute( *countsNative, *offsetsNative, N_SPLIT*N_SPLIT );
+		}
+
+		{	//	5. sort constraints by cellIdx
+			//	todo. preallocate this
+//			b3Assert( contactsIn->getType() == TYPE_HOST );
+//			b3OpenCLArray<b3Contact4>* out = BufferUtils::map<TYPE_CL, false>( data->m_device, contactsIn );	//	copying contacts to this buffer
+
+			{
+				
+
+				b3Int4 cdata; cdata.x = nContacts;
+				b3BufferInfoCL bInfo[] = { b3BufferInfoCL( contactsIn->getBufferCL() ), b3BufferInfoCL( m_contactBuffer->getBufferCL() ), b3BufferInfoCL( m_sortDataBuffer->getBufferCL() ) };
+				b3LauncherCL launcher( m_queue, m_reorderContactKernel );
+				launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+				launcher.setConst(  cdata );
+				launcher.launch1D( nContacts, 64 );
+			}
+//			BufferUtils::unmap<true>( out, contactsIn, nContacts );
+		}
+	}
+
+	
+}
+
+*/
+
+void	b3Solver::batchContacts(  b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* nNative, b3OpenCLArray<unsigned int>* offsetsNative, int staticIdx )
+{
+	
+	int numWorkItems = 64*N_SPLIT*N_SPLIT;
+	{
+		B3_PROFILE("batch generation");
+		
+		b3Int4 cdata;
+		cdata.x = nContacts;
+		cdata.y = 0;
+		cdata.z = staticIdx;
+
+		
+#ifdef BATCH_DEBUG
+		SolverDebugInfo* debugInfo = new  SolverDebugInfo[numWorkItems];
+		adl::b3OpenCLArray<SolverDebugInfo> gpuDebugInfo(data->m_device,numWorkItems);
+		memset(debugInfo,0,sizeof(SolverDebugInfo)*numWorkItems);
+		gpuDebugInfo.write(debugInfo,numWorkItems);
+#endif
+
+		
+
+
+		b3BufferInfoCL bInfo[] = { 
+			b3BufferInfoCL( contacts->getBufferCL() ), 
+			b3BufferInfoCL(  m_contactBuffer2->getBufferCL()),
+			b3BufferInfoCL( nNative->getBufferCL() ), 
+			b3BufferInfoCL( offsetsNative->getBufferCL() ),
+#ifdef BATCH_DEBUG
+			,	b3BufferInfoCL(&gpuDebugInfo)
+#endif
+		};
+
+		
+		
+
+		{
+			B3_PROFILE("batchingKernel");
+			//b3LauncherCL launcher( m_queue, m_batchingKernel);
+			cl_kernel k = useNewBatchingKernel ? m_batchingKernelNew : m_batchingKernel;
+
+			b3LauncherCL launcher( m_queue, k);
+			if (!useNewBatchingKernel )
+			{
+				launcher.setBuffer( contacts->getBufferCL() );
+			}
+			launcher.setBuffer( m_contactBuffer2->getBufferCL() );
+			launcher.setBuffer( nNative->getBufferCL());
+			launcher.setBuffer( offsetsNative->getBufferCL());
+
+			//launcher.setConst(  cdata );
+            launcher.setConst(staticIdx);
+            
+			launcher.launch1D( numWorkItems, 64 );
+			clFinish(m_queue);
+		}
+
+#ifdef BATCH_DEBUG
+	aaaa
+		b3Contact4* hostContacts = new b3Contact4[nContacts];
+		m_contactBuffer->read(hostContacts,nContacts);
+		clFinish(m_queue);
+
+		gpuDebugInfo.read(debugInfo,numWorkItems);
+		clFinish(m_queue);
+
+		for (int i=0;i<numWorkItems;i++)
+		{
+			if (debugInfo[i].m_valInt1>0)
+			{
+				printf("catch\n");
+			}
+			if (debugInfo[i].m_valInt2>0)
+			{
+				printf("catch22\n");
+			}
+
+			if (debugInfo[i].m_valInt3>0)
+			{
+				printf("catch666\n");
+			}
+
+			if (debugInfo[i].m_valInt4>0)
+			{
+				printf("catch777\n");
+			}
+		}
+		delete[] debugInfo;
+#endif //BATCH_DEBUG
+
+	}
+
+//	copy buffer to buffer
+	//b3Assert(m_contactBuffer->size()==nContacts);
+	//contacts->copyFromOpenCLArray( *m_contactBuffer);
+	//clFinish(m_queue);//needed?
+	
+	
+	
+}
+

src/Bullet3OpenCL/RigidBody/b3Solver.h

@@ -0,0 +1,124 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Takahiro Harada
+
+
+#ifndef __ADL_SOLVER_H
+#define __ADL_SOLVER_H
+
+#include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
+#include "b3GpuConstraint4.h"
+
+#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
+
+#include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h"
+#include "Bullet3OpenCL/ParallelPrimitives/b3RadixSort32CL.h"
+#include "Bullet3OpenCL/ParallelPrimitives/b3BoundSearchCL.h"
+
+#include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
+
+
+#define B3NEXTMULTIPLEOF(num, alignment) (((num)/(alignment) + (((num)%(alignment)==0)?0:1))*(alignment))
+
+class b3SolverBase
+{
+	public:
+		
+
+		struct ConstraintCfg
+		{
+			ConstraintCfg( float dt = 0.f ): m_positionDrift( 0.005f ), m_positionConstraintCoeff( 0.2f ), m_dt(dt), m_staticIdx(-1) {}
+
+			float m_positionDrift;
+			float m_positionConstraintCoeff;
+			float m_dt;
+			bool m_enableParallelSolve;
+			float m_averageExtent;
+			int m_staticIdx;
+		};
+
+		
+
+		enum
+		{
+			N_SPLIT = 16,
+			N_BATCHES = 4,
+			N_OBJ_PER_SPLIT = 10,
+			N_TASKS_PER_BATCH = N_SPLIT*N_SPLIT,
+		};
+};
+
+class b3Solver : public b3SolverBase
+{
+	public:
+
+		cl_context m_context;
+		cl_device_id m_device;
+		cl_command_queue m_queue;
+				
+
+		b3OpenCLArray<unsigned int>* m_numConstraints;
+		b3OpenCLArray<unsigned int>* m_offsets;
+		
+		
+		int m_nIterations;
+		cl_kernel m_batchingKernel;
+		cl_kernel m_batchingKernelNew;
+		cl_kernel m_solveContactKernel;
+		cl_kernel m_solveFrictionKernel;
+		cl_kernel m_contactToConstraintKernel;
+		cl_kernel m_setSortDataKernel;
+		cl_kernel m_reorderContactKernel;
+		cl_kernel m_copyConstraintKernel;
+
+		class b3RadixSort32CL*	m_sort32;
+		class b3BoundSearchCL*	m_search;
+		class b3PrefixScanCL*	m_scan;
+
+		b3OpenCLArray<b3SortData>* m_sortDataBuffer;
+		b3OpenCLArray<b3Contact4>* m_contactBuffer2;
+
+		enum
+		{
+			DYNAMIC_CONTACT_ALLOCATION_THRESHOLD = 2000000,
+		};
+
+		
+
+		
+		b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity);
+
+		virtual ~b3Solver();
+		
+		void solveContactConstraint( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, const b3OpenCLArray<b3InertiaCL>* inertiaBuf, 
+			b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches);
+
+		void solveContactConstraintHost(  b3OpenCLArray<b3RigidBodyCL>* bodyBuf, b3OpenCLArray<b3InertiaCL>* shapeBuf, 
+			b3OpenCLArray<b3GpuConstraint4>* constraint, void* additionalData, int n ,int maxNumBatches);
+
+
+		void convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf, 
+			const b3OpenCLArray<b3InertiaCL>* shapeBuf, 
+			b3OpenCLArray<b3Contact4>* contactsIn, b3OpenCLArray<b3GpuConstraint4>* contactCOut, void* additionalData, 
+			int nContacts, const ConstraintCfg& cfg );
+
+		void	batchContacts( b3OpenCLArray<b3Contact4>* contacts, int nContacts, b3OpenCLArray<unsigned int>* n, b3OpenCLArray<unsigned int>* offsets, int staticIdx );
+
+};
+
+
+
+
+#endif //__ADL_SOLVER_H

src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.cl

@@ -0,0 +1,344 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Takahiro Harada
+
+
+#pragma OPENCL EXTENSION cl_amd_printf : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable
+
+#ifdef cl_ext_atomic_counters_32
+#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
+#else
+#define counter32_t volatile __global int*
+#endif
+
+
+typedef unsigned int u32;
+typedef unsigned short u16;
+typedef unsigned char u8;
+
+#define GET_GROUP_IDX get_group_id(0)
+#define GET_LOCAL_IDX get_local_id(0)
+#define GET_GLOBAL_IDX get_global_id(0)
+#define GET_GROUP_SIZE get_local_size(0)
+#define GET_NUM_GROUPS get_num_groups(0)
+#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)
+#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)
+#define AtomInc(x) atom_inc(&(x))
+#define AtomInc1(x, out) out = atom_inc(&(x))
+#define AppendInc(x, out) out = atomic_inc(x)
+#define AtomAdd(x, value) atom_add(&(x), value)
+#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )
+#define AtomXhg(x, value) atom_xchg ( &(x), value )
+
+
+#define SELECT_UINT4( b, a, condition ) select( b,a,condition )
+
+#define make_float4 (float4)
+#define make_float2 (float2)
+#define make_uint4 (uint4)
+#define make_int4 (int4)
+#define make_uint2 (uint2)
+#define make_int2 (int2)
+
+
+#define max2 max
+#define min2 min
+
+
+#define WG_SIZE 64
+
+
+
+typedef struct 
+{
+	float4 m_worldPos[4];
+	float4 m_worldNormal;
+	u32 m_coeffs;
+	int m_batchIdx;
+
+	int m_bodyA;//sign bit set for fixed objects
+	int m_bodyB;
+}Contact4;
+
+typedef struct 
+{
+	int m_n;
+	int m_start;
+	int m_staticIdx;
+	int m_paddings[1];
+} ConstBuffer;
+
+typedef struct 
+{
+	int m_a;
+	int m_b;
+	u32 m_idx;
+}Elem;
+
+#define STACK_SIZE (WG_SIZE*10)
+//#define STACK_SIZE (WG_SIZE)
+#define RING_SIZE 1024
+#define RING_SIZE_MASK (RING_SIZE-1)
+#define CHECK_SIZE (WG_SIZE)
+
+
+#define GET_RING_CAPACITY (RING_SIZE - ldsRingEnd)
+#define RING_END ldsTmp
+
+u32 readBuf(__local u32* buff, int idx)
+{
+	idx = idx % (32*CHECK_SIZE);
+	int bitIdx = idx%32;
+	int bufIdx = idx/32;
+	return buff[bufIdx] & (1<<bitIdx);
+}
+
+void writeBuf(__local u32* buff, int idx)
+{
+	idx = idx % (32*CHECK_SIZE);
+	int bitIdx = idx%32;
+	int bufIdx = idx/32;
+//	buff[bufIdx] |= (1<<bitIdx);
+	atom_or( &buff[bufIdx], (1<<bitIdx) );
+}
+
+u32 tryWrite(__local u32* buff, int idx)
+{
+	idx = idx % (32*CHECK_SIZE);
+	int bitIdx = idx%32;
+	int bufIdx = idx/32;
+	u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) );
+	return ((ans >> bitIdx)&1) == 0;
+}
+
+//	batching on the GPU
+__kernel void CreateBatches( __global const Contact4* gConstraints, __global Contact4* gConstraintsOut,
+		__global const u32* gN, __global const u32* gStart, 
+		int m_staticIdx )
+{
+	__local u32 ldsStackIdx[STACK_SIZE];
+	__local u32 ldsStackEnd;
+	__local Elem ldsRingElem[RING_SIZE];
+	__local u32 ldsRingEnd;
+	__local u32 ldsTmp;
+	__local u32 ldsCheckBuffer[CHECK_SIZE];
+	__local u32 ldsFixedBuffer[CHECK_SIZE];
+	__local u32 ldsGEnd;
+	__local u32 ldsDstEnd;
+
+	int wgIdx = GET_GROUP_IDX;
+	int lIdx = GET_LOCAL_IDX;
+	
+	const int m_n = gN[wgIdx];
+	const int m_start = gStart[wgIdx];
+		
+	if( lIdx == 0 )
+	{
+		ldsRingEnd = 0;
+		ldsGEnd = 0;
+		ldsStackEnd = 0;
+		ldsDstEnd = m_start;
+	}
+	
+//	while(1)
+//was 250
+	for(int ie=0; ie<50; ie++)
+	{
+		ldsFixedBuffer[lIdx] = 0;
+
+		for(int giter=0; giter<4; giter++)
+		{
+			int ringCap = GET_RING_CAPACITY;
+		
+			//	1. fill ring
+			if( ldsGEnd < m_n )
+			{
+				while( ringCap > WG_SIZE )
+				{
+					if( ldsGEnd >= m_n ) break;
+					if( lIdx < ringCap - WG_SIZE )
+					{
+						int srcIdx;
+						AtomInc1( ldsGEnd, srcIdx );
+						if( srcIdx < m_n )
+						{
+							int dstIdx;
+							AtomInc1( ldsRingEnd, dstIdx );
+							
+							int a = gConstraints[m_start+srcIdx].m_bodyA;
+							int b = gConstraints[m_start+srcIdx].m_bodyB;
+							ldsRingElem[dstIdx].m_a = (a>b)? b:a;
+							ldsRingElem[dstIdx].m_b = (a>b)? a:b;
+							ldsRingElem[dstIdx].m_idx = srcIdx;
+						}
+					}
+					ringCap = GET_RING_CAPACITY;
+				}
+			}
+
+			GROUP_LDS_BARRIER;
+	
+			//	2. fill stack
+			__local Elem* dst = ldsRingElem;
+			if( lIdx == 0 ) RING_END = 0;
+
+			int srcIdx=lIdx;
+			int end = ldsRingEnd;
+
+			{
+				for(int ii=0; ii<end; ii+=WG_SIZE, srcIdx+=WG_SIZE)
+				{
+					Elem e;
+					if(srcIdx<end) e = ldsRingElem[srcIdx];
+					bool done = (srcIdx<end)?false:true;
+
+					for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) ldsCheckBuffer[lIdx] = 0;
+					
+					if( !done )
+					{
+						int aUsed = readBuf( ldsFixedBuffer, abs(e.m_a));
+						int bUsed = readBuf( ldsFixedBuffer, abs(e.m_b));
+
+						if( aUsed==0 && bUsed==0 )
+						{
+							int aAvailable;
+							int bAvailable;
+							int ea = abs(e.m_a);
+							int eb = abs(e.m_b);
+
+							aAvailable = tryWrite( ldsCheckBuffer, ea );
+							bAvailable = tryWrite( ldsCheckBuffer, eb );
+
+							bool aStatic = (e.m_a<0) ||(ea==m_staticIdx);
+							bool bStatic = (e.m_b<0) ||(eb==m_staticIdx);
+							
+							aAvailable = aStatic? 1: aAvailable;
+							bAvailable = bStatic? 1: bAvailable;
+
+							bool success = (aAvailable && bAvailable);
+							if(success)
+							{
+							
+								if (!aStatic)
+									writeBuf( ldsFixedBuffer, ea );
+								if (!bStatic)
+									writeBuf( ldsFixedBuffer, eb );
+							}
+							done = success;
+						}
+					}
+
+					//	put it aside
+					if(srcIdx<end)
+					{
+						if( done )
+						{
+							int dstIdx; AtomInc1( ldsStackEnd, dstIdx );
+							if( dstIdx < STACK_SIZE )
+								ldsStackIdx[dstIdx] = e.m_idx;
+							else{
+								done = false;
+								AtomAdd( ldsStackEnd, -1 );
+							}
+						}
+						if( !done )
+						{
+							int dstIdx; AtomInc1( RING_END, dstIdx );
+							dst[dstIdx] = e;
+						}
+					}
+
+					//	if filled, flush
+					if( ldsStackEnd == STACK_SIZE )
+					{
+						for(int i=lIdx; i<STACK_SIZE; i+=WG_SIZE)
+						{
+							int idx = m_start + ldsStackIdx[i];
+							int dstIdx; AtomInc1( ldsDstEnd, dstIdx );
+							gConstraintsOut[ dstIdx ] = gConstraints[ idx ];
+							gConstraintsOut[ dstIdx ].m_batchIdx = ie;
+						}
+						if( lIdx == 0 ) ldsStackEnd = 0;
+
+						//for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) 
+						ldsFixedBuffer[lIdx] = 0;
+					}
+				}
+			}
+
+			if( lIdx == 0 ) ldsRingEnd = RING_END;
+		}
+
+		GROUP_LDS_BARRIER;
+
+		for(int i=lIdx; i<ldsStackEnd; i+=WG_SIZE)
+		{
+			int idx = m_start + ldsStackIdx[i];
+			int dstIdx; AtomInc1( ldsDstEnd, dstIdx );
+			gConstraintsOut[ dstIdx ] = gConstraints[ idx ];
+			gConstraintsOut[ dstIdx ].m_batchIdx = ie;
+		}
+
+		//	in case it couldn't consume any pair. Flush them
+		//	todo. Serial batch worth while?
+		if( ldsStackEnd == 0 )
+		{
+			for(int i=lIdx; i<ldsRingEnd; i+=WG_SIZE)
+			{
+				int idx = m_start + ldsRingElem[i].m_idx;
+				int dstIdx; AtomInc1( ldsDstEnd, dstIdx );
+				gConstraintsOut[ dstIdx ] = gConstraints[ idx ];
+				gConstraintsOut[ dstIdx ].m_batchIdx = 100+i;
+			}
+			GROUP_LDS_BARRIER;
+			if( lIdx == 0 ) ldsRingEnd = 0;
+		}
+
+		if( lIdx == 0 ) ldsStackEnd = 0;
+
+		GROUP_LDS_BARRIER;
+
+		//	termination
+		if( ldsGEnd == m_n && ldsRingEnd == 0 )
+			break;
+	}
+
+
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h

@@ -0,0 +1,348 @@
+//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
+static const char* batchingKernelsCL= \
+"/*\n"
+"Copyright (c) 2012 Advanced Micro Devices, Inc.  \n"
+"\n"
+"This software is provided 'as-is', without any express or implied warranty.\n"
+"In no event will the authors be held liable for any damages arising from the use of this software.\n"
+"Permission is granted to anyone to use this software for any purpose, \n"
+"including commercial applications, and to alter it and redistribute it freely, \n"
+"subject to the following restrictions:\n"
+"\n"
+"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
+"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
+"3. This notice may not be removed or altered from any source distribution.\n"
+"*/\n"
+"//Originally written by Takahiro Harada\n"
+"\n"
+"\n"
+"#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n"
+"\n"
+"#ifdef cl_ext_atomic_counters_32\n"
+"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n"
+"#else\n"
+"#define counter32_t volatile __global int*\n"
+"#endif\n"
+"\n"
+"\n"
+"typedef unsigned int u32;\n"
+"typedef unsigned short u16;\n"
+"typedef unsigned char u8;\n"
+"\n"
+"#define GET_GROUP_IDX get_group_id(0)\n"
+"#define GET_LOCAL_IDX get_local_id(0)\n"
+"#define GET_GLOBAL_IDX get_global_id(0)\n"
+"#define GET_GROUP_SIZE get_local_size(0)\n"
+"#define GET_NUM_GROUPS get_num_groups(0)\n"
+"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n"
+"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n"
+"#define AtomInc(x) atom_inc(&(x))\n"
+"#define AtomInc1(x, out) out = atom_inc(&(x))\n"
+"#define AppendInc(x, out) out = atomic_inc(x)\n"
+"#define AtomAdd(x, value) atom_add(&(x), value)\n"
+"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n"
+"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n"
+"\n"
+"\n"
+"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n"
+"\n"
+"#define make_float4 (float4)\n"
+"#define make_float2 (float2)\n"
+"#define make_uint4 (uint4)\n"
+"#define make_int4 (int4)\n"
+"#define make_uint2 (uint2)\n"
+"#define make_int2 (int2)\n"
+"\n"
+"\n"
+"#define max2 max\n"
+"#define min2 min\n"
+"\n"
+"\n"
+"#define WG_SIZE 64\n"
+"\n"
+"\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_worldNormal;\n"
+"	u32 m_coeffs;\n"
+"	int m_batchIdx;\n"
+"\n"
+"	int m_bodyA;//sign bit set for fixed objects\n"
+"	int m_bodyB;\n"
+"}Contact4;\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	int m_n;\n"
+"	int m_start;\n"
+"	int m_staticIdx;\n"
+"	int m_paddings[1];\n"
+"} ConstBuffer;\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	int m_a;\n"
+"	int m_b;\n"
+"	u32 m_idx;\n"
+"}Elem;\n"
+"\n"
+"#define STACK_SIZE (WG_SIZE*10)\n"
+"//#define STACK_SIZE (WG_SIZE)\n"
+"#define RING_SIZE 1024\n"
+"#define RING_SIZE_MASK (RING_SIZE-1)\n"
+"#define CHECK_SIZE (WG_SIZE)\n"
+"\n"
+"\n"
+"#define GET_RING_CAPACITY (RING_SIZE - ldsRingEnd)\n"
+"#define RING_END ldsTmp\n"
+"\n"
+"u32 readBuf(__local u32* buff, int idx)\n"
+"{\n"
+"	idx = idx % (32*CHECK_SIZE);\n"
+"	int bitIdx = idx%32;\n"
+"	int bufIdx = idx/32;\n"
+"	return buff[bufIdx] & (1<<bitIdx);\n"
+"}\n"
+"\n"
+"void writeBuf(__local u32* buff, int idx)\n"
+"{\n"
+"	idx = idx % (32*CHECK_SIZE);\n"
+"	int bitIdx = idx%32;\n"
+"	int bufIdx = idx/32;\n"
+"//	buff[bufIdx] |= (1<<bitIdx);\n"
+"	atom_or( &buff[bufIdx], (1<<bitIdx) );\n"
+"}\n"
+"\n"
+"u32 tryWrite(__local u32* buff, int idx)\n"
+"{\n"
+"	idx = idx % (32*CHECK_SIZE);\n"
+"	int bitIdx = idx%32;\n"
+"	int bufIdx = idx/32;\n"
+"	u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) );\n"
+"	return ((ans >> bitIdx)&1) == 0;\n"
+"}\n"
+"\n"
+"//	batching on the GPU\n"
+"__kernel void CreateBatches( __global const Contact4* gConstraints, __global Contact4* gConstraintsOut,\n"
+"		__global const u32* gN, __global const u32* gStart, \n"
+"		int m_staticIdx )\n"
+"{\n"
+"	__local u32 ldsStackIdx[STACK_SIZE];\n"
+"	__local u32 ldsStackEnd;\n"
+"	__local Elem ldsRingElem[RING_SIZE];\n"
+"	__local u32 ldsRingEnd;\n"
+"	__local u32 ldsTmp;\n"
+"	__local u32 ldsCheckBuffer[CHECK_SIZE];\n"
+"	__local u32 ldsFixedBuffer[CHECK_SIZE];\n"
+"	__local u32 ldsGEnd;\n"
+"	__local u32 ldsDstEnd;\n"
+"\n"
+"	int wgIdx = GET_GROUP_IDX;\n"
+"	int lIdx = GET_LOCAL_IDX;\n"
+"	\n"
+"	const int m_n = gN[wgIdx];\n"
+"	const int m_start = gStart[wgIdx];\n"
+"		\n"
+"	if( lIdx == 0 )\n"
+"	{\n"
+"		ldsRingEnd = 0;\n"
+"		ldsGEnd = 0;\n"
+"		ldsStackEnd = 0;\n"
+"		ldsDstEnd = m_start;\n"
+"	}\n"
+"	\n"
+"//	while(1)\n"
+"//was 250\n"
+"	for(int ie=0; ie<50; ie++)\n"
+"	{\n"
+"		ldsFixedBuffer[lIdx] = 0;\n"
+"\n"
+"		for(int giter=0; giter<4; giter++)\n"
+"		{\n"
+"			int ringCap = GET_RING_CAPACITY;\n"
+"		\n"
+"			//	1. fill ring\n"
+"			if( ldsGEnd < m_n )\n"
+"			{\n"
+"				while( ringCap > WG_SIZE )\n"
+"				{\n"
+"					if( ldsGEnd >= m_n ) break;\n"
+"					if( lIdx < ringCap - WG_SIZE )\n"
+"					{\n"
+"						int srcIdx;\n"
+"						AtomInc1( ldsGEnd, srcIdx );\n"
+"						if( srcIdx < m_n )\n"
+"						{\n"
+"							int dstIdx;\n"
+"							AtomInc1( ldsRingEnd, dstIdx );\n"
+"							\n"
+"							int a = gConstraints[m_start+srcIdx].m_bodyA;\n"
+"							int b = gConstraints[m_start+srcIdx].m_bodyB;\n"
+"							ldsRingElem[dstIdx].m_a = (a>b)? b:a;\n"
+"							ldsRingElem[dstIdx].m_b = (a>b)? a:b;\n"
+"							ldsRingElem[dstIdx].m_idx = srcIdx;\n"
+"						}\n"
+"					}\n"
+"					ringCap = GET_RING_CAPACITY;\n"
+"				}\n"
+"			}\n"
+"\n"
+"			GROUP_LDS_BARRIER;\n"
+"	\n"
+"			//	2. fill stack\n"
+"			__local Elem* dst = ldsRingElem;\n"
+"			if( lIdx == 0 ) RING_END = 0;\n"
+"\n"
+"			int srcIdx=lIdx;\n"
+"			int end = ldsRingEnd;\n"
+"\n"
+"			{\n"
+"				for(int ii=0; ii<end; ii+=WG_SIZE, srcIdx+=WG_SIZE)\n"
+"				{\n"
+"					Elem e;\n"
+"					if(srcIdx<end) e = ldsRingElem[srcIdx];\n"
+"					bool done = (srcIdx<end)?false:true;\n"
+"\n"
+"					for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) ldsCheckBuffer[lIdx] = 0;\n"
+"					\n"
+"					if( !done )\n"
+"					{\n"
+"						int aUsed = readBuf( ldsFixedBuffer, abs(e.m_a));\n"
+"						int bUsed = readBuf( ldsFixedBuffer, abs(e.m_b));\n"
+"\n"
+"						if( aUsed==0 && bUsed==0 )\n"
+"						{\n"
+"							int aAvailable;\n"
+"							int bAvailable;\n"
+"							int ea = abs(e.m_a);\n"
+"							int eb = abs(e.m_b);\n"
+"\n"
+"							aAvailable = tryWrite( ldsCheckBuffer, ea );\n"
+"							bAvailable = tryWrite( ldsCheckBuffer, eb );\n"
+"\n"
+"							bool aStatic = (e.m_a<0) ||(ea==m_staticIdx);\n"
+"							bool bStatic = (e.m_b<0) ||(eb==m_staticIdx);\n"
+"							\n"
+"							aAvailable = aStatic? 1: aAvailable;\n"
+"							bAvailable = bStatic? 1: bAvailable;\n"
+"\n"
+"							bool success = (aAvailable && bAvailable);\n"
+"							if(success)\n"
+"							{\n"
+"							\n"
+"								if (!aStatic)\n"
+"									writeBuf( ldsFixedBuffer, ea );\n"
+"								if (!bStatic)\n"
+"									writeBuf( ldsFixedBuffer, eb );\n"
+"							}\n"
+"							done = success;\n"
+"						}\n"
+"					}\n"
+"\n"
+"					//	put it aside\n"
+"					if(srcIdx<end)\n"
+"					{\n"
+"						if( done )\n"
+"						{\n"
+"							int dstIdx; AtomInc1( ldsStackEnd, dstIdx );\n"
+"							if( dstIdx < STACK_SIZE )\n"
+"								ldsStackIdx[dstIdx] = e.m_idx;\n"
+"							else{\n"
+"								done = false;\n"
+"								AtomAdd( ldsStackEnd, -1 );\n"
+"							}\n"
+"						}\n"
+"						if( !done )\n"
+"						{\n"
+"							int dstIdx; AtomInc1( RING_END, dstIdx );\n"
+"							dst[dstIdx] = e;\n"
+"						}\n"
+"					}\n"
+"\n"
+"					//	if filled, flush\n"
+"					if( ldsStackEnd == STACK_SIZE )\n"
+"					{\n"
+"						for(int i=lIdx; i<STACK_SIZE; i+=WG_SIZE)\n"
+"						{\n"
+"							int idx = m_start + ldsStackIdx[i];\n"
+"							int dstIdx; AtomInc1( ldsDstEnd, dstIdx );\n"
+"							gConstraintsOut[ dstIdx ] = gConstraints[ idx ];\n"
+"							gConstraintsOut[ dstIdx ].m_batchIdx = ie;\n"
+"						}\n"
+"						if( lIdx == 0 ) ldsStackEnd = 0;\n"
+"\n"
+"						//for(int i=lIdx; i<CHECK_SIZE; i+=WG_SIZE) \n"
+"						ldsFixedBuffer[lIdx] = 0;\n"
+"					}\n"
+"				}\n"
+"			}\n"
+"\n"
+"			if( lIdx == 0 ) ldsRingEnd = RING_END;\n"
+"		}\n"
+"\n"
+"		GROUP_LDS_BARRIER;\n"
+"\n"
+"		for(int i=lIdx; i<ldsStackEnd; i+=WG_SIZE)\n"
+"		{\n"
+"			int idx = m_start + ldsStackIdx[i];\n"
+"			int dstIdx; AtomInc1( ldsDstEnd, dstIdx );\n"
+"			gConstraintsOut[ dstIdx ] = gConstraints[ idx ];\n"
+"			gConstraintsOut[ dstIdx ].m_batchIdx = ie;\n"
+"		}\n"
+"\n"
+"		//	in case it couldn't consume any pair. Flush them\n"
+"		//	todo. Serial batch worth while?\n"
+"		if( ldsStackEnd == 0 )\n"
+"		{\n"
+"			for(int i=lIdx; i<ldsRingEnd; i+=WG_SIZE)\n"
+"			{\n"
+"				int idx = m_start + ldsRingElem[i].m_idx;\n"
+"				int dstIdx; AtomInc1( ldsDstEnd, dstIdx );\n"
+"				gConstraintsOut[ dstIdx ] = gConstraints[ idx ];\n"
+"				gConstraintsOut[ dstIdx ].m_batchIdx = 100+i;\n"
+"			}\n"
+"			GROUP_LDS_BARRIER;\n"
+"			if( lIdx == 0 ) ldsRingEnd = 0;\n"
+"		}\n"
+"\n"
+"		if( lIdx == 0 ) ldsStackEnd = 0;\n"
+"\n"
+"		GROUP_LDS_BARRIER;\n"
+"\n"
+"		//	termination\n"
+"		if( ldsGEnd == m_n && ldsRingEnd == 0 )\n"
+"			break;\n"
+"	}\n"
+"\n"
+"\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+;

src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.cl

@@ -0,0 +1,236 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Erwin Coumans
+
+
+#pragma OPENCL EXTENSION cl_amd_printf : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable
+
+#ifdef cl_ext_atomic_counters_32
+#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
+#else
+#define counter32_t volatile __global int*
+#endif
+
+#define SIMD_WIDTH 64
+
+typedef unsigned int u32;
+typedef unsigned short u16;
+typedef unsigned char u8;
+
+#define GET_GROUP_IDX get_group_id(0)
+#define GET_LOCAL_IDX get_local_id(0)
+#define GET_GLOBAL_IDX get_global_id(0)
+#define GET_GROUP_SIZE get_local_size(0)
+#define GET_NUM_GROUPS get_num_groups(0)
+#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)
+#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)
+#define AtomInc(x) atom_inc(&(x))
+#define AtomInc1(x, out) out = atom_inc(&(x))
+#define AppendInc(x, out) out = atomic_inc(x)
+#define AtomAdd(x, value) atom_add(&(x), value)
+#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )
+#define AtomXhg(x, value) atom_xchg ( &(x), value )
+
+
+#define SELECT_UINT4( b, a, condition ) select( b,a,condition )
+
+#define make_float4 (float4)
+#define make_float2 (float2)
+#define make_uint4 (uint4)
+#define make_int4 (int4)
+#define make_uint2 (uint2)
+#define make_int2 (int2)
+
+
+#define max2 max
+#define min2 min
+
+
+#define WG_SIZE 64
+
+
+
+typedef struct 
+{
+	float4 m_worldPos[4];
+	float4 m_worldNormal;
+	u32 m_coeffs;
+	int m_batchIdx;
+
+	int m_bodyAPtrAndSignBit;//sign bit set for fixed objects
+	int m_bodyBPtrAndSignBit;
+}Contact4;
+
+typedef struct 
+{
+	int m_n;
+	int m_start;
+	int m_staticIdx;
+	int m_paddings[1];
+} ConstBuffer;
+
+typedef struct 
+{
+	int m_a;
+	int m_b;
+	u32 m_idx;
+}Elem;
+
+
+
+
+
+//	batching on the GPU
+__kernel void CreateBatchesBruteForce( __global Contact4* gConstraints, 	__global const u32* gN, __global const u32* gStart, int m_staticIdx )
+{
+	int wgIdx = GET_GROUP_IDX;
+	int lIdx = GET_LOCAL_IDX;
+	
+	const int m_n = gN[wgIdx];
+	const int m_start = gStart[wgIdx];
+		
+	if( lIdx == 0 )
+	{
+		for (int i=0;i<m_n;i++)
+		{
+			int srcIdx = i+m_start;
+			int batchIndex = i;
+			gConstraints[ srcIdx ].m_batchIdx = batchIndex;	
+		}
+	}
+}
+
+
+#define CHECK_SIZE (WG_SIZE)
+
+
+
+
+u32 readBuf(__local u32* buff, int idx)
+{
+	idx = idx % (32*CHECK_SIZE);
+	int bitIdx = idx%32;
+	int bufIdx = idx/32;
+	return buff[bufIdx] & (1<<bitIdx);
+}
+
+void writeBuf(__local u32* buff, int idx)
+{
+	idx = idx % (32*CHECK_SIZE);
+	int bitIdx = idx%32;
+	int bufIdx = idx/32;
+	buff[bufIdx] |= (1<<bitIdx);
+	//atom_or( &buff[bufIdx], (1<<bitIdx) );
+}
+
+u32 tryWrite(__local u32* buff, int idx)
+{
+	idx = idx % (32*CHECK_SIZE);
+	int bitIdx = idx%32;
+	int bufIdx = idx/32;
+	u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) );
+	return ((ans >> bitIdx)&1) == 0;
+}
+
+
+//	batching on the GPU
+__kernel void CreateBatchesNew( __global Contact4* gConstraints, __global const u32* gN, __global const u32* gStart, int staticIdx )
+{
+	int wgIdx = GET_GROUP_IDX;
+	int lIdx = GET_LOCAL_IDX;
+	const int numConstraints = gN[wgIdx];
+	const int m_start = gStart[wgIdx];
+		
+	
+	__local u32 ldsFixedBuffer[CHECK_SIZE];
+		
+	
+	
+	
+	
+	if( lIdx == 0 )
+	{
+	
+		
+		__global Contact4* cs = &gConstraints[m_start];	
+	
+		
+		int numValidConstraints = 0;
+		int batchIdx = 0;
+
+		while( numValidConstraints < numConstraints)
+		{
+			int nCurrentBatch = 0;
+			//	clear flag
+	
+			for(int i=0; i<CHECK_SIZE; i++) 
+				ldsFixedBuffer[i] = 0;		
+
+			for(int i=numValidConstraints; i<numConstraints; i++)
+			{
+
+				int bodyAS = cs[i].m_bodyAPtrAndSignBit;
+				int bodyBS = cs[i].m_bodyBPtrAndSignBit;
+				int bodyA = abs(bodyAS);
+				int bodyB = abs(bodyBS);
+				bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx;
+				bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx;
+				int aUnavailable = aIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyA);
+				int bUnavailable = bIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyB);
+				
+				if( aUnavailable==0 && bUnavailable==0 ) // ok
+				{
+					if (!aIsStatic)
+					{
+						writeBuf( ldsFixedBuffer, bodyA );
+					}
+					if (!bIsStatic)
+					{
+						writeBuf( ldsFixedBuffer, bodyB );
+					}
+
+					cs[i].m_batchIdx = batchIdx;
+
+					if (i!=numValidConstraints)
+					{
+						//btSwap(cs[i],cs[numValidConstraints]);
+						
+						Contact4 tmp = cs[i];
+						cs[i] = cs[numValidConstraints];
+						cs[numValidConstraints] = tmp;
+						
+					}
+
+					numValidConstraints++;
+					
+					nCurrentBatch++;
+					if( nCurrentBatch == SIMD_WIDTH)
+					{
+						nCurrentBatch = 0;
+						for(int i=0; i<CHECK_SIZE; i++) 
+							ldsFixedBuffer[i] = 0;
+						
+					}
+				}
+			}//for
+			batchIdx ++;
+		}//while
+	}//if( lIdx == 0 )
+	
+	//return batchIdx;
+}

src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h

@@ -0,0 +1,240 @@
+//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
+static const char* batchingKernelsNewCL= \
+"/*\n"
+"Copyright (c) 2012 Advanced Micro Devices, Inc.  \n"
+"\n"
+"This software is provided 'as-is', without any express or implied warranty.\n"
+"In no event will the authors be held liable for any damages arising from the use of this software.\n"
+"Permission is granted to anyone to use this software for any purpose, \n"
+"including commercial applications, and to alter it and redistribute it freely, \n"
+"subject to the following restrictions:\n"
+"\n"
+"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
+"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
+"3. This notice may not be removed or altered from any source distribution.\n"
+"*/\n"
+"//Originally written by Erwin Coumans\n"
+"\n"
+"\n"
+"#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n"
+"\n"
+"#ifdef cl_ext_atomic_counters_32\n"
+"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n"
+"#else\n"
+"#define counter32_t volatile __global int*\n"
+"#endif\n"
+"\n"
+"#define SIMD_WIDTH 64\n"
+"\n"
+"typedef unsigned int u32;\n"
+"typedef unsigned short u16;\n"
+"typedef unsigned char u8;\n"
+"\n"
+"#define GET_GROUP_IDX get_group_id(0)\n"
+"#define GET_LOCAL_IDX get_local_id(0)\n"
+"#define GET_GLOBAL_IDX get_global_id(0)\n"
+"#define GET_GROUP_SIZE get_local_size(0)\n"
+"#define GET_NUM_GROUPS get_num_groups(0)\n"
+"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n"
+"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n"
+"#define AtomInc(x) atom_inc(&(x))\n"
+"#define AtomInc1(x, out) out = atom_inc(&(x))\n"
+"#define AppendInc(x, out) out = atomic_inc(x)\n"
+"#define AtomAdd(x, value) atom_add(&(x), value)\n"
+"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n"
+"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n"
+"\n"
+"\n"
+"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n"
+"\n"
+"#define make_float4 (float4)\n"
+"#define make_float2 (float2)\n"
+"#define make_uint4 (uint4)\n"
+"#define make_int4 (int4)\n"
+"#define make_uint2 (uint2)\n"
+"#define make_int2 (int2)\n"
+"\n"
+"\n"
+"#define max2 max\n"
+"#define min2 min\n"
+"\n"
+"\n"
+"#define WG_SIZE 64\n"
+"\n"
+"\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_worldNormal;\n"
+"	u32 m_coeffs;\n"
+"	int m_batchIdx;\n"
+"\n"
+"	int m_bodyAPtrAndSignBit;//sign bit set for fixed objects\n"
+"	int m_bodyBPtrAndSignBit;\n"
+"}Contact4;\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	int m_n;\n"
+"	int m_start;\n"
+"	int m_staticIdx;\n"
+"	int m_paddings[1];\n"
+"} ConstBuffer;\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	int m_a;\n"
+"	int m_b;\n"
+"	u32 m_idx;\n"
+"}Elem;\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"//	batching on the GPU\n"
+"__kernel void CreateBatchesBruteForce( __global Contact4* gConstraints, 	__global const u32* gN, __global const u32* gStart, int m_staticIdx )\n"
+"{\n"
+"	int wgIdx = GET_GROUP_IDX;\n"
+"	int lIdx = GET_LOCAL_IDX;\n"
+"	\n"
+"	const int m_n = gN[wgIdx];\n"
+"	const int m_start = gStart[wgIdx];\n"
+"		\n"
+"	if( lIdx == 0 )\n"
+"	{\n"
+"		for (int i=0;i<m_n;i++)\n"
+"		{\n"
+"			int srcIdx = i+m_start;\n"
+"			int batchIndex = i;\n"
+"			gConstraints[ srcIdx ].m_batchIdx = batchIndex;	\n"
+"		}\n"
+"	}\n"
+"}\n"
+"\n"
+"\n"
+"#define CHECK_SIZE (WG_SIZE)\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"u32 readBuf(__local u32* buff, int idx)\n"
+"{\n"
+"	idx = idx % (32*CHECK_SIZE);\n"
+"	int bitIdx = idx%32;\n"
+"	int bufIdx = idx/32;\n"
+"	return buff[bufIdx] & (1<<bitIdx);\n"
+"}\n"
+"\n"
+"void writeBuf(__local u32* buff, int idx)\n"
+"{\n"
+"	idx = idx % (32*CHECK_SIZE);\n"
+"	int bitIdx = idx%32;\n"
+"	int bufIdx = idx/32;\n"
+"	buff[bufIdx] |= (1<<bitIdx);\n"
+"	//atom_or( &buff[bufIdx], (1<<bitIdx) );\n"
+"}\n"
+"\n"
+"u32 tryWrite(__local u32* buff, int idx)\n"
+"{\n"
+"	idx = idx % (32*CHECK_SIZE);\n"
+"	int bitIdx = idx%32;\n"
+"	int bufIdx = idx/32;\n"
+"	u32 ans = (u32)atom_or( &buff[bufIdx], (1<<bitIdx) );\n"
+"	return ((ans >> bitIdx)&1) == 0;\n"
+"}\n"
+"\n"
+"\n"
+"//	batching on the GPU\n"
+"__kernel void CreateBatchesNew( __global Contact4* gConstraints, __global const u32* gN, __global const u32* gStart, int staticIdx )\n"
+"{\n"
+"	int wgIdx = GET_GROUP_IDX;\n"
+"	int lIdx = GET_LOCAL_IDX;\n"
+"	const int numConstraints = gN[wgIdx];\n"
+"	const int m_start = gStart[wgIdx];\n"
+"		\n"
+"	\n"
+"	__local u32 ldsFixedBuffer[CHECK_SIZE];\n"
+"		\n"
+"	\n"
+"	\n"
+"	\n"
+"	\n"
+"	if( lIdx == 0 )\n"
+"	{\n"
+"	\n"
+"		\n"
+"		__global Contact4* cs = &gConstraints[m_start];	\n"
+"	\n"
+"		\n"
+"		int numValidConstraints = 0;\n"
+"		int batchIdx = 0;\n"
+"\n"
+"		while( numValidConstraints < numConstraints)\n"
+"		{\n"
+"			int nCurrentBatch = 0;\n"
+"			//	clear flag\n"
+"	\n"
+"			for(int i=0; i<CHECK_SIZE; i++) \n"
+"				ldsFixedBuffer[i] = 0;		\n"
+"\n"
+"			for(int i=numValidConstraints; i<numConstraints; i++)\n"
+"			{\n"
+"\n"
+"				int bodyAS = cs[i].m_bodyAPtrAndSignBit;\n"
+"				int bodyBS = cs[i].m_bodyBPtrAndSignBit;\n"
+"				int bodyA = abs(bodyAS);\n"
+"				int bodyB = abs(bodyBS);\n"
+"				bool aIsStatic = (bodyAS<0) || bodyAS==staticIdx;\n"
+"				bool bIsStatic = (bodyBS<0) || bodyBS==staticIdx;\n"
+"				int aUnavailable = aIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyA);\n"
+"				int bUnavailable = bIsStatic ? 0 : readBuf( ldsFixedBuffer, bodyB);\n"
+"				\n"
+"				if( aUnavailable==0 && bUnavailable==0 ) // ok\n"
+"				{\n"
+"					if (!aIsStatic)\n"
+"					{\n"
+"						writeBuf( ldsFixedBuffer, bodyA );\n"
+"					}\n"
+"					if (!bIsStatic)\n"
+"					{\n"
+"						writeBuf( ldsFixedBuffer, bodyB );\n"
+"					}\n"
+"\n"
+"					cs[i].m_batchIdx = batchIdx;\n"
+"\n"
+"					if (i!=numValidConstraints)\n"
+"					{\n"
+"						//b3Swap(cs[i],cs[numValidConstraints]);\n"
+"						\n"
+"						Contact4 tmp = cs[i];\n"
+"						cs[i] = cs[numValidConstraints];\n"
+"						cs[numValidConstraints] = tmp;\n"
+"						\n"
+"					}\n"
+"\n"
+"					numValidConstraints++;\n"
+"					\n"
+"					nCurrentBatch++;\n"
+"					if( nCurrentBatch == SIMD_WIDTH)\n"
+"					{\n"
+"						nCurrentBatch = 0;\n"
+"						for(int i=0; i<CHECK_SIZE; i++) \n"
+"							ldsFixedBuffer[i] = 0;\n"
+"						\n"
+"					}\n"
+"				}\n"
+"			}//for\n"
+"			batchIdx ++;\n"
+"		}//while\n"
+"	}//if( lIdx == 0 )\n"
+"	\n"
+"	//return batchIdx;\n"
+"}\n"
+"\n"
+;

src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.cl

@@ -0,0 +1,92 @@
+
+float4 quatMult(float4 q1, float4 q2)
+{
+	float4 q;
+	q.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
+	q.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
+	q.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
+	q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z; 
+	return q;
+}
+
+float4 quatNorm(float4 q)
+{
+	float len = native_sqrt(dot(q, q));
+	if(len > 0.f)
+	{
+		q *= 1.f / len;
+	}
+	else
+	{
+		q.x = q.y = q.z = 0.f;
+		q.w = 1.f;
+	}
+	return q;
+}
+
+
+typedef struct
+{
+	float4 m_pos;
+	float4 m_quat;
+	float4 m_linVel;
+	float4 m_angVel;
+
+	unsigned int m_collidableIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
+} Body;
+
+
+
+
+__kernel void 
+  integrateTransformsKernel( __global Body* bodies,const int numNodes, float timeStep, float angularDamping, float4 gravityAcceleration)
+{
+	int nodeID = get_global_id(0);
+	float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
+	if( nodeID < numNodes && (bodies[nodeID].m_invMass != 0.f))
+	{
+		//angular velocity
+		{
+			float4 axis;
+			//add some hardcoded angular damping
+			bodies[nodeID].m_angVel.x *= angularDamping;
+			bodies[nodeID].m_angVel.y *= angularDamping;
+			bodies[nodeID].m_angVel.z *= angularDamping;
+			
+			float4 angvel = bodies[nodeID].m_angVel;
+			float fAngle = native_sqrt(dot(angvel, angvel));
+			//limit the angular motion
+			if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+			{
+				fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
+			}
+			if(fAngle < 0.001f)
+			{
+				// use Taylor's expansions of sync function
+				axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
+			}
+			else
+			{
+				// sync(fAngle) = sin(c*fAngle)/t
+				axis = angvel * ( native_sin(0.5f * fAngle * timeStep) / fAngle);
+			}
+			float4 dorn = axis;
+			dorn.w = native_cos(fAngle * timeStep * 0.5f);
+			float4 orn0 = bodies[nodeID].m_quat;
+
+			float4 predictedOrn = quatMult(dorn, orn0);
+			predictedOrn = quatNorm(predictedOrn);
+			bodies[nodeID].m_quat=predictedOrn;
+		}
+
+		//linear velocity		
+		bodies[nodeID].m_pos +=  bodies[nodeID].m_linVel * timeStep;
+		
+		//apply gravity
+		bodies[nodeID].m_linVel += gravityAcceleration * timeStep;
+		
+	}
+}

src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h

@@ -0,0 +1,96 @@
+//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
+static const char* integrateKernelCL= \
+"\n"
+"float4 quatMult(float4 q1, float4 q2)\n"
+"{\n"
+"	float4 q;\n"
+"	q.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;\n"
+"	q.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;\n"
+"	q.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;\n"
+"	q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z; \n"
+"	return q;\n"
+"}\n"
+"\n"
+"float4 quatNorm(float4 q)\n"
+"{\n"
+"	float len = native_sqrt(dot(q, q));\n"
+"	if(len > 0.f)\n"
+"	{\n"
+"		q *= 1.f / len;\n"
+"	}\n"
+"	else\n"
+"	{\n"
+"		q.x = q.y = q.z = 0.f;\n"
+"		q.w = 1.f;\n"
+"	}\n"
+"	return q;\n"
+"}\n"
+"\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_pos;\n"
+"	float4 m_quat;\n"
+"	float4 m_linVel;\n"
+"	float4 m_angVel;\n"
+"\n"
+"	unsigned int m_collidableIdx;\n"
+"	float m_invMass;\n"
+"	float m_restituitionCoeff;\n"
+"	float m_frictionCoeff;\n"
+"} Body;\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__kernel void \n"
+"  integrateTransformsKernel( __global Body* bodies,const int numNodes, float timeStep, float angularDamping, float4 gravityAcceleration)\n"
+"{\n"
+"	int nodeID = get_global_id(0);\n"
+"	float B3_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);\n"
+"	if( nodeID < numNodes && (bodies[nodeID].m_invMass != 0.f))\n"
+"	{\n"
+"		//angular velocity\n"
+"		{\n"
+"			float4 axis;\n"
+"			//add some hardcoded angular damping\n"
+"			bodies[nodeID].m_angVel.x *= angularDamping;\n"
+"			bodies[nodeID].m_angVel.y *= angularDamping;\n"
+"			bodies[nodeID].m_angVel.z *= angularDamping;\n"
+"			\n"
+"			float4 angvel = bodies[nodeID].m_angVel;\n"
+"			float fAngle = native_sqrt(dot(angvel, angvel));\n"
+"			//limit the angular motion\n"
+"			if(fAngle*timeStep > B3_GPU_ANGULAR_MOTION_THRESHOLD)\n"
+"			{\n"
+"				fAngle = B3_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;\n"
+"			}\n"
+"			if(fAngle < 0.001f)\n"
+"			{\n"
+"				// use Taylor's expansions of sync function\n"
+"				axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);\n"
+"			}\n"
+"			else\n"
+"			{\n"
+"				// sync(fAngle) = sin(c*fAngle)/t\n"
+"				axis = angvel * ( native_sin(0.5f * fAngle * timeStep) / fAngle);\n"
+"			}\n"
+"			float4 dorn = axis;\n"
+"			dorn.w = native_cos(fAngle * timeStep * 0.5f);\n"
+"			float4 orn0 = bodies[nodeID].m_quat;\n"
+"\n"
+"			float4 predictedOrn = quatMult(dorn, orn0);\n"
+"			predictedOrn = quatNorm(predictedOrn);\n"
+"			bodies[nodeID].m_quat=predictedOrn;\n"
+"		}\n"
+"\n"
+"		//linear velocity		\n"
+"		bodies[nodeID].m_pos +=  bodies[nodeID].m_linVel * timeStep;\n"
+"		\n"
+"		//apply gravity\n"
+"		bodies[nodeID].m_linVel += gravityAcceleration * timeStep;\n"
+"		\n"
+"	}\n"
+"}\n"
+"\n"
+;

src/Bullet3OpenCL/RigidBody/kernels/solveContact.cl

@@ -0,0 +1,476 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Takahiro Harada
+
+
+//#pragma OPENCL EXTENSION cl_amd_printf : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable
+
+
+#ifdef cl_ext_atomic_counters_32
+#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
+#else
+#define counter32_t volatile global int*
+#endif
+
+typedef unsigned int u32;
+typedef unsigned short u16;
+typedef unsigned char u8;
+
+#define GET_GROUP_IDX get_group_id(0)
+#define GET_LOCAL_IDX get_local_id(0)
+#define GET_GLOBAL_IDX get_global_id(0)
+#define GET_GROUP_SIZE get_local_size(0)
+#define GET_NUM_GROUPS get_num_groups(0)
+#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)
+#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)
+#define AtomInc(x) atom_inc(&(x))
+#define AtomInc1(x, out) out = atom_inc(&(x))
+#define AppendInc(x, out) out = atomic_inc(x)
+#define AtomAdd(x, value) atom_add(&(x), value)
+#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )
+#define AtomXhg(x, value) atom_xchg ( &(x), value )
+
+
+#define SELECT_UINT4( b, a, condition ) select( b,a,condition )
+
+#define mymake_float4 (float4)
+//#define make_float2 (float2)
+//#define make_uint4 (uint4)
+//#define make_int4 (int4)
+//#define make_uint2 (uint2)
+//#define make_int2 (int2)
+
+
+#define max2 max
+#define min2 min
+
+
+///////////////////////////////////////
+//	Vector
+///////////////////////////////////////
+
+
+
+
+__inline
+float4 fastNormalize4(float4 v)
+{
+	return fast_normalize(v);
+}
+
+
+
+__inline
+float4 cross3(float4 a, float4 b)
+{
+	return cross(a,b);
+}
+
+__inline
+float dot3F4(float4 a, float4 b)
+{
+	float4 a1 = mymake_float4(a.xyz,0.f);
+	float4 b1 = mymake_float4(b.xyz,0.f);
+	return dot(a1, b1);
+}
+
+
+
+
+__inline
+float4 normalize3(const float4 a)
+{
+	float4 n = mymake_float4(a.x, a.y, a.z, 0.f);
+	return fastNormalize4( n );
+//	float length = sqrtf(dot3F4(a, a));
+//	return 1.f/length * a;
+}
+
+
+
+
+///////////////////////////////////////
+//	Matrix3x3
+///////////////////////////////////////
+
+typedef struct
+{
+	float4 m_row[3];
+}Matrix3x3;
+
+
+
+
+
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b);
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b);
+
+
+
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b)
+{
+	float4 ans;
+	ans.x = dot3F4( a.m_row[0], b );
+	ans.y = dot3F4( a.m_row[1], b );
+	ans.z = dot3F4( a.m_row[2], b );
+	ans.w = 0.f;
+	return ans;
+}
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b)
+{
+	float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
+	float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
+	float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
+
+	float4 ans;
+	ans.x = dot3F4( a, colx );
+	ans.y = dot3F4( a, coly );
+	ans.z = dot3F4( a, colz );
+	return ans;
+}
+
+///////////////////////////////////////
+//	Quaternion
+///////////////////////////////////////
+
+typedef float4 Quaternion;
+
+
+
+
+
+
+
+#define WG_SIZE 64
+
+typedef struct
+{
+	float4 m_pos;
+	Quaternion m_quat;
+	float4 m_linVel;
+	float4 m_angVel;
+
+	u32 m_shapeIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
+} Body;
+
+typedef struct
+{
+	Matrix3x3 m_invInertia;
+	Matrix3x3 m_initInvInertia;
+} Shape;
+
+typedef struct
+{
+	float4 m_linear;
+	float4 m_worldPos[4];
+	float4 m_center;	
+	float m_jacCoeffInv[4];
+	float m_b[4];
+	float m_appliedRambdaDt[4];
+
+	float m_fJacCoeffInv[2];	
+	float m_fAppliedRambdaDt[2];	
+
+	u32 m_bodyA;
+	u32 m_bodyB;
+
+	int m_batchIdx;
+	u32 m_paddings[1];
+} Constraint4;
+
+typedef struct
+{
+	float4 m_worldPos[4];
+	float4 m_worldNormal;
+	u32 m_coeffs;
+	int m_batchIdx;
+
+	int m_bodyAPtrAndSignBit;
+	int m_bodyBPtrAndSignBit;
+} Contact4;
+
+typedef struct
+{
+	int m_nConstraints;
+	int m_start;
+	int m_batchIdx;
+	int m_nSplit;
+//	int m_paddings[1];
+} ConstBuffer;
+
+typedef struct
+{
+	int m_solveFriction;
+	int m_maxBatch;	//	long batch really kills the performance
+	int m_batchIdx;
+	int m_nSplit;
+//	int m_paddings[1];
+} ConstBufferBatchSolve;
+
+void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1);
+
+void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
+{
+	*linear = mymake_float4(-n.xyz,0.f);
+	*angular0 = -cross3(r0, n);
+	*angular1 = cross3(r1, n);
+}
+
+float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 );
+
+float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )
+{
+	return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);
+}
+
+
+float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,
+				   float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1);
+
+float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,
+					float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)
+{
+	//	linear0,1 are normlized
+	float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;
+	float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);
+	float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;
+	float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);
+	return -1.f/(jmj0+jmj1+jmj2+jmj3);
+}
+
+
+void solveContact(__global Constraint4* cs,
+				  float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,
+				  float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB);
+
+void solveContact(__global Constraint4* cs,
+			float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,
+			float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB)
+{
+	float minRambdaDt = 0;
+	float maxRambdaDt = FLT_MAX;
+
+	for(int ic=0; ic<4; ic++)
+	{
+		if( cs->m_jacCoeffInv[ic] == 0.f ) continue;
+
+		float4 angular0, angular1, linear;
+		float4 r0 = cs->m_worldPos[ic] - posA;
+		float4 r1 = cs->m_worldPos[ic] - posB;
+		setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );
+
+		float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, 
+			*linVelA, *angVelA, *linVelB, *angVelB ) + cs->m_b[ic];
+		rambdaDt *= cs->m_jacCoeffInv[ic];
+
+		{
+			float prevSum = cs->m_appliedRambdaDt[ic];
+			float updated = prevSum;
+			updated += rambdaDt;
+			updated = max2( updated, minRambdaDt );
+			updated = min2( updated, maxRambdaDt );
+			rambdaDt = updated - prevSum;
+			cs->m_appliedRambdaDt[ic] = updated;
+		}
+
+		float4 linImp0 = invMassA*linear*rambdaDt;
+		float4 linImp1 = invMassB*(-linear)*rambdaDt;
+		float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
+		float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
+
+		*linVelA += linImp0;
+		*angVelA += angImp0;
+		*linVelB += linImp1;
+		*angVelB += angImp1;
+	}
+}
+
+void btPlaneSpace1 (const float4* n, float4* p, float4* q);
+ void btPlaneSpace1 (const float4* n, float4* p, float4* q)
+{
+  if (fabs(n[0].z) > 0.70710678f) {
+    // choose p in y-z plane
+    float a = n[0].y*n[0].y + n[0].z*n[0].z;
+    float k = 1.f/sqrt(a);
+    p[0].x = 0;
+	p[0].y = -n[0].z*k;
+	p[0].z = n[0].y*k;
+    // set q = n x p
+    q[0].x = a*k;
+	q[0].y = -n[0].x*p[0].z;
+	q[0].z = n[0].x*p[0].y;
+  }
+  else {
+    // choose p in x-y plane
+    float a = n[0].x*n[0].x + n[0].y*n[0].y;
+    float k = 1.f/sqrt(a);
+    p[0].x = -n[0].y*k;
+	p[0].y = n[0].x*k;
+	p[0].z = 0;
+    // set q = n x p
+    q[0].x = -n[0].z*p[0].y;
+	q[0].y = n[0].z*p[0].x;
+	q[0].z = a*k;
+  }
+}
+
+void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);
+void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)
+{
+	//float frictionCoeff = ldsCs[0].m_linear.w;
+	int aIdx = ldsCs[0].m_bodyA;
+	int bIdx = ldsCs[0].m_bodyB;
+
+	float4 posA = gBodies[aIdx].m_pos;
+	float4 linVelA = gBodies[aIdx].m_linVel;
+	float4 angVelA = gBodies[aIdx].m_angVel;
+	float invMassA = gBodies[aIdx].m_invMass;
+	Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;
+
+	float4 posB = gBodies[bIdx].m_pos;
+	float4 linVelB = gBodies[bIdx].m_linVel;
+	float4 angVelB = gBodies[bIdx].m_angVel;
+	float invMassB = gBodies[bIdx].m_invMass;
+	Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;
+
+	solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,
+			posB, &linVelB, &angVelB, invMassB, invInertiaB );
+
+  if (gBodies[aIdx].m_invMass)
+  {
+		gBodies[aIdx].m_linVel = linVelA;
+		gBodies[aIdx].m_angVel = angVelA;
+	} else
+	{
+		gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);
+		gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);
+	
+	}
+	if (gBodies[bIdx].m_invMass)
+  {
+		gBodies[bIdx].m_linVel = linVelB;
+		gBodies[bIdx].m_angVel = angVelB;
+	} else
+	{
+		gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);
+		gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);
+	
+	}
+
+}
+
+
+
+typedef struct 
+{
+	int m_valInt0;
+	int m_valInt1;
+	int m_valInt2;
+	int m_valInt3;
+
+	float m_val0;
+	float m_val1;
+	float m_val2;
+	float m_val3;
+} SolverDebugInfo;
+
+
+
+
+__kernel
+__attribute__((reqd_work_group_size(WG_SIZE,1,1)))
+void BatchSolveKernelContact(__global Body* gBodies,
+                      __global Shape* gShapes,
+                      __global Constraint4* gConstraints,
+                      __global int* gN,
+                      __global int* gOffsets,
+                       int maxBatch,
+                       int bIdx,
+                       int nSplit
+                      )
+{
+	//__local int ldsBatchIdx[WG_SIZE+1];
+	__local int ldsCurBatch;
+	__local int ldsNextBatch;
+	__local int ldsStart;
+
+	int lIdx = GET_LOCAL_IDX;
+	int wgIdx = GET_GROUP_IDX;
+
+//	int gIdx = GET_GLOBAL_IDX;
+//	debugInfo[gIdx].m_valInt0 = gIdx;
+	//debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;
+
+
+	int xIdx = (wgIdx/(nSplit/2))*2 + (bIdx&1);
+	int yIdx = (wgIdx%(nSplit/2))*2 + (bIdx>>1);
+	int cellIdx = xIdx+yIdx*nSplit;
+	
+	if( gN[cellIdx] == 0 ) 
+		return;
+
+	const int start = gOffsets[cellIdx];
+	const int end = start + gN[cellIdx];
+
+	
+	if( lIdx == 0 )
+	{
+		ldsCurBatch = 0;
+		ldsNextBatch = 0;
+		ldsStart = start;
+	}
+
+
+	GROUP_LDS_BARRIER;
+
+	int idx=ldsStart+lIdx;
+	while (ldsCurBatch < maxBatch)
+	{
+		for(; idx<end; )
+		{
+			if (gConstraints[idx].m_batchIdx == ldsCurBatch)
+			{
+					solveContactConstraint( gBodies, gShapes, &gConstraints[idx] );
+
+				 idx+=64;
+			} else
+			{
+				break;
+			}
+		}
+		GROUP_LDS_BARRIER;
+		if( lIdx == 0 )
+		{
+			ldsCurBatch++;
+		}
+		GROUP_LDS_BARRIER;
+	}
+	
+    
+}

src/Bullet3OpenCL/RigidBody/kernels/solveContact.h

@@ -0,0 +1,480 @@
+//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
+static const char* solveContactCL= \
+"/*\n"
+"Copyright (c) 2012 Advanced Micro Devices, Inc.  \n"
+"\n"
+"This software is provided 'as-is', without any express or implied warranty.\n"
+"In no event will the authors be held liable for any damages arising from the use of this software.\n"
+"Permission is granted to anyone to use this software for any purpose, \n"
+"including commercial applications, and to alter it and redistribute it freely, \n"
+"subject to the following restrictions:\n"
+"\n"
+"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
+"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
+"3. This notice may not be removed or altered from any source distribution.\n"
+"*/\n"
+"//Originally written by Takahiro Harada\n"
+"\n"
+"\n"
+"//#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n"
+"\n"
+"\n"
+"#ifdef cl_ext_atomic_counters_32\n"
+"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n"
+"#else\n"
+"#define counter32_t volatile global int*\n"
+"#endif\n"
+"\n"
+"typedef unsigned int u32;\n"
+"typedef unsigned short u16;\n"
+"typedef unsigned char u8;\n"
+"\n"
+"#define GET_GROUP_IDX get_group_id(0)\n"
+"#define GET_LOCAL_IDX get_local_id(0)\n"
+"#define GET_GLOBAL_IDX get_global_id(0)\n"
+"#define GET_GROUP_SIZE get_local_size(0)\n"
+"#define GET_NUM_GROUPS get_num_groups(0)\n"
+"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n"
+"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n"
+"#define AtomInc(x) atom_inc(&(x))\n"
+"#define AtomInc1(x, out) out = atom_inc(&(x))\n"
+"#define AppendInc(x, out) out = atomic_inc(x)\n"
+"#define AtomAdd(x, value) atom_add(&(x), value)\n"
+"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n"
+"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n"
+"\n"
+"\n"
+"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n"
+"\n"
+"#define mymake_float4 (float4)\n"
+"//#define make_float2 (float2)\n"
+"//#define make_uint4 (uint4)\n"
+"//#define make_int4 (int4)\n"
+"//#define make_uint2 (uint2)\n"
+"//#define make_int2 (int2)\n"
+"\n"
+"\n"
+"#define max2 max\n"
+"#define min2 min\n"
+"\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Vector\n"
+"///////////////////////////////////////\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 fastNormalize4(float4 v)\n"
+"{\n"
+"	return fast_normalize(v);\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 cross3(float4 a, float4 b)\n"
+"{\n"
+"	return cross(a,b);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float dot3F4(float4 a, float4 b)\n"
+"{\n"
+"	float4 a1 = mymake_float4(a.xyz,0.f);\n"
+"	float4 b1 = mymake_float4(b.xyz,0.f);\n"
+"	return dot(a1, b1);\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 normalize3(const float4 a)\n"
+"{\n"
+"	float4 n = mymake_float4(a.x, a.y, a.z, 0.f);\n"
+"	return fastNormalize4( n );\n"
+"//	float length = sqrtf(dot3F4(a, a));\n"
+"//	return 1.f/length * a;\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Matrix3x3\n"
+"///////////////////////////////////////\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_row[3];\n"
+"}Matrix3x3;\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b);\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b);\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b)\n"
+"{\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a.m_row[0], b );\n"
+"	ans.y = dot3F4( a.m_row[1], b );\n"
+"	ans.z = dot3F4( a.m_row[2], b );\n"
+"	ans.w = 0.f;\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b)\n"
+"{\n"
+"	float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n"
+"	float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n"
+"	float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n"
+"\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a, colx );\n"
+"	ans.y = dot3F4( a, coly );\n"
+"	ans.z = dot3F4( a, colz );\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Quaternion\n"
+"///////////////////////////////////////\n"
+"\n"
+"typedef float4 Quaternion;\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"#define WG_SIZE 64\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_pos;\n"
+"	Quaternion m_quat;\n"
+"	float4 m_linVel;\n"
+"	float4 m_angVel;\n"
+"\n"
+"	u32 m_shapeIdx;\n"
+"	float m_invMass;\n"
+"	float m_restituitionCoeff;\n"
+"	float m_frictionCoeff;\n"
+"} Body;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	Matrix3x3 m_invInertia;\n"
+"	Matrix3x3 m_initInvInertia;\n"
+"} Shape;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_linear;\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_center;	\n"
+"	float m_jacCoeffInv[4];\n"
+"	float m_b[4];\n"
+"	float m_appliedRambdaDt[4];\n"
+"\n"
+"	float m_fJacCoeffInv[2];	\n"
+"	float m_fAppliedRambdaDt[2];	\n"
+"\n"
+"	u32 m_bodyA;\n"
+"	u32 m_bodyB;\n"
+"\n"
+"	int m_batchIdx;\n"
+"	u32 m_paddings[1];\n"
+"} Constraint4;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_worldNormal;\n"
+"	u32 m_coeffs;\n"
+"	int m_batchIdx;\n"
+"\n"
+"	int m_bodyAPtrAndSignBit;\n"
+"	int m_bodyBPtrAndSignBit;\n"
+"} Contact4;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_nConstraints;\n"
+"	int m_start;\n"
+"	int m_batchIdx;\n"
+"	int m_nSplit;\n"
+"//	int m_paddings[1];\n"
+"} ConstBuffer;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_solveFriction;\n"
+"	int m_maxBatch;	//	long batch really kills the performance\n"
+"	int m_batchIdx;\n"
+"	int m_nSplit;\n"
+"//	int m_paddings[1];\n"
+"} ConstBufferBatchSolve;\n"
+"\n"
+"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1);\n"
+"\n"
+"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
+"{\n"
+"	*linear = mymake_float4(-n.xyz,0.f);\n"
+"	*angular0 = -cross3(r0, n);\n"
+"	*angular1 = cross3(r1, n);\n"
+"}\n"
+"\n"
+"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 );\n"
+"\n"
+"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n"
+"{\n"
+"	return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n"
+"}\n"
+"\n"
+"\n"
+"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n"
+"				   float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1);\n"
+"\n"
+"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n"
+"					float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)\n"
+"{\n"
+"	//	linear0,1 are normlized\n"
+"	float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n"
+"	float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n"
+"	float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n"
+"	float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n"
+"	return -1.f/(jmj0+jmj1+jmj2+jmj3);\n"
+"}\n"
+"\n"
+"\n"
+"void solveContact(__global Constraint4* cs,\n"
+"				  float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n"
+"				  float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB);\n"
+"\n"
+"void solveContact(__global Constraint4* cs,\n"
+"			float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n"
+"			float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB)\n"
+"{\n"
+"	float minRambdaDt = 0;\n"
+"	float maxRambdaDt = FLT_MAX;\n"
+"\n"
+"	for(int ic=0; ic<4; ic++)\n"
+"	{\n"
+"		if( cs->m_jacCoeffInv[ic] == 0.f ) continue;\n"
+"\n"
+"		float4 angular0, angular1, linear;\n"
+"		float4 r0 = cs->m_worldPos[ic] - posA;\n"
+"		float4 r1 = cs->m_worldPos[ic] - posB;\n"
+"		setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n"
+"\n"
+"		float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n"
+"			*linVelA, *angVelA, *linVelB, *angVelB ) + cs->m_b[ic];\n"
+"		rambdaDt *= cs->m_jacCoeffInv[ic];\n"
+"\n"
+"		{\n"
+"			float prevSum = cs->m_appliedRambdaDt[ic];\n"
+"			float updated = prevSum;\n"
+"			updated += rambdaDt;\n"
+"			updated = max2( updated, minRambdaDt );\n"
+"			updated = min2( updated, maxRambdaDt );\n"
+"			rambdaDt = updated - prevSum;\n"
+"			cs->m_appliedRambdaDt[ic] = updated;\n"
+"		}\n"
+"\n"
+"		float4 linImp0 = invMassA*linear*rambdaDt;\n"
+"		float4 linImp1 = invMassB*(-linear)*rambdaDt;\n"
+"		float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n"
+"		float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n"
+"\n"
+"		*linVelA += linImp0;\n"
+"		*angVelA += angImp0;\n"
+"		*linVelB += linImp1;\n"
+"		*angVelB += angImp1;\n"
+"	}\n"
+"}\n"
+"\n"
+"void b3PlaneSpace1 (const float4* n, float4* p, float4* q);\n"
+" void b3PlaneSpace1 (const float4* n, float4* p, float4* q)\n"
+"{\n"
+"  if (fabs(n[0].z) > 0.70710678f) {\n"
+"    // choose p in y-z plane\n"
+"    float a = n[0].y*n[0].y + n[0].z*n[0].z;\n"
+"    float k = 1.f/sqrt(a);\n"
+"    p[0].x = 0;\n"
+"	p[0].y = -n[0].z*k;\n"
+"	p[0].z = n[0].y*k;\n"
+"    // set q = n x p\n"
+"    q[0].x = a*k;\n"
+"	q[0].y = -n[0].x*p[0].z;\n"
+"	q[0].z = n[0].x*p[0].y;\n"
+"  }\n"
+"  else {\n"
+"    // choose p in x-y plane\n"
+"    float a = n[0].x*n[0].x + n[0].y*n[0].y;\n"
+"    float k = 1.f/sqrt(a);\n"
+"    p[0].x = -n[0].y*k;\n"
+"	p[0].y = n[0].x*k;\n"
+"	p[0].z = 0;\n"
+"    // set q = n x p\n"
+"    q[0].x = -n[0].z*p[0].y;\n"
+"	q[0].y = n[0].z*p[0].x;\n"
+"	q[0].z = a*k;\n"
+"  }\n"
+"}\n"
+"\n"
+"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);\n"
+"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)\n"
+"{\n"
+"	//float frictionCoeff = ldsCs[0].m_linear.w;\n"
+"	int aIdx = ldsCs[0].m_bodyA;\n"
+"	int bIdx = ldsCs[0].m_bodyB;\n"
+"\n"
+"	float4 posA = gBodies[aIdx].m_pos;\n"
+"	float4 linVelA = gBodies[aIdx].m_linVel;\n"
+"	float4 angVelA = gBodies[aIdx].m_angVel;\n"
+"	float invMassA = gBodies[aIdx].m_invMass;\n"
+"	Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n"
+"\n"
+"	float4 posB = gBodies[bIdx].m_pos;\n"
+"	float4 linVelB = gBodies[bIdx].m_linVel;\n"
+"	float4 angVelB = gBodies[bIdx].m_angVel;\n"
+"	float invMassB = gBodies[bIdx].m_invMass;\n"
+"	Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n"
+"\n"
+"	solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n"
+"			posB, &linVelB, &angVelB, invMassB, invInertiaB );\n"
+"\n"
+"  if (gBodies[aIdx].m_invMass)\n"
+"  {\n"
+"		gBodies[aIdx].m_linVel = linVelA;\n"
+"		gBodies[aIdx].m_angVel = angVelA;\n"
+"	} else\n"
+"	{\n"
+"		gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);\n"
+"		gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);\n"
+"	\n"
+"	}\n"
+"	if (gBodies[bIdx].m_invMass)\n"
+"  {\n"
+"		gBodies[bIdx].m_linVel = linVelB;\n"
+"		gBodies[bIdx].m_angVel = angVelB;\n"
+"	} else\n"
+"	{\n"
+"		gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);\n"
+"		gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);\n"
+"	\n"
+"	}\n"
+"\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	int m_valInt0;\n"
+"	int m_valInt1;\n"
+"	int m_valInt2;\n"
+"	int m_valInt3;\n"
+"\n"
+"	float m_val0;\n"
+"	float m_val1;\n"
+"	float m_val2;\n"
+"	float m_val3;\n"
+"} SolverDebugInfo;\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__kernel\n"
+"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n"
+"void BatchSolveKernelContact(__global Body* gBodies,\n"
+"                      __global Shape* gShapes,\n"
+"                      __global Constraint4* gConstraints,\n"
+"                      __global int* gN,\n"
+"                      __global int* gOffsets,\n"
+"                       int maxBatch,\n"
+"                       int bIdx,\n"
+"                       int nSplit\n"
+"                      )\n"
+"{\n"
+"	//__local int ldsBatchIdx[WG_SIZE+1];\n"
+"	__local int ldsCurBatch;\n"
+"	__local int ldsNextBatch;\n"
+"	__local int ldsStart;\n"
+"\n"
+"	int lIdx = GET_LOCAL_IDX;\n"
+"	int wgIdx = GET_GROUP_IDX;\n"
+"\n"
+"//	int gIdx = GET_GLOBAL_IDX;\n"
+"//	debugInfo[gIdx].m_valInt0 = gIdx;\n"
+"	//debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;\n"
+"\n"
+"\n"
+"	int xIdx = (wgIdx/(nSplit/2))*2 + (bIdx&1);\n"
+"	int yIdx = (wgIdx%(nSplit/2))*2 + (bIdx>>1);\n"
+"	int cellIdx = xIdx+yIdx*nSplit;\n"
+"	\n"
+"	if( gN[cellIdx] == 0 ) \n"
+"		return;\n"
+"\n"
+"	const int start = gOffsets[cellIdx];\n"
+"	const int end = start + gN[cellIdx];\n"
+"\n"
+"	\n"
+"	if( lIdx == 0 )\n"
+"	{\n"
+"		ldsCurBatch = 0;\n"
+"		ldsNextBatch = 0;\n"
+"		ldsStart = start;\n"
+"	}\n"
+"\n"
+"\n"
+"	GROUP_LDS_BARRIER;\n"
+"\n"
+"	int idx=ldsStart+lIdx;\n"
+"	while (ldsCurBatch < maxBatch)\n"
+"	{\n"
+"		for(; idx<end; )\n"
+"		{\n"
+"			if (gConstraints[idx].m_batchIdx == ldsCurBatch)\n"
+"			{\n"
+"					solveContactConstraint( gBodies, gShapes, &gConstraints[idx] );\n"
+"\n"
+"				 idx+=64;\n"
+"			} else\n"
+"			{\n"
+"				break;\n"
+"			}\n"
+"		}\n"
+"		GROUP_LDS_BARRIER;\n"
+"		if( lIdx == 0 )\n"
+"		{\n"
+"			ldsCurBatch++;\n"
+"		}\n"
+"		GROUP_LDS_BARRIER;\n"
+"	}\n"
+"	\n"
+"    \n"
+"}\n"
+"\n"
+;

src/Bullet3OpenCL/RigidBody/kernels/solveFriction.cl

@@ -0,0 +1,506 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Takahiro Harada
+
+
+//#pragma OPENCL EXTENSION cl_amd_printf : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable
+
+
+#ifdef cl_ext_atomic_counters_32
+#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
+#else
+#define counter32_t volatile global int*
+#endif
+
+typedef unsigned int u32;
+typedef unsigned short u16;
+typedef unsigned char u8;
+
+#define GET_GROUP_IDX get_group_id(0)
+#define GET_LOCAL_IDX get_local_id(0)
+#define GET_GLOBAL_IDX get_global_id(0)
+#define GET_GROUP_SIZE get_local_size(0)
+#define GET_NUM_GROUPS get_num_groups(0)
+#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)
+#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)
+#define AtomInc(x) atom_inc(&(x))
+#define AtomInc1(x, out) out = atom_inc(&(x))
+#define AppendInc(x, out) out = atomic_inc(x)
+#define AtomAdd(x, value) atom_add(&(x), value)
+#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )
+#define AtomXhg(x, value) atom_xchg ( &(x), value )
+
+
+#define SELECT_UINT4( b, a, condition ) select( b,a,condition )
+
+#define mymake_float4 (float4)
+//#define make_float2 (float2)
+//#define make_uint4 (uint4)
+//#define make_int4 (int4)
+//#define make_uint2 (uint2)
+//#define make_int2 (int2)
+
+
+#define max2 max
+#define min2 min
+
+
+///////////////////////////////////////
+//	Vector
+///////////////////////////////////////
+
+
+
+
+__inline
+float4 fastNormalize4(float4 v)
+{
+	return fast_normalize(v);
+}
+
+
+
+__inline
+float4 cross3(float4 a, float4 b)
+{
+	return cross(a,b);
+}
+
+__inline
+float dot3F4(float4 a, float4 b)
+{
+	float4 a1 = mymake_float4(a.xyz,0.f);
+	float4 b1 = mymake_float4(b.xyz,0.f);
+	return dot(a1, b1);
+}
+
+
+
+
+__inline
+float4 normalize3(const float4 a)
+{
+	float4 n = mymake_float4(a.x, a.y, a.z, 0.f);
+	return fastNormalize4( n );
+//	float length = sqrtf(dot3F4(a, a));
+//	return 1.f/length * a;
+}
+
+
+
+
+///////////////////////////////////////
+//	Matrix3x3
+///////////////////////////////////////
+
+typedef struct
+{
+	float4 m_row[3];
+}Matrix3x3;
+
+
+
+
+
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b);
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b);
+
+
+
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b)
+{
+	float4 ans;
+	ans.x = dot3F4( a.m_row[0], b );
+	ans.y = dot3F4( a.m_row[1], b );
+	ans.z = dot3F4( a.m_row[2], b );
+	ans.w = 0.f;
+	return ans;
+}
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b)
+{
+	float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
+	float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
+	float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
+
+	float4 ans;
+	ans.x = dot3F4( a, colx );
+	ans.y = dot3F4( a, coly );
+	ans.z = dot3F4( a, colz );
+	return ans;
+}
+
+///////////////////////////////////////
+//	Quaternion
+///////////////////////////////////////
+
+typedef float4 Quaternion;
+
+
+
+
+
+
+
+#define WG_SIZE 64
+
+typedef struct
+{
+	float4 m_pos;
+	Quaternion m_quat;
+	float4 m_linVel;
+	float4 m_angVel;
+
+	u32 m_shapeIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
+} Body;
+
+typedef struct
+{
+	Matrix3x3 m_invInertia;
+	Matrix3x3 m_initInvInertia;
+} Shape;
+
+typedef struct
+{
+	float4 m_linear;
+	float4 m_worldPos[4];
+	float4 m_center;	
+	float m_jacCoeffInv[4];
+	float m_b[4];
+	float m_appliedRambdaDt[4];
+
+	float m_fJacCoeffInv[2];	
+	float m_fAppliedRambdaDt[2];	
+
+	u32 m_bodyA;
+	u32 m_bodyB;
+
+	int m_batchIdx;
+	u32 m_paddings[1];
+} Constraint4;
+
+typedef struct
+{
+	float4 m_worldPos[4];
+	float4 m_worldNormal;
+	u32 m_coeffs;
+	int m_batchIdx;
+
+	int m_bodyAPtrAndSignBit;
+	int m_bodyBPtrAndSignBit;
+} Contact4;
+
+typedef struct
+{
+	int m_nConstraints;
+	int m_start;
+	int m_batchIdx;
+	int m_nSplit;
+//	int m_paddings[1];
+} ConstBuffer;
+
+typedef struct
+{
+	int m_solveFriction;
+	int m_maxBatch;	//	long batch really kills the performance
+	int m_batchIdx;
+	int m_nSplit;
+//	int m_paddings[1];
+} ConstBufferBatchSolve;
+
+void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1);
+
+void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
+{
+	*linear = mymake_float4(-n.xyz,0.f);
+	*angular0 = -cross3(r0, n);
+	*angular1 = cross3(r1, n);
+}
+
+float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 );
+
+float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )
+{
+	return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);
+}
+
+
+float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,
+				   float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1);
+
+float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,
+					float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)
+{
+	//	linear0,1 are normlized
+	float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;
+	float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);
+	float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;
+	float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);
+	return -1.f/(jmj0+jmj1+jmj2+jmj3);
+}
+void btPlaneSpace1 (const float4* n, float4* p, float4* q);
+ void btPlaneSpace1 (const float4* n, float4* p, float4* q)
+{
+  if (fabs(n[0].z) > 0.70710678f) {
+    // choose p in y-z plane
+    float a = n[0].y*n[0].y + n[0].z*n[0].z;
+    float k = 1.f/sqrt(a);
+    p[0].x = 0;
+	p[0].y = -n[0].z*k;
+	p[0].z = n[0].y*k;
+    // set q = n x p
+    q[0].x = a*k;
+	q[0].y = -n[0].x*p[0].z;
+	q[0].z = n[0].x*p[0].y;
+  }
+  else {
+    // choose p in x-y plane
+    float a = n[0].x*n[0].x + n[0].y*n[0].y;
+    float k = 1.f/sqrt(a);
+    p[0].x = -n[0].y*k;
+	p[0].y = n[0].x*k;
+	p[0].z = 0;
+    // set q = n x p
+    q[0].x = -n[0].z*p[0].y;
+	q[0].y = n[0].z*p[0].x;
+	q[0].z = a*k;
+  }
+}
+
+
+void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);
+void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)
+{
+	float frictionCoeff = ldsCs[0].m_linear.w;
+	int aIdx = ldsCs[0].m_bodyA;
+	int bIdx = ldsCs[0].m_bodyB;
+
+
+	float4 posA = gBodies[aIdx].m_pos;
+	float4 linVelA = gBodies[aIdx].m_linVel;
+	float4 angVelA = gBodies[aIdx].m_angVel;
+	float invMassA = gBodies[aIdx].m_invMass;
+	Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;
+
+	float4 posB = gBodies[bIdx].m_pos;
+	float4 linVelB = gBodies[bIdx].m_linVel;
+	float4 angVelB = gBodies[bIdx].m_angVel;
+	float invMassB = gBodies[bIdx].m_invMass;
+	Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;
+	
+
+	{
+		float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
+		float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
+
+		float sum = 0;
+		for(int j=0; j<4; j++)
+		{
+			sum +=ldsCs[0].m_appliedRambdaDt[j];
+		}
+		frictionCoeff = 0.7f;
+		for(int j=0; j<4; j++)
+		{
+			maxRambdaDt[j] = frictionCoeff*sum;
+			minRambdaDt[j] = -maxRambdaDt[j];
+		}
+
+		
+//		solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,
+//			posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );
+		
+		
+		{
+			
+			__global Constraint4* cs = ldsCs;
+			
+			if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;
+			const float4 center = cs->m_center;
+			
+			float4 n = -cs->m_linear;
+			
+			float4 tangent[2];
+			btPlaneSpace1(&n,&tangent[0],&tangent[1]);
+			float4 angular0, angular1, linear;
+			float4 r0 = center - posA;
+			float4 r1 = center - posB;
+			for(int i=0; i<2; i++)
+			{
+				setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );
+				float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
+											linVelA, angVelA, linVelB, angVelB );
+				rambdaDt *= cs->m_fJacCoeffInv[i];
+				
+				{
+					float prevSum = cs->m_fAppliedRambdaDt[i];
+					float updated = prevSum;
+					updated += rambdaDt;
+					updated = max2( updated, minRambdaDt[i] );
+					updated = min2( updated, maxRambdaDt[i] );
+					rambdaDt = updated - prevSum;
+					cs->m_fAppliedRambdaDt[i] = updated;
+				}
+				
+				float4 linImp0 = invMassA*linear*rambdaDt;
+				float4 linImp1 = invMassB*(-linear)*rambdaDt;
+				float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
+				float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
+				
+				linVelA += linImp0;
+				angVelA += angImp0;
+				linVelB += linImp1;
+				angVelB += angImp1;
+			}
+			{	//	angular damping for point constraint
+				float4 ab = normalize3( posB - posA );
+				float4 ac = normalize3( center - posA );
+				if( dot3F4( ab, ac ) > 0.95f  || (invMassA == 0.f || invMassB == 0.f))
+				{
+					float angNA = dot3F4( n, angVelA );
+					float angNB = dot3F4( n, angVelB );
+					
+					angVelA -= (angNA*0.1f)*n;
+					angVelB -= (angNB*0.1f)*n;
+				}
+			}
+		}
+
+		
+		
+	}
+
+	if (gBodies[aIdx].m_invMass)
+	{
+		gBodies[aIdx].m_linVel = linVelA;
+		gBodies[aIdx].m_angVel = angVelA;
+	} else
+	{
+		gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);
+		gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);
+	}
+	if (gBodies[bIdx].m_invMass)
+	{
+		gBodies[bIdx].m_linVel = linVelB;
+		gBodies[bIdx].m_angVel = angVelB;
+	} else
+	{
+		gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);
+		gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);
+	}
+ 
+
+}
+
+typedef struct 
+{
+	int m_valInt0;
+	int m_valInt1;
+	int m_valInt2;
+	int m_valInt3;
+
+	float m_val0;
+	float m_val1;
+	float m_val2;
+	float m_val3;
+} SolverDebugInfo;
+
+
+
+
+__kernel
+__attribute__((reqd_work_group_size(WG_SIZE,1,1)))
+void BatchSolveKernelFriction(__global Body* gBodies,
+                      __global Shape* gShapes,
+                      __global Constraint4* gConstraints,
+                      __global int* gN,
+                      __global int* gOffsets,
+                       int maxBatch,
+                       int bIdx,
+                       int nSplit
+                      )
+{
+	//__local int ldsBatchIdx[WG_SIZE+1];
+	__local int ldsCurBatch;
+	__local int ldsNextBatch;
+	__local int ldsStart;
+
+	int lIdx = GET_LOCAL_IDX;
+	int wgIdx = GET_GROUP_IDX;
+
+//	int gIdx = GET_GLOBAL_IDX;
+//	debugInfo[gIdx].m_valInt0 = gIdx;
+	//debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;
+
+
+	int xIdx = (wgIdx/(nSplit/2))*2 + (bIdx&1);
+	int yIdx = (wgIdx%(nSplit/2))*2 + (bIdx>>1);
+	int cellIdx = xIdx+yIdx*nSplit;
+	
+	if( gN[cellIdx] == 0 ) 
+		return;
+
+	const int start = gOffsets[cellIdx];
+	const int end = start + gN[cellIdx];
+
+	
+	if( lIdx == 0 )
+	{
+		ldsCurBatch = 0;
+		ldsNextBatch = 0;
+		ldsStart = start;
+	}
+
+
+	GROUP_LDS_BARRIER;
+
+	int idx=ldsStart+lIdx;
+	while (ldsCurBatch < maxBatch)
+	{
+		for(; idx<end; )
+		{
+			if (gConstraints[idx].m_batchIdx == ldsCurBatch)
+			{
+
+					solveFrictionConstraint( gBodies, gShapes, &gConstraints[idx] );
+
+				 idx+=64;
+			} else
+			{
+				break;
+			}
+		}
+		GROUP_LDS_BARRIER;
+		if( lIdx == 0 )
+		{
+			ldsCurBatch++;
+		}
+		GROUP_LDS_BARRIER;
+	}
+	
+    
+}

src/Bullet3OpenCL/RigidBody/kernels/solveFriction.h

@@ -0,0 +1,510 @@
+//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
+static const char* solveFrictionCL= \
+"/*\n"
+"Copyright (c) 2012 Advanced Micro Devices, Inc.  \n"
+"\n"
+"This software is provided 'as-is', without any express or implied warranty.\n"
+"In no event will the authors be held liable for any damages arising from the use of this software.\n"
+"Permission is granted to anyone to use this software for any purpose, \n"
+"including commercial applications, and to alter it and redistribute it freely, \n"
+"subject to the following restrictions:\n"
+"\n"
+"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
+"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
+"3. This notice may not be removed or altered from any source distribution.\n"
+"*/\n"
+"//Originally written by Takahiro Harada\n"
+"\n"
+"\n"
+"//#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n"
+"\n"
+"\n"
+"#ifdef cl_ext_atomic_counters_32\n"
+"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n"
+"#else\n"
+"#define counter32_t volatile global int*\n"
+"#endif\n"
+"\n"
+"typedef unsigned int u32;\n"
+"typedef unsigned short u16;\n"
+"typedef unsigned char u8;\n"
+"\n"
+"#define GET_GROUP_IDX get_group_id(0)\n"
+"#define GET_LOCAL_IDX get_local_id(0)\n"
+"#define GET_GLOBAL_IDX get_global_id(0)\n"
+"#define GET_GROUP_SIZE get_local_size(0)\n"
+"#define GET_NUM_GROUPS get_num_groups(0)\n"
+"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n"
+"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n"
+"#define AtomInc(x) atom_inc(&(x))\n"
+"#define AtomInc1(x, out) out = atom_inc(&(x))\n"
+"#define AppendInc(x, out) out = atomic_inc(x)\n"
+"#define AtomAdd(x, value) atom_add(&(x), value)\n"
+"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n"
+"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n"
+"\n"
+"\n"
+"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n"
+"\n"
+"#define mymake_float4 (float4)\n"
+"//#define make_float2 (float2)\n"
+"//#define make_uint4 (uint4)\n"
+"//#define make_int4 (int4)\n"
+"//#define make_uint2 (uint2)\n"
+"//#define make_int2 (int2)\n"
+"\n"
+"\n"
+"#define max2 max\n"
+"#define min2 min\n"
+"\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Vector\n"
+"///////////////////////////////////////\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 fastNormalize4(float4 v)\n"
+"{\n"
+"	return fast_normalize(v);\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 cross3(float4 a, float4 b)\n"
+"{\n"
+"	return cross(a,b);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float dot3F4(float4 a, float4 b)\n"
+"{\n"
+"	float4 a1 = mymake_float4(a.xyz,0.f);\n"
+"	float4 b1 = mymake_float4(b.xyz,0.f);\n"
+"	return dot(a1, b1);\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 normalize3(const float4 a)\n"
+"{\n"
+"	float4 n = mymake_float4(a.x, a.y, a.z, 0.f);\n"
+"	return fastNormalize4( n );\n"
+"//	float length = sqrtf(dot3F4(a, a));\n"
+"//	return 1.f/length * a;\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Matrix3x3\n"
+"///////////////////////////////////////\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_row[3];\n"
+"}Matrix3x3;\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b);\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b);\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b)\n"
+"{\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a.m_row[0], b );\n"
+"	ans.y = dot3F4( a.m_row[1], b );\n"
+"	ans.z = dot3F4( a.m_row[2], b );\n"
+"	ans.w = 0.f;\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b)\n"
+"{\n"
+"	float4 colx = mymake_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n"
+"	float4 coly = mymake_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n"
+"	float4 colz = mymake_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n"
+"\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a, colx );\n"
+"	ans.y = dot3F4( a, coly );\n"
+"	ans.z = dot3F4( a, colz );\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Quaternion\n"
+"///////////////////////////////////////\n"
+"\n"
+"typedef float4 Quaternion;\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"#define WG_SIZE 64\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_pos;\n"
+"	Quaternion m_quat;\n"
+"	float4 m_linVel;\n"
+"	float4 m_angVel;\n"
+"\n"
+"	u32 m_shapeIdx;\n"
+"	float m_invMass;\n"
+"	float m_restituitionCoeff;\n"
+"	float m_frictionCoeff;\n"
+"} Body;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	Matrix3x3 m_invInertia;\n"
+"	Matrix3x3 m_initInvInertia;\n"
+"} Shape;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_linear;\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_center;	\n"
+"	float m_jacCoeffInv[4];\n"
+"	float m_b[4];\n"
+"	float m_appliedRambdaDt[4];\n"
+"\n"
+"	float m_fJacCoeffInv[2];	\n"
+"	float m_fAppliedRambdaDt[2];	\n"
+"\n"
+"	u32 m_bodyA;\n"
+"	u32 m_bodyB;\n"
+"\n"
+"	int m_batchIdx;\n"
+"	u32 m_paddings[1];\n"
+"} Constraint4;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_worldNormal;\n"
+"	u32 m_coeffs;\n"
+"	int m_batchIdx;\n"
+"\n"
+"	int m_bodyAPtrAndSignBit;\n"
+"	int m_bodyBPtrAndSignBit;\n"
+"} Contact4;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_nConstraints;\n"
+"	int m_start;\n"
+"	int m_batchIdx;\n"
+"	int m_nSplit;\n"
+"//	int m_paddings[1];\n"
+"} ConstBuffer;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_solveFriction;\n"
+"	int m_maxBatch;	//	long batch really kills the performance\n"
+"	int m_batchIdx;\n"
+"	int m_nSplit;\n"
+"//	int m_paddings[1];\n"
+"} ConstBufferBatchSolve;\n"
+"\n"
+"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1);\n"
+"\n"
+"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
+"{\n"
+"	*linear = mymake_float4(-n.xyz,0.f);\n"
+"	*angular0 = -cross3(r0, n);\n"
+"	*angular1 = cross3(r1, n);\n"
+"}\n"
+"\n"
+"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 );\n"
+"\n"
+"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n"
+"{\n"
+"	return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n"
+"}\n"
+"\n"
+"\n"
+"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n"
+"				   float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1);\n"
+"\n"
+"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n"
+"					float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)\n"
+"{\n"
+"	//	linear0,1 are normlized\n"
+"	float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n"
+"	float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n"
+"	float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n"
+"	float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n"
+"	return -1.f/(jmj0+jmj1+jmj2+jmj3);\n"
+"}\n"
+"void b3PlaneSpace1 (const float4* n, float4* p, float4* q);\n"
+" void b3PlaneSpace1 (const float4* n, float4* p, float4* q)\n"
+"{\n"
+"  if (fabs(n[0].z) > 0.70710678f) {\n"
+"    // choose p in y-z plane\n"
+"    float a = n[0].y*n[0].y + n[0].z*n[0].z;\n"
+"    float k = 1.f/sqrt(a);\n"
+"    p[0].x = 0;\n"
+"	p[0].y = -n[0].z*k;\n"
+"	p[0].z = n[0].y*k;\n"
+"    // set q = n x p\n"
+"    q[0].x = a*k;\n"
+"	q[0].y = -n[0].x*p[0].z;\n"
+"	q[0].z = n[0].x*p[0].y;\n"
+"  }\n"
+"  else {\n"
+"    // choose p in x-y plane\n"
+"    float a = n[0].x*n[0].x + n[0].y*n[0].y;\n"
+"    float k = 1.f/sqrt(a);\n"
+"    p[0].x = -n[0].y*k;\n"
+"	p[0].y = n[0].x*k;\n"
+"	p[0].z = 0;\n"
+"    // set q = n x p\n"
+"    q[0].x = -n[0].z*p[0].y;\n"
+"	q[0].y = n[0].z*p[0].x;\n"
+"	q[0].z = a*k;\n"
+"  }\n"
+"}\n"
+"\n"
+"\n"
+"void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs);\n"
+"void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs)\n"
+"{\n"
+"	float frictionCoeff = ldsCs[0].m_linear.w;\n"
+"	int aIdx = ldsCs[0].m_bodyA;\n"
+"	int bIdx = ldsCs[0].m_bodyB;\n"
+"\n"
+"\n"
+"	float4 posA = gBodies[aIdx].m_pos;\n"
+"	float4 linVelA = gBodies[aIdx].m_linVel;\n"
+"	float4 angVelA = gBodies[aIdx].m_angVel;\n"
+"	float invMassA = gBodies[aIdx].m_invMass;\n"
+"	Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n"
+"\n"
+"	float4 posB = gBodies[bIdx].m_pos;\n"
+"	float4 linVelB = gBodies[bIdx].m_linVel;\n"
+"	float4 angVelB = gBodies[bIdx].m_angVel;\n"
+"	float invMassB = gBodies[bIdx].m_invMass;\n"
+"	Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n"
+"	\n"
+"\n"
+"	{\n"
+"		float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};\n"
+"		float minRambdaDt[4] = {0.f,0.f,0.f,0.f};\n"
+"\n"
+"		float sum = 0;\n"
+"		for(int j=0; j<4; j++)\n"
+"		{\n"
+"			sum +=ldsCs[0].m_appliedRambdaDt[j];\n"
+"		}\n"
+"		frictionCoeff = 0.7f;\n"
+"		for(int j=0; j<4; j++)\n"
+"		{\n"
+"			maxRambdaDt[j] = frictionCoeff*sum;\n"
+"			minRambdaDt[j] = -maxRambdaDt[j];\n"
+"		}\n"
+"\n"
+"		\n"
+"//		solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n"
+"//			posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );\n"
+"		\n"
+"		\n"
+"		{\n"
+"			\n"
+"			__global Constraint4* cs = ldsCs;\n"
+"			\n"
+"			if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;\n"
+"			const float4 center = cs->m_center;\n"
+"			\n"
+"			float4 n = -cs->m_linear;\n"
+"			\n"
+"			float4 tangent[2];\n"
+"			b3PlaneSpace1(&n,&tangent[0],&tangent[1]);\n"
+"			float4 angular0, angular1, linear;\n"
+"			float4 r0 = center - posA;\n"
+"			float4 r1 = center - posB;\n"
+"			for(int i=0; i<2; i++)\n"
+"			{\n"
+"				setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );\n"
+"				float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,\n"
+"											linVelA, angVelA, linVelB, angVelB );\n"
+"				rambdaDt *= cs->m_fJacCoeffInv[i];\n"
+"				\n"
+"				{\n"
+"					float prevSum = cs->m_fAppliedRambdaDt[i];\n"
+"					float updated = prevSum;\n"
+"					updated += rambdaDt;\n"
+"					updated = max2( updated, minRambdaDt[i] );\n"
+"					updated = min2( updated, maxRambdaDt[i] );\n"
+"					rambdaDt = updated - prevSum;\n"
+"					cs->m_fAppliedRambdaDt[i] = updated;\n"
+"				}\n"
+"				\n"
+"				float4 linImp0 = invMassA*linear*rambdaDt;\n"
+"				float4 linImp1 = invMassB*(-linear)*rambdaDt;\n"
+"				float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n"
+"				float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n"
+"				\n"
+"				linVelA += linImp0;\n"
+"				angVelA += angImp0;\n"
+"				linVelB += linImp1;\n"
+"				angVelB += angImp1;\n"
+"			}\n"
+"			{	//	angular damping for point constraint\n"
+"				float4 ab = normalize3( posB - posA );\n"
+"				float4 ac = normalize3( center - posA );\n"
+"				if( dot3F4( ab, ac ) > 0.95f  || (invMassA == 0.f || invMassB == 0.f))\n"
+"				{\n"
+"					float angNA = dot3F4( n, angVelA );\n"
+"					float angNB = dot3F4( n, angVelB );\n"
+"					\n"
+"					angVelA -= (angNA*0.1f)*n;\n"
+"					angVelB -= (angNB*0.1f)*n;\n"
+"				}\n"
+"			}\n"
+"		}\n"
+"\n"
+"		\n"
+"		\n"
+"	}\n"
+"\n"
+"	if (gBodies[aIdx].m_invMass)\n"
+"	{\n"
+"		gBodies[aIdx].m_linVel = linVelA;\n"
+"		gBodies[aIdx].m_angVel = angVelA;\n"
+"	} else\n"
+"	{\n"
+"		gBodies[aIdx].m_linVel = mymake_float4(0,0,0,0);\n"
+"		gBodies[aIdx].m_angVel = mymake_float4(0,0,0,0);\n"
+"	}\n"
+"	if (gBodies[bIdx].m_invMass)\n"
+"	{\n"
+"		gBodies[bIdx].m_linVel = linVelB;\n"
+"		gBodies[bIdx].m_angVel = angVelB;\n"
+"	} else\n"
+"	{\n"
+"		gBodies[bIdx].m_linVel = mymake_float4(0,0,0,0);\n"
+"		gBodies[bIdx].m_angVel = mymake_float4(0,0,0,0);\n"
+"	}\n"
+" \n"
+"\n"
+"}\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	int m_valInt0;\n"
+"	int m_valInt1;\n"
+"	int m_valInt2;\n"
+"	int m_valInt3;\n"
+"\n"
+"	float m_val0;\n"
+"	float m_val1;\n"
+"	float m_val2;\n"
+"	float m_val3;\n"
+"} SolverDebugInfo;\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"__kernel\n"
+"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n"
+"void BatchSolveKernelFriction(__global Body* gBodies,\n"
+"                      __global Shape* gShapes,\n"
+"                      __global Constraint4* gConstraints,\n"
+"                      __global int* gN,\n"
+"                      __global int* gOffsets,\n"
+"                       int maxBatch,\n"
+"                       int bIdx,\n"
+"                       int nSplit\n"
+"                      )\n"
+"{\n"
+"	//__local int ldsBatchIdx[WG_SIZE+1];\n"
+"	__local int ldsCurBatch;\n"
+"	__local int ldsNextBatch;\n"
+"	__local int ldsStart;\n"
+"\n"
+"	int lIdx = GET_LOCAL_IDX;\n"
+"	int wgIdx = GET_GROUP_IDX;\n"
+"\n"
+"//	int gIdx = GET_GLOBAL_IDX;\n"
+"//	debugInfo[gIdx].m_valInt0 = gIdx;\n"
+"	//debugInfo[gIdx].m_valInt1 = GET_GROUP_SIZE;\n"
+"\n"
+"\n"
+"	int xIdx = (wgIdx/(nSplit/2))*2 + (bIdx&1);\n"
+"	int yIdx = (wgIdx%(nSplit/2))*2 + (bIdx>>1);\n"
+"	int cellIdx = xIdx+yIdx*nSplit;\n"
+"	\n"
+"	if( gN[cellIdx] == 0 ) \n"
+"		return;\n"
+"\n"
+"	const int start = gOffsets[cellIdx];\n"
+"	const int end = start + gN[cellIdx];\n"
+"\n"
+"	\n"
+"	if( lIdx == 0 )\n"
+"	{\n"
+"		ldsCurBatch = 0;\n"
+"		ldsNextBatch = 0;\n"
+"		ldsStart = start;\n"
+"	}\n"
+"\n"
+"\n"
+"	GROUP_LDS_BARRIER;\n"
+"\n"
+"	int idx=ldsStart+lIdx;\n"
+"	while (ldsCurBatch < maxBatch)\n"
+"	{\n"
+"		for(; idx<end; )\n"
+"		{\n"
+"			if (gConstraints[idx].m_batchIdx == ldsCurBatch)\n"
+"			{\n"
+"\n"
+"					solveFrictionConstraint( gBodies, gShapes, &gConstraints[idx] );\n"
+"\n"
+"				 idx+=64;\n"
+"			} else\n"
+"			{\n"
+"				break;\n"
+"			}\n"
+"		}\n"
+"		GROUP_LDS_BARRIER;\n"
+"		if( lIdx == 0 )\n"
+"		{\n"
+"			ldsCurBatch++;\n"
+"		}\n"
+"		GROUP_LDS_BARRIER;\n"
+"	}\n"
+"	\n"
+"    \n"
+"}\n"
+"\n"
+;

src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl

@@ -0,0 +1,660 @@
+
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Takahiro Harada
+
+
+#pragma OPENCL EXTENSION cl_amd_printf : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable
+
+
+#ifdef cl_ext_atomic_counters_32
+#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
+#else
+#define counter32_t volatile global int*
+#endif
+
+typedef unsigned int u32;
+typedef unsigned short u16;
+typedef unsigned char u8;
+
+#define GET_GROUP_IDX get_group_id(0)
+#define GET_LOCAL_IDX get_local_id(0)
+#define GET_GLOBAL_IDX get_global_id(0)
+#define GET_GROUP_SIZE get_local_size(0)
+#define GET_NUM_GROUPS get_num_groups(0)
+#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)
+#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)
+#define AtomInc(x) atom_inc(&(x))
+#define AtomInc1(x, out) out = atom_inc(&(x))
+#define AppendInc(x, out) out = atomic_inc(x)
+#define AtomAdd(x, value) atom_add(&(x), value)
+#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )
+#define AtomXhg(x, value) atom_xchg ( &(x), value )
+
+
+#define SELECT_UINT4( b, a, condition ) select( b,a,condition )
+
+#define make_float4 (float4)
+#define make_float2 (float2)
+#define make_uint4 (uint4)
+#define make_int4 (int4)
+#define make_uint2 (uint2)
+#define make_int2 (int2)
+
+
+#define max2 max
+#define min2 min
+
+
+///////////////////////////////////////
+//	Vector
+///////////////////////////////////////
+__inline
+float fastDiv(float numerator, float denominator)
+{
+	return native_divide(numerator, denominator);	
+//	return numerator/denominator;	
+}
+
+__inline
+float4 fastDiv4(float4 numerator, float4 denominator)
+{
+	return native_divide(numerator, denominator);	
+}
+
+__inline
+float fastSqrtf(float f2)
+{
+	return native_sqrt(f2);
+//	return sqrt(f2);
+}
+
+__inline
+float fastRSqrt(float f2)
+{
+	return native_rsqrt(f2);
+}
+
+__inline
+float fastLength4(float4 v)
+{
+	return fast_length(v);
+}
+
+__inline
+float4 fastNormalize4(float4 v)
+{
+	return fast_normalize(v);
+}
+
+
+__inline
+float sqrtf(float a)
+{
+//	return sqrt(a);
+	return native_sqrt(a);
+}
+
+__inline
+float4 cross3(float4 a, float4 b)
+{
+	return cross(a,b);
+}
+
+__inline
+float dot3F4(float4 a, float4 b)
+{
+	float4 a1 = make_float4(a.xyz,0.f);
+	float4 b1 = make_float4(b.xyz,0.f);
+	return dot(a1, b1);
+}
+
+__inline
+float length3(const float4 a)
+{
+	return sqrtf(dot3F4(a,a));
+}
+
+__inline
+float dot4(const float4 a, const float4 b)
+{
+	return dot( a, b );
+}
+
+//	for height
+__inline
+float dot3w1(const float4 point, const float4 eqn)
+{
+	return dot3F4(point,eqn) + eqn.w;
+}
+
+__inline
+float4 normalize3(const float4 a)
+{
+	float4 n = make_float4(a.x, a.y, a.z, 0.f);
+	return fastNormalize4( n );
+//	float length = sqrtf(dot3F4(a, a));
+//	return 1.f/length * a;
+}
+
+__inline
+float4 normalize4(const float4 a)
+{
+	float length = sqrtf(dot4(a, a));
+	return 1.f/length * a;
+}
+
+__inline
+float4 createEquation(const float4 a, const float4 b, const float4 c)
+{
+	float4 eqn;
+	float4 ab = b-a;
+	float4 ac = c-a;
+	eqn = normalize3( cross3(ab, ac) );
+	eqn.w = -dot3F4(eqn,a);
+	return eqn;
+}
+
+///////////////////////////////////////
+//	Matrix3x3
+///////////////////////////////////////
+
+typedef struct
+{
+	float4 m_row[3];
+}Matrix3x3;
+
+__inline
+Matrix3x3 mtZero();
+
+__inline
+Matrix3x3 mtIdentity();
+
+__inline
+Matrix3x3 mtTranspose(Matrix3x3 m);
+
+__inline
+Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b);
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b);
+
+__inline
+Matrix3x3 mtZero()
+{
+	Matrix3x3 m;
+	m.m_row[0] = (float4)(0.f);
+	m.m_row[1] = (float4)(0.f);
+	m.m_row[2] = (float4)(0.f);
+	return m;
+}
+
+__inline
+Matrix3x3 mtIdentity()
+{
+	Matrix3x3 m;
+	m.m_row[0] = (float4)(1,0,0,0);
+	m.m_row[1] = (float4)(0,1,0,0);
+	m.m_row[2] = (float4)(0,0,1,0);
+	return m;
+}
+
+__inline
+Matrix3x3 mtTranspose(Matrix3x3 m)
+{
+	Matrix3x3 out;
+	out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
+	out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);
+	out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);
+	return out;
+}
+
+__inline
+Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)
+{
+	Matrix3x3 transB;
+	transB = mtTranspose( b );
+	Matrix3x3 ans;
+	//	why this doesn't run when 0ing in the for{}
+	a.m_row[0].w = 0.f;
+	a.m_row[1].w = 0.f;
+	a.m_row[2].w = 0.f;
+	for(int i=0; i<3; i++)
+	{
+//	a.m_row[i].w = 0.f;
+		ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);
+		ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);
+		ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);
+		ans.m_row[i].w = 0.f;
+	}
+	return ans;
+}
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b)
+{
+	float4 ans;
+	ans.x = dot3F4( a.m_row[0], b );
+	ans.y = dot3F4( a.m_row[1], b );
+	ans.z = dot3F4( a.m_row[2], b );
+	ans.w = 0.f;
+	return ans;
+}
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b)
+{
+	float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
+	float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
+	float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
+
+	float4 ans;
+	ans.x = dot3F4( a, colx );
+	ans.y = dot3F4( a, coly );
+	ans.z = dot3F4( a, colz );
+	return ans;
+}
+
+///////////////////////////////////////
+//	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
+Matrix3x3 qtGetRotationMatrix(Quaternion q);
+
+
+
+__inline
+Quaternion qtMul(Quaternion a, Quaternion b)
+{
+	Quaternion ans;
+	ans = cross3( 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 fastNormalize4(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
+Matrix3x3 qtGetRotationMatrix(Quaternion quat)
+{
+	float4 quat2 = (float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);
+	Matrix3x3 out;
+
+	out.m_row[0].x=1-2*quat2.y-2*quat2.z;
+	out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;
+	out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;
+	out.m_row[0].w = 0.f;
+
+	out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;
+	out.m_row[1].y=1-2*quat2.x-2*quat2.z;
+	out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;
+	out.m_row[1].w = 0.f;
+
+	out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;
+	out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;
+	out.m_row[2].z=1-2*quat2.x-2*quat2.y;
+	out.m_row[2].w = 0.f;
+
+	return out;
+}
+
+
+
+
+#define WG_SIZE 64
+
+typedef struct
+{
+	float4 m_pos;
+	Quaternion m_quat;
+	float4 m_linVel;
+	float4 m_angVel;
+
+	u32 m_shapeIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
+} Body;
+
+typedef struct
+{
+	Matrix3x3 m_invInertia;
+	Matrix3x3 m_initInvInertia;
+} Shape;
+
+typedef struct
+{
+	float4 m_linear;
+	float4 m_worldPos[4];
+	float4 m_center;	
+	float m_jacCoeffInv[4];
+	float m_b[4];
+	float m_appliedRambdaDt[4];
+
+	float m_fJacCoeffInv[2];	
+	float m_fAppliedRambdaDt[2];	
+
+	u32 m_bodyA;
+	u32 m_bodyB;
+
+	int m_batchIdx;
+	u32 m_paddings[1];
+} Constraint4;
+
+typedef struct
+{
+	float4 m_worldPos[4];
+	float4 m_worldNormal;
+	u32 m_coeffs;
+	int m_batchIdx;
+
+	int m_bodyAPtrAndSignBit;
+	int m_bodyBPtrAndSignBit;
+} Contact4;
+
+typedef struct
+{
+	int m_nConstraints;
+	int m_start;
+	int m_batchIdx;
+	int m_nSplit;
+//	int m_paddings[1];
+} ConstBuffer;
+
+typedef struct
+{
+	int m_solveFriction;
+	int m_maxBatch;	//	long batch really kills the performance
+	int m_batchIdx;
+	int m_nSplit;
+//	int m_paddings[1];
+} ConstBufferBatchSolve;
+
+
+void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
+{
+	*linear = make_float4(-n.xyz,0.f);
+	*angular0 = -cross3(r0, n);
+	*angular1 = cross3(r1, n);
+}
+
+
+float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )
+{
+	return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);
+}
+
+
+float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,
+					float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)
+{
+	//	linear0,1 are normlized
+	float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;
+	float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);
+	float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;
+	float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);
+	return -1.f/(jmj0+jmj1+jmj2+jmj3);
+}
+
+
+
+ 
+
+
+typedef struct 
+{
+	int m_valInt0;
+	int m_valInt1;
+	int m_valInt2;
+	int m_valInt3;
+
+	float m_val0;
+	float m_val1;
+	float m_val2;
+	float m_val3;
+} SolverDebugInfo;
+
+
+
+typedef struct
+{
+	int m_nContacts;
+	int m_staticIdx;
+	float m_scale;
+	int m_nSplit;
+} ConstBufferSSD;
+
+
+void btPlaneSpace1 (float4 n, float4* p, float4* q);
+ void btPlaneSpace1 (float4 n, float4* p, float4* q)
+{
+  if (fabs(n.z) > 0.70710678f) {
+    // choose p in y-z plane
+    float a = n.y*n.y + n.z*n.z;
+    float k = 1.f/sqrt(a);
+    p[0].x = 0;
+	p[0].y = -n.z*k;
+	p[0].z = n.y*k;
+    // set q = n x p
+    q[0].x = a*k;
+	q[0].y = -n.x*p[0].z;
+	q[0].z = n.x*p[0].y;
+  }
+  else {
+    // choose p in x-y plane
+    float a = n.x*n.x + n.y*n.y;
+    float k = 1.f/sqrt(a);
+    p[0].x = -n.y*k;
+	p[0].y = n.x*k;
+	p[0].z = 0;
+    // set q = n x p
+    q[0].x = -n.z*p[0].y;
+	q[0].y = n.z*p[0].x;
+	q[0].z = a*k;
+  }
+}
+
+
+void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVelA, float invMassA, const Matrix3x3 invInertiaA,
+	const float4 posB, const float4 linVelB, const float4 angVelB, float invMassB, const Matrix3x3 invInertiaB, 
+	__global Contact4* src, float dt, float positionDrift, float positionConstraintCoeff,
+	Constraint4* dstC )
+{
+	dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
+	dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
+
+	float dtInv = 1.f/dt;
+	for(int ic=0; ic<4; ic++)
+	{
+		dstC->m_appliedRambdaDt[ic] = 0.f;
+	}
+	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
+
+
+	dstC->m_linear = -src->m_worldNormal;
+	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );
+	for(int ic=0; ic<4; ic++)
+	{
+		float4 r0 = src->m_worldPos[ic] - posA;
+		float4 r1 = src->m_worldPos[ic] - posB;
+
+		if( ic >= src->m_worldNormal.w )//npoints
+		{
+			dstC->m_jacCoeffInv[ic] = 0.f;
+			continue;
+		}
+
+		float relVelN;
+		{
+			float4 linear, angular0, angular1;
+			setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1);
+
+			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
+				invMassA, &invInertiaA, invMassB, &invInertiaB );
+
+			relVelN = calcRelVel(linear, -linear, angular0, angular1,
+				linVelA, angVelA, linVelB, angVelB);
+
+			float e = 0.f;//src->getRestituitionCoeff();
+			if( relVelN*relVelN < 0.004f ) e = 0.f;
+
+			dstC->m_b[ic] = e*relVelN;
+			//float penetration = src->m_worldPos[ic].w;
+			dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
+			dstC->m_appliedRambdaDt[ic] = 0.f;
+		}
+	}
+
+	if( src->m_worldNormal.w > 0 )//npoints
+	{	//	prepare friction
+		float4 center = make_float4(0.f);
+		for(int i=0; i<src->m_worldNormal.w; i++) 
+			center += src->m_worldPos[i];
+		center /= (float)src->m_worldNormal.w;
+
+		float4 tangent[2];
+		btPlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]);
+		
+		float4 r[2];
+		r[0] = center - posA;
+		r[1] = center - posB;
+
+		for(int i=0; i<2; i++)
+		{
+			float4 linear, angular0, angular1;
+			setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
+
+			dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
+				invMassA, &invInertiaA, invMassB, &invInertiaB );
+			dstC->m_fAppliedRambdaDt[i] = 0.f;
+		}
+		dstC->m_center = center;
+	}
+
+	for(int i=0; i<4; i++)
+	{
+		if( i<src->m_worldNormal.w )
+		{
+			dstC->m_worldPos[i] = src->m_worldPos[i];
+		}
+		else
+		{
+			dstC->m_worldPos[i] = make_float4(0.f);
+		}
+	}
+}
+
+typedef struct
+{
+	int m_nContacts;
+	float m_dt;
+	float m_positionDrift;
+	float m_positionConstraintCoeff;
+} ConstBufferCTC;
+
+__kernel
+__attribute__((reqd_work_group_size(WG_SIZE,1,1)))
+void ContactToConstraintKernel(__global Contact4* gContact, __global Body* gBodies, __global Shape* gShapes, __global Constraint4* gConstraintOut, 
+int nContacts,
+float dt,
+float positionDrift,
+float positionConstraintCoeff
+)
+{
+	int gIdx = GET_GLOBAL_IDX;
+	
+	if( gIdx < nContacts )
+	{
+		int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);
+		int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);
+
+		float4 posA = gBodies[aIdx].m_pos;
+		float4 linVelA = gBodies[aIdx].m_linVel;
+		float4 angVelA = gBodies[aIdx].m_angVel;
+		float invMassA = gBodies[aIdx].m_invMass;
+		Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;
+
+		float4 posB = gBodies[bIdx].m_pos;
+		float4 linVelB = gBodies[bIdx].m_linVel;
+		float4 angVelB = gBodies[bIdx].m_angVel;
+		float invMassB = gBodies[bIdx].m_invMass;
+		Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;
+
+		Constraint4 cs;
+
+    	setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,
+			&gContact[gIdx], dt, positionDrift, positionConstraintCoeff,
+			&cs );
+		
+		cs.m_batchIdx = gContact[gIdx].m_batchIdx;
+
+		gConstraintOut[gIdx] = cs;
+	}
+}
+
+
+
+
+

src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h

@@ -0,0 +1,664 @@
+//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
+static const char* solverSetupCL= \
+"\n"
+"/*\n"
+"Copyright (c) 2012 Advanced Micro Devices, Inc.  \n"
+"\n"
+"This software is provided 'as-is', without any express or implied warranty.\n"
+"In no event will the authors be held liable for any damages arising from the use of this software.\n"
+"Permission is granted to anyone to use this software for any purpose, \n"
+"including commercial applications, and to alter it and redistribute it freely, \n"
+"subject to the following restrictions:\n"
+"\n"
+"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
+"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
+"3. This notice may not be removed or altered from any source distribution.\n"
+"*/\n"
+"//Originally written by Takahiro Harada\n"
+"\n"
+"\n"
+"#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n"
+"\n"
+"\n"
+"#ifdef cl_ext_atomic_counters_32\n"
+"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n"
+"#else\n"
+"#define counter32_t volatile global int*\n"
+"#endif\n"
+"\n"
+"typedef unsigned int u32;\n"
+"typedef unsigned short u16;\n"
+"typedef unsigned char u8;\n"
+"\n"
+"#define GET_GROUP_IDX get_group_id(0)\n"
+"#define GET_LOCAL_IDX get_local_id(0)\n"
+"#define GET_GLOBAL_IDX get_global_id(0)\n"
+"#define GET_GROUP_SIZE get_local_size(0)\n"
+"#define GET_NUM_GROUPS get_num_groups(0)\n"
+"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n"
+"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n"
+"#define AtomInc(x) atom_inc(&(x))\n"
+"#define AtomInc1(x, out) out = atom_inc(&(x))\n"
+"#define AppendInc(x, out) out = atomic_inc(x)\n"
+"#define AtomAdd(x, value) atom_add(&(x), value)\n"
+"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n"
+"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n"
+"\n"
+"\n"
+"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n"
+"\n"
+"#define make_float4 (float4)\n"
+"#define make_float2 (float2)\n"
+"#define make_uint4 (uint4)\n"
+"#define make_int4 (int4)\n"
+"#define make_uint2 (uint2)\n"
+"#define make_int2 (int2)\n"
+"\n"
+"\n"
+"#define max2 max\n"
+"#define min2 min\n"
+"\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Vector\n"
+"///////////////////////////////////////\n"
+"__inline\n"
+"float fastDiv(float numerator, float denominator)\n"
+"{\n"
+"	return native_divide(numerator, denominator);	\n"
+"//	return numerator/denominator;	\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 fastDiv4(float4 numerator, float4 denominator)\n"
+"{\n"
+"	return native_divide(numerator, denominator);	\n"
+"}\n"
+"\n"
+"__inline\n"
+"float fastSqrtf(float f2)\n"
+"{\n"
+"	return native_sqrt(f2);\n"
+"//	return sqrt(f2);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float fastRSqrt(float f2)\n"
+"{\n"
+"	return native_rsqrt(f2);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float fastLength4(float4 v)\n"
+"{\n"
+"	return fast_length(v);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 fastNormalize4(float4 v)\n"
+"{\n"
+"	return fast_normalize(v);\n"
+"}\n"
+"\n"
+"\n"
+"__inline\n"
+"float sqrtf(float a)\n"
+"{\n"
+"//	return sqrt(a);\n"
+"	return native_sqrt(a);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 cross3(float4 a, float4 b)\n"
+"{\n"
+"	return cross(a,b);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float dot3F4(float4 a, float4 b)\n"
+"{\n"
+"	float4 a1 = make_float4(a.xyz,0.f);\n"
+"	float4 b1 = make_float4(b.xyz,0.f);\n"
+"	return dot(a1, b1);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float length3(const float4 a)\n"
+"{\n"
+"	return sqrtf(dot3F4(a,a));\n"
+"}\n"
+"\n"
+"__inline\n"
+"float dot4(const float4 a, const float4 b)\n"
+"{\n"
+"	return dot( a, b );\n"
+"}\n"
+"\n"
+"//	for height\n"
+"__inline\n"
+"float dot3w1(const float4 point, const float4 eqn)\n"
+"{\n"
+"	return dot3F4(point,eqn) + eqn.w;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 normalize3(const float4 a)\n"
+"{\n"
+"	float4 n = make_float4(a.x, a.y, a.z, 0.f);\n"
+"	return fastNormalize4( n );\n"
+"//	float length = sqrtf(dot3F4(a, a));\n"
+"//	return 1.f/length * a;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 normalize4(const float4 a)\n"
+"{\n"
+"	float length = sqrtf(dot4(a, a));\n"
+"	return 1.f/length * a;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 createEquation(const float4 a, const float4 b, const float4 c)\n"
+"{\n"
+"	float4 eqn;\n"
+"	float4 ab = b-a;\n"
+"	float4 ac = c-a;\n"
+"	eqn = normalize3( cross3(ab, ac) );\n"
+"	eqn.w = -dot3F4(eqn,a);\n"
+"	return eqn;\n"
+"}\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Matrix3x3\n"
+"///////////////////////////////////////\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_row[3];\n"
+"}Matrix3x3;\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtZero();\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtIdentity();\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtTranspose(Matrix3x3 m);\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b);\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b);\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtZero()\n"
+"{\n"
+"	Matrix3x3 m;\n"
+"	m.m_row[0] = (float4)(0.f);\n"
+"	m.m_row[1] = (float4)(0.f);\n"
+"	m.m_row[2] = (float4)(0.f);\n"
+"	return m;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtIdentity()\n"
+"{\n"
+"	Matrix3x3 m;\n"
+"	m.m_row[0] = (float4)(1,0,0,0);\n"
+"	m.m_row[1] = (float4)(0,1,0,0);\n"
+"	m.m_row[2] = (float4)(0,0,1,0);\n"
+"	return m;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtTranspose(Matrix3x3 m)\n"
+"{\n"
+"	Matrix3x3 out;\n"
+"	out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n"
+"	out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n"
+"	out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n"
+"	return out;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n"
+"{\n"
+"	Matrix3x3 transB;\n"
+"	transB = mtTranspose( b );\n"
+"	Matrix3x3 ans;\n"
+"	//	why this doesn't run when 0ing in the for{}\n"
+"	a.m_row[0].w = 0.f;\n"
+"	a.m_row[1].w = 0.f;\n"
+"	a.m_row[2].w = 0.f;\n"
+"	for(int i=0; i<3; i++)\n"
+"	{\n"
+"//	a.m_row[i].w = 0.f;\n"
+"		ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n"
+"		ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n"
+"		ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n"
+"		ans.m_row[i].w = 0.f;\n"
+"	}\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b)\n"
+"{\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a.m_row[0], b );\n"
+"	ans.y = dot3F4( a.m_row[1], b );\n"
+"	ans.z = dot3F4( a.m_row[2], b );\n"
+"	ans.w = 0.f;\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b)\n"
+"{\n"
+"	float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n"
+"	float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n"
+"	float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n"
+"\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a, colx );\n"
+"	ans.y = dot3F4( a, coly );\n"
+"	ans.z = dot3F4( a, colz );\n"
+"	return ans;\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"
+"__inline\n"
+"Matrix3x3 qtGetRotationMatrix(Quaternion q);\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"Quaternion qtMul(Quaternion a, Quaternion b)\n"
+"{\n"
+"	Quaternion ans;\n"
+"	ans = cross3( 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 fastNormalize4(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"
+"Matrix3x3 qtGetRotationMatrix(Quaternion quat)\n"
+"{\n"
+"	float4 quat2 = (float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n"
+"	Matrix3x3 out;\n"
+"\n"
+"	out.m_row[0].x=1-2*quat2.y-2*quat2.z;\n"
+"	out.m_row[0].y=2*quat.x*quat.y-2*quat.w*quat.z;\n"
+"	out.m_row[0].z=2*quat.x*quat.z+2*quat.w*quat.y;\n"
+"	out.m_row[0].w = 0.f;\n"
+"\n"
+"	out.m_row[1].x=2*quat.x*quat.y+2*quat.w*quat.z;\n"
+"	out.m_row[1].y=1-2*quat2.x-2*quat2.z;\n"
+"	out.m_row[1].z=2*quat.y*quat.z-2*quat.w*quat.x;\n"
+"	out.m_row[1].w = 0.f;\n"
+"\n"
+"	out.m_row[2].x=2*quat.x*quat.z-2*quat.w*quat.y;\n"
+"	out.m_row[2].y=2*quat.y*quat.z+2*quat.w*quat.x;\n"
+"	out.m_row[2].z=1-2*quat2.x-2*quat2.y;\n"
+"	out.m_row[2].w = 0.f;\n"
+"\n"
+"	return out;\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"#define WG_SIZE 64\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_pos;\n"
+"	Quaternion m_quat;\n"
+"	float4 m_linVel;\n"
+"	float4 m_angVel;\n"
+"\n"
+"	u32 m_shapeIdx;\n"
+"	float m_invMass;\n"
+"	float m_restituitionCoeff;\n"
+"	float m_frictionCoeff;\n"
+"} Body;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	Matrix3x3 m_invInertia;\n"
+"	Matrix3x3 m_initInvInertia;\n"
+"} Shape;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_linear;\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_center;	\n"
+"	float m_jacCoeffInv[4];\n"
+"	float m_b[4];\n"
+"	float m_appliedRambdaDt[4];\n"
+"\n"
+"	float m_fJacCoeffInv[2];	\n"
+"	float m_fAppliedRambdaDt[2];	\n"
+"\n"
+"	u32 m_bodyA;\n"
+"	u32 m_bodyB;\n"
+"\n"
+"	int m_batchIdx;\n"
+"	u32 m_paddings[1];\n"
+"} Constraint4;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_worldNormal;\n"
+"	u32 m_coeffs;\n"
+"	int m_batchIdx;\n"
+"\n"
+"	int m_bodyAPtrAndSignBit;\n"
+"	int m_bodyBPtrAndSignBit;\n"
+"} Contact4;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_nConstraints;\n"
+"	int m_start;\n"
+"	int m_batchIdx;\n"
+"	int m_nSplit;\n"
+"//	int m_paddings[1];\n"
+"} ConstBuffer;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_solveFriction;\n"
+"	int m_maxBatch;	//	long batch really kills the performance\n"
+"	int m_batchIdx;\n"
+"	int m_nSplit;\n"
+"//	int m_paddings[1];\n"
+"} ConstBufferBatchSolve;\n"
+"\n"
+"\n"
+"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
+"{\n"
+"	*linear = make_float4(-n.xyz,0.f);\n"
+"	*angular0 = -cross3(r0, n);\n"
+"	*angular1 = cross3(r1, n);\n"
+"}\n"
+"\n"
+"\n"
+"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n"
+"{\n"
+"	return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n"
+"}\n"
+"\n"
+"\n"
+"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n"
+"					float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1)\n"
+"{\n"
+"	//	linear0,1 are normlized\n"
+"	float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n"
+"	float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n"
+"	float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n"
+"	float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n"
+"	return -1.f/(jmj0+jmj1+jmj2+jmj3);\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+" \n"
+"\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	int m_valInt0;\n"
+"	int m_valInt1;\n"
+"	int m_valInt2;\n"
+"	int m_valInt3;\n"
+"\n"
+"	float m_val0;\n"
+"	float m_val1;\n"
+"	float m_val2;\n"
+"	float m_val3;\n"
+"} SolverDebugInfo;\n"
+"\n"
+"\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_nContacts;\n"
+"	int m_staticIdx;\n"
+"	float m_scale;\n"
+"	int m_nSplit;\n"
+"} ConstBufferSSD;\n"
+"\n"
+"\n"
+"void b3PlaneSpace1 (float4 n, float4* p, float4* q);\n"
+" void b3PlaneSpace1 (float4 n, float4* p, float4* q)\n"
+"{\n"
+"  if (fabs(n.z) > 0.70710678f) {\n"
+"    // choose p in y-z plane\n"
+"    float a = n.y*n.y + n.z*n.z;\n"
+"    float k = 1.f/sqrt(a);\n"
+"    p[0].x = 0;\n"
+"	p[0].y = -n.z*k;\n"
+"	p[0].z = n.y*k;\n"
+"    // set q = n x p\n"
+"    q[0].x = a*k;\n"
+"	q[0].y = -n.x*p[0].z;\n"
+"	q[0].z = n.x*p[0].y;\n"
+"  }\n"
+"  else {\n"
+"    // choose p in x-y plane\n"
+"    float a = n.x*n.x + n.y*n.y;\n"
+"    float k = 1.f/sqrt(a);\n"
+"    p[0].x = -n.y*k;\n"
+"	p[0].y = n.x*k;\n"
+"	p[0].z = 0;\n"
+"    // set q = n x p\n"
+"    q[0].x = -n.z*p[0].y;\n"
+"	q[0].y = n.z*p[0].x;\n"
+"	q[0].z = a*k;\n"
+"  }\n"
+"}\n"
+"\n"
+"\n"
+"void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVelA, float invMassA, const Matrix3x3 invInertiaA,\n"
+"	const float4 posB, const float4 linVelB, const float4 angVelB, float invMassB, const Matrix3x3 invInertiaB, \n"
+"	__global Contact4* src, float dt, float positionDrift, float positionConstraintCoeff,\n"
+"	Constraint4* dstC )\n"
+"{\n"
+"	dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);\n"
+"	dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);\n"
+"\n"
+"	float dtInv = 1.f/dt;\n"
+"	for(int ic=0; ic<4; ic++)\n"
+"	{\n"
+"		dstC->m_appliedRambdaDt[ic] = 0.f;\n"
+"	}\n"
+"	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n"
+"\n"
+"\n"
+"	dstC->m_linear = -src->m_worldNormal;\n"
+"	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n"
+"	for(int ic=0; ic<4; ic++)\n"
+"	{\n"
+"		float4 r0 = src->m_worldPos[ic] - posA;\n"
+"		float4 r1 = src->m_worldPos[ic] - posB;\n"
+"\n"
+"		if( ic >= src->m_worldNormal.w )//npoints\n"
+"		{\n"
+"			dstC->m_jacCoeffInv[ic] = 0.f;\n"
+"			continue;\n"
+"		}\n"
+"\n"
+"		float relVelN;\n"
+"		{\n"
+"			float4 linear, angular0, angular1;\n"
+"			setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1);\n"
+"\n"
+"			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n"
+"				invMassA, &invInertiaA, invMassB, &invInertiaB );\n"
+"\n"
+"			relVelN = calcRelVel(linear, -linear, angular0, angular1,\n"
+"				linVelA, angVelA, linVelB, angVelB);\n"
+"\n"
+"			float e = 0.f;//src->getRestituitionCoeff();\n"
+"			if( relVelN*relVelN < 0.004f ) e = 0.f;\n"
+"\n"
+"			dstC->m_b[ic] = e*relVelN;\n"
+"			//float penetration = src->m_worldPos[ic].w;\n"
+"			dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n"
+"			dstC->m_appliedRambdaDt[ic] = 0.f;\n"
+"		}\n"
+"	}\n"
+"\n"
+"	if( src->m_worldNormal.w > 0 )//npoints\n"
+"	{	//	prepare friction\n"
+"		float4 center = make_float4(0.f);\n"
+"		for(int i=0; i<src->m_worldNormal.w; i++) \n"
+"			center += src->m_worldPos[i];\n"
+"		center /= (float)src->m_worldNormal.w;\n"
+"\n"
+"		float4 tangent[2];\n"
+"		b3PlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]);\n"
+"		\n"
+"		float4 r[2];\n"
+"		r[0] = center - posA;\n"
+"		r[1] = center - posB;\n"
+"\n"
+"		for(int i=0; i<2; i++)\n"
+"		{\n"
+"			float4 linear, angular0, angular1;\n"
+"			setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);\n"
+"\n"
+"			dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,\n"
+"				invMassA, &invInertiaA, invMassB, &invInertiaB );\n"
+"			dstC->m_fAppliedRambdaDt[i] = 0.f;\n"
+"		}\n"
+"		dstC->m_center = center;\n"
+"	}\n"
+"\n"
+"	for(int i=0; i<4; i++)\n"
+"	{\n"
+"		if( i<src->m_worldNormal.w )\n"
+"		{\n"
+"			dstC->m_worldPos[i] = src->m_worldPos[i];\n"
+"		}\n"
+"		else\n"
+"		{\n"
+"			dstC->m_worldPos[i] = make_float4(0.f);\n"
+"		}\n"
+"	}\n"
+"}\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_nContacts;\n"
+"	float m_dt;\n"
+"	float m_positionDrift;\n"
+"	float m_positionConstraintCoeff;\n"
+"} ConstBufferCTC;\n"
+"\n"
+"__kernel\n"
+"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n"
+"void ContactToConstraintKernel(__global Contact4* gContact, __global Body* gBodies, __global Shape* gShapes, __global Constraint4* gConstraintOut, \n"
+"int nContacts,\n"
+"float dt,\n"
+"float positionDrift,\n"
+"float positionConstraintCoeff\n"
+")\n"
+"{\n"
+"	int gIdx = GET_GLOBAL_IDX;\n"
+"	\n"
+"	if( gIdx < nContacts )\n"
+"	{\n"
+"		int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n"
+"		int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n"
+"\n"
+"		float4 posA = gBodies[aIdx].m_pos;\n"
+"		float4 linVelA = gBodies[aIdx].m_linVel;\n"
+"		float4 angVelA = gBodies[aIdx].m_angVel;\n"
+"		float invMassA = gBodies[aIdx].m_invMass;\n"
+"		Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n"
+"\n"
+"		float4 posB = gBodies[bIdx].m_pos;\n"
+"		float4 linVelB = gBodies[bIdx].m_linVel;\n"
+"		float4 angVelB = gBodies[bIdx].m_angVel;\n"
+"		float invMassB = gBodies[bIdx].m_invMass;\n"
+"		Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n"
+"\n"
+"		Constraint4 cs;\n"
+"\n"
+"    	setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n"
+"			&gContact[gIdx], dt, positionDrift, positionConstraintCoeff,\n"
+"			&cs );\n"
+"		\n"
+"		cs.m_batchIdx = gContact[gIdx].m_batchIdx;\n"
+"\n"
+"		gConstraintOut[gIdx] = cs;\n"
+"	}\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+;

src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.cl

@@ -0,0 +1,494 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Takahiro Harada
+
+
+#pragma OPENCL EXTENSION cl_amd_printf : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable
+
+
+#ifdef cl_ext_atomic_counters_32
+#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
+#else
+#define counter32_t volatile global int*
+#endif
+
+typedef unsigned int u32;
+typedef unsigned short u16;
+typedef unsigned char u8;
+
+#define GET_GROUP_IDX get_group_id(0)
+#define GET_LOCAL_IDX get_local_id(0)
+#define GET_GLOBAL_IDX get_global_id(0)
+#define GET_GROUP_SIZE get_local_size(0)
+#define GET_NUM_GROUPS get_num_groups(0)
+#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)
+#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)
+#define AtomInc(x) atom_inc(&(x))
+#define AtomInc1(x, out) out = atom_inc(&(x))
+#define AppendInc(x, out) out = atomic_inc(x)
+#define AtomAdd(x, value) atom_add(&(x), value)
+#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )
+#define AtomXhg(x, value) atom_xchg ( &(x), value )
+
+
+#define SELECT_UINT4( b, a, condition ) select( b,a,condition )
+
+#define make_float4 (float4)
+#define make_float2 (float2)
+#define make_uint4 (uint4)
+#define make_int4 (int4)
+#define make_uint2 (uint2)
+#define make_int2 (int2)
+
+
+#define max2 max
+#define min2 min
+
+
+///////////////////////////////////////
+//	Vector
+///////////////////////////////////////
+__inline
+float fastDiv(float numerator, float denominator)
+{
+	return native_divide(numerator, denominator);	
+//	return numerator/denominator;	
+}
+
+__inline
+float4 fastDiv4(float4 numerator, float4 denominator)
+{
+	return native_divide(numerator, denominator);	
+}
+
+__inline
+float fastSqrtf(float f2)
+{
+	return native_sqrt(f2);
+//	return sqrt(f2);
+}
+
+__inline
+float fastRSqrt(float f2)
+{
+	return native_rsqrt(f2);
+}
+
+__inline
+float fastLength4(float4 v)
+{
+	return fast_length(v);
+}
+
+__inline
+float4 fastNormalize4(float4 v)
+{
+	return fast_normalize(v);
+}
+
+
+__inline
+float sqrtf(float a)
+{
+//	return sqrt(a);
+	return native_sqrt(a);
+}
+
+__inline
+float4 cross3(float4 a, float4 b)
+{
+	return cross(a,b);
+}
+
+__inline
+float dot3F4(float4 a, float4 b)
+{
+	float4 a1 = make_float4(a.xyz,0.f);
+	float4 b1 = make_float4(b.xyz,0.f);
+	return dot(a1, b1);
+}
+
+__inline
+float length3(const float4 a)
+{
+	return sqrtf(dot3F4(a,a));
+}
+
+__inline
+float dot4(const float4 a, const float4 b)
+{
+	return dot( a, b );
+}
+
+//	for height
+__inline
+float dot3w1(const float4 point, const float4 eqn)
+{
+	return dot3F4(point,eqn) + eqn.w;
+}
+
+__inline
+float4 normalize3(const float4 a)
+{
+	float4 n = make_float4(a.x, a.y, a.z, 0.f);
+	return fastNormalize4( n );
+//	float length = sqrtf(dot3F4(a, a));
+//	return 1.f/length * a;
+}
+
+__inline
+float4 normalize4(const float4 a)
+{
+	float length = sqrtf(dot4(a, a));
+	return 1.f/length * a;
+}
+
+__inline
+float4 createEquation(const float4 a, const float4 b, const float4 c)
+{
+	float4 eqn;
+	float4 ab = b-a;
+	float4 ac = c-a;
+	eqn = normalize3( cross3(ab, ac) );
+	eqn.w = -dot3F4(eqn,a);
+	return eqn;
+}
+
+///////////////////////////////////////
+//	Matrix3x3
+///////////////////////////////////////
+
+typedef struct
+{
+	float4 m_row[3];
+}Matrix3x3;
+
+__inline
+Matrix3x3 mtZero();
+
+__inline
+Matrix3x3 mtIdentity();
+
+__inline
+Matrix3x3 mtTranspose(Matrix3x3 m);
+
+__inline
+Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b);
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b);
+
+__inline
+Matrix3x3 mtZero()
+{
+	Matrix3x3 m;
+	m.m_row[0] = (float4)(0.f);
+	m.m_row[1] = (float4)(0.f);
+	m.m_row[2] = (float4)(0.f);
+	return m;
+}
+
+__inline
+Matrix3x3 mtIdentity()
+{
+	Matrix3x3 m;
+	m.m_row[0] = (float4)(1,0,0,0);
+	m.m_row[1] = (float4)(0,1,0,0);
+	m.m_row[2] = (float4)(0,0,1,0);
+	return m;
+}
+
+__inline
+Matrix3x3 mtTranspose(Matrix3x3 m)
+{
+	Matrix3x3 out;
+	out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
+	out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);
+	out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);
+	return out;
+}
+
+__inline
+Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)
+{
+	Matrix3x3 transB;
+	transB = mtTranspose( b );
+	Matrix3x3 ans;
+	//	why this doesn't run when 0ing in the for{}
+	a.m_row[0].w = 0.f;
+	a.m_row[1].w = 0.f;
+	a.m_row[2].w = 0.f;
+	for(int i=0; i<3; i++)
+	{
+//	a.m_row[i].w = 0.f;
+		ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);
+		ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);
+		ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);
+		ans.m_row[i].w = 0.f;
+	}
+	return ans;
+}
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b)
+{
+	float4 ans;
+	ans.x = dot3F4( a.m_row[0], b );
+	ans.y = dot3F4( a.m_row[1], b );
+	ans.z = dot3F4( a.m_row[2], b );
+	ans.w = 0.f;
+	return ans;
+}
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b)
+{
+	float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
+	float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
+	float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
+
+	float4 ans;
+	ans.x = dot3F4( a, colx );
+	ans.y = dot3F4( a, coly );
+	ans.z = dot3F4( a, colz );
+	return ans;
+}
+
+///////////////////////////////////////
+//	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
+Quaternion qtMul(Quaternion a, Quaternion b)
+{
+	Quaternion ans;
+	ans = cross3( 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 fastNormalize4(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 );
+}
+
+
+
+
+#define WG_SIZE 64
+
+typedef struct
+{
+	float4 m_pos;
+	Quaternion m_quat;
+	float4 m_linVel;
+	float4 m_angVel;
+
+	u32 m_shapeIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
+} Body;
+
+typedef struct
+{
+	Matrix3x3 m_invInertia;
+	Matrix3x3 m_initInvInertia;
+} Shape;
+
+typedef struct
+{
+	float4 m_linear;
+	float4 m_worldPos[4];
+	float4 m_center;	
+	float m_jacCoeffInv[4];
+	float m_b[4];
+	float m_appliedRambdaDt[4];
+
+	float m_fJacCoeffInv[2];	
+	float m_fAppliedRambdaDt[2];	
+
+	u32 m_bodyA;
+	u32 m_bodyB;
+
+	int m_batchIdx;
+	u32 m_paddings[1];
+} Constraint4;
+
+typedef struct
+{
+	float4 m_worldPos[4];
+	float4 m_worldNormal;
+	u32 m_coeffs;
+	int m_batchIdx;
+
+	int m_bodyAPtrAndSignBit;
+	int m_bodyBPtrAndSignBit;
+} Contact4;
+
+typedef struct
+{
+	int m_nConstraints;
+	int m_start;
+	int m_batchIdx;
+	int m_nSplit;
+//	int m_paddings[1];
+} ConstBuffer;
+
+typedef struct
+{
+	int m_solveFriction;
+	int m_maxBatch;	//	long batch really kills the performance
+	int m_batchIdx;
+	int m_nSplit;
+//	int m_paddings[1];
+} ConstBufferBatchSolve;
+
+
+ 
+
+
+typedef struct 
+{
+	int m_valInt0;
+	int m_valInt1;
+	int m_valInt2;
+	int m_valInt3;
+
+	float m_val0;
+	float m_val1;
+	float m_val2;
+	float m_val3;
+} SolverDebugInfo;
+
+
+
+
+//	others
+__kernel
+__attribute__((reqd_work_group_size(WG_SIZE,1,1)))
+void ReorderContactKernel(__global Contact4* in, __global Contact4* out, __global int2* sortData, int4 cb )
+{
+	int nContacts = cb.x;
+	int gIdx = GET_GLOBAL_IDX;
+
+	if( gIdx < nContacts )
+	{
+		int srcIdx = sortData[gIdx].y;
+		out[gIdx] = in[srcIdx];
+	}
+}
+
+typedef struct
+{
+	int m_nContacts;
+	int m_staticIdx;
+	float m_scale;
+	int m_nSplit;
+} ConstBufferSSD;
+
+__kernel
+__attribute__((reqd_work_group_size(WG_SIZE,1,1)))
+void SetSortDataKernel(__global Contact4* gContact, __global Body* gBodies, __global int2* gSortDataOut, 
+int nContacts,
+float scale,
+int N_SPLIT
+)
+
+{
+	int gIdx = GET_GLOBAL_IDX;
+	
+	if( gIdx < nContacts )
+	{
+		int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);
+		int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);
+
+		int idx = (gContact[gIdx].m_bodyAPtrAndSignBit<0)? bIdx: aIdx;
+		float4 p = gBodies[idx].m_pos;
+		int xIdx = (int)((p.x-((p.x<0.f)?1.f:0.f))*scale) & (N_SPLIT-1);
+		int zIdx = (int)((p.z-((p.z<0.f)?1.f:0.f))*scale) & (N_SPLIT-1);
+
+		gSortDataOut[gIdx].x = (xIdx+zIdx*N_SPLIT);
+		gSortDataOut[gIdx].y = gIdx;
+	}
+	else
+	{
+		gSortDataOut[gIdx].x = 0xffffffff;
+	}
+}
+
+__kernel
+__attribute__((reqd_work_group_size(WG_SIZE,1,1)))
+void CopyConstraintKernel(__global Contact4* gIn, __global Contact4* gOut, int4 cb )
+{
+	int gIdx = GET_GLOBAL_IDX;
+	if( gIdx < cb.x )
+	{
+		gOut[gIdx] = gIn[gIdx];
+	}
+}
+
+
+

src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h

@@ -0,0 +1,498 @@
+//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
+static const char* solverSetup2CL= \
+"/*\n"
+"Copyright (c) 2012 Advanced Micro Devices, Inc.  \n"
+"\n"
+"This software is provided 'as-is', without any express or implied warranty.\n"
+"In no event will the authors be held liable for any damages arising from the use of this software.\n"
+"Permission is granted to anyone to use this software for any purpose, \n"
+"including commercial applications, and to alter it and redistribute it freely, \n"
+"subject to the following restrictions:\n"
+"\n"
+"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
+"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
+"3. This notice may not be removed or altered from any source distribution.\n"
+"*/\n"
+"//Originally written by Takahiro Harada\n"
+"\n"
+"\n"
+"#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n"
+"\n"
+"\n"
+"#ifdef cl_ext_atomic_counters_32\n"
+"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n"
+"#else\n"
+"#define counter32_t volatile global int*\n"
+"#endif\n"
+"\n"
+"typedef unsigned int u32;\n"
+"typedef unsigned short u16;\n"
+"typedef unsigned char u8;\n"
+"\n"
+"#define GET_GROUP_IDX get_group_id(0)\n"
+"#define GET_LOCAL_IDX get_local_id(0)\n"
+"#define GET_GLOBAL_IDX get_global_id(0)\n"
+"#define GET_GROUP_SIZE get_local_size(0)\n"
+"#define GET_NUM_GROUPS get_num_groups(0)\n"
+"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n"
+"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n"
+"#define AtomInc(x) atom_inc(&(x))\n"
+"#define AtomInc1(x, out) out = atom_inc(&(x))\n"
+"#define AppendInc(x, out) out = atomic_inc(x)\n"
+"#define AtomAdd(x, value) atom_add(&(x), value)\n"
+"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n"
+"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n"
+"\n"
+"\n"
+"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n"
+"\n"
+"#define make_float4 (float4)\n"
+"#define make_float2 (float2)\n"
+"#define make_uint4 (uint4)\n"
+"#define make_int4 (int4)\n"
+"#define make_uint2 (uint2)\n"
+"#define make_int2 (int2)\n"
+"\n"
+"\n"
+"#define max2 max\n"
+"#define min2 min\n"
+"\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Vector\n"
+"///////////////////////////////////////\n"
+"__inline\n"
+"float fastDiv(float numerator, float denominator)\n"
+"{\n"
+"	return native_divide(numerator, denominator);	\n"
+"//	return numerator/denominator;	\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 fastDiv4(float4 numerator, float4 denominator)\n"
+"{\n"
+"	return native_divide(numerator, denominator);	\n"
+"}\n"
+"\n"
+"__inline\n"
+"float fastSqrtf(float f2)\n"
+"{\n"
+"	return native_sqrt(f2);\n"
+"//	return sqrt(f2);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float fastRSqrt(float f2)\n"
+"{\n"
+"	return native_rsqrt(f2);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float fastLength4(float4 v)\n"
+"{\n"
+"	return fast_length(v);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 fastNormalize4(float4 v)\n"
+"{\n"
+"	return fast_normalize(v);\n"
+"}\n"
+"\n"
+"\n"
+"__inline\n"
+"float sqrtf(float a)\n"
+"{\n"
+"//	return sqrt(a);\n"
+"	return native_sqrt(a);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 cross3(float4 a, float4 b)\n"
+"{\n"
+"	return cross(a,b);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float dot3F4(float4 a, float4 b)\n"
+"{\n"
+"	float4 a1 = make_float4(a.xyz,0.f);\n"
+"	float4 b1 = make_float4(b.xyz,0.f);\n"
+"	return dot(a1, b1);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float length3(const float4 a)\n"
+"{\n"
+"	return sqrtf(dot3F4(a,a));\n"
+"}\n"
+"\n"
+"__inline\n"
+"float dot4(const float4 a, const float4 b)\n"
+"{\n"
+"	return dot( a, b );\n"
+"}\n"
+"\n"
+"//	for height\n"
+"__inline\n"
+"float dot3w1(const float4 point, const float4 eqn)\n"
+"{\n"
+"	return dot3F4(point,eqn) + eqn.w;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 normalize3(const float4 a)\n"
+"{\n"
+"	float4 n = make_float4(a.x, a.y, a.z, 0.f);\n"
+"	return fastNormalize4( n );\n"
+"//	float length = sqrtf(dot3F4(a, a));\n"
+"//	return 1.f/length * a;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 normalize4(const float4 a)\n"
+"{\n"
+"	float length = sqrtf(dot4(a, a));\n"
+"	return 1.f/length * a;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 createEquation(const float4 a, const float4 b, const float4 c)\n"
+"{\n"
+"	float4 eqn;\n"
+"	float4 ab = b-a;\n"
+"	float4 ac = c-a;\n"
+"	eqn = normalize3( cross3(ab, ac) );\n"
+"	eqn.w = -dot3F4(eqn,a);\n"
+"	return eqn;\n"
+"}\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Matrix3x3\n"
+"///////////////////////////////////////\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_row[3];\n"
+"}Matrix3x3;\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtZero();\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtIdentity();\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtTranspose(Matrix3x3 m);\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b);\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b);\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtZero()\n"
+"{\n"
+"	Matrix3x3 m;\n"
+"	m.m_row[0] = (float4)(0.f);\n"
+"	m.m_row[1] = (float4)(0.f);\n"
+"	m.m_row[2] = (float4)(0.f);\n"
+"	return m;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtIdentity()\n"
+"{\n"
+"	Matrix3x3 m;\n"
+"	m.m_row[0] = (float4)(1,0,0,0);\n"
+"	m.m_row[1] = (float4)(0,1,0,0);\n"
+"	m.m_row[2] = (float4)(0,0,1,0);\n"
+"	return m;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtTranspose(Matrix3x3 m)\n"
+"{\n"
+"	Matrix3x3 out;\n"
+"	out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n"
+"	out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n"
+"	out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n"
+"	return out;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n"
+"{\n"
+"	Matrix3x3 transB;\n"
+"	transB = mtTranspose( b );\n"
+"	Matrix3x3 ans;\n"
+"	//	why this doesn't run when 0ing in the for{}\n"
+"	a.m_row[0].w = 0.f;\n"
+"	a.m_row[1].w = 0.f;\n"
+"	a.m_row[2].w = 0.f;\n"
+"	for(int i=0; i<3; i++)\n"
+"	{\n"
+"//	a.m_row[i].w = 0.f;\n"
+"		ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n"
+"		ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n"
+"		ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n"
+"		ans.m_row[i].w = 0.f;\n"
+"	}\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b)\n"
+"{\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a.m_row[0], b );\n"
+"	ans.y = dot3F4( a.m_row[1], b );\n"
+"	ans.z = dot3F4( a.m_row[2], b );\n"
+"	ans.w = 0.f;\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b)\n"
+"{\n"
+"	float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n"
+"	float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n"
+"	float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n"
+"\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a, colx );\n"
+"	ans.y = dot3F4( a, coly );\n"
+"	ans.z = dot3F4( a, colz );\n"
+"	return ans;\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"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"Quaternion qtMul(Quaternion a, Quaternion b)\n"
+"{\n"
+"	Quaternion ans;\n"
+"	ans = cross3( 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 fastNormalize4(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"
+"\n"
+"\n"
+"\n"
+"#define WG_SIZE 64\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_pos;\n"
+"	Quaternion m_quat;\n"
+"	float4 m_linVel;\n"
+"	float4 m_angVel;\n"
+"\n"
+"	u32 m_shapeIdx;\n"
+"	float m_invMass;\n"
+"	float m_restituitionCoeff;\n"
+"	float m_frictionCoeff;\n"
+"} Body;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	Matrix3x3 m_invInertia;\n"
+"	Matrix3x3 m_initInvInertia;\n"
+"} Shape;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_linear;\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_center;	\n"
+"	float m_jacCoeffInv[4];\n"
+"	float m_b[4];\n"
+"	float m_appliedRambdaDt[4];\n"
+"\n"
+"	float m_fJacCoeffInv[2];	\n"
+"	float m_fAppliedRambdaDt[2];	\n"
+"\n"
+"	u32 m_bodyA;\n"
+"	u32 m_bodyB;\n"
+"\n"
+"	int m_batchIdx;\n"
+"	u32 m_paddings[1];\n"
+"} Constraint4;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_worldNormal;\n"
+"	u32 m_coeffs;\n"
+"	int m_batchIdx;\n"
+"\n"
+"	int m_bodyAPtrAndSignBit;\n"
+"	int m_bodyBPtrAndSignBit;\n"
+"} Contact4;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_nConstraints;\n"
+"	int m_start;\n"
+"	int m_batchIdx;\n"
+"	int m_nSplit;\n"
+"//	int m_paddings[1];\n"
+"} ConstBuffer;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_solveFriction;\n"
+"	int m_maxBatch;	//	long batch really kills the performance\n"
+"	int m_batchIdx;\n"
+"	int m_nSplit;\n"
+"//	int m_paddings[1];\n"
+"} ConstBufferBatchSolve;\n"
+"\n"
+"\n"
+" \n"
+"\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	int m_valInt0;\n"
+"	int m_valInt1;\n"
+"	int m_valInt2;\n"
+"	int m_valInt3;\n"
+"\n"
+"	float m_val0;\n"
+"	float m_val1;\n"
+"	float m_val2;\n"
+"	float m_val3;\n"
+"} SolverDebugInfo;\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"//	others\n"
+"__kernel\n"
+"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n"
+"void ReorderContactKernel(__global Contact4* in, __global Contact4* out, __global int2* sortData, int4 cb )\n"
+"{\n"
+"	int nContacts = cb.x;\n"
+"	int gIdx = GET_GLOBAL_IDX;\n"
+"\n"
+"	if( gIdx < nContacts )\n"
+"	{\n"
+"		int srcIdx = sortData[gIdx].y;\n"
+"		out[gIdx] = in[srcIdx];\n"
+"	}\n"
+"}\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	int m_nContacts;\n"
+"	int m_staticIdx;\n"
+"	float m_scale;\n"
+"	int m_nSplit;\n"
+"} ConstBufferSSD;\n"
+"\n"
+"__kernel\n"
+"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n"
+"void SetSortDataKernel(__global Contact4* gContact, __global Body* gBodies, __global int2* gSortDataOut, \n"
+"int nContacts,\n"
+"float scale,\n"
+"int N_SPLIT\n"
+")\n"
+"\n"
+"{\n"
+"	int gIdx = GET_GLOBAL_IDX;\n"
+"	\n"
+"	if( gIdx < nContacts )\n"
+"	{\n"
+"		int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n"
+"		int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n"
+"\n"
+"		int idx = (gContact[gIdx].m_bodyAPtrAndSignBit<0)? bIdx: aIdx;\n"
+"		float4 p = gBodies[idx].m_pos;\n"
+"		int xIdx = (int)((p.x-((p.x<0.f)?1.f:0.f))*scale) & (N_SPLIT-1);\n"
+"		int zIdx = (int)((p.z-((p.z<0.f)?1.f:0.f))*scale) & (N_SPLIT-1);\n"
+"\n"
+"		gSortDataOut[gIdx].x = (xIdx+zIdx*N_SPLIT);\n"
+"		gSortDataOut[gIdx].y = gIdx;\n"
+"	}\n"
+"	else\n"
+"	{\n"
+"		gSortDataOut[gIdx].x = 0xffffffff;\n"
+"	}\n"
+"}\n"
+"\n"
+"__kernel\n"
+"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n"
+"void CopyConstraintKernel(__global Contact4* gIn, __global Contact4* gOut, int4 cb )\n"
+"{\n"
+"	int gIdx = GET_GLOBAL_IDX;\n"
+"	if( gIdx < cb.x )\n"
+"	{\n"
+"		gOut[gIdx] = gIn[gIdx];\n"
+"	}\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+;

src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl

@@ -0,0 +1,971 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#pragma OPENCL EXTENSION cl_amd_printf : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable
+#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable
+
+
+#ifdef cl_ext_atomic_counters_32
+#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
+#else
+#define counter32_t volatile global int*
+#endif
+
+typedef unsigned int u32;
+typedef unsigned short u16;
+typedef unsigned char u8;
+
+#define GET_GROUP_IDX get_group_id(0)
+#define GET_LOCAL_IDX get_local_id(0)
+#define GET_GLOBAL_IDX get_global_id(0)
+#define GET_GROUP_SIZE get_local_size(0)
+#define GET_NUM_GROUPS get_num_groups(0)
+#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)
+#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)
+#define AtomInc(x) atom_inc(&(x))
+#define AtomInc1(x, out) out = atom_inc(&(x))
+#define AppendInc(x, out) out = atomic_inc(x)
+#define AtomAdd(x, value) atom_add(&(x), value)
+#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )
+#define AtomXhg(x, value) atom_xchg ( &(x), value )
+
+
+#define SELECT_UINT4( b, a, condition ) select( b,a,condition )
+
+#define make_float4 (float4)
+#define make_float2 (float2)
+#define make_uint4 (uint4)
+#define make_int4 (int4)
+#define make_uint2 (uint2)
+#define make_int2 (int2)
+
+
+#define max2 max
+#define min2 min
+
+
+///////////////////////////////////////
+//	Vector
+///////////////////////////////////////
+__inline
+float fastDiv(float numerator, float denominator)
+{
+	return native_divide(numerator, denominator);	
+//	return numerator/denominator;	
+}
+
+__inline
+float4 fastDiv4(float4 numerator, float4 denominator)
+{
+	return native_divide(numerator, denominator);	
+}
+
+__inline
+float fastSqrtf(float f2)
+{
+	return native_sqrt(f2);
+//	return sqrt(f2);
+}
+
+__inline
+float fastRSqrt(float f2)
+{
+	return native_rsqrt(f2);
+}
+
+__inline
+float fastLength4(float4 v)
+{
+	return fast_length(v);
+}
+
+__inline
+float4 fastNormalize4(float4 v)
+{
+	return fast_normalize(v);
+}
+
+
+__inline
+float sqrtf(float a)
+{
+//	return sqrt(a);
+	return native_sqrt(a);
+}
+
+__inline
+float4 cross3(float4 a1, float4 b1)
+{
+
+	float4 	a=make_float4(a1.xyz,0.f);
+	float4 	b=make_float4(b1.xyz,0.f);
+	//float4 	a=a1;
+	//float4 	b=b1;
+	return cross(a,b);
+}
+
+__inline
+float dot3F4(float4 a, float4 b)
+{
+	float4 a1 = make_float4(a.xyz,0.f);
+	float4 b1 = make_float4(b.xyz,0.f);
+	return dot(a1, b1);
+}
+
+__inline
+float length3(const float4 a)
+{
+	return sqrtf(dot3F4(a,a));
+}
+
+__inline
+float dot4(const float4 a, const float4 b)
+{
+	return dot( a, b );
+}
+
+//	for height
+__inline
+float dot3w1(const float4 point, const float4 eqn)
+{
+	return dot3F4(point,eqn) + eqn.w;
+}
+
+__inline
+float4 normalize3(const float4 a)
+{
+	float4 n = make_float4(a.x, a.y, a.z, 0.f);
+	return fastNormalize4( n );
+//	float length = sqrtf(dot3F4(a, a));
+//	return 1.f/length * a;
+}
+
+__inline
+float4 normalize4(const float4 a)
+{
+	float length = sqrtf(dot4(a, a));
+	return 1.f/length * a;
+}
+
+__inline
+float4 createEquation(const float4 a, const float4 b, const float4 c)
+{
+	float4 eqn;
+	float4 ab = b-a;
+	float4 ac = c-a;
+	eqn = normalize3( cross3(ab, ac) );
+	eqn.w = -dot3F4(eqn,a);
+	return eqn;
+}
+
+///////////////////////////////////////
+//	Matrix3x3
+///////////////////////////////////////
+
+typedef struct
+{
+	float4 m_row[3];
+}Matrix3x3;
+
+__inline
+Matrix3x3 mtZero();
+
+__inline
+Matrix3x3 mtIdentity();
+
+__inline
+Matrix3x3 mtTranspose(Matrix3x3 m);
+
+__inline
+Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b);
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b);
+
+__inline
+Matrix3x3 mtZero()
+{
+	Matrix3x3 m;
+	m.m_row[0] = (float4)(0.f);
+	m.m_row[1] = (float4)(0.f);
+	m.m_row[2] = (float4)(0.f);
+	return m;
+}
+
+__inline
+Matrix3x3 mtIdentity()
+{
+	Matrix3x3 m;
+	m.m_row[0] = (float4)(1,0,0,0);
+	m.m_row[1] = (float4)(0,1,0,0);
+	m.m_row[2] = (float4)(0,0,1,0);
+	return m;
+}
+
+__inline
+Matrix3x3 mtTranspose(Matrix3x3 m)
+{
+	Matrix3x3 out;
+	out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
+	out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);
+	out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);
+	return out;
+}
+
+__inline
+Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)
+{
+	Matrix3x3 transB;
+	transB = mtTranspose( b );
+	Matrix3x3 ans;
+	//	why this doesn't run when 0ing in the for{}
+	a.m_row[0].w = 0.f;
+	a.m_row[1].w = 0.f;
+	a.m_row[2].w = 0.f;
+	for(int i=0; i<3; i++)
+	{
+//	a.m_row[i].w = 0.f;
+		ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);
+		ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);
+		ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);
+		ans.m_row[i].w = 0.f;
+	}
+	return ans;
+}
+
+__inline
+float4 mtMul1(Matrix3x3 a, float4 b)
+{
+	float4 ans;
+	ans.x = dot3F4( a.m_row[0], b );
+	ans.y = dot3F4( a.m_row[1], b );
+	ans.z = dot3F4( a.m_row[2], b );
+	ans.w = 0.f;
+	return ans;
+}
+
+__inline
+float4 mtMul3(float4 a, Matrix3x3 b)
+{
+	float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);
+	float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);
+	float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);
+
+	float4 ans;
+	ans.x = dot3F4( a, colx );
+	ans.y = dot3F4( a, coly );
+	ans.z = dot3F4( a, colz );
+	return ans;
+}
+
+///////////////////////////////////////
+//	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
+Quaternion qtMul(Quaternion a, Quaternion b)
+{
+	Quaternion ans;
+	ans = cross3( 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 fastNormalize4(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 );
+}
+
+
+
+
+#define WG_SIZE 64
+
+typedef struct
+{
+	float4 m_pos;
+	Quaternion m_quat;
+	float4 m_linVel;
+	float4 m_angVel;
+
+	u32 m_shapeIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
+} Body;
+
+typedef struct
+{
+	Matrix3x3 m_invInertia;
+	Matrix3x3 m_initInvInertia;
+} Shape;
+
+typedef struct
+{
+	float4 m_linear;
+	float4 m_worldPos[4];
+	float4 m_center;	
+	float m_jacCoeffInv[4];
+	float m_b[4];
+	float m_appliedRambdaDt[4];
+
+	float m_fJacCoeffInv[2];	
+	float m_fAppliedRambdaDt[2];	
+
+	u32 m_bodyA;
+	u32 m_bodyB;
+	int m_batchIdx;
+	u32 m_paddings;
+} Constraint4;
+
+typedef struct
+{
+	float4 m_worldPos[4];
+	float4 m_worldNormal;
+	u32 m_coeffs;
+	int m_batchIdx;
+
+	int m_bodyAPtrAndSignBit;
+	int m_bodyBPtrAndSignBit;
+} Contact4;
+
+
+__kernel void CountBodiesKernel(__global Contact4* manifoldPtr, __global unsigned int* bodyCount, __global int2* contactConstraintOffsets, int numContactManifolds, int fixedBodyIndex)
+{
+	int i = GET_GLOBAL_IDX;
+	
+	if( i < numContactManifolds)
+	{
+		int pa = manifoldPtr[i].m_bodyAPtrAndSignBit;
+		bool isFixedA = (pa <0) || (pa == fixedBodyIndex);
+		int bodyIndexA = abs(pa);
+		if (!isFixedA)
+		{
+			 AtomInc1(bodyCount[bodyIndexA],contactConstraintOffsets[i].x);
+		}
+		barrier(CLK_GLOBAL_MEM_FENCE);
+		int pb = manifoldPtr[i].m_bodyBPtrAndSignBit;
+		bool isFixedB = (pb <0) || (pb == fixedBodyIndex);
+		int bodyIndexB = abs(pb);
+		if (!isFixedB)
+		{
+			AtomInc1(bodyCount[bodyIndexB],contactConstraintOffsets[i].y);
+		} 
+	}
+}
+
+__kernel void ClearVelocitiesKernel(__global float4* linearVelocities,__global float4* angularVelocities, int numSplitBodies)
+{
+	int i = GET_GLOBAL_IDX;
+	
+	if( i < numSplitBodies)
+	{
+		linearVelocities[i] = make_float4(0);
+		angularVelocities[i] = make_float4(0);
+	}
+}
+
+
+__kernel void AverageVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,
+__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)
+{
+	int i = GET_GLOBAL_IDX;
+	if (i<numBodies)
+	{
+		if (gBodies[i].m_invMass)
+		{
+			int bodyOffset = offsetSplitBodies[i];
+			int count = bodyCount[i];
+			float factor = 1.f/((float)count);
+			float4 averageLinVel = make_float4(0.f);
+			float4 averageAngVel = make_float4(0.f);
+			
+			for (int j=0;j<count;j++)
+			{
+				averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;
+				averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;
+			}
+			
+			for (int j=0;j<count;j++)
+			{
+				deltaLinearVelocities[bodyOffset+j] = averageLinVel;
+				deltaAngularVelocities[bodyOffset+j] = averageAngVel;
+			}
+			
+		}//bodies[i].m_invMass
+	}//i<numBodies
+}
+
+
+
+void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
+{
+	*linear = make_float4(-n.xyz,0.f);
+	*angular0 = -cross3(r0, n);
+	*angular1 = cross3(r1, n);
+}
+
+
+float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )
+{
+	return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);
+}
+
+
+float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,
+					float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1, float countA, float countB)
+{
+	//	linear0,1 are normlized
+	float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;
+	float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);
+	float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;
+	float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);
+	return -1.f/((jmj0+jmj1)*countA+(jmj2+jmj3)*countB);
+}
+
+
+void btPlaneSpace1 (float4 n, float4* p, float4* q);
+ void btPlaneSpace1 (float4 n, float4* p, float4* q)
+{
+  if (fabs(n.z) > 0.70710678f) {
+    // choose p in y-z plane
+    float a = n.y*n.y + n.z*n.z;
+    float k = 1.f/sqrt(a);
+    p[0].x = 0;
+	p[0].y = -n.z*k;
+	p[0].z = n.y*k;
+    // set q = n x p
+    q[0].x = a*k;
+	q[0].y = -n.x*p[0].z;
+	q[0].z = n.x*p[0].y;
+  }
+  else {
+    // choose p in x-y plane
+    float a = n.x*n.x + n.y*n.y;
+    float k = 1.f/sqrt(a);
+    p[0].x = -n.y*k;
+	p[0].y = n.x*k;
+	p[0].z = 0;
+    // set q = n x p
+    q[0].x = -n.z*p[0].y;
+	q[0].y = n.z*p[0].x;
+	q[0].z = a*k;
+  }
+}
+
+
+
+
+
+void solveContact(__global Constraint4* cs,
+			float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,
+			float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB,
+			float4* dLinVelA, float4* dAngVelA, float4* dLinVelB, float4* dAngVelB)
+{
+	float minRambdaDt = 0;
+	float maxRambdaDt = FLT_MAX;
+
+	for(int ic=0; ic<4; ic++)
+	{
+		if( cs->m_jacCoeffInv[ic] == 0.f ) continue;
+
+		float4 angular0, angular1, linear;
+		float4 r0 = cs->m_worldPos[ic] - posA;
+		float4 r1 = cs->m_worldPos[ic] - posB;
+		setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );
+	
+
+
+		float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, 
+			*linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];
+		rambdaDt *= cs->m_jacCoeffInv[ic];
+
+		
+		{
+			float prevSum = cs->m_appliedRambdaDt[ic];
+			float updated = prevSum;
+			updated += rambdaDt;
+			updated = max2( updated, minRambdaDt );
+			updated = min2( updated, maxRambdaDt );
+			rambdaDt = updated - prevSum;
+			cs->m_appliedRambdaDt[ic] = updated;
+		}
+
+			
+		float4 linImp0 = invMassA*linear*rambdaDt;
+		float4 linImp1 = invMassB*(-linear)*rambdaDt;
+		float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
+		float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
+
+		
+		if (invMassA)
+		{
+			*dLinVelA += linImp0;
+			*dAngVelA += angImp0;
+		}
+		if (invMassB)
+		{
+			*dLinVelB += linImp1;
+			*dAngVelB += angImp1;
+		}
+	}
+}
+
+
+//	solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,contactConstraintOffsets,offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);
+
+
+void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, 
+__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,
+__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)
+{
+
+	//float frictionCoeff = ldsCs[0].m_linear.w;
+	int aIdx = ldsCs[0].m_bodyA;
+	int bIdx = ldsCs[0].m_bodyB;
+
+	float4 posA = gBodies[aIdx].m_pos;
+	float4 linVelA = gBodies[aIdx].m_linVel;
+	float4 angVelA = gBodies[aIdx].m_angVel;
+	float invMassA = gBodies[aIdx].m_invMass;
+	Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;
+
+	float4 posB = gBodies[bIdx].m_pos;
+	float4 linVelB = gBodies[bIdx].m_linVel;
+	float4 angVelB = gBodies[bIdx].m_angVel;
+	float invMassB = gBodies[bIdx].m_invMass;
+	Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;
+
+			
+	float4 dLinVelA = make_float4(0,0,0,0);
+	float4 dAngVelA = make_float4(0,0,0,0);
+	float4 dLinVelB = make_float4(0,0,0,0);
+	float4 dAngVelB = make_float4(0,0,0,0);
+			
+	int bodyOffsetA = offsetSplitBodies[aIdx];
+	int constraintOffsetA = contactConstraintOffsets[0].x;
+	int splitIndexA = bodyOffsetA+constraintOffsetA;
+	
+	if (invMassA)
+	{
+		dLinVelA = deltaLinearVelocities[splitIndexA];
+		dAngVelA = deltaAngularVelocities[splitIndexA];
+	}
+
+	int bodyOffsetB = offsetSplitBodies[bIdx];
+	int constraintOffsetB = contactConstraintOffsets[0].y;
+	int splitIndexB= bodyOffsetB+constraintOffsetB;
+
+	if (invMassB)
+	{
+		dLinVelB = deltaLinearVelocities[splitIndexB];
+		dAngVelB = deltaAngularVelocities[splitIndexB];
+	}
+
+	solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,
+			posB, &linVelB, &angVelB, invMassB, invInertiaB ,&dLinVelA, &dAngVelA, &dLinVelB, &dAngVelB);
+
+	if (invMassA)
+	{
+		deltaLinearVelocities[splitIndexA] = dLinVelA;
+		deltaAngularVelocities[splitIndexA] = dAngVelA;
+	} 
+	if (invMassB)
+	{
+		deltaLinearVelocities[splitIndexB] = dLinVelB;
+		deltaAngularVelocities[splitIndexB] = dAngVelB;
+	}
+
+}
+
+
+__kernel void SolveContactJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,
+__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,
+float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds
+)
+{
+	int i = GET_GLOBAL_IDX;
+	if (i<numManifolds)
+	{
+		solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);
+	}
+}
+
+
+
+
+void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs,
+							__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,
+							__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)
+{
+	float frictionCoeff = 0.7f;//ldsCs[0].m_linear.w;
+	int aIdx = ldsCs[0].m_bodyA;
+	int bIdx = ldsCs[0].m_bodyB;
+
+
+	float4 posA = gBodies[aIdx].m_pos;
+	float4 linVelA = gBodies[aIdx].m_linVel;
+	float4 angVelA = gBodies[aIdx].m_angVel;
+	float invMassA = gBodies[aIdx].m_invMass;
+	Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;
+
+	float4 posB = gBodies[bIdx].m_pos;
+	float4 linVelB = gBodies[bIdx].m_linVel;
+	float4 angVelB = gBodies[bIdx].m_angVel;
+	float invMassB = gBodies[bIdx].m_invMass;
+	Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;
+	
+
+	float4 dLinVelA = make_float4(0,0,0,0);
+	float4 dAngVelA = make_float4(0,0,0,0);
+	float4 dLinVelB = make_float4(0,0,0,0);
+	float4 dAngVelB = make_float4(0,0,0,0);
+			
+	int bodyOffsetA = offsetSplitBodies[aIdx];
+	int constraintOffsetA = contactConstraintOffsets[0].x;
+	int splitIndexA = bodyOffsetA+constraintOffsetA;
+	
+	if (invMassA)
+	{
+		dLinVelA = deltaLinearVelocities[splitIndexA];
+		dAngVelA = deltaAngularVelocities[splitIndexA];
+	}
+
+	int bodyOffsetB = offsetSplitBodies[bIdx];
+	int constraintOffsetB = contactConstraintOffsets[0].y;
+	int splitIndexB= bodyOffsetB+constraintOffsetB;
+
+	if (invMassB)
+	{
+		dLinVelB = deltaLinearVelocities[splitIndexB];
+		dAngVelB = deltaAngularVelocities[splitIndexB];
+	}
+
+
+
+
+	{
+		float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
+		float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
+
+		float sum = 0;
+		for(int j=0; j<4; j++)
+		{
+			sum +=ldsCs[0].m_appliedRambdaDt[j];
+		}
+		frictionCoeff = 0.7f;
+		for(int j=0; j<4; j++)
+		{
+			maxRambdaDt[j] = frictionCoeff*sum;
+			minRambdaDt[j] = -maxRambdaDt[j];
+		}
+
+		
+//		solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,
+//			posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );
+		
+		
+		{
+			
+			__global Constraint4* cs = ldsCs;
+			
+			if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;
+			const float4 center = cs->m_center;
+			
+			float4 n = -cs->m_linear;
+			
+			float4 tangent[2];
+			btPlaneSpace1(n,&tangent[0],&tangent[1]);
+			float4 angular0, angular1, linear;
+			float4 r0 = center - posA;
+			float4 r1 = center - posB;
+			for(int i=0; i<2; i++)
+			{
+				setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );
+				float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
+											linVelA+dLinVelA, angVelA+dAngVelA, linVelB+dLinVelB, angVelB+dAngVelB );
+				rambdaDt *= cs->m_fJacCoeffInv[i];
+				
+				{
+					float prevSum = cs->m_fAppliedRambdaDt[i];
+					float updated = prevSum;
+					updated += rambdaDt;
+					updated = max2( updated, minRambdaDt[i] );
+					updated = min2( updated, maxRambdaDt[i] );
+					rambdaDt = updated - prevSum;
+					cs->m_fAppliedRambdaDt[i] = updated;
+				}
+				
+				float4 linImp0 = invMassA*linear*rambdaDt;
+				float4 linImp1 = invMassB*(-linear)*rambdaDt;
+				float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
+				float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
+				
+				dLinVelA += linImp0;
+				dAngVelA += angImp0;
+				dLinVelB += linImp1;
+				dAngVelB += angImp1;
+			}
+			{	//	angular damping for point constraint
+				float4 ab = normalize3( posB - posA );
+				float4 ac = normalize3( center - posA );
+				if( dot3F4( ab, ac ) > 0.95f  || (invMassA == 0.f || invMassB == 0.f))
+				{
+					float angNA = dot3F4( n, angVelA );
+					float angNB = dot3F4( n, angVelB );
+					
+					dAngVelA -= (angNA*0.1f)*n;
+					dAngVelB -= (angNB*0.1f)*n;
+				}
+			}
+		}
+
+		
+		
+	}
+
+	if (invMassA)
+	{
+		deltaLinearVelocities[splitIndexA] = dLinVelA;
+		deltaAngularVelocities[splitIndexA] = dAngVelA;
+	} 
+	if (invMassB)
+	{
+		deltaLinearVelocities[splitIndexB] = dLinVelB;
+		deltaAngularVelocities[splitIndexB] = dAngVelB;
+	}
+ 
+
+}
+
+
+__kernel void SolveFrictionJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,
+										__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,
+										__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,
+										float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds
+)
+{
+	int i = GET_GLOBAL_IDX;
+	if (i<numManifolds)
+	{
+		solveFrictionConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);
+	}
+}
+
+
+__kernel void UpdateBodyVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,
+									__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)
+{
+	int i = GET_GLOBAL_IDX;
+	if (i<numBodies)
+	{
+		if (gBodies[i].m_invMass)
+		{
+			int bodyOffset = offsetSplitBodies[i];
+			int count = bodyCount[i];
+			if (count)
+			{
+				gBodies[i].m_linVel += deltaLinearVelocities[bodyOffset];
+				gBodies[i].m_angVel += deltaAngularVelocities[bodyOffset];
+			}
+		}
+	}
+}
+
+
+
+void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVelA, float invMassA, const Matrix3x3 invInertiaA,
+	const float4 posB, const float4 linVelB, const float4 angVelB, float invMassB, const Matrix3x3 invInertiaB, 
+	__global Contact4* src, float dt, float positionDrift, float positionConstraintCoeff,float countA, float countB,
+	Constraint4* dstC )
+{
+	dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);
+	dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);
+
+	float dtInv = 1.f/dt;
+	for(int ic=0; ic<4; ic++)
+	{
+		dstC->m_appliedRambdaDt[ic] = 0.f;
+	}
+	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
+
+
+	dstC->m_linear = -src->m_worldNormal;
+	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );
+	for(int ic=0; ic<4; ic++)
+	{
+		float4 r0 = src->m_worldPos[ic] - posA;
+		float4 r1 = src->m_worldPos[ic] - posB;
+
+		if( ic >= src->m_worldNormal.w )//npoints
+		{
+			dstC->m_jacCoeffInv[ic] = 0.f;
+			continue;
+		}
+
+		float relVelN;
+		{
+			float4 linear, angular0, angular1;
+			setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1);
+
+			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
+				invMassA, &invInertiaA, invMassB, &invInertiaB , countA, countB);
+
+			relVelN = calcRelVel(linear, -linear, angular0, angular1,
+				linVelA, angVelA, linVelB, angVelB);
+
+			float e = 0.f;//src->getRestituitionCoeff();
+			if( relVelN*relVelN < 0.004f ) e = 0.f;
+
+			dstC->m_b[ic] = e*relVelN;
+			//float penetration = src->m_worldPos[ic].w;
+			dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
+			dstC->m_appliedRambdaDt[ic] = 0.f;
+		}
+	}
+
+	if( src->m_worldNormal.w > 0 )//npoints
+	{	//	prepare friction
+		float4 center = make_float4(0.f);
+		for(int i=0; i<src->m_worldNormal.w; i++) 
+			center += src->m_worldPos[i];
+		center /= (float)src->m_worldNormal.w;
+
+		float4 tangent[2];
+		btPlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]);
+		
+		float4 r[2];
+		r[0] = center - posA;
+		r[1] = center - posB;
+
+		for(int i=0; i<2; i++)
+		{
+			float4 linear, angular0, angular1;
+			setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);
+
+			dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,
+				invMassA, &invInertiaA, invMassB, &invInertiaB ,countA, countB);
+			dstC->m_fAppliedRambdaDt[i] = 0.f;
+		}
+		dstC->m_center = center;
+	}
+
+	for(int i=0; i<4; i++)
+	{
+		if( i<src->m_worldNormal.w )
+		{
+			dstC->m_worldPos[i] = src->m_worldPos[i];
+		}
+		else
+		{
+			dstC->m_worldPos[i] = make_float4(0.f);
+		}
+	}
+}
+
+
+__kernel
+__attribute__((reqd_work_group_size(WG_SIZE,1,1)))
+void ContactToConstraintSplitKernel(__global const Contact4* gContact, __global const Body* gBodies, __global const Shape* gShapes, __global Constraint4* gConstraintOut, 
+__global const unsigned int* bodyCount,
+int nContacts,
+float dt,
+float positionDrift,
+float positionConstraintCoeff
+)
+{
+	int gIdx = GET_GLOBAL_IDX;
+	
+	if( gIdx < nContacts )
+	{
+		int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);
+		int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);
+
+		float4 posA = gBodies[aIdx].m_pos;
+		float4 linVelA = gBodies[aIdx].m_linVel;
+		float4 angVelA = gBodies[aIdx].m_angVel;
+		float invMassA = gBodies[aIdx].m_invMass;
+		Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;
+
+		float4 posB = gBodies[bIdx].m_pos;
+		float4 linVelB = gBodies[bIdx].m_linVel;
+		float4 angVelB = gBodies[bIdx].m_angVel;
+		float invMassB = gBodies[bIdx].m_invMass;
+		Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;
+
+		Constraint4 cs;
+
+		float countA = invMassA ? (float)bodyCount[aIdx] : 1;
+		float countB = invMassB ? (float)bodyCount[bIdx] : 1;
+
+    	setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,
+			&gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB,
+			&cs  );
+		
+		cs.m_batchIdx = gContact[gIdx].m_batchIdx;
+
+		gConstraintOut[gIdx] = cs;
+	}
+}

src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h

@@ -0,0 +1,974 @@
+//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
+static const char* solverUtilsCL= \
+"/*\n"
+"Copyright (c) 2012 Advanced Micro Devices, Inc.  \n"
+"\n"
+"This software is provided 'as-is', without any express or implied warranty.\n"
+"In no event will the authors be held liable for any damages arising from the use of this software.\n"
+"Permission is granted to anyone to use this software for any purpose, \n"
+"including commercial applications, and to alter it and redistribute it freely, \n"
+"subject to the following restrictions:\n"
+"\n"
+"1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.\n"
+"2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.\n"
+"3. This notice may not be removed or altered from any source distribution.\n"
+"*/\n"
+"\n"
+"\n"
+"#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_local_int32_extended_atomics : enable\n"
+"#pragma OPENCL EXTENSION cl_khr_global_int32_extended_atomics : enable\n"
+"\n"
+"\n"
+"#ifdef cl_ext_atomic_counters_32\n"
+"#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable\n"
+"#else\n"
+"#define counter32_t volatile global int*\n"
+"#endif\n"
+"\n"
+"typedef unsigned int u32;\n"
+"typedef unsigned short u16;\n"
+"typedef unsigned char u8;\n"
+"\n"
+"#define GET_GROUP_IDX get_group_id(0)\n"
+"#define GET_LOCAL_IDX get_local_id(0)\n"
+"#define GET_GLOBAL_IDX get_global_id(0)\n"
+"#define GET_GROUP_SIZE get_local_size(0)\n"
+"#define GET_NUM_GROUPS get_num_groups(0)\n"
+"#define GROUP_LDS_BARRIER barrier(CLK_LOCAL_MEM_FENCE)\n"
+"#define GROUP_MEM_FENCE mem_fence(CLK_LOCAL_MEM_FENCE)\n"
+"#define AtomInc(x) atom_inc(&(x))\n"
+"#define AtomInc1(x, out) out = atom_inc(&(x))\n"
+"#define AppendInc(x, out) out = atomic_inc(x)\n"
+"#define AtomAdd(x, value) atom_add(&(x), value)\n"
+"#define AtomCmpxhg(x, cmp, value) atom_cmpxchg( &(x), cmp, value )\n"
+"#define AtomXhg(x, value) atom_xchg ( &(x), value )\n"
+"\n"
+"\n"
+"#define SELECT_UINT4( b, a, condition ) select( b,a,condition )\n"
+"\n"
+"#define make_float4 (float4)\n"
+"#define make_float2 (float2)\n"
+"#define make_uint4 (uint4)\n"
+"#define make_int4 (int4)\n"
+"#define make_uint2 (uint2)\n"
+"#define make_int2 (int2)\n"
+"\n"
+"\n"
+"#define max2 max\n"
+"#define min2 min\n"
+"\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Vector\n"
+"///////////////////////////////////////\n"
+"__inline\n"
+"float fastDiv(float numerator, float denominator)\n"
+"{\n"
+"	return native_divide(numerator, denominator);	\n"
+"//	return numerator/denominator;	\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 fastDiv4(float4 numerator, float4 denominator)\n"
+"{\n"
+"	return native_divide(numerator, denominator);	\n"
+"}\n"
+"\n"
+"__inline\n"
+"float fastSqrtf(float f2)\n"
+"{\n"
+"	return native_sqrt(f2);\n"
+"//	return sqrt(f2);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float fastRSqrt(float f2)\n"
+"{\n"
+"	return native_rsqrt(f2);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float fastLength4(float4 v)\n"
+"{\n"
+"	return fast_length(v);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 fastNormalize4(float4 v)\n"
+"{\n"
+"	return fast_normalize(v);\n"
+"}\n"
+"\n"
+"\n"
+"__inline\n"
+"float sqrtf(float a)\n"
+"{\n"
+"//	return sqrt(a);\n"
+"	return native_sqrt(a);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 cross3(float4 a1, float4 b1)\n"
+"{\n"
+"\n"
+"	float4 	a=make_float4(a1.xyz,0.f);\n"
+"	float4 	b=make_float4(b1.xyz,0.f);\n"
+"	//float4 	a=a1;\n"
+"	//float4 	b=b1;\n"
+"	return cross(a,b);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float dot3F4(float4 a, float4 b)\n"
+"{\n"
+"	float4 a1 = make_float4(a.xyz,0.f);\n"
+"	float4 b1 = make_float4(b.xyz,0.f);\n"
+"	return dot(a1, b1);\n"
+"}\n"
+"\n"
+"__inline\n"
+"float length3(const float4 a)\n"
+"{\n"
+"	return sqrtf(dot3F4(a,a));\n"
+"}\n"
+"\n"
+"__inline\n"
+"float dot4(const float4 a, const float4 b)\n"
+"{\n"
+"	return dot( a, b );\n"
+"}\n"
+"\n"
+"//	for height\n"
+"__inline\n"
+"float dot3w1(const float4 point, const float4 eqn)\n"
+"{\n"
+"	return dot3F4(point,eqn) + eqn.w;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 normalize3(const float4 a)\n"
+"{\n"
+"	float4 n = make_float4(a.x, a.y, a.z, 0.f);\n"
+"	return fastNormalize4( n );\n"
+"//	float length = sqrtf(dot3F4(a, a));\n"
+"//	return 1.f/length * a;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 normalize4(const float4 a)\n"
+"{\n"
+"	float length = sqrtf(dot4(a, a));\n"
+"	return 1.f/length * a;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 createEquation(const float4 a, const float4 b, const float4 c)\n"
+"{\n"
+"	float4 eqn;\n"
+"	float4 ab = b-a;\n"
+"	float4 ac = c-a;\n"
+"	eqn = normalize3( cross3(ab, ac) );\n"
+"	eqn.w = -dot3F4(eqn,a);\n"
+"	return eqn;\n"
+"}\n"
+"\n"
+"///////////////////////////////////////\n"
+"//	Matrix3x3\n"
+"///////////////////////////////////////\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_row[3];\n"
+"}Matrix3x3;\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtZero();\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtIdentity();\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtTranspose(Matrix3x3 m);\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b);\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b);\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b);\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtZero()\n"
+"{\n"
+"	Matrix3x3 m;\n"
+"	m.m_row[0] = (float4)(0.f);\n"
+"	m.m_row[1] = (float4)(0.f);\n"
+"	m.m_row[2] = (float4)(0.f);\n"
+"	return m;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtIdentity()\n"
+"{\n"
+"	Matrix3x3 m;\n"
+"	m.m_row[0] = (float4)(1,0,0,0);\n"
+"	m.m_row[1] = (float4)(0,1,0,0);\n"
+"	m.m_row[2] = (float4)(0,0,1,0);\n"
+"	return m;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtTranspose(Matrix3x3 m)\n"
+"{\n"
+"	Matrix3x3 out;\n"
+"	out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n"
+"	out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n"
+"	out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n"
+"	return out;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n"
+"{\n"
+"	Matrix3x3 transB;\n"
+"	transB = mtTranspose( b );\n"
+"	Matrix3x3 ans;\n"
+"	//	why this doesn't run when 0ing in the for{}\n"
+"	a.m_row[0].w = 0.f;\n"
+"	a.m_row[1].w = 0.f;\n"
+"	a.m_row[2].w = 0.f;\n"
+"	for(int i=0; i<3; i++)\n"
+"	{\n"
+"//	a.m_row[i].w = 0.f;\n"
+"		ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n"
+"		ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n"
+"		ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n"
+"		ans.m_row[i].w = 0.f;\n"
+"	}\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 mtMul1(Matrix3x3 a, float4 b)\n"
+"{\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a.m_row[0], b );\n"
+"	ans.y = dot3F4( a.m_row[1], b );\n"
+"	ans.z = dot3F4( a.m_row[2], b );\n"
+"	ans.w = 0.f;\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"float4 mtMul3(float4 a, Matrix3x3 b)\n"
+"{\n"
+"	float4 colx = make_float4(b.m_row[0].x, b.m_row[1].x, b.m_row[2].x, 0);\n"
+"	float4 coly = make_float4(b.m_row[0].y, b.m_row[1].y, b.m_row[2].y, 0);\n"
+"	float4 colz = make_float4(b.m_row[0].z, b.m_row[1].z, b.m_row[2].z, 0);\n"
+"\n"
+"	float4 ans;\n"
+"	ans.x = dot3F4( a, colx );\n"
+"	ans.y = dot3F4( a, coly );\n"
+"	ans.z = dot3F4( a, colz );\n"
+"	return ans;\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"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"Quaternion qtMul(Quaternion a, Quaternion b)\n"
+"{\n"
+"	Quaternion ans;\n"
+"	ans = cross3( 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 fastNormalize4(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"
+"\n"
+"\n"
+"\n"
+"#define WG_SIZE 64\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_pos;\n"
+"	Quaternion m_quat;\n"
+"	float4 m_linVel;\n"
+"	float4 m_angVel;\n"
+"\n"
+"	u32 m_shapeIdx;\n"
+"	float m_invMass;\n"
+"	float m_restituitionCoeff;\n"
+"	float m_frictionCoeff;\n"
+"} Body;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	Matrix3x3 m_invInertia;\n"
+"	Matrix3x3 m_initInvInertia;\n"
+"} Shape;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_linear;\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_center;	\n"
+"	float m_jacCoeffInv[4];\n"
+"	float m_b[4];\n"
+"	float m_appliedRambdaDt[4];\n"
+"\n"
+"	float m_fJacCoeffInv[2];	\n"
+"	float m_fAppliedRambdaDt[2];	\n"
+"\n"
+"	u32 m_bodyA;\n"
+"	u32 m_bodyB;\n"
+"	int m_batchIdx;\n"
+"	u32 m_paddings;\n"
+"} Constraint4;\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_worldPos[4];\n"
+"	float4 m_worldNormal;\n"
+"	u32 m_coeffs;\n"
+"	int m_batchIdx;\n"
+"\n"
+"	int m_bodyAPtrAndSignBit;\n"
+"	int m_bodyBPtrAndSignBit;\n"
+"} Contact4;\n"
+"\n"
+"\n"
+"__kernel void CountBodiesKernel(__global Contact4* manifoldPtr, __global unsigned int* bodyCount, __global int2* contactConstraintOffsets, int numContactManifolds, int fixedBodyIndex)\n"
+"{\n"
+"	int i = GET_GLOBAL_IDX;\n"
+"	\n"
+"	if( i < numContactManifolds)\n"
+"	{\n"
+"		int pa = manifoldPtr[i].m_bodyAPtrAndSignBit;\n"
+"		bool isFixedA = (pa <0) || (pa == fixedBodyIndex);\n"
+"		int bodyIndexA = abs(pa);\n"
+"		if (!isFixedA)\n"
+"		{\n"
+"			 AtomInc1(bodyCount[bodyIndexA],contactConstraintOffsets[i].x);\n"
+"		}\n"
+"		barrier(CLK_GLOBAL_MEM_FENCE);\n"
+"		int pb = manifoldPtr[i].m_bodyBPtrAndSignBit;\n"
+"		bool isFixedB = (pb <0) || (pb == fixedBodyIndex);\n"
+"		int bodyIndexB = abs(pb);\n"
+"		if (!isFixedB)\n"
+"		{\n"
+"			AtomInc1(bodyCount[bodyIndexB],contactConstraintOffsets[i].y);\n"
+"		} \n"
+"	}\n"
+"}\n"
+"\n"
+"__kernel void ClearVelocitiesKernel(__global float4* linearVelocities,__global float4* angularVelocities, int numSplitBodies)\n"
+"{\n"
+"	int i = GET_GLOBAL_IDX;\n"
+"	\n"
+"	if( i < numSplitBodies)\n"
+"	{\n"
+"		linearVelocities[i] = make_float4(0);\n"
+"		angularVelocities[i] = make_float4(0);\n"
+"	}\n"
+"}\n"
+"\n"
+"\n"
+"__kernel void AverageVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,\n"
+"__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)\n"
+"{\n"
+"	int i = GET_GLOBAL_IDX;\n"
+"	if (i<numBodies)\n"
+"	{\n"
+"		if (gBodies[i].m_invMass)\n"
+"		{\n"
+"			int bodyOffset = offsetSplitBodies[i];\n"
+"			int count = bodyCount[i];\n"
+"			float factor = 1.f/((float)count);\n"
+"			float4 averageLinVel = make_float4(0.f);\n"
+"			float4 averageAngVel = make_float4(0.f);\n"
+"			\n"
+"			for (int j=0;j<count;j++)\n"
+"			{\n"
+"				averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;\n"
+"				averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;\n"
+"			}\n"
+"			\n"
+"			for (int j=0;j<count;j++)\n"
+"			{\n"
+"				deltaLinearVelocities[bodyOffset+j] = averageLinVel;\n"
+"				deltaAngularVelocities[bodyOffset+j] = averageAngVel;\n"
+"			}\n"
+"			\n"
+"		}//bodies[i].m_invMass\n"
+"	}//i<numBodies\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
+"{\n"
+"	*linear = make_float4(-n.xyz,0.f);\n"
+"	*angular0 = -cross3(r0, n);\n"
+"	*angular1 = cross3(r1, n);\n"
+"}\n"
+"\n"
+"\n"
+"float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n"
+"{\n"
+"	return dot3F4(l0, linVel0) + dot3F4(a0, angVel0) + dot3F4(l1, linVel1) + dot3F4(a1, angVel1);\n"
+"}\n"
+"\n"
+"\n"
+"float calcJacCoeff(const float4 linear0, const float4 linear1, const float4 angular0, const float4 angular1,\n"
+"					float invMass0, const Matrix3x3* invInertia0, float invMass1, const Matrix3x3* invInertia1, float countA, float countB)\n"
+"{\n"
+"	//	linear0,1 are normlized\n"
+"	float jmj0 = invMass0;//dot3F4(linear0, linear0)*invMass0;\n"
+"	float jmj1 = dot3F4(mtMul3(angular0,*invInertia0), angular0);\n"
+"	float jmj2 = invMass1;//dot3F4(linear1, linear1)*invMass1;\n"
+"	float jmj3 = dot3F4(mtMul3(angular1,*invInertia1), angular1);\n"
+"	return -1.f/((jmj0+jmj1)*countA+(jmj2+jmj3)*countB);\n"
+"}\n"
+"\n"
+"\n"
+"void b3PlaneSpace1 (float4 n, float4* p, float4* q);\n"
+" void b3PlaneSpace1 (float4 n, float4* p, float4* q)\n"
+"{\n"
+"  if (fabs(n.z) > 0.70710678f) {\n"
+"    // choose p in y-z plane\n"
+"    float a = n.y*n.y + n.z*n.z;\n"
+"    float k = 1.f/sqrt(a);\n"
+"    p[0].x = 0;\n"
+"	p[0].y = -n.z*k;\n"
+"	p[0].z = n.y*k;\n"
+"    // set q = n x p\n"
+"    q[0].x = a*k;\n"
+"	q[0].y = -n.x*p[0].z;\n"
+"	q[0].z = n.x*p[0].y;\n"
+"  }\n"
+"  else {\n"
+"    // choose p in x-y plane\n"
+"    float a = n.x*n.x + n.y*n.y;\n"
+"    float k = 1.f/sqrt(a);\n"
+"    p[0].x = -n.y*k;\n"
+"	p[0].y = n.x*k;\n"
+"	p[0].z = 0;\n"
+"    // set q = n x p\n"
+"    q[0].x = -n.z*p[0].y;\n"
+"	q[0].y = n.z*p[0].x;\n"
+"	q[0].z = a*k;\n"
+"  }\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"void solveContact(__global Constraint4* cs,\n"
+"			float4 posA, float4* linVelA, float4* angVelA, float invMassA, Matrix3x3 invInertiaA,\n"
+"			float4 posB, float4* linVelB, float4* angVelB, float invMassB, Matrix3x3 invInertiaB,\n"
+"			float4* dLinVelA, float4* dAngVelA, float4* dLinVelB, float4* dAngVelB)\n"
+"{\n"
+"	float minRambdaDt = 0;\n"
+"	float maxRambdaDt = FLT_MAX;\n"
+"\n"
+"	for(int ic=0; ic<4; ic++)\n"
+"	{\n"
+"		if( cs->m_jacCoeffInv[ic] == 0.f ) continue;\n"
+"\n"
+"		float4 angular0, angular1, linear;\n"
+"		float4 r0 = cs->m_worldPos[ic] - posA;\n"
+"		float4 r1 = cs->m_worldPos[ic] - posB;\n"
+"		setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n"
+"	\n"
+"\n"
+"\n"
+"		float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n"
+"			*linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];\n"
+"		rambdaDt *= cs->m_jacCoeffInv[ic];\n"
+"\n"
+"		\n"
+"		{\n"
+"			float prevSum = cs->m_appliedRambdaDt[ic];\n"
+"			float updated = prevSum;\n"
+"			updated += rambdaDt;\n"
+"			updated = max2( updated, minRambdaDt );\n"
+"			updated = min2( updated, maxRambdaDt );\n"
+"			rambdaDt = updated - prevSum;\n"
+"			cs->m_appliedRambdaDt[ic] = updated;\n"
+"		}\n"
+"\n"
+"			\n"
+"		float4 linImp0 = invMassA*linear*rambdaDt;\n"
+"		float4 linImp1 = invMassB*(-linear)*rambdaDt;\n"
+"		float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n"
+"		float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n"
+"\n"
+"		\n"
+"		if (invMassA)\n"
+"		{\n"
+"			*dLinVelA += linImp0;\n"
+"			*dAngVelA += angImp0;\n"
+"		}\n"
+"		if (invMassB)\n"
+"		{\n"
+"			*dLinVelB += linImp1;\n"
+"			*dAngVelB += angImp1;\n"
+"		}\n"
+"	}\n"
+"}\n"
+"\n"
+"\n"
+"//	solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,contactConstraintOffsets,offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n"
+"\n"
+"\n"
+"void solveContactConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs, \n"
+"__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n"
+"__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)\n"
+"{\n"
+"\n"
+"	//float frictionCoeff = ldsCs[0].m_linear.w;\n"
+"	int aIdx = ldsCs[0].m_bodyA;\n"
+"	int bIdx = ldsCs[0].m_bodyB;\n"
+"\n"
+"	float4 posA = gBodies[aIdx].m_pos;\n"
+"	float4 linVelA = gBodies[aIdx].m_linVel;\n"
+"	float4 angVelA = gBodies[aIdx].m_angVel;\n"
+"	float invMassA = gBodies[aIdx].m_invMass;\n"
+"	Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n"
+"\n"
+"	float4 posB = gBodies[bIdx].m_pos;\n"
+"	float4 linVelB = gBodies[bIdx].m_linVel;\n"
+"	float4 angVelB = gBodies[bIdx].m_angVel;\n"
+"	float invMassB = gBodies[bIdx].m_invMass;\n"
+"	Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n"
+"\n"
+"			\n"
+"	float4 dLinVelA = make_float4(0,0,0,0);\n"
+"	float4 dAngVelA = make_float4(0,0,0,0);\n"
+"	float4 dLinVelB = make_float4(0,0,0,0);\n"
+"	float4 dAngVelB = make_float4(0,0,0,0);\n"
+"			\n"
+"	int bodyOffsetA = offsetSplitBodies[aIdx];\n"
+"	int constraintOffsetA = contactConstraintOffsets[0].x;\n"
+"	int splitIndexA = bodyOffsetA+constraintOffsetA;\n"
+"	\n"
+"	if (invMassA)\n"
+"	{\n"
+"		dLinVelA = deltaLinearVelocities[splitIndexA];\n"
+"		dAngVelA = deltaAngularVelocities[splitIndexA];\n"
+"	}\n"
+"\n"
+"	int bodyOffsetB = offsetSplitBodies[bIdx];\n"
+"	int constraintOffsetB = contactConstraintOffsets[0].y;\n"
+"	int splitIndexB= bodyOffsetB+constraintOffsetB;\n"
+"\n"
+"	if (invMassB)\n"
+"	{\n"
+"		dLinVelB = deltaLinearVelocities[splitIndexB];\n"
+"		dAngVelB = deltaAngularVelocities[splitIndexB];\n"
+"	}\n"
+"\n"
+"	solveContact( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n"
+"			posB, &linVelB, &angVelB, invMassB, invInertiaB ,&dLinVelA, &dAngVelA, &dLinVelB, &dAngVelB);\n"
+"\n"
+"	if (invMassA)\n"
+"	{\n"
+"		deltaLinearVelocities[splitIndexA] = dLinVelA;\n"
+"		deltaAngularVelocities[splitIndexA] = dAngVelA;\n"
+"	} \n"
+"	if (invMassB)\n"
+"	{\n"
+"		deltaLinearVelocities[splitIndexB] = dLinVelB;\n"
+"		deltaAngularVelocities[splitIndexB] = dAngVelB;\n"
+"	}\n"
+"\n"
+"}\n"
+"\n"
+"\n"
+"__kernel void SolveContactJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n"
+"__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,\n"
+"float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds\n"
+")\n"
+"{\n"
+"	int i = GET_GLOBAL_IDX;\n"
+"	if (i<numManifolds)\n"
+"	{\n"
+"		solveContactConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n"
+"	}\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"void solveFrictionConstraint(__global Body* gBodies, __global Shape* gShapes, __global Constraint4* ldsCs,\n"
+"							__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n"
+"							__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities)\n"
+"{\n"
+"	float frictionCoeff = 0.7f;//ldsCs[0].m_linear.w;\n"
+"	int aIdx = ldsCs[0].m_bodyA;\n"
+"	int bIdx = ldsCs[0].m_bodyB;\n"
+"\n"
+"\n"
+"	float4 posA = gBodies[aIdx].m_pos;\n"
+"	float4 linVelA = gBodies[aIdx].m_linVel;\n"
+"	float4 angVelA = gBodies[aIdx].m_angVel;\n"
+"	float invMassA = gBodies[aIdx].m_invMass;\n"
+"	Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n"
+"\n"
+"	float4 posB = gBodies[bIdx].m_pos;\n"
+"	float4 linVelB = gBodies[bIdx].m_linVel;\n"
+"	float4 angVelB = gBodies[bIdx].m_angVel;\n"
+"	float invMassB = gBodies[bIdx].m_invMass;\n"
+"	Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n"
+"	\n"
+"\n"
+"	float4 dLinVelA = make_float4(0,0,0,0);\n"
+"	float4 dAngVelA = make_float4(0,0,0,0);\n"
+"	float4 dLinVelB = make_float4(0,0,0,0);\n"
+"	float4 dAngVelB = make_float4(0,0,0,0);\n"
+"			\n"
+"	int bodyOffsetA = offsetSplitBodies[aIdx];\n"
+"	int constraintOffsetA = contactConstraintOffsets[0].x;\n"
+"	int splitIndexA = bodyOffsetA+constraintOffsetA;\n"
+"	\n"
+"	if (invMassA)\n"
+"	{\n"
+"		dLinVelA = deltaLinearVelocities[splitIndexA];\n"
+"		dAngVelA = deltaAngularVelocities[splitIndexA];\n"
+"	}\n"
+"\n"
+"	int bodyOffsetB = offsetSplitBodies[bIdx];\n"
+"	int constraintOffsetB = contactConstraintOffsets[0].y;\n"
+"	int splitIndexB= bodyOffsetB+constraintOffsetB;\n"
+"\n"
+"	if (invMassB)\n"
+"	{\n"
+"		dLinVelB = deltaLinearVelocities[splitIndexB];\n"
+"		dAngVelB = deltaAngularVelocities[splitIndexB];\n"
+"	}\n"
+"\n"
+"\n"
+"\n"
+"\n"
+"	{\n"
+"		float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};\n"
+"		float minRambdaDt[4] = {0.f,0.f,0.f,0.f};\n"
+"\n"
+"		float sum = 0;\n"
+"		for(int j=0; j<4; j++)\n"
+"		{\n"
+"			sum +=ldsCs[0].m_appliedRambdaDt[j];\n"
+"		}\n"
+"		frictionCoeff = 0.7f;\n"
+"		for(int j=0; j<4; j++)\n"
+"		{\n"
+"			maxRambdaDt[j] = frictionCoeff*sum;\n"
+"			minRambdaDt[j] = -maxRambdaDt[j];\n"
+"		}\n"
+"\n"
+"		\n"
+"//		solveFriction( ldsCs, posA, &linVelA, &angVelA, invMassA, invInertiaA,\n"
+"//			posB, &linVelB, &angVelB, invMassB, invInertiaB, maxRambdaDt, minRambdaDt );\n"
+"		\n"
+"		\n"
+"		{\n"
+"			\n"
+"			__global Constraint4* cs = ldsCs;\n"
+"			\n"
+"			if( cs->m_fJacCoeffInv[0] == 0 && cs->m_fJacCoeffInv[0] == 0 ) return;\n"
+"			const float4 center = cs->m_center;\n"
+"			\n"
+"			float4 n = -cs->m_linear;\n"
+"			\n"
+"			float4 tangent[2];\n"
+"			b3PlaneSpace1(n,&tangent[0],&tangent[1]);\n"
+"			float4 angular0, angular1, linear;\n"
+"			float4 r0 = center - posA;\n"
+"			float4 r1 = center - posB;\n"
+"			for(int i=0; i<2; i++)\n"
+"			{\n"
+"				setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 );\n"
+"				float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,\n"
+"											linVelA+dLinVelA, angVelA+dAngVelA, linVelB+dLinVelB, angVelB+dAngVelB );\n"
+"				rambdaDt *= cs->m_fJacCoeffInv[i];\n"
+"				\n"
+"				{\n"
+"					float prevSum = cs->m_fAppliedRambdaDt[i];\n"
+"					float updated = prevSum;\n"
+"					updated += rambdaDt;\n"
+"					updated = max2( updated, minRambdaDt[i] );\n"
+"					updated = min2( updated, maxRambdaDt[i] );\n"
+"					rambdaDt = updated - prevSum;\n"
+"					cs->m_fAppliedRambdaDt[i] = updated;\n"
+"				}\n"
+"				\n"
+"				float4 linImp0 = invMassA*linear*rambdaDt;\n"
+"				float4 linImp1 = invMassB*(-linear)*rambdaDt;\n"
+"				float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n"
+"				float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n"
+"				\n"
+"				dLinVelA += linImp0;\n"
+"				dAngVelA += angImp0;\n"
+"				dLinVelB += linImp1;\n"
+"				dAngVelB += angImp1;\n"
+"			}\n"
+"			{	//	angular damping for point constraint\n"
+"				float4 ab = normalize3( posB - posA );\n"
+"				float4 ac = normalize3( center - posA );\n"
+"				if( dot3F4( ab, ac ) > 0.95f  || (invMassA == 0.f || invMassB == 0.f))\n"
+"				{\n"
+"					float angNA = dot3F4( n, angVelA );\n"
+"					float angNB = dot3F4( n, angVelB );\n"
+"					\n"
+"					dAngVelA -= (angNA*0.1f)*n;\n"
+"					dAngVelB -= (angNB*0.1f)*n;\n"
+"				}\n"
+"			}\n"
+"		}\n"
+"\n"
+"		\n"
+"		\n"
+"	}\n"
+"\n"
+"	if (invMassA)\n"
+"	{\n"
+"		deltaLinearVelocities[splitIndexA] = dLinVelA;\n"
+"		deltaAngularVelocities[splitIndexA] = dAngVelA;\n"
+"	} \n"
+"	if (invMassB)\n"
+"	{\n"
+"		deltaLinearVelocities[splitIndexB] = dLinVelB;\n"
+"		deltaAngularVelocities[splitIndexB] = dAngVelB;\n"
+"	}\n"
+" \n"
+"\n"
+"}\n"
+"\n"
+"\n"
+"__kernel void SolveFrictionJacobiKernel(__global Constraint4* gConstraints, __global Body* gBodies, __global Shape* gShapes ,\n"
+"										__global int2* contactConstraintOffsets,__global unsigned int* offsetSplitBodies,\n"
+"										__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,\n"
+"										float deltaTime, float positionDrift, float positionConstraintCoeff, int fixedBodyIndex, int numManifolds\n"
+")\n"
+"{\n"
+"	int i = GET_GLOBAL_IDX;\n"
+"	if (i<numManifolds)\n"
+"	{\n"
+"		solveFrictionConstraint( gBodies, gShapes, &gConstraints[i] ,&contactConstraintOffsets[i],offsetSplitBodies, deltaLinearVelocities, deltaAngularVelocities);\n"
+"	}\n"
+"}\n"
+"\n"
+"\n"
+"__kernel void UpdateBodyVelocitiesKernel(__global Body* gBodies,__global int* offsetSplitBodies,__global const unsigned int* bodyCount,\n"
+"									__global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities, int numBodies)\n"
+"{\n"
+"	int i = GET_GLOBAL_IDX;\n"
+"	if (i<numBodies)\n"
+"	{\n"
+"		if (gBodies[i].m_invMass)\n"
+"		{\n"
+"			int bodyOffset = offsetSplitBodies[i];\n"
+"			int count = bodyCount[i];\n"
+"			if (count)\n"
+"			{\n"
+"				gBodies[i].m_linVel += deltaLinearVelocities[bodyOffset];\n"
+"				gBodies[i].m_angVel += deltaAngularVelocities[bodyOffset];\n"
+"			}\n"
+"		}\n"
+"	}\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVelA, float invMassA, const Matrix3x3 invInertiaA,\n"
+"	const float4 posB, const float4 linVelB, const float4 angVelB, float invMassB, const Matrix3x3 invInertiaB, \n"
+"	__global Contact4* src, float dt, float positionDrift, float positionConstraintCoeff,float countA, float countB,\n"
+"	Constraint4* dstC )\n"
+"{\n"
+"	dstC->m_bodyA = abs(src->m_bodyAPtrAndSignBit);\n"
+"	dstC->m_bodyB = abs(src->m_bodyBPtrAndSignBit);\n"
+"\n"
+"	float dtInv = 1.f/dt;\n"
+"	for(int ic=0; ic<4; ic++)\n"
+"	{\n"
+"		dstC->m_appliedRambdaDt[ic] = 0.f;\n"
+"	}\n"
+"	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n"
+"\n"
+"\n"
+"	dstC->m_linear = -src->m_worldNormal;\n"
+"	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n"
+"	for(int ic=0; ic<4; ic++)\n"
+"	{\n"
+"		float4 r0 = src->m_worldPos[ic] - posA;\n"
+"		float4 r1 = src->m_worldPos[ic] - posB;\n"
+"\n"
+"		if( ic >= src->m_worldNormal.w )//npoints\n"
+"		{\n"
+"			dstC->m_jacCoeffInv[ic] = 0.f;\n"
+"			continue;\n"
+"		}\n"
+"\n"
+"		float relVelN;\n"
+"		{\n"
+"			float4 linear, angular0, angular1;\n"
+"			setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1);\n"
+"\n"
+"			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n"
+"				invMassA, &invInertiaA, invMassB, &invInertiaB , countA, countB);\n"
+"\n"
+"			relVelN = calcRelVel(linear, -linear, angular0, angular1,\n"
+"				linVelA, angVelA, linVelB, angVelB);\n"
+"\n"
+"			float e = 0.f;//src->getRestituitionCoeff();\n"
+"			if( relVelN*relVelN < 0.004f ) e = 0.f;\n"
+"\n"
+"			dstC->m_b[ic] = e*relVelN;\n"
+"			//float penetration = src->m_worldPos[ic].w;\n"
+"			dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n"
+"			dstC->m_appliedRambdaDt[ic] = 0.f;\n"
+"		}\n"
+"	}\n"
+"\n"
+"	if( src->m_worldNormal.w > 0 )//npoints\n"
+"	{	//	prepare friction\n"
+"		float4 center = make_float4(0.f);\n"
+"		for(int i=0; i<src->m_worldNormal.w; i++) \n"
+"			center += src->m_worldPos[i];\n"
+"		center /= (float)src->m_worldNormal.w;\n"
+"\n"
+"		float4 tangent[2];\n"
+"		b3PlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]);\n"
+"		\n"
+"		float4 r[2];\n"
+"		r[0] = center - posA;\n"
+"		r[1] = center - posB;\n"
+"\n"
+"		for(int i=0; i<2; i++)\n"
+"		{\n"
+"			float4 linear, angular0, angular1;\n"
+"			setLinearAndAngular(tangent[i], r[0], r[1], &linear, &angular0, &angular1);\n"
+"\n"
+"			dstC->m_fJacCoeffInv[i] = calcJacCoeff(linear, -linear, angular0, angular1,\n"
+"				invMassA, &invInertiaA, invMassB, &invInertiaB ,countA, countB);\n"
+"			dstC->m_fAppliedRambdaDt[i] = 0.f;\n"
+"		}\n"
+"		dstC->m_center = center;\n"
+"	}\n"
+"\n"
+"	for(int i=0; i<4; i++)\n"
+"	{\n"
+"		if( i<src->m_worldNormal.w )\n"
+"		{\n"
+"			dstC->m_worldPos[i] = src->m_worldPos[i];\n"
+"		}\n"
+"		else\n"
+"		{\n"
+"			dstC->m_worldPos[i] = make_float4(0.f);\n"
+"		}\n"
+"	}\n"
+"}\n"
+"\n"
+"\n"
+"__kernel\n"
+"__attribute__((reqd_work_group_size(WG_SIZE,1,1)))\n"
+"void ContactToConstraintSplitKernel(__global const Contact4* gContact, __global const Body* gBodies, __global const Shape* gShapes, __global Constraint4* gConstraintOut, \n"
+"__global const unsigned int* bodyCount,\n"
+"int nContacts,\n"
+"float dt,\n"
+"float positionDrift,\n"
+"float positionConstraintCoeff\n"
+")\n"
+"{\n"
+"	int gIdx = GET_GLOBAL_IDX;\n"
+"	\n"
+"	if( gIdx < nContacts )\n"
+"	{\n"
+"		int aIdx = abs(gContact[gIdx].m_bodyAPtrAndSignBit);\n"
+"		int bIdx = abs(gContact[gIdx].m_bodyBPtrAndSignBit);\n"
+"\n"
+"		float4 posA = gBodies[aIdx].m_pos;\n"
+"		float4 linVelA = gBodies[aIdx].m_linVel;\n"
+"		float4 angVelA = gBodies[aIdx].m_angVel;\n"
+"		float invMassA = gBodies[aIdx].m_invMass;\n"
+"		Matrix3x3 invInertiaA = gShapes[aIdx].m_invInertia;\n"
+"\n"
+"		float4 posB = gBodies[bIdx].m_pos;\n"
+"		float4 linVelB = gBodies[bIdx].m_linVel;\n"
+"		float4 angVelB = gBodies[bIdx].m_angVel;\n"
+"		float invMassB = gBodies[bIdx].m_invMass;\n"
+"		Matrix3x3 invInertiaB = gShapes[bIdx].m_invInertia;\n"
+"\n"
+"		Constraint4 cs;\n"
+"\n"
+"		float countA = invMassA ? (float)bodyCount[aIdx] : 1;\n"
+"		float countB = invMassB ? (float)bodyCount[bIdx] : 1;\n"
+"\n"
+"    	setConstraint4( posA, linVelA, angVelA, invMassA, invInertiaA, posB, linVelB, angVelB, invMassB, invInertiaB,\n"
+"			&gContact[gIdx], dt, positionDrift, positionConstraintCoeff,countA,countB,\n"
+"			&cs  );\n"
+"		\n"
+"		cs.m_batchIdx = gContact[gIdx].m_batchIdx;\n"
+"\n"
+"		gConstraintOut[gIdx] = cs;\n"
+"	}\n"
+"}\n"
+;

src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.cl

@@ -0,0 +1,195 @@
+
+#define SHAPE_CONVEX_HULL 3
+
+typedef float4 Quaternion;
+
+__inline
+float4 cross3(float4 a, float4 b)
+{
+	return cross(a,b);
+}
+
+__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 = cross3( a, b );
+	ans += a.w*b+b.w*a;
+	ans.w = a.w*b.w - dot3F4(a, b);
+	return ans;
+}
+
+__inline
+Quaternion qtInvert(Quaternion q)
+{
+	return (Quaternion)(-q.xyz, q.w);
+}
+
+__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
+float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)
+{
+	return qtRotate( *orientation, *p ) + (*translation);
+}
+
+typedef struct
+{
+	float4	m_row[3];
+} Matrix3x3;
+
+typedef unsigned int u32;
+
+
+typedef struct
+{
+	float4 m_pos;
+	float4 m_quat;
+	float4 m_linVel;
+	float4 m_angVel;
+
+	u32 m_collidableIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
+} Body;
+
+typedef struct Collidable
+{
+	int m_unused1;
+	int m_unused2;
+	int m_shapeType;
+	int m_shapeIndex;
+} Collidable;
+
+
+typedef struct
+{
+	Matrix3x3 m_invInertia;
+	Matrix3x3 m_initInvInertia;
+} Shape;
+
+
+__inline
+Matrix3x3 qtGetRotationMatrix(float4 quat)
+{
+	float4 quat2 = (float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);
+	Matrix3x3 out;
+
+	out.m_row[0].x=fabs(1-2*quat2.y-2*quat2.z);
+	out.m_row[0].y=fabs(2*quat.x*quat.y-2*quat.w*quat.z);
+	out.m_row[0].z=fabs(2*quat.x*quat.z+2*quat.w*quat.y);
+	out.m_row[0].w = 0.f;
+
+	out.m_row[1].x=fabs(2*quat.x*quat.y+2*quat.w*quat.z);
+	out.m_row[1].y=fabs(1-2*quat2.x-2*quat2.z);
+	out.m_row[1].z=fabs(2*quat.y*quat.z-2*quat.w*quat.x);
+	out.m_row[1].w = 0.f;
+
+	out.m_row[2].x=fabs(2*quat.x*quat.z-2*quat.w*quat.y);
+	out.m_row[2].y=fabs(2*quat.y*quat.z+2*quat.w*quat.x);
+	out.m_row[2].z=fabs(1-2*quat2.x-2*quat2.y);
+	out.m_row[2].w = 0.f;
+
+	return out;
+}
+
+
+typedef struct 
+{
+	float			fx;
+	float			fy;
+	float			fz;
+	int	uw;
+} btAABBCL;
+
+__inline
+Matrix3x3 mtTranspose(Matrix3x3 m)
+{
+	Matrix3x3 out;
+	out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);
+	out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);
+	out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);
+	return out;
+}
+
+
+
+__inline
+Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)
+{
+	Matrix3x3 transB;
+	transB = mtTranspose( b );
+	Matrix3x3 ans;
+	//	why this doesn't run when 0ing in the for{}
+	a.m_row[0].w = 0.f;
+	a.m_row[1].w = 0.f;
+	a.m_row[2].w = 0.f;
+	for(int i=0; i<3; i++)
+	{
+//	a.m_row[i].w = 0.f;
+		ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);
+		ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);
+		ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);
+		ans.m_row[i].w = 0.f;
+	}
+	return ans;
+}
+
+
+__kernel void initializeGpuAabbsFull(  const int numNodes, __global Body* gBodies,__global Collidable* collidables, __global btAABBCL* plocalShapeAABB, __global btAABBCL* pAABB)
+{
+	int nodeID = get_global_id(0);
+		
+	if( nodeID < numNodes )
+	{
+		float4 position = gBodies[nodeID].m_pos;
+		float4 orientation = gBodies[nodeID].m_quat;
+		
+			
+		int collidableIndex = gBodies[nodeID].m_collidableIdx;
+		int shapeIndex = collidables[collidableIndex].m_shapeIndex;
+			
+		if (shapeIndex>=0)
+		{
+			btAABBCL minAabb = plocalShapeAABB[collidableIndex*2];
+			btAABBCL maxAabb = plocalShapeAABB[collidableIndex*2+1];
+				
+			float4 halfExtents = ((float4)(maxAabb.fx - minAabb.fx,maxAabb.fy - minAabb.fy,maxAabb.fz - minAabb.fz,0.f))*0.5f;
+			float4 localCenter = ((float4)(maxAabb.fx + minAabb.fx,maxAabb.fy + minAabb.fy,maxAabb.fz + minAabb.fz,0.f))*0.5f;
+				
+			float4 worldCenter = transform(&localCenter,&position,&orientation);
+				
+			Matrix3x3 abs_b = qtGetRotationMatrix(orientation);
+			float4 extent = (float4) (	dot(abs_b.m_row[0],halfExtents),dot(abs_b.m_row[1],halfExtents),dot(abs_b.m_row[2],halfExtents),0.f);
+			
+	
+			pAABB[nodeID*2].fx = worldCenter.x-extent.x;
+			pAABB[nodeID*2].fy = worldCenter.y-extent.y;
+			pAABB[nodeID*2].fz = worldCenter.z-extent.z;
+			pAABB[nodeID*2].uw = nodeID;
+	
+			pAABB[nodeID*2+1].fx = worldCenter.x+extent.x;
+			pAABB[nodeID*2+1].fy = worldCenter.y+extent.y;
+			pAABB[nodeID*2+1].fz = worldCenter.z+extent.z;
+			pAABB[nodeID*2+1].uw = gBodies[nodeID].m_invMass==0.f? 0 : 1;
+		}
+	} 
+}

src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h

@@ -0,0 +1,199 @@
+//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project
+static const char* updateAabbsKernelCL= \
+"\n"
+"#define SHAPE_CONVEX_HULL 3\n"
+"\n"
+"typedef float4 Quaternion;\n"
+"\n"
+"__inline\n"
+"float4 cross3(float4 a, float4 b)\n"
+"{\n"
+"	return cross(a,b);\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 = cross3( a, b );\n"
+"	ans += a.w*b+b.w*a;\n"
+"	ans.w = a.w*b.w - dot3F4(a, b);\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"__inline\n"
+"Quaternion qtInvert(Quaternion q)\n"
+"{\n"
+"	return (Quaternion)(-q.xyz, q.w);\n"
+"}\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"
+"float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)\n"
+"{\n"
+"	return qtRotate( *orientation, *p ) + (*translation);\n"
+"}\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4	m_row[3];\n"
+"} Matrix3x3;\n"
+"\n"
+"typedef unsigned int u32;\n"
+"\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	float4 m_pos;\n"
+"	float4 m_quat;\n"
+"	float4 m_linVel;\n"
+"	float4 m_angVel;\n"
+"\n"
+"	u32 m_collidableIdx;\n"
+"	float m_invMass;\n"
+"	float m_restituitionCoeff;\n"
+"	float m_frictionCoeff;\n"
+"} Body;\n"
+"\n"
+"typedef struct Collidable\n"
+"{\n"
+"	int m_unused1;\n"
+"	int m_unused2;\n"
+"	int m_shapeType;\n"
+"	int m_shapeIndex;\n"
+"} Collidable;\n"
+"\n"
+"\n"
+"typedef struct\n"
+"{\n"
+"	Matrix3x3 m_invInertia;\n"
+"	Matrix3x3 m_initInvInertia;\n"
+"} Shape;\n"
+"\n"
+"\n"
+"__inline\n"
+"Matrix3x3 qtGetRotationMatrix(float4 quat)\n"
+"{\n"
+"	float4 quat2 = (float4)(quat.x*quat.x, quat.y*quat.y, quat.z*quat.z, 0.f);\n"
+"	Matrix3x3 out;\n"
+"\n"
+"	out.m_row[0].x=fabs(1-2*quat2.y-2*quat2.z);\n"
+"	out.m_row[0].y=fabs(2*quat.x*quat.y-2*quat.w*quat.z);\n"
+"	out.m_row[0].z=fabs(2*quat.x*quat.z+2*quat.w*quat.y);\n"
+"	out.m_row[0].w = 0.f;\n"
+"\n"
+"	out.m_row[1].x=fabs(2*quat.x*quat.y+2*quat.w*quat.z);\n"
+"	out.m_row[1].y=fabs(1-2*quat2.x-2*quat2.z);\n"
+"	out.m_row[1].z=fabs(2*quat.y*quat.z-2*quat.w*quat.x);\n"
+"	out.m_row[1].w = 0.f;\n"
+"\n"
+"	out.m_row[2].x=fabs(2*quat.x*quat.z-2*quat.w*quat.y);\n"
+"	out.m_row[2].y=fabs(2*quat.y*quat.z+2*quat.w*quat.x);\n"
+"	out.m_row[2].z=fabs(1-2*quat2.x-2*quat2.y);\n"
+"	out.m_row[2].w = 0.f;\n"
+"\n"
+"	return out;\n"
+"}\n"
+"\n"
+"\n"
+"typedef struct \n"
+"{\n"
+"	float			fx;\n"
+"	float			fy;\n"
+"	float			fz;\n"
+"	int	uw;\n"
+"} b3AABBCL;\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtTranspose(Matrix3x3 m)\n"
+"{\n"
+"	Matrix3x3 out;\n"
+"	out.m_row[0] = (float4)(m.m_row[0].x, m.m_row[1].x, m.m_row[2].x, 0.f);\n"
+"	out.m_row[1] = (float4)(m.m_row[0].y, m.m_row[1].y, m.m_row[2].y, 0.f);\n"
+"	out.m_row[2] = (float4)(m.m_row[0].z, m.m_row[1].z, m.m_row[2].z, 0.f);\n"
+"	return out;\n"
+"}\n"
+"\n"
+"\n"
+"\n"
+"__inline\n"
+"Matrix3x3 mtMul(Matrix3x3 a, Matrix3x3 b)\n"
+"{\n"
+"	Matrix3x3 transB;\n"
+"	transB = mtTranspose( b );\n"
+"	Matrix3x3 ans;\n"
+"	//	why this doesn't run when 0ing in the for{}\n"
+"	a.m_row[0].w = 0.f;\n"
+"	a.m_row[1].w = 0.f;\n"
+"	a.m_row[2].w = 0.f;\n"
+"	for(int i=0; i<3; i++)\n"
+"	{\n"
+"//	a.m_row[i].w = 0.f;\n"
+"		ans.m_row[i].x = dot3F4(a.m_row[i],transB.m_row[0]);\n"
+"		ans.m_row[i].y = dot3F4(a.m_row[i],transB.m_row[1]);\n"
+"		ans.m_row[i].z = dot3F4(a.m_row[i],transB.m_row[2]);\n"
+"		ans.m_row[i].w = 0.f;\n"
+"	}\n"
+"	return ans;\n"
+"}\n"
+"\n"
+"\n"
+"__kernel void initializeGpuAabbsFull(  const int numNodes, __global Body* gBodies,__global Collidable* collidables, __global b3AABBCL* plocalShapeAABB, __global b3AABBCL* pAABB)\n"
+"{\n"
+"	int nodeID = get_global_id(0);\n"
+"		\n"
+"	if( nodeID < numNodes )\n"
+"	{\n"
+"		float4 position = gBodies[nodeID].m_pos;\n"
+"		float4 orientation = gBodies[nodeID].m_quat;\n"
+"		\n"
+"			\n"
+"		int collidableIndex = gBodies[nodeID].m_collidableIdx;\n"
+"		int shapeIndex = collidables[collidableIndex].m_shapeIndex;\n"
+"			\n"
+"		if (shapeIndex>=0)\n"
+"		{\n"
+"			b3AABBCL minAabb = plocalShapeAABB[collidableIndex*2];\n"
+"			b3AABBCL maxAabb = plocalShapeAABB[collidableIndex*2+1];\n"
+"				\n"
+"			float4 halfExtents = ((float4)(maxAabb.fx - minAabb.fx,maxAabb.fy - minAabb.fy,maxAabb.fz - minAabb.fz,0.f))*0.5f;\n"
+"			float4 localCenter = ((float4)(maxAabb.fx + minAabb.fx,maxAabb.fy + minAabb.fy,maxAabb.fz + minAabb.fz,0.f))*0.5f;\n"
+"				\n"
+"			float4 worldCenter = transform(&localCenter,&position,&orientation);\n"
+"				\n"
+"			Matrix3x3 abs_b = qtGetRotationMatrix(orientation);\n"
+"			float4 extent = (float4) (	dot(abs_b.m_row[0],halfExtents),dot(abs_b.m_row[1],halfExtents),dot(abs_b.m_row[2],halfExtents),0.f);\n"
+"			\n"
+"	\n"
+"			pAABB[nodeID*2].fx = worldCenter.x-extent.x;\n"
+"			pAABB[nodeID*2].fy = worldCenter.y-extent.y;\n"
+"			pAABB[nodeID*2].fz = worldCenter.z-extent.z;\n"
+"			pAABB[nodeID*2].uw = nodeID;\n"
+"	\n"
+"			pAABB[nodeID*2+1].fx = worldCenter.x+extent.x;\n"
+"			pAABB[nodeID*2+1].fy = worldCenter.y+extent.y;\n"
+"			pAABB[nodeID*2+1].fz = worldCenter.z+extent.z;\n"
+"			pAABB[nodeID*2+1].uw = gBodies[nodeID].m_invMass==0.f? 0 : 1;\n"
+"		}\n"
+"	} \n"
+"}\n"
+"\n"
+;