diff --git a/Demos3/GpuDemos/GpuDemo.h b/Demos3/GpuDemos/GpuDemo.h
index 0cfe70ef0..686eaba55 100644
--- a/Demos3/GpuDemos/GpuDemo.h
+++ b/Demos3/GpuDemos/GpuDemo.h
@@ -48,9 +48,9 @@ public:
 	arraySizeZ(10),
 	#else
 
-		arraySizeX(30),
-		arraySizeY(30),
-		arraySizeZ(30),
+		arraySizeX(1),
+		arraySizeY(10),
+		arraySizeZ(1),
 #endif
 		m_useConcaveMesh(false),
 		gapX(16.3),
diff --git a/Demos3/GpuDemos/main_opengl3core.cpp b/Demos3/GpuDemos/main_opengl3core.cpp
index 5b4c3b2a7..303a77c84 100644
--- a/Demos3/GpuDemos/main_opengl3core.cpp
+++ b/Demos3/GpuDemos/main_opengl3core.cpp
@@ -101,7 +101,7 @@ enum
 };
 
 b3AlignedObjectArray<const char*> demoNames;
-int selectedDemo = 1;
+int selectedDemo = 0;
 GpuDemo::CreateFunc* allDemos[]=
 {
 		//ConcaveCompound2Scene::MyCreateFunc,
@@ -247,9 +247,21 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
 }
 
 extern bool useShadowMap;
-
+static bool wireframe=false;
 void MyKeyboardCallback(int key, int state)
 {
+	if (key=='w' && state)
+	{
+		wireframe=!wireframe;
+		if (wireframe)
+		{
+			glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+		} else
+		{
+			glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+		}
+	}
+
 	if (key=='s' && state)
 	{
 		useShadowMap=!useShadowMap;
diff --git a/Demos3/bullet2/BasicDemo/main.cpp b/Demos3/bullet2/BasicDemo/main.cpp
index 8d90b8416..3c9f77f45 100644
--- a/Demos3/bullet2/BasicDemo/main.cpp
+++ b/Demos3/bullet2/BasicDemo/main.cpp
@@ -64,11 +64,23 @@ public:
 
 class BasicDemo : public Bullet2RigidBodyDemo
 {
-	SimpleOpenGL3App* m_glApp;
+
+	btRigidBody*	m_pickedBody;
+	btTypedConstraint* m_pickedConstraint;
+	btVector3 m_oldPickingPos;
+	btVector3 m_hitPos;
+	btScalar m_oldPickingDist;
+
+
 
 public:
+	SimpleOpenGL3App* m_glApp;
+
+
 	BasicDemo(SimpleOpenGL3App* app)
-	:m_glApp(app)
+	:m_glApp(app),
+	m_pickedBody(0),
+	m_pickedConstraint(0)
 	{
 	}
 	virtual ~BasicDemo()
@@ -141,7 +153,7 @@ public:
 						curColor&=3;
 						startTransform.setOrigin(btVector3(
 											btScalar(2.0*i),
-											btScalar(1+2.0*k),
+											btScalar(20+2.0*k),
 											btScalar(2.0*j)));
 
 						m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents);
@@ -179,12 +191,215 @@ public:
 
 		m_glApp->m_instancingRenderer->renderScene();
 	}
+
+	btVector3	getRayTo(int x,int y)
+	{
+		if (!m_glApp->m_instancingRenderer)
+		{
+			btAssert(0);
+			return btVector3(0,0,0);
+		}
+
+		float top = 1.f;
+		float bottom = -1.f;
+		float nearPlane = 1.f;
+		float tanFov = (top-bottom)*0.5f / nearPlane;
+		float fov = b3Scalar(2.0) * b3Atan(tanFov);
+
+		btVector3 camPos,camTarget;
+		m_glApp->m_instancingRenderer->getCameraPosition(camPos);
+		m_glApp->m_instancingRenderer->getCameraTargetPosition(camTarget);
+
+		btVector3	rayFrom = camPos;
+		btVector3 rayForward = (camTarget-camPos);
+		rayForward.normalize();
+		float farPlane = 10000.f;
+		rayForward*= farPlane;
+
+		btVector3 rightOffset;
+		btVector3 m_cameraUp=btVector3(0,1,0);
+		btVector3 vertical = m_cameraUp;
+
+		btVector3 hor;
+		hor = rayForward.cross(vertical);
+		hor.normalize();
+		vertical = hor.cross(rayForward);
+		vertical.normalize();
+
+		float tanfov = tanf(0.5f*fov);
+
+
+		hor *= 2.f * farPlane * tanfov;
+		vertical *= 2.f * farPlane * tanfov;
+
+		b3Scalar aspect;
+		float width = m_glApp->m_instancingRenderer->getScreenWidth();
+		float height = m_glApp->m_instancingRenderer->getScreenHeight();
+
+		aspect =  width / height;
+	
+		hor*=aspect;
+
+
+		btVector3 rayToCenter = rayFrom + rayForward;
+		btVector3 dHor = hor * 1.f/width;
+		btVector3 dVert = vertical * 1.f/height;
+
+
+		btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
+		rayTo += btScalar(x) * dHor;
+		rayTo -= btScalar(y) * dVert;
+		return rayTo;
+	}
+
+	
+	bool	mouseMoveCallback(float x,float y)
+	{
+//		if (m_data->m_altPressed!=0 || m_data->m_controlPressed!=0)
+	//		return false;
+		
+		if (m_pickedBody  && m_pickedConstraint)
+		{
+			btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickedConstraint);
+			if (pickCon)
+			{
+				//keep it at the same picking distance
+				btVector3 newRayTo = getRayTo(x,y);
+				btVector3 rayFrom;
+				btVector3 oldPivotInB = pickCon->getPivotInB();
+				btVector3 newPivotB;
+				m_glApp->m_instancingRenderer->getCameraPosition(rayFrom);
+				btVector3 dir = newRayTo-rayFrom;
+				dir.normalize();
+				dir *= m_oldPickingDist;
+
+				newPivotB = rayFrom + dir;
+				pickCon->setPivotB(newPivotB);
+			}
+		}
+		
+		return false;
+	}
+	bool	mouseButtonCallback(int button, int state, float x, float y)
+	{
+
+		if (state==1)
+		{
+			if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0))
+			{
+				btVector3 camPos;
+				m_glApp->m_instancingRenderer->getCameraPosition(camPos);
+
+				btVector3 rayFrom = camPos;
+				btVector3 rayTo = getRayTo(x,y);
+
+				btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
+				m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
+				if (rayCallback.hasHit())
+				{
+
+					btVector3 pickPos = rayCallback.m_hitPointWorld;
+					btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
+					if (body)
+					{
+						//other exclusions?
+						if (!(body->isStaticObject() || body->isKinematicObject()))
+						{
+							m_pickedBody = body;
+							m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
+							//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
+							btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
+							btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
+							m_dynamicsWorld->addConstraint(p2p,true);
+							m_pickedConstraint = p2p;
+							btScalar mousePickClamping = 30.f;
+							p2p->m_setting.m_impulseClamp = mousePickClamping;
+							//very weak constraint for picking
+							p2p->m_setting.m_tau = 0.001f;
+						}
+					}
+
+
+//					pickObject(pickPos, rayCallback.m_collisionObject);
+					m_oldPickingPos = rayTo;
+					m_hitPos = pickPos;
+					m_oldPickingDist  = (pickPos-rayFrom).length();
+//					printf("hit !\n");
+				//add p2p
+				}
+				
+			}
+		} else
+		{
+			if (button==0)
+			{
+				if (m_pickedConstraint)
+				{
+					m_dynamicsWorld->removeConstraint(m_pickedConstraint);
+					delete m_pickedConstraint;
+					m_pickedConstraint=0;
+					m_pickedBody = 0;
+				}
+				//remove p2p
+			}
+		}
+
+		//printf("button=%d, state=%d\n",button,state);
+		return false;
+	}
+
 	void	stepSimulation()
 	{
 		m_dynamicsWorld->stepSimulation(1./60,0);
 	}
 };
 
+
+
+BasicDemo* sDemo = 0;
+
+static void MyMouseMoveCallback( float x, float y)
+{
+	bool handled = false;
+	if (sDemo)
+		handled = sDemo->mouseMoveCallback(x,y);
+	if (!handled)
+		b3DefaultMouseMoveCallback(x,y);
+}
+static void MyMouseButtonCallback(int button, int state, float x, float y)
+{
+	bool handled = false;
+	//try picking first
+	if (sDemo)
+		handled = sDemo->mouseButtonCallback(button,state,x,y);
+
+	if (!handled)
+		b3DefaultMouseButtonCallback(button,state,x,y);
+}
+
+void MyKeyboardCallback(int key, int state)
+{
+
+	if (key==B3G_ESCAPE && sDemo->m_glApp->m_window)
+	{
+		sDemo->m_glApp->m_window->setRequestExit();
+	}
+	if (key=='w')
+	{
+		glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+	}
+	if (key=='s')
+	{
+		glPolygonMode( GL_FRONT_AND_BACK, GL_FILL);
+	}
+
+//	if (sDemo)
+	//	sDemo->keyboardCallback(key,state);
+
+	b3DefaultKeyboardCallback(key,state);
+}
+
+
 int main(int argc, char* argv[])
 {
 	
@@ -201,8 +416,13 @@ int main(int argc, char* argv[])
 	app->m_instancingRenderer->setCameraPitch(0);
 	app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0));
 
+	app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
+	app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
+	app->m_window->setKeyboardCallback(MyKeyboardCallback);
+
 	BasicDemo* demo = new BasicDemo(app);
 	demo->initPhysics();
+	sDemo = demo;
 
 	GLint err = glGetError();
     assert(err==GL_NO_ERROR);
diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp
new file mode 100644
index 000000000..685603917
--- /dev/null
+++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp
@@ -0,0 +1,742 @@
+
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_Z 5
+float scaling = 1.f;
+float friction = 1.;
+
+
+#include "OpenGLWindow/SimpleOpenGL3App.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "assert.h"
+#include <stdio.h>
+
+#include "btBulletDynamicsCommon.h"
+
+#include "BulletDynamics/Featherstone/btMultiBody.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+
+static b3Vector4 colors[4] =
+{
+	b3MakeVector4(1,0,0,1),
+	b3MakeVector4(0,1,0,1),
+	b3MakeVector4(0,1,1,1),
+	b3MakeVector4(1,1,0,1),
+};
+
+
+struct btMultiBodySettings
+{
+	btMultiBodySettings()
+	{
+		m_numLinks = 0;
+		m_basePosition.setZero();
+		m_isFixedBase = true;
+		m_usePrismatic = false;
+		m_canSleep = true;
+		m_createConstraints = false;
+		m_disableParentCollision = false;
+	}
+	int			m_numLinks;
+	btVector3	m_basePosition;
+	bool		m_isFixedBase;
+	bool		m_usePrismatic;
+	bool		m_canSleep;
+	bool		m_createConstraints;
+	bool		m_disableParentCollision;
+};
+
+class Bullet2MultiBodyDemo
+{
+protected:
+	btMultiBodyDynamicsWorld* m_dynamicsWorld;
+	btCollisionDispatcher*	m_dispatcher;
+	btBroadphaseInterface*	m_bp;
+	btCollisionConfiguration* m_config;
+	btMultiBodyConstraintSolver* m_solver;
+
+public:
+	Bullet2MultiBodyDemo()
+	{
+		m_config = 0;
+		m_dispatcher = 0;
+		m_bp = 0;
+		m_solver = 0;
+		m_dynamicsWorld = 0;
+	}
+	virtual void initPhysics()
+	{
+		m_config = new btDefaultCollisionConfiguration;
+		m_dispatcher = new btCollisionDispatcher(m_config);
+		m_bp = new btDbvtBroadphase();
+		m_solver = new btMultiBodyConstraintSolver();
+		m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config);
+	}
+	virtual void exitPhysics()
+	{
+		delete m_dynamicsWorld;
+		m_dynamicsWorld=0;
+		delete m_solver;
+		m_solver=0;
+		delete m_bp;
+		m_bp=0;
+		delete m_dispatcher;
+		m_dispatcher=0;
+		delete m_config;
+		m_config=0;
+	}
+
+	virtual ~Bullet2MultiBodyDemo()
+	{
+		btAssert(m_config == 0);
+		btAssert(m_dispatcher == 0);
+		btAssert(m_bp == 0);
+		btAssert(m_solver == 0);
+		btAssert(m_dynamicsWorld == 0);
+	}
+
+};
+
+class BasicDemo : public Bullet2MultiBodyDemo
+{
+	SimpleOpenGL3App* m_glApp;
+
+	btRigidBody*	m_pickedBody;
+	btTypedConstraint* m_pickedConstraint;
+	btVector3 m_oldPickingPos;
+	btVector3 m_hitPos;
+	btScalar m_oldPickingDist;
+
+	class btMultiBodyPoint2Point*		m_pickingMultiBodyPoint2Point;
+	btAlignedObjectArray<btMultiBodyLinkCollider*> m_linkColliders;
+
+public:
+	BasicDemo(SimpleOpenGL3App* app)
+	:m_glApp(app),
+	m_pickedBody(0),
+	m_pickedConstraint(0),
+	m_pickingMultiBodyPoint2Point(0)
+	{
+	}
+	virtual ~BasicDemo()
+	{
+	}
+
+	btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, const btMultiBodySettings& settings)
+	{
+		static int curColor=0;
+					
+	
+		int cubeShapeId = m_glApp->registerCubeShape();
+		
+		int n_links = settings.m_numLinks;
+		float mass = 13.5*scaling;
+		btVector3 inertia = btVector3 (91,344,253)*scaling*scaling;
+	
+	
+		btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep);
+	//		bod->setHasSelfCollision(false);
+
+		//btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1);
+		btQuaternion orn(0,0,0,1);
+		bod->setBasePos(settings.m_basePosition);
+		bod->setWorldToBaseRot(orn);
+		btVector3 vel(0,0,0);
+		bod->setBaseVel(vel);
+
+		{
+			
+			btVector3 joint_axis_hinge(1,0,0);
+			btVector3 joint_axis_prismatic(0,0,1);
+			btQuaternion parent_to_child = orn.inverse();
+			btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic);
+			btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge);
+        
+			int this_link_num = -1;
+			int link_num_counter = 0;
+
+		
+
+			btVector3 pos = btVector3 (0,0,9.0500002)*scaling;
+
+			btVector3 joint_axis_position = btVector3 (0,0,4.5250001)*scaling;
+
+			for (int i=0;i<n_links;i++)
+			{
+				float initial_joint_angle=0.3;
+				if (i>0)
+					initial_joint_angle = -0.06f;
+
+				const int child_link_num = link_num_counter++;
+
+			
+
+				if (settings.m_usePrismatic)// && i==(n_links-1))
+				{
+						bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
+							parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),settings.m_disableParentCollision);
+
+				} else
+				{
+					bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge,
+											joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)),settings.m_disableParentCollision);
+				}
+				bod->setJointPos(child_link_num, initial_joint_angle);
+				this_link_num = i;
+		
+				if (0)//!useGroundShape && i==4)
+				{
+					btVector3 pivotInAworld(0,20,46);
+					btVector3 pivotInAlocal = bod->worldPosToLocal(i, pivotInAworld);
+					btVector3 pivotInBworld = pivotInAworld;
+					btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(bod,i,&btTypedConstraint::getFixedBody(),pivotInAlocal,pivotInBworld);
+					world->addMultiBodyConstraint(p2p);
+				}
+				//add some constraint limit
+				if (settings.m_usePrismatic)
+				{
+		//			btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
+			
+					if (settings.m_createConstraints)
+					{	
+						btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
+						world->addMultiBodyConstraint(con);
+					}
+			
+				} else
+				{
+					if (settings.m_createConstraints)
+					{	
+						if (1)
+						{
+							btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000); 
+							world->addMultiBodyConstraint(con);
+						}
+
+						btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
+						world->addMultiBodyConstraint(con);
+					}
+
+				}
+			}
+		}
+
+		//add a collider for the base
+		{
+			
+			btAlignedObjectArray<btQuaternion> world_to_local;
+			world_to_local.resize(n_links+1);
+
+			btAlignedObjectArray<btVector3> local_origin;
+			local_origin.resize(n_links+1);
+			world_to_local[0] = bod->getWorldToBaseRot();
+			local_origin[0] = bod->getBasePos();
+			//float halfExtents[3]={7.5,0.05,4.5};
+			float halfExtents[3]={7.5,0.45,4.5};
+			{
+			
+				float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+				float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
+
+			
+				if (1)
+				{
+					btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling);
+					btRigidBody* body = new btRigidBody(mass,0,box,inertia);
+					btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1);
+
+
+
+
+					body->setCollisionShape(box);
+					col->setCollisionShape(box);
+								
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(local_origin[0]);
+					tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+					body->setWorldTransform(tr);
+					col->setWorldTransform(tr);
+				
+					b3Vector4 color = colors[curColor++];
+					curColor&=3;
+
+					int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents);
+					col->setUserIndex(index);
+
+
+
+
+					world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter));
+					col->setFriction(friction);
+					bod->setBaseCollider(col);
+				
+				}
+			}
+
+
+			for (int i=0;i<bod->getNumLinks();i++)
+			{
+				const int parent = bod->getParent(i);
+				world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1];
+				local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i)));
+			}
+
+		
+			for (int i=0;i<bod->getNumLinks();i++)
+			{
+		
+				btVector3 posr = local_origin[i+1];
+				float pos[4]={posr.x(),posr.y(),posr.z(),1};
+			
+				float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
+
+				btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling);
+				btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i);
+
+				col->setCollisionShape(box);
+				btTransform tr;
+				tr.setIdentity();
+				tr.setOrigin(posr);
+				tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+				col->setWorldTransform(tr);
+				col->setFriction(friction);
+
+								
+				b3Vector4 color = colors[curColor++];
+				curColor&=3;
+
+				int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents);
+				col->setUserIndex(index);
+
+
+
+				world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter));
+			
+				bod->getLink(i).m_collider=col;
+				//app->drawBox(halfExtents, pos,quat);
+			}
+
+		}
+		world->addMultiBody(bod);
+
+		return bod;
+	}
+	
+	void addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents)
+	{
+	}
+	void addBoxes_testMultiDof()
+	{
+	}
+
+	void	initPhysics()
+	{
+
+		Bullet2MultiBodyDemo::initPhysics();
+
+		//create ground
+		int cubeShapeId = m_glApp->registerCubeShape();
+		float pos[]={0,0,0};
+		float orn[]={0,0,0,1};
+		
+
+		{
+			float color[]={0.3,0.3,1,1};
+			float halfExtents[]={50,50,50,1};
+			btTransform groundTransform;
+			groundTransform.setIdentity();
+			groundTransform.setOrigin(btVector3(0,-50,0));
+			btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(halfExtents[0]),btScalar(halfExtents[1]),btScalar(halfExtents[2])));
+			//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
+			{
+				btScalar mass(0.);
+				//rigidbody is dynamic if and only if mass is non zero, otherwise static
+				bool isDynamic = (mass != 0.f);
+				btVector3 localInertia(0,0,0);
+				if (isDynamic)
+					groundShape->calculateLocalInertia(mass,localInertia);
+				//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+				btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+				btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
+				btRigidBody* body = new btRigidBody(rbInfo);
+
+				int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,groundTransform.getOrigin(),groundTransform.getRotation(),color,halfExtents);
+				body ->setUserIndex(index);
+
+				//add the body to the dynamics world
+				m_dynamicsWorld->addRigidBody(body);
+			}
+		}
+#if 0
+
+		{
+			float halfExtents[]={1,1,1,1};
+		
+
+
+			btTransform startTransform;
+			startTransform.setIdentity();
+			btScalar mass = 1.f;
+			btVector3 localInertia;
+			btBoxShape* colShape = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
+			colShape ->calculateLocalInertia(mass,localInertia);
+
+			for (int k=0;k<ARRAY_SIZE_Y;k++)
+			{
+				for (int i=0;i<ARRAY_SIZE_X;i++)
+				{
+					for(int j = 0;j<ARRAY_SIZE_Z;j++)
+					{
+						static int curColor=0;
+						b3Vector4 color = colors[curColor];
+						curColor++;
+						startTransform.setOrigin(btVector3(
+											btScalar(2.0*i),
+											btScalar(20+2.0*k),
+											btScalar(2.0*j)));
+
+						int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents);
+			
+						//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+						btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+						btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
+						btRigidBody* body = new btRigidBody(rbInfo);
+						body->setUserIndex(index);
+
+						m_dynamicsWorld->addRigidBody(body);
+					}
+				}
+			}
+		}
+#endif
+		btMultiBodySettings settings;
+		settings.m_isFixedBase = false;
+		settings.m_basePosition.setValue(0,10,0);
+		settings.m_numLinks = 10;
+		btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings);
+
+		m_glApp->m_instancingRenderer->writeTransforms();
+	}
+
+
+
+
+
+	void	exitPhysics()
+	{
+		Bullet2MultiBodyDemo::exitPhysics();
+	}
+	void	drawObjects()
+	{
+		//sync graphics -> physics world transforms
+		{
+			for (int i=0;i<m_dynamicsWorld->getNumCollisionObjects();i++)
+			{
+				btCollisionObject* col = m_dynamicsWorld->getCollisionObjectArray()[i];
+
+				btVector3 pos = col->getWorldTransform().getOrigin();
+				btQuaternion orn = col->getWorldTransform().getRotation();
+				int index = col->getUserIndex();
+				m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos,orn,index);
+			}
+			m_glApp->m_instancingRenderer->writeTransforms();
+		}
+
+		m_glApp->m_instancingRenderer->renderScene();
+	}
+
+	btVector3	getRayTo(int x,int y)
+	{
+		if (!m_glApp->m_instancingRenderer)
+		{
+			btAssert(0);
+			return btVector3(0,0,0);
+		}
+
+		float top = 1.f;
+		float bottom = -1.f;
+		float nearPlane = 1.f;
+		float tanFov = (top-bottom)*0.5f / nearPlane;
+		float fov = b3Scalar(2.0) * b3Atan(tanFov);
+
+		btVector3 camPos,camTarget;
+		m_glApp->m_instancingRenderer->getCameraPosition(camPos);
+		m_glApp->m_instancingRenderer->getCameraTargetPosition(camTarget);
+
+		btVector3	rayFrom = camPos;
+		btVector3 rayForward = (camTarget-camPos);
+		rayForward.normalize();
+		float farPlane = 10000.f;
+		rayForward*= farPlane;
+
+		btVector3 rightOffset;
+		btVector3 m_cameraUp=btVector3(0,1,0);
+		btVector3 vertical = m_cameraUp;
+
+		btVector3 hor;
+		hor = rayForward.cross(vertical);
+		hor.normalize();
+		vertical = hor.cross(rayForward);
+		vertical.normalize();
+
+		float tanfov = tanf(0.5f*fov);
+
+
+		hor *= 2.f * farPlane * tanfov;
+		vertical *= 2.f * farPlane * tanfov;
+
+		b3Scalar aspect;
+		float width = m_glApp->m_instancingRenderer->getScreenWidth();
+		float height = m_glApp->m_instancingRenderer->getScreenHeight();
+
+		aspect =  width / height;
+	
+		hor*=aspect;
+
+
+		btVector3 rayToCenter = rayFrom + rayForward;
+		btVector3 dHor = hor * 1.f/width;
+		btVector3 dVert = vertical * 1.f/height;
+
+
+		btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
+		rayTo += btScalar(x) * dHor;
+		rayTo -= btScalar(y) * dVert;
+		return rayTo;
+	}
+
+	
+	bool	mouseMoveCallback(float x,float y)
+	{
+//		if (m_data->m_altPressed!=0 || m_data->m_controlPressed!=0)
+	//		return false;
+		
+		if (m_pickedBody  && m_pickedConstraint)
+		{
+			btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickedConstraint);
+			if (pickCon)
+			{
+				//keep it at the same picking distance
+				btVector3 newRayTo = getRayTo(x,y);
+				btVector3 rayFrom;
+				btVector3 oldPivotInB = pickCon->getPivotInB();
+				btVector3 newPivotB;
+				m_glApp->m_instancingRenderer->getCameraPosition(rayFrom);
+				btVector3 dir = newRayTo-rayFrom;
+				dir.normalize();
+				dir *= m_oldPickingDist;
+
+				newPivotB = rayFrom + dir;
+				pickCon->setPivotB(newPivotB);
+			}
+		}
+		if (m_pickingMultiBodyPoint2Point)
+		{
+			//keep it at the same picking distance
+
+			btVector3 newRayTo = getRayTo(x,y);
+			btVector3 rayFrom;
+			btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB();
+			btVector3 newPivotB;
+			btVector3 camPos;
+			m_glApp->m_instancingRenderer->getCameraPosition(camPos);
+			rayFrom = camPos;
+			btVector3 dir = newRayTo-rayFrom;
+			dir.normalize();
+			dir *= m_oldPickingDist;
+
+			newPivotB = rayFrom + dir;
+			
+			m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
+		}
+		
+		return false;
+	}
+	bool	mouseButtonCallback(int button, int state, float x, float y)
+	{
+
+		if (state==1)
+		{
+			if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0))
+			{
+				btVector3 camPos;
+				m_glApp->m_instancingRenderer->getCameraPosition(camPos);
+
+				btVector3 rayFrom = camPos;
+				btVector3 rayTo = getRayTo(x,y);
+
+				btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
+				m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
+				if (rayCallback.hasHit())
+				{
+
+					btVector3 pickPos = rayCallback.m_hitPointWorld;
+					btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
+					if (body)
+					{
+						//other exclusions?
+						if (!(body->isStaticObject() || body->isKinematicObject()))
+						{
+							m_pickedBody = body;
+							m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
+							//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
+							btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
+							btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
+							m_dynamicsWorld->addConstraint(p2p,true);
+							m_pickedConstraint = p2p;
+							btScalar mousePickClamping = 30.f;
+							p2p->m_setting.m_impulseClamp = mousePickClamping;
+							//very weak constraint for picking
+							p2p->m_setting.m_tau = 0.001f;
+						}
+					} else
+					{
+						btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
+						if (multiCol && multiCol->m_multiBody)
+						{
+							multiCol->m_multiBody->setCanSleep(false);
+
+							btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
+
+							btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos);
+							//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
+							//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
+							//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
+							//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
+
+							p2p->setMaxAppliedImpulse(20*scaling);
+		
+							btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
+							world->addMultiBodyConstraint(p2p);
+							m_pickingMultiBodyPoint2Point =p2p; 
+						}
+					}
+
+
+//					pickObject(pickPos, rayCallback.m_collisionObject);
+					m_oldPickingPos = rayTo;
+					m_hitPos = pickPos;
+					m_oldPickingDist  = (pickPos-rayFrom).length();
+//					printf("hit !\n");
+				//add p2p
+				}
+				
+			}
+		} else
+		{
+			if (button==0)
+			{
+				if (m_pickedConstraint)
+				{
+					m_dynamicsWorld->removeConstraint(m_pickedConstraint);
+					delete m_pickedConstraint;
+					m_pickedConstraint=0;
+					m_pickedBody = 0;
+				}
+
+				if (m_pickingMultiBodyPoint2Point)
+				{
+					m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(true);
+					btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
+					world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point);
+					delete m_pickingMultiBodyPoint2Point;
+					m_pickingMultiBodyPoint2Point = 0;
+				}
+				//remove p2p
+			}
+		}
+
+		//printf("button=%d, state=%d\n",button,state);
+		return false;
+	}
+
+	void	stepSimulation()
+	{
+		m_dynamicsWorld->stepSimulation(1./60,0);
+//		CProfileManager::dumpAll();
+	}
+};
+
+
+
+BasicDemo* sDemo = 0;
+
+static void MyMouseMoveCallback( float x, float y)
+{
+	bool handled = false;
+	if (sDemo)
+		handled = sDemo->mouseMoveCallback(x,y);
+	if (!handled)
+		b3DefaultMouseMoveCallback(x,y);
+}
+static void MyMouseButtonCallback(int button, int state, float x, float y)
+{
+	bool handled = false;
+	//try picking first
+	if (sDemo)
+		handled = sDemo->mouseButtonCallback(button,state,x,y);
+
+	if (!handled)
+		b3DefaultMouseButtonCallback(button,state,x,y);
+}
+
+
+int main(int argc, char* argv[])
+{
+	
+	float dt = 1./120.f;
+#ifdef BT_DEBUG
+	char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo (Debug build=SLOW)";
+#else
+	char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo";
+#endif
+
+	
+	SimpleOpenGL3App* app = new SimpleOpenGL3App(name,1024,768);
+	app->m_instancingRenderer->setCameraDistance(40);
+	app->m_instancingRenderer->setCameraPitch(0);
+	app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0));
+
+	app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
+	app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
+
+	BasicDemo* demo = new BasicDemo(app);
+	demo->initPhysics();
+	sDemo = demo;
+
+	GLint err = glGetError();
+    assert(err==GL_NO_ERROR);
+	
+	do
+	{
+		GLint err = glGetError();
+		assert(err==GL_NO_ERROR);
+		app->m_instancingRenderer->init();
+		app->m_instancingRenderer->updateCamera();
+		
+		demo->stepSimulation();
+		demo->drawObjects();
+		app->drawGrid(10,0.01);
+		char bla[1024];
+		static int frameCount = 0;
+		frameCount++;
+		sprintf(bla,"Simulation frame %d", frameCount);
+		
+		app->drawText(bla,10,10);
+		app->swapBuffer();
+	} while (!app->m_window->requestedExit());
+
+
+	demo->exitPhysics();
+	delete demo;
+
+	delete app;
+	return 0;
+}
diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua b/Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua
new file mode 100644
index 000000000..fc4bafc8e
--- /dev/null
+++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua
@@ -0,0 +1,38 @@
+	
+		project "App2_FeatherstoneMultiBodyDemo"
+
+		language "C++"
+				
+		kind "ConsoleApp"
+		targetdir "../../../bin"
+
+  	includedirs {
+                ".",
+                "../../../src",
+                "../../../btgui"
+                }
+
+		initOpenGL()
+		initGlew()
+			
+		links{"gwen", "BulletDynamics", "BulletCollision","LinearMath",
+			"OpenGL_Window", "OpenGL_TrueTypeFont"
+		}
+		
+		files {
+		"**.cpp",
+		"**.h",
+		"../../../src/Bullet3Common/**.cpp",
+ 		"../../../src/Bullet3Common/**.h",
+		"../../../btgui/Timing/b3Clock.cpp",
+		"../../../btgui/Timing/b3Clock.h"
+
+		}
+
+	if os.is("Linux") then 
+		links ("X11")
+	end
+
+	if os.is("MacOSX") then
+		links{"Cocoa.framework"}
+	end
diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.cpp b/btgui/OpenGLWindow/GLInstancingRenderer.cpp
index bddd6dd5f..98c8fbffb 100644
--- a/btgui/OpenGLWindow/GLInstancingRenderer.cpp
+++ b/btgui/OpenGLWindow/GLInstancingRenderer.cpp
@@ -407,6 +407,8 @@ GLInstancingRenderer::~GLInstancingRenderer()
 
 void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)
 {
+	b3Assert(srcIndex<m_data->m_totalNumInstances);
+	b3Assert(srcIndex>=0);
 	m_data->m_instance_positions_ptr[srcIndex*4+0]=position[0];
 	m_data->m_instance_positions_ptr[srcIndex*4+1]=position[1];
 	m_data->m_instance_positions_ptr[srcIndex*4+2]=position[2];
@@ -605,7 +607,7 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float*
 		b3Error("registerGraphicsInstance out of range, %d\n", maxElements);
 		return -1;
 	}
-	return gfxObj->m_numGraphicsInstances;
+	return index;//gfxObj->m_numGraphicsInstances;
 }
 
 
diff --git a/build3/premake4.lua b/build3/premake4.lua
index 5b9086664..10d5460f0 100644
--- a/build3/premake4.lua
+++ b/build3/premake4.lua
@@ -115,6 +115,7 @@ include "../Demos3/SimpleOpenGL3"
 		include "../src/BulletCollision"
 		include "../src/LinearMath"
 		include "../Demos3/bullet2/BasicDemo"
+		include "../Demos3/bullet2/FeatherstoneMultiBodyDemo"
 
 		include "../src/Bullet3Dynamics"
 		include "../src/Bullet3Common"
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
new file mode 100644
index 000000000..8788ccbb4
--- /dev/null
+++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
@@ -0,0 +1,20 @@
+
+#ifndef B3_BVH_SUBTREE_INFO_DATA_H
+#define B3_BVH_SUBTREE_INFO_DATA_H
+
+typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
+
+struct b3BvhSubtreeInfoData
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes, points to the root of the subtree
+	int			m_rootNodeIndex;
+	//4 bytes
+	int			m_subtreeSize;
+	int			m_padding[3];
+};
+
+#endif //B3_BVH_SUBTREE_INFO_DATA_H
+
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h
new file mode 100644
index 000000000..2618da24b
--- /dev/null
+++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h
@@ -0,0 +1,126 @@
+
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+
+
+
+// work-in-progress
+void   b3BvhTraversal( __global const b3Int4* pairs, 
+									__global const b3RigidBodyData* rigidBodies, 
+									__global const b3Collidable* collidables,
+									__global b3Aabb* aabbs,
+									__global b3Int4* concavePairsOut,
+									__global volatile int* numConcavePairsOut,
+									__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
+									__global const b3QuantizedBvhNode* quantizedNodesRoot,
+									__global const b3BvhInfo* bvhInfos,
+									int numPairs,
+									int maxNumConcavePairsCapacity,
+									int id)
+{
+	
+	int bodyIndexA = pairs[id].x;
+	int bodyIndexB = pairs[id].y;
+	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
+	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
+	
+	//once the broadphase avoids static-static pairs, we can remove this test
+	if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
+	{
+		return;
+	}
+		
+	if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)
+		return;
+
+	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
+		
+	if (shapeTypeB!=SHAPE_CONVEX_HULL &&
+		shapeTypeB!=SHAPE_SPHERE	&&
+		shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
+		)
+		return;
+
+	b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
+
+	b3Float4	bvhAabbMin = bvhInfo.m_aabbMin;
+	b3Float4	bvhAabbMax = bvhInfo.m_aabbMax;
+	b3Float4	bvhQuantization = bvhInfo.m_quantization;
+	int numSubtreeHeaders = bvhInfo.m_numSubTrees;
+	__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
+	__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
+	
+
+	unsigned short int quantizedQueryAabbMin[3];
+	unsigned short int quantizedQueryAabbMax[3];
+	b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
+	b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
+	
+	for (int i=0;i<numSubtreeHeaders;i++)
+	{
+		b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
+				
+		int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+		if (overlap != 0)
+		{
+			int startNodeIndex = subtree.m_rootNodeIndex;
+			int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize;
+			int curIndex = startNodeIndex;
+			int escapeIndex;
+			int isLeafNode;
+			int aabbOverlap;
+			while (curIndex < endNodeIndex)
+			{
+				b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
+				aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax);
+				isLeafNode = b3IsLeaf(&rootNode);
+				if (aabbOverlap)
+				{
+					if (isLeafNode)
+					{
+						int triangleIndex = b3GetTriangleIndex(&rootNode);
+						if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS)
+						{
+								int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
+								int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB);
+								for (int b=0;b<numChildrenB;b++)
+								{
+									if ((pairIdx+b)<maxNumConcavePairsCapacity)
+									{
+										int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b;
+										b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB);
+										concavePairsOut[pairIdx+b] = newPair;
+									}
+								}
+						} else
+						{
+							int pairIdx = b3AtomicInc(numConcavePairsOut);
+							if (pairIdx<maxNumConcavePairsCapacity)
+							{
+								b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0);
+								concavePairsOut[pairIdx] = newPair;
+							}
+						}
+					} 
+					curIndex++;
+				} else
+				{
+					if (isLeafNode)
+					{
+						curIndex++;
+					} else
+					{
+						escapeIndex = b3GetEscapeIndex(&rootNode);
+						curIndex += escapeIndex;
+					}
+				}
+			}
+		}
+	}
+
+}
\ No newline at end of file
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3CollidableData.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3CollidableData.h
deleted file mode 100644
index e69de29bb..000000000
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
new file mode 100644
index 000000000..bc7bdaa85
--- /dev/null
+++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
@@ -0,0 +1,597 @@
+#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
+#define B3_FIND_CONCAVE_SEPARATING_AXIS_H
+
+#define B3_TRIANGLE_NUM_CONVEX_FACES 5
+
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+
+
+inline void b3Project(__global const b3ConvexPolyhedronData* hull,  b3Float4ConstArg pos, b3QuatConstArg orn, 
+const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
+{
+	min[0] = FLT_MAX;
+	max[0] = -FLT_MAX;
+	int numVerts = hull->m_numVertices;
+
+	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),*dir);
+	float offset = b3Dot(pos,*dir);
+	for(int i=0;i<numVerts;i++)
+	{
+		float dp = b3Dot(vertices[hull->m_vertexOffset+i],localDir);
+		if(dp < min[0])	
+			min[0] = dp;
+		if(dp > max[0])	
+			max[0] = dp;
+	}
+	if(min[0]>max[0])
+	{
+		float tmp = min[0];
+		min[0] = max[0];
+		max[0] = tmp;
+	}
+	min[0] += offset;
+	max[0] += offset;
+}
+
+
+inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, 
+	b3Float4ConstArg posA,b3QuatConstArg ornA,
+	b3Float4ConstArg posB,b3QuatConstArg ornB,
+	b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB,float* depth)
+{
+	float Min0,Max0;
+	float Min1,Max1;
+	b3Project(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0);
+	b3Project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1);
+
+	if(Max0<Min1 || Max1<Min0)
+		return false;
+
+	float d0 = Max0 - Min1;
+	float d1 = Max1 - Min0;
+	*depth = d0<d1 ? d0:d1;
+	return true;
+}
+
+
+bool b3FindSeparatingAxis(	const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, 
+	b3Float4ConstArg posA1,
+	b3QuatConstArg ornA,
+	b3Float4ConstArg posB1,
+	b3QuatConstArg ornB,
+	b3Float4ConstArg DeltaC2,
+	
+	const b3Float4* verticesA, 
+	const b3Float4* uniqueEdgesA, 
+	const b3GpuFace* facesA,
+	const int*  indicesA,
+
+	__global const b3Float4* verticesB, 
+	__global const b3Float4* uniqueEdgesB, 
+	__global const b3GpuFace* facesB,
+	__global const int*  indicesB,
+	b3Float4* sep,
+	float* dmin)
+{
+	
+
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+	int curPlaneTests=0;
+	{
+		int numFacesA = hullA->m_numFaces;
+		// Test normals from hullA
+		for(int i=0;i<numFacesA;i++)
+		{
+			const b3Float4 normal = facesA[hullA->m_faceOffset+i].m_plane;
+			b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
+			if (b3Dot(DeltaC2,faceANormalWS)<0)
+				faceANormalWS*=-1.f;
+			curPlaneTests++;
+			float d;
+			if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d))
+				return false;
+			if(d<*dmin)
+			{
+				*dmin = d;
+				*sep = faceANormalWS;
+			}
+		}
+	}
+	if((b3Dot(-DeltaC2,*sep))>0.0f)
+	{
+		*sep = -(*sep);
+	}
+	return true;
+}
+
+
+
+
+
+bool b3FindSeparatingAxisEdgeEdge(	const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, 
+	b3Float4ConstArg posA1,
+	b3QuatConstArg ornA,
+	b3Float4ConstArg posB1,
+	b3QuatConstArg ornB,
+	b3Float4ConstArg DeltaC2,
+	const b3Float4* verticesA, 
+	const b3Float4* uniqueEdgesA, 
+	const b3GpuFace* facesA,
+	const int*  indicesA,
+	__global const b3Float4* verticesB, 
+	__global const b3Float4* uniqueEdgesB, 
+	__global const b3GpuFace* facesB,
+	__global const int*  indicesB,
+		b3Float4* sep,
+	float* dmin)
+{
+
+
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+
+	int curPlaneTests=0;
+
+	int curEdgeEdge = 0;
+	// Test edges
+	for(int e0=0;e0<hullA->m_numUniqueEdges;e0++)
+	{
+		const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0];
+		b3Float4 edge0World = b3QuatRotate(ornA,edge0);
+
+		for(int e1=0;e1<hullB->m_numUniqueEdges;e1++)
+		{
+			const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1];
+			b3Float4 edge1World = b3QuatRotate(ornB,edge1);
+
+
+			b3Float4 crossje = b3Cross(edge0World,edge1World);
+
+			curEdgeEdge++;
+			if(!b3IsAlmostZero(crossje))
+			{
+				crossje = b3Normalized(crossje);
+				if (b3Dot(DeltaC2,crossje)<0)
+					crossje *= -1.f;
+
+				float dist;
+				bool result = true;
+				{
+					float Min0,Max0;
+					float Min1,Max1;
+					b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0);
+					b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1);
+				
+					if(Max0<Min1 || Max1<Min0)
+						result = false;
+				
+					float d0 = Max0 - Min1;
+					float d1 = Max1 - Min0;
+					dist = d0<d1 ? d0:d1;
+					result = true;
+
+				}
+				
+
+				if(dist<*dmin)
+				{
+					*dmin = dist;
+					*sep = crossje;
+				}
+			}
+		}
+
+	}
+
+	
+	if((b3Dot(-DeltaC2,*sep))>0.0f)
+	{
+		*sep = -(*sep);
+	}
+	return true;
+}
+
+
+
+inline int	b3FindClippingFaces(b3Float4ConstArg separatingNormal,
+                      __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
+                      b3Float4ConstArg posA, b3QuatConstArg ornA,b3Float4ConstArg posB, b3QuatConstArg ornB,
+                       __global b3Float4* worldVertsA1,
+                      __global b3Float4* worldNormalsA1,
+                      __global b3Float4* worldVertsB1,
+                      int capacityWorldVerts,
+                      const float minDist, float maxDist,
+                      __global const b3Float4* verticesA,
+                      __global const b3GpuFace_t* facesA,
+                      __global const int* indicesA,
+						__global const b3Float4* verticesB,
+                      __global const b3GpuFace_t* facesB,
+                      __global const int* indicesB,
+
+                      __global b3Int4* clippingFaces, int pairIndex)
+{
+	int numContactsOut = 0;
+	int numWorldVertsB1= 0;
+    
+    
+	int closestFaceB=-1;
+	float dmax = -FLT_MAX;
+    
+	{
+		for(int face=0;face<hullB->m_numFaces;face++)
+		{
+			const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset+face].m_plane.x,
+                                              facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);
+			const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
+			float d = b3Dot(WorldNormal,separatingNormal);
+			if (d > dmax)
+			{
+				dmax = d;
+				closestFaceB = face;
+			}
+		}
+	}
+    
+	{
+		const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB];
+		const int numVertices = polyB.m_numIndices;
+		for(int e0=0;e0<numVertices;e0++)
+		{
+			const b3Float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
+			worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
+		}
+	}
+    
+    int closestFaceA=-1;
+	{
+		float dmin = FLT_MAX;
+		for(int face=0;face<hullA->m_numFaces;face++)
+		{
+			const b3Float4 Normal = b3MakeFloat4(
+                                              facesA[hullA->m_faceOffset+face].m_plane.x,
+                                              facesA[hullA->m_faceOffset+face].m_plane.y,
+                                              facesA[hullA->m_faceOffset+face].m_plane.z,
+                                              0.f);
+			const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
+            
+			float d = b3Dot(faceANormalWS,separatingNormal);
+			if (d < dmin)
+			{
+				dmin = d;
+				closestFaceA = face;
+                worldNormalsA1[pairIndex] = faceANormalWS;
+			}
+		}
+	}
+    
+    int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;
+	for(int e0=0;e0<numVerticesA;e0++)
+	{
+        const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];
+        worldVertsA1[pairIndex*capacityWorldVerts+e0] = b3TransformPoint(a, posA,ornA);
+    }
+    
+    clippingFaces[pairIndex].x = closestFaceA;
+    clippingFaces[pairIndex].y = closestFaceB;
+    clippingFaces[pairIndex].z = numVerticesA;
+    clippingFaces[pairIndex].w = numWorldVertsB1;
+    
+    
+	return numContactsOut;
+}
+
+
+        
+
+__kernel void   b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs,
+																					__global const b3RigidBodyData* rigidBodies,
+																					__global const b3Collidable* collidables,
+																					__global const b3ConvexPolyhedronData* convexShapes, 
+																					__global const b3Float4* vertices,
+																					__global const b3Float4* uniqueEdges,
+																					__global const b3GpuFace* faces,
+																					__global const int* indices,
+																					__global const b3GpuChildShape* gpuChildShapes,
+																					__global b3Aabb* aabbs,
+																					__global b3Float4* concaveSeparatingNormalsOut,
+																					__global b3Int4* clippingFacesOut,
+																					__global b3Vector3* worldVertsA1Out,
+																					__global b3Vector3* worldNormalsA1Out,
+																					__global b3Vector3* worldVertsB1Out,
+																					__global int* hasSeparatingNormals,
+																					int vertexFaceCapacity,
+																					int numConcavePairs,
+																					int pairIdx
+																					)
+{
+	int i = pairIdx;
+/*	int i = get_global_id(0);
+	if (i>=numConcavePairs)
+		return;
+	int pairIdx = i;
+	*/
+
+	int bodyIndexA = concavePairs[i].x;
+	int bodyIndexB = concavePairs[i].y;
+
+	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
+	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
+
+	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
+	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
+
+	if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&&
+		collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS)
+	{
+		concavePairs[pairIdx].w = -1;
+		return;
+	}
+
+	hasSeparatingNormals[i] = 0;
+
+	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
+	int numActualConcaveConvexTests = 0;
+	
+	int f = concavePairs[i].z;
+	
+	bool overlap = false;
+	
+	b3ConvexPolyhedronData convexPolyhedronA;
+
+	//add 3 vertices of the triangle
+	convexPolyhedronA.m_numVertices = 3;
+	convexPolyhedronA.m_vertexOffset = 0;
+	b3Float4	localCenter = b3MakeFloat4(0.f,0.f,0.f,0.f);
+
+	b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];
+	b3Float4 triMinAabb, triMaxAabb;
+	b3Aabb triAabb;
+	triAabb.m_minVec = b3MakeFloat4(1e30f,1e30f,1e30f,0.f);
+	triAabb.m_maxVec = b3MakeFloat4(-1e30f,-1e30f,-1e30f,0.f);
+	
+	b3Float4 verticesA[3];
+	for (int i=0;i<3;i++)
+	{
+		int index = indices[face.m_indexOffset+i];
+		b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];
+		verticesA[i] = vert;
+		localCenter += vert;
+			
+		triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec,vert);		
+		triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec,vert);		
+
+	}
+
+	overlap = true;
+	overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
+	overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
+	overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
+		
+	if (overlap)
+	{
+		float dmin = FLT_MAX;
+		int hasSeparatingAxis=5;
+		b3Float4 sepAxis=b3MakeFloat4(1,2,3,4);
+
+		int localCC=0;
+		numActualConcaveConvexTests++;
+
+		//a triangle has 3 unique edges
+		convexPolyhedronA.m_numUniqueEdges = 3;
+		convexPolyhedronA.m_uniqueEdgesOffset = 0;
+		b3Float4 uniqueEdgesA[3];
+		
+		uniqueEdgesA[0] = (verticesA[1]-verticesA[0]);
+		uniqueEdgesA[1] = (verticesA[2]-verticesA[1]);
+		uniqueEdgesA[2] = (verticesA[0]-verticesA[2]);
+
+
+		convexPolyhedronA.m_faceOffset = 0;
+                                  
+		b3Float4 normal = b3MakeFloat4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
+                             
+		b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
+		int indicesA[3+3+2+2+2];
+		int curUsedIndices=0;
+		int fidx=0;
+
+		//front size of triangle
+		{
+			facesA[fidx].m_indexOffset=curUsedIndices;
+			indicesA[0] = 0;
+			indicesA[1] = 1;
+			indicesA[2] = 2;
+			curUsedIndices+=3;
+			float c = face.m_plane.w;
+			facesA[fidx].m_plane.x = normal.x;
+			facesA[fidx].m_plane.y = normal.y;
+			facesA[fidx].m_plane.z = normal.z;
+			facesA[fidx].m_plane.w = c;
+			facesA[fidx].m_numIndices=3;
+		}
+		fidx++;
+		//back size of triangle
+		{
+			facesA[fidx].m_indexOffset=curUsedIndices;
+			indicesA[3]=2;
+			indicesA[4]=1;
+			indicesA[5]=0;
+			curUsedIndices+=3;
+			float c = b3Dot(normal,verticesA[0]);
+			float c1 = -face.m_plane.w;
+			facesA[fidx].m_plane.x = -normal.x;
+			facesA[fidx].m_plane.y = -normal.y;
+			facesA[fidx].m_plane.z = -normal.z;
+			facesA[fidx].m_plane.w = c;
+			facesA[fidx].m_numIndices=3;
+		}
+		fidx++;
+
+		bool addEdgePlanes = true;
+		if (addEdgePlanes)
+		{
+			int numVertices=3;
+			int prevVertex = numVertices-1;
+			for (int i=0;i<numVertices;i++)
+			{
+				b3Float4 v0 = verticesA[i];
+				b3Float4 v1 = verticesA[prevVertex];
+                                            
+				b3Float4 edgeNormal = b3Normalized(b3Cross(normal,v1-v0));
+				float c = -b3Dot(edgeNormal,v0);
+
+				facesA[fidx].m_numIndices = 2;
+				facesA[fidx].m_indexOffset=curUsedIndices;
+				indicesA[curUsedIndices++]=i;
+				indicesA[curUsedIndices++]=prevVertex;
+                                            
+				facesA[fidx].m_plane.x = edgeNormal.x;
+				facesA[fidx].m_plane.y = edgeNormal.y;
+				facesA[fidx].m_plane.z = edgeNormal.z;
+				facesA[fidx].m_plane.w = c;
+				fidx++;
+				prevVertex = i;
+			}
+		}
+		convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
+		convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f);
+
+
+		b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
+		posA.w = 0.f;
+		b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
+		posB.w = 0.f;
+
+		b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
+		b3Quaternion ornB =rigidBodies[bodyIndexB].m_quat;
+
+		
+
+
+		///////////////////
+		///compound shape support
+
+		if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		{
+			int compoundChild = concavePairs[pairIdx].w;
+			int childShapeIndexB = compoundChild;//collidables[collidableIndexB].m_shapeIndex+compoundChild;
+			int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
+			b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
+			b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
+			b3Float4 newPosB = b3TransformPoint(childPosB,posB,ornB);
+			b3Quaternion newOrnB = b3QuatMul(ornB,childOrnB);
+			posB = newPosB;
+			ornB = newOrnB;
+			shapeIndexB = collidables[childColIndexB].m_shapeIndex;
+		}
+		//////////////////
+
+		b3Float4 c0local = convexPolyhedronA.m_localCenter;
+		b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
+		b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
+		b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
+		const b3Float4 DeltaC2 = c0 - c1;
+
+
+		bool sepA = b3FindSeparatingAxis(	&convexPolyhedronA, &convexShapes[shapeIndexB],
+												posA,ornA,
+												posB,ornB,
+												DeltaC2,
+												verticesA,uniqueEdgesA,facesA,indicesA,
+												vertices,uniqueEdges,faces,indices,
+												&sepAxis,&dmin);
+		hasSeparatingAxis = 4;
+		if (!sepA)
+		{
+			hasSeparatingAxis = 0;
+		} else
+		{
+			bool sepB = b3FindSeparatingAxis(	&convexShapes[shapeIndexB],&convexPolyhedronA,
+												posB,ornB,
+												posA,ornA,
+												DeltaC2,
+												vertices,uniqueEdges,faces,indices,
+												verticesA,uniqueEdgesA,facesA,indicesA,
+												&sepAxis,&dmin);
+
+			if (!sepB)
+			{
+				hasSeparatingAxis = 0;
+			} else
+			{
+				bool sepEE = b3FindSeparatingAxisEdgeEdge(	&convexPolyhedronA, &convexShapes[shapeIndexB],
+															posA,ornA,
+															posB,ornB,
+															DeltaC2,
+															verticesA,uniqueEdgesA,facesA,indicesA,
+															vertices,uniqueEdges,faces,indices,
+															&sepAxis,&dmin);
+	
+				if (!sepEE)
+				{
+					hasSeparatingAxis = 0;
+				} else
+				{
+					hasSeparatingAxis = 1;
+				}
+			}
+		}	
+		
+		if (hasSeparatingAxis)
+		{
+			hasSeparatingNormals[i]=1;
+			sepAxis.w = dmin;
+			concaveSeparatingNormalsOut[pairIdx]=sepAxis;
+
+			//now compute clipping faces A and B, and world-space clipping vertices A and B...
+
+			float minDist = -1e30f;
+			float maxDist = 0.02f;
+
+			b3FindClippingFaces(sepAxis,
+                     &convexPolyhedronA,
+					 &convexShapes[shapeIndexB],
+					 posA,ornA,
+					 posB,ornB,
+                       worldVertsA1Out,
+                      worldNormalsA1Out,
+                      worldVertsB1Out,
+					  vertexFaceCapacity,
+                      minDist, maxDist,
+                      verticesA,
+                      facesA,
+                      indicesA,
+ 
+					  vertices,
+                      faces,
+                      indices,
+                      clippingFacesOut, pairIdx);
+
+		} else
+		{	
+			//mark this pair as in-active
+			concavePairs[pairIdx].w = -1;
+		}
+	}
+	else
+	{	
+		//mark this pair as in-active
+		concavePairs[pairIdx].w = -1;
+	}
+}
+
+
+#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H
+
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
new file mode 100644
index 000000000..3661e43cf
--- /dev/null
+++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
@@ -0,0 +1,90 @@
+
+
+#ifndef B3_QUANTIZED_BVH_NODE_H
+#define B3_QUANTIZED_BVH_NODE_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+
+#define B3_MAX_NUM_PARTS_IN_BITS 10
+
+///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
+
+struct b3QuantizedBvhNodeData
+{
+	//12 bytes
+	unsigned short int	m_quantizedAabbMin[3];
+	unsigned short int	m_quantizedAabbMax[3];
+	//4 bytes
+	int	m_escapeIndexOrTriangleIndex;
+};
+
+inline int	b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
+{
+	unsigned int x=0;
+	unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS);
+	// Get only the lower bits where the triangle index is stored
+	return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
+}
+
+inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
+{
+	//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+	return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
+}
+	
+inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
+{
+	return -rootNode->m_escapeIndexOrTriangleIndex;
+}
+
+inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
+{
+	b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin);
+	clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax);
+
+	b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
+	if (isMax)
+	{
+		out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
+		out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
+		out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
+	} else
+	{
+		out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
+		out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
+		out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
+	}
+
+}
+
+
+inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
+								const unsigned short int* aabbMin1,
+								const unsigned short int* aabbMax1,
+								const unsigned short int* aabbMin2,
+								const unsigned short int* aabbMax2)
+{
+	//int overlap = 1;
+	if (aabbMin1[0] > aabbMax2[0])
+		return 0;
+	if (aabbMax1[0] < aabbMin2[0])
+		return 0;
+	if (aabbMin1[1] > aabbMax2[1])
+		return 0;
+	if (aabbMax1[1] < aabbMin2[1])
+		return 0;
+	if (aabbMin1[2] > aabbMax2[2])
+		return 0;
+	if (aabbMax1[2] < aabbMin2[2])
+		return 0;
+	return 1;
+	//overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
+	//overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
+	//overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
+	//return overlap;
+}
+
+
+#endif //B3_QUANTIZED_BVH_NODE_H
diff --git a/src/Bullet3Common/shared/b3Float4.h b/src/Bullet3Common/shared/b3Float4.h
index a8159e51f..fd92b641c 100644
--- a/src/Bullet3Common/shared/b3Float4.h
+++ b/src/Bullet3Common/shared/b3Float4.h
@@ -10,12 +10,30 @@
 	#define b3Dot3F4 b3Dot
 	#define b3Cross3 b3Cross
 	#define	b3MakeFloat4  b3MakeVector3
+	inline b3Vector3 b3Normalized(const b3Vector3& vec)
+	{
+		return vec.normalized();
+	}
 
 	inline b3Float4 b3FastNormalized3(b3Float4ConstArg v)
 	{
 		return v.normalized();
 	}
 
+	inline b3Float4 b3MaxFloat4 (const b3Float4& a, const b3Float4& b)
+	{
+		b3Float4 tmp = a;
+		tmp.setMax(b);
+		return tmp;
+	}
+	inline b3Float4 b3MinFloat4 (const b3Float4& a, const b3Float4& b)
+	{
+		b3Float4 tmp = a;
+		tmp.setMin(b);
+		return tmp;
+	}
+
+
 
 #else
 	typedef float4	b3Float4;
@@ -33,6 +51,11 @@
 		float4 b1 = b3MakeFloat4(v1.xyz,0.f);
 		return cross(a1, b1);
 	}
+	#define b3MinFloat4 min
+	#define b3MaxFloat4 max
+
+	#define b3Normalized(a) normalize(a)
+
 #endif 
 
 
diff --git a/src/Bullet3Common/shared/b3Int4.h b/src/Bullet3Common/shared/b3Int4.h
index 41e049cb9..aa02d6bee 100644
--- a/src/Bullet3Common/shared/b3Int4.h
+++ b/src/Bullet3Common/shared/b3Int4.h
@@ -1,8 +1,11 @@
 #ifndef B3_INT4_H
 #define B3_INT4_H
 
+#ifdef __cplusplus
+
 #include "Bullet3Common/b3Scalar.h"
 
+
 B3_ATTRIBUTE_ALIGNED16(struct) b3UnsignedInt4
 {
 	B3_DECLARE_ALIGNED_ALLOCATOR();
@@ -51,5 +54,15 @@ B3_FORCE_INLINE b3UnsignedInt4 b3MakeUnsignedInt4(unsigned int x, unsigned int y
 	return v;
 }
 
+#else
+
+
+#define b3UnsignedInt4 uint4
+#define b3Int4 int4
+#define b3MakeInt4 (int4)
+#define b3MakeUnsignedInt4 (uint4)
+
+
+#endif //__cplusplus
 
 #endif //B3_INT4_H
diff --git a/src/Bullet3Common/shared/b3PlatformDefinitions.h b/src/Bullet3Common/shared/b3PlatformDefinitions.h
index 51af689f8..01243ed1f 100644
--- a/src/Bullet3Common/shared/b3PlatformDefinitions.h
+++ b/src/Bullet3Common/shared/b3PlatformDefinitions.h
@@ -8,9 +8,19 @@ struct MyTest
 
 #ifdef __cplusplus
 #define b3AtomicInc(a) ((*a)++)
+
+inline int b3AtomicAdd (volatile int *p, int val)
+{
+	int oldValue = *p;
+	int newValue = oldValue+val;
+	*p = newValue;
+	return oldValue;
+}
+
 #define __global 
 #else
 #define b3AtomicInc atomic_inc
+#define b3AtomicAdd atomic_add
 #define b3Fabs fabs
 #define b3Sqrt native_sqrt
 #define b3Sin native_sin
diff --git a/src/Bullet3Common/shared/b3Quat.h b/src/Bullet3Common/shared/b3Quat.h
index 8f2fe8301..81b688108 100644
--- a/src/Bullet3Common/shared/b3Quat.h
+++ b/src/Bullet3Common/shared/b3Quat.h
@@ -10,6 +10,10 @@
 
 	#define b3Quat b3Quaternion
 	#define b3QuatConstArg const b3Quaternion&
+	inline b3Quat b3QuatInverse(b3QuatConstArg orn)
+	{
+		return orn.inverse();
+	}
 
 	inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg  orientation)
 	{
diff --git a/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp b/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp
index 598e8ce32..53846a6a4 100644
--- a/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp
+++ b/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp
@@ -6,7 +6,7 @@
 #include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
-#include "Bullet3Collision/NarrowPhaseCollision/shared/b3CollidableData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
 #include "Bullet3Common/b3Vector3.h"
 #include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
 #include "Bullet3Dynamics/shared/b3Inertia.h"
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
index f08f284b4..9b7a109ff 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
@@ -13,6 +13,10 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+bool findSeparatingAxisOnGpu = true;
+
+bool bvhTraversalKernelGPU = true;
+bool findConcaveSeparatingAxisKernelGPU = true;
 
 ///This file was written by Erwin Coumans
 ///Separating axis rest based on work from Pierre Terdiman, see
@@ -65,6 +69,11 @@ typedef b3AlignedObjectArray<b3Vector3> b3VertexArray;
 #endif
 
 
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h"
+
+
+
 #define dot3F4 b3Dot
 
 GpuSatCollision::GpuSatCollision(cl_context ctx,cl_device_id device, cl_command_queue  q )
@@ -76,6 +85,7 @@ m_totalContactsOut(m_context, m_queue),
 m_sepNormals(m_context, m_queue),
 m_hasSeparatingNormals(m_context, m_queue),
 m_concaveSepNormals(m_context, m_queue),
+m_concaveHasSeparatingNormals(m_context,m_queue),
 m_numConcavePairsOut(m_context, m_queue),
 m_gpuCompoundPairs(m_context, m_queue),
 m_gpuCompoundSepNormals(m_context, m_queue),
@@ -1197,7 +1207,7 @@ int clipHullHullSingle(
 			int numPoints = 0;
 					
 			{
-				B3_PROFILE("extractManifold");
+			//	B3_PROFILE("extractManifold");
 				numPoints = extractManifold(contactsOut, numContactsOut, normalOnSurfaceB,  &contactIdx);
 			}
 					
@@ -2723,6 +2733,9 @@ int computeContactConvexConvex2(
 }
 
 
+
+																
+																
 void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>* pairs, int nPairs,
 			const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
 			b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
@@ -2898,15 +2911,13 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 			hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
 		{
 			//printf("hostPairs[i].z=%d\n",hostPairs[i].z);
-			int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
-					hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
-			//int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
-			//		hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,
-			//		oldHostContacts);
+			int contactIndex = 0;//computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf, hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
+			//int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
 
 
 			if (contactIndex>=0)
 			{
+//				printf("convex convex contactIndex = %d\n",contactIndex);
 				hostPairs[i].z = contactIndex;
 			}
 //			printf("plane-convex\n");
@@ -2932,7 +2943,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 		contactOut->resize(0);
 		}
 
-	return;
+		m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true);
+
 #else
 
 	{
@@ -2979,7 +2991,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 	
 	int concaveCapacity=maxTriConvexPairCapacity;
 	m_concaveSepNormals.resize(concaveCapacity);
-
+	m_concaveHasSeparatingNormals.resize(concaveCapacity);
 	m_numConcavePairsOut.resize(0);
 	m_numConcavePairsOut.push_back(0);
 
@@ -2996,7 +3008,6 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 
 	int numCompoundPairs = 0;
 
-	bool findSeparatingAxisOnGpu = true;//false;
 	int numConcavePairs =0;
 
 	{
@@ -3029,84 +3040,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 				clFinish(m_queue);
 			}
 
-			//now perform the tree query on GPU
-			{
-				
-				
-				
-				{
-				
-					if (treeNodesGPU->size() && treeNodesGPU->size())
-					{
-						B3_PROFILE("m_bvhTraversalKernel");
-						
-						
-						numConcavePairs = m_numConcavePairsOut.at(0);
-						
-						b3LauncherCL launcher(m_queue, m_bvhTraversalKernel,"m_bvhTraversalKernel");
-						launcher.setBuffer( pairs->getBufferCL());
-						launcher.setBuffer(  bodyBuf->getBufferCL());
-						launcher.setBuffer( gpuCollidables.getBufferCL());
-						launcher.setBuffer( clAabbsWorldSpace.getBufferCL());
-						launcher.setBuffer( triangleConvexPairsOut.getBufferCL());
-						launcher.setBuffer( m_numConcavePairsOut.getBufferCL());
-						launcher.setBuffer( subTreesGPU->getBufferCL());
-						launcher.setBuffer( treeNodesGPU->getBufferCL());
-						launcher.setBuffer( bvhInfo->getBufferCL());
-						
-						launcher.setConst( nPairs  );
-						launcher.setConst( maxTriConvexPairCapacity);
-						int num = nPairs;
-						launcher.launch1D( num);
-						clFinish(m_queue);
-						numConcavePairs = m_numConcavePairsOut.at(0);
-						//printf("numConcavePairs=%d (max = %d\n",numConcavePairs,maxTriConvexPairCapacity);
-						
-						if (numConcavePairs > maxTriConvexPairCapacity)
-						{
-							static int exceeded_maxTriConvexPairCapacity_count = 0;
-							b3Error("Rxceeded %d times the maxTriConvexPairCapacity (found %d but max is %d)\n", exceeded_maxTriConvexPairCapacity_count++,
-								numConcavePairs,maxTriConvexPairCapacity);
-							numConcavePairs = maxTriConvexPairCapacity;
-						}
-						triangleConvexPairsOut.resize(numConcavePairs);
-						if (numConcavePairs)
-						{
-							//now perform a SAT test for each triangle-convex element (stored in triangleConvexPairsOut)
-							B3_PROFILE("findConcaveSeparatingAxisKernel");
-							b3BufferInfoCL bInfo[] = { 
-								b3BufferInfoCL( triangleConvexPairsOut.getBufferCL() ), 
-								b3BufferInfoCL( bodyBuf->getBufferCL(),true), 
-								b3BufferInfoCL( gpuCollidables.getBufferCL(),true), 
-								b3BufferInfoCL( convexData.getBufferCL(),true),
-								b3BufferInfoCL( gpuVertices.getBufferCL(),true),
-								b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
-								b3BufferInfoCL( gpuFaces.getBufferCL(),true),
-								b3BufferInfoCL( gpuIndices.getBufferCL(),true),
-								b3BufferInfoCL( gpuChildShapes.getBufferCL(),true),
-								b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true),
-								b3BufferInfoCL( m_concaveSepNormals.getBufferCL())
-							};
-
-							b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisKernel,"m_findConcaveSeparatingAxisKernel");
-							launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
-
-							launcher.setConst( numConcavePairs  );
-
-							int num = numConcavePairs;
-							launcher.launch1D( num);
-							clFinish(m_queue);
-
-//							b3AlignedObjectArray<b3Vector3> cpuCompoundSepNormals;
-	//						m_concaveSepNormals.copyToHost(cpuCompoundSepNormals);
-		//					b3AlignedObjectArray<b3Int4> cpuConcavePairs;
-			//				triangleConvexPairsOut.copyToHost(cpuConcavePairs);
-
-
-						}
-					}
-				}
-			}
+			
 			
 			numCompoundPairs = m_numCompoundPairsOut.at(0);
 			bool useGpuFindCompoundPairs=true;
@@ -3325,8 +3259,252 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 		
 		
 	}
+	int vertexFaceCapacity = 64;
 
 
+		
+	{
+		//now perform the tree query on GPU
+			
+					
+				
+				
+		if (treeNodesGPU->size() && treeNodesGPU->size())
+		{
+			if (bvhTraversalKernelGPU)
+			{
+						
+				B3_PROFILE("m_bvhTraversalKernel");
+						
+						
+				numConcavePairs = m_numConcavePairsOut.at(0);
+						
+				b3LauncherCL launcher(m_queue, m_bvhTraversalKernel,"m_bvhTraversalKernel");
+				launcher.setBuffer( pairs->getBufferCL());
+				launcher.setBuffer(  bodyBuf->getBufferCL());
+				launcher.setBuffer( gpuCollidables.getBufferCL());
+				launcher.setBuffer( clAabbsWorldSpace.getBufferCL());
+				launcher.setBuffer( triangleConvexPairsOut.getBufferCL());
+				launcher.setBuffer( m_numConcavePairsOut.getBufferCL());
+				launcher.setBuffer( subTreesGPU->getBufferCL());
+				launcher.setBuffer( treeNodesGPU->getBufferCL());
+				launcher.setBuffer( bvhInfo->getBufferCL());
+						
+				launcher.setConst( nPairs  );
+				launcher.setConst( maxTriConvexPairCapacity);
+				int num = nPairs;
+				launcher.launch1D( num);
+				clFinish(m_queue);
+				numConcavePairs = m_numConcavePairsOut.at(0);
+			} else
+			{
+					b3AlignedObjectArray<b3Int4> hostPairs;
+					pairs->copyToHost(hostPairs);
+					b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
+					bodyBuf->copyToHost(hostBodyBuf);
+					b3AlignedObjectArray<b3Collidable> hostCollidables;
+					gpuCollidables.copyToHost(hostCollidables);
+					b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
+					clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
+
+					//int maxTriConvexPairCapacity,
+					b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
+					triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
+
+					int numTriConvexPairsOutHost=0;
+					numConcavePairs = 0;
+					//m_numConcavePairsOut
+
+					b3AlignedObjectArray<b3QuantizedBvhNode>	treeNodesCPU;
+					treeNodesGPU->copyToHost(treeNodesCPU);
+					b3AlignedObjectArray<b3BvhSubtreeInfo>	subTreesCPU;
+					subTreesGPU->copyToHost(subTreesCPU);
+					b3AlignedObjectArray<b3BvhInfo>	bvhInfoCPU;
+					bvhInfo->copyToHost(bvhInfoCPU);
+					//compute it...
+
+					volatile int hostNumConcavePairsOut=0;
+
+					//
+					for (int i=0;i<nPairs;i++)
+					{
+						b3BvhTraversal( &hostPairs.at(0), 
+						&hostBodyBuf.at(0),
+						&hostCollidables.at(0),
+						&hostAabbsWorldSpace.at(0),
+						&triangleConvexPairsOutHost.at(0),
+						&hostNumConcavePairsOut,
+						&subTreesCPU.at(0),
+						&treeNodesCPU.at(0),
+						&bvhInfoCPU.at(0),
+						nPairs,
+						maxTriConvexPairCapacity,
+						i);
+					}
+					numConcavePairs = hostNumConcavePairsOut;
+
+					if (hostNumConcavePairsOut)
+					{
+						triangleConvexPairsOutHost.resize(hostNumConcavePairsOut);
+						triangleConvexPairsOut.copyFromHost(triangleConvexPairsOutHost);
+					}
+					//
+
+					m_numConcavePairsOut.resize(0);
+					m_numConcavePairsOut.push_back(numConcavePairs);
+			}
+
+				//printf("numConcavePairs=%d (max = %d\n",numConcavePairs,maxTriConvexPairCapacity);
+						
+			if (numConcavePairs > maxTriConvexPairCapacity)
+			{
+				static int exceeded_maxTriConvexPairCapacity_count = 0;
+				b3Error("Exceeded the maxTriConvexPairCapacity (found %d but max is %d, it happened %d times)\n",
+					numConcavePairs,maxTriConvexPairCapacity,exceeded_maxTriConvexPairCapacity_count++);
+				numConcavePairs = maxTriConvexPairCapacity;
+			}
+			triangleConvexPairsOut.resize(numConcavePairs);
+	
+			if (numConcavePairs)
+			{
+
+				
+						
+	
+				clippingFacesOutGPU.resize(numConcavePairs);
+				worldNormalsAGPU.resize(numConcavePairs);
+				worldVertsA1GPU.resize(vertexFaceCapacity*numConcavePairs);
+				worldVertsB1GPU.resize(vertexFaceCapacity*numConcavePairs);
+
+
+				if (findConcaveSeparatingAxisKernelGPU)
+				{
+
+					/*
+					m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
+						clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
+						worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
+						worldNormalsAGPU.copyFromHost(worldNormalsACPU);
+						worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
+					*/
+
+					//now perform a SAT test for each triangle-convex element (stored in triangleConvexPairsOut)
+					B3_PROFILE("findConcaveSeparatingAxisKernel");
+					b3BufferInfoCL bInfo[] = { 
+						b3BufferInfoCL( triangleConvexPairsOut.getBufferCL() ), 
+						b3BufferInfoCL( bodyBuf->getBufferCL(),true), 
+						b3BufferInfoCL( gpuCollidables.getBufferCL(),true), 
+						b3BufferInfoCL( convexData.getBufferCL(),true),
+						b3BufferInfoCL( gpuVertices.getBufferCL(),true),
+						b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
+						b3BufferInfoCL( gpuFaces.getBufferCL(),true),
+						b3BufferInfoCL( gpuIndices.getBufferCL(),true),
+						b3BufferInfoCL( gpuChildShapes.getBufferCL(),true),
+						b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true),
+						b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
+						b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()),
+						b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()),
+						b3BufferInfoCL( worldVertsA1GPU.getBufferCL()),
+						b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
+						b3BufferInfoCL(worldVertsB1GPU.getBufferCL())
+					};
+
+					b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisKernel,"m_findConcaveSeparatingAxisKernel");
+					launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+					launcher.setConst(vertexFaceCapacity);
+					launcher.setConst( numConcavePairs  );
+
+					int num = numConcavePairs;
+					launcher.launch1D( num);
+					clFinish(m_queue);
+				} else
+				{
+
+						b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
+						b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
+						b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
+						b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
+						b3AlignedObjectArray<int>concaveHasSeparatingNormalsCPU;
+
+						b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
+						triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
+						//triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
+						b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
+						bodyBuf->copyToHost(hostBodyBuf);
+						b3AlignedObjectArray<b3Collidable> hostCollidables;
+						gpuCollidables.copyToHost(hostCollidables);
+						b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
+						clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
+
+						b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
+						convexData.copyToHost(hostConvexData);
+
+						b3AlignedObjectArray<b3Vector3> hostVertices;
+						gpuVertices.copyToHost(hostVertices);
+
+						b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
+						gpuUniqueEdges.copyToHost(hostUniqueEdges);
+						b3AlignedObjectArray<b3GpuFace> hostFaces;
+						gpuFaces.copyToHost(hostFaces);
+						b3AlignedObjectArray<int> hostIndices;
+						gpuIndices.copyToHost(hostIndices);
+						b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
+						gpuChildShapes.copyToHost(cpuChildShapes);
+
+
+								
+						b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
+						m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
+						concaveHasSeparatingNormalsCPU.resize(concaveSepNormalsHost.size());
+
+						b3GpuChildShape* childShapePointerCPU = 0;
+						if (cpuChildShapes.size())
+							childShapePointerCPU  = &cpuChildShapes.at(0);
+
+						clippingFacesOutCPU.resize(clippingFacesOutGPU.size());
+						worldVertsA1CPU.resize(worldVertsA1GPU.size());
+    					worldNormalsACPU.resize(worldNormalsAGPU.size());
+						worldVertsB1CPU.resize(worldVertsB1GPU.size());
+
+						for (int i=0;i<numConcavePairs;i++)
+						{
+							b3FindConcaveSeparatingAxisKernel(&triangleConvexPairsOutHost.at(0),
+								&hostBodyBuf.at(0),
+								&hostCollidables.at(0),
+								&hostConvexData.at(0), &hostVertices.at(0),&hostUniqueEdges.at(0),
+								&hostFaces.at(0),&hostIndices.at(0),childShapePointerCPU,
+								&hostAabbsWorldSpace.at(0),
+								&concaveSepNormalsHost.at(0),
+								&clippingFacesOutCPU.at(0),
+								&worldVertsA1CPU.at(0),
+								&worldNormalsACPU.at(0),
+								&worldVertsB1CPU.at(0),
+								&concaveHasSeparatingNormalsCPU.at(0),
+								vertexFaceCapacity,
+								numConcavePairs,i);
+						};
+
+						m_concaveSepNormals.copyFromHost(concaveSepNormalsHost);
+						m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
+						clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
+						worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
+						worldNormalsAGPU.copyFromHost(worldNormalsACPU);
+						worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
+
+
+
+				}
+//							b3AlignedObjectArray<b3Vector3> cpuCompoundSepNormals;
+//						m_concaveSepNormals.copyToHost(cpuCompoundSepNormals);
+//					b3AlignedObjectArray<b3Int4> cpuConcavePairs;
+//				triangleConvexPairsOut.copyToHost(cpuConcavePairs);
+
+
+			}
+		}
+		
+		
+	}
 
 	if (numConcavePairs)
 	{
@@ -3377,45 +3555,130 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 	
 	if (contactClippingOnGpu)
 	{
-		//B3_PROFILE("clipHullHullKernel");
-
-		
 		m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true);
 
-		//concave-convex contact clipping
+		//B3_PROFILE("clipHullHullKernel");
 
+		bool breakupConcaveConvexKernel = false;
+
+#ifdef __APPLE__
+		//actually, some Apple OpenCL platform/device combinations work fine...
+		breakupConcaveConvexKernel = true;
+#endif
+		//concave-convex contact clipping
 		if (numConcavePairs)
 		{
 //			printf("numConcavePairs = %d\n", numConcavePairs);
 	//		nContacts = m_totalContactsOut.at(0);
 		//	printf("nContacts before = %d\n", nContacts);
 
-			B3_PROFILE("clipHullHullConcaveConvexKernel");
-			nContacts = m_totalContactsOut.at(0);
-			b3BufferInfoCL bInfo[] = { 
-				b3BufferInfoCL( triangleConvexPairsOut.getBufferCL(), true ), 
-				b3BufferInfoCL( bodyBuf->getBufferCL(),true), 
-				b3BufferInfoCL( gpuCollidables.getBufferCL(),true), 
-				b3BufferInfoCL( convexData.getBufferCL(),true),
-				b3BufferInfoCL( gpuVertices.getBufferCL(),true),
-				b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
-				b3BufferInfoCL( gpuFaces.getBufferCL(),true),
-				b3BufferInfoCL( gpuIndices.getBufferCL(),true),
-				b3BufferInfoCL( gpuChildShapes.getBufferCL(),true),
-				b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
-				b3BufferInfoCL( contactOut->getBufferCL()),
-				b3BufferInfoCL( m_totalContactsOut.getBufferCL())	
-			};
-			b3LauncherCL launcher(m_queue, m_clipHullHullConcaveConvexKernel,"m_clipHullHullConcaveConvexKernel");
-			launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
-			launcher.setConst( numConcavePairs  );
-			int num = numConcavePairs;
-			launcher.launch1D( num);
-			clFinish(m_queue);
-			nContacts = m_totalContactsOut.at(0);
-			contactOut->resize(nContacts);
-			b3AlignedObjectArray<b3Contact4> cpuContacts;
-			contactOut->copyToHost(cpuContacts);
+			if (breakupConcaveConvexKernel)
+			{
+
+				worldVertsB2GPU.resize(vertexFaceCapacity*numConcavePairs);
+				
+
+				//clipFacesAndFindContacts
+				bool clipFacesAndFindContactsCPU = false;
+				if (clipFacesAndFindContactsCPU)
+				{
+				} else
+				{
+
+					if (1)
+					{
+
+					
+						B3_PROFILE("clipFacesAndFindContacts");
+						//nContacts = m_totalContactsOut.at(0);
+						//int h = m_hasSeparatingNormals.at(0);
+						//int4 p = clippingFacesOutGPU.at(0);
+						b3BufferInfoCL bInfo[] = {
+							b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
+							b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()),
+							b3BufferInfoCL( contactOut->getBufferCL()),
+							b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()),
+							b3BufferInfoCL( worldVertsA1GPU.getBufferCL()),
+							b3BufferInfoCL( worldNormalsAGPU.getBufferCL()),
+							b3BufferInfoCL( worldVertsB1GPU.getBufferCL()),
+							b3BufferInfoCL( worldVertsB2GPU.getBufferCL()),
+							b3BufferInfoCL( m_totalContactsOut.getBufferCL())
+						};
+						b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts,"m_clipFacesAndFindContacts");
+						launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+						launcher.setConst(vertexFaceCapacity);
+
+						launcher.setConst( numConcavePairs  );
+						int debugMode = 0;
+						launcher.setConst( debugMode);
+						int num = numConcavePairs;
+						launcher.launch1D( num);
+						clFinish(m_queue);
+						//int bla = m_totalContactsOut.at(0);
+					}
+				}
+				//contactReduction
+				{
+					contactOut->reserve(nContacts+numConcavePairs);
+                    
+                    {
+                        B3_PROFILE("newContactReductionKernel");
+                            b3BufferInfoCL bInfo[] =
+                        {
+							b3BufferInfoCL( triangleConvexPairsOut.getBufferCL(), true ),
+                            b3BufferInfoCL( bodyBuf->getBufferCL(),true),
+                            b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
+                            b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()),
+                            b3BufferInfoCL( contactOut->getBufferCL()),
+                            b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()),
+                            b3BufferInfoCL( worldVertsB2GPU.getBufferCL()),
+                            b3BufferInfoCL( m_totalContactsOut.getBufferCL())
+                        };
+                        
+                        b3LauncherCL launcher(m_queue, m_newContactReductionKernel,"m_newContactReductionKernel");
+                        launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+                        launcher.setConst(vertexFaceCapacity);
+                        launcher.setConst( numConcavePairs  );
+                        int num = numConcavePairs;
+                        
+                        launcher.launch1D( num);
+                    }
+                    nContacts = m_totalContactsOut.at(0);
+                    contactOut->resize(nContacts);
+                    
+				}
+				//re-use?
+
+
+			} else
+			{
+				B3_PROFILE("clipHullHullConcaveConvexKernel");
+				nContacts = m_totalContactsOut.at(0);
+				b3BufferInfoCL bInfo[] = { 
+					b3BufferInfoCL( triangleConvexPairsOut.getBufferCL(), true ), 
+					b3BufferInfoCL( bodyBuf->getBufferCL(),true), 
+					b3BufferInfoCL( gpuCollidables.getBufferCL(),true), 
+					b3BufferInfoCL( convexData.getBufferCL(),true),
+					b3BufferInfoCL( gpuVertices.getBufferCL(),true),
+					b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
+					b3BufferInfoCL( gpuFaces.getBufferCL(),true),
+					b3BufferInfoCL( gpuIndices.getBufferCL(),true),
+					b3BufferInfoCL( gpuChildShapes.getBufferCL(),true),
+					b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
+					b3BufferInfoCL( contactOut->getBufferCL()),
+					b3BufferInfoCL( m_totalContactsOut.getBufferCL())	
+				};
+				b3LauncherCL launcher(m_queue, m_clipHullHullConcaveConvexKernel,"m_clipHullHullConcaveConvexKernel");
+				launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
+				launcher.setConst( numConcavePairs  );
+				int num = numConcavePairs;
+				launcher.launch1D( num);
+				clFinish(m_queue);
+				nContacts = m_totalContactsOut.at(0);
+				contactOut->resize(nContacts);
+				b3AlignedObjectArray<b3Contact4> cpuContacts;
+				contactOut->copyToHost(cpuContacts);
+			}
 //			printf("nContacts after = %d\n", nContacts);
 		}
 
@@ -3436,24 +3699,12 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 
 
 			
-            int vertexFaceCapacity = 64;
-            
             
             worldVertsB1GPU.resize(vertexFaceCapacity*nPairs);
-            
-            
             clippingFacesOutGPU.resize(nPairs);
-            
-            
             worldNormalsAGPU.resize(nPairs);
-            
-            
             worldVertsA1GPU.resize(vertexFaceCapacity*nPairs);
-            
-             
             worldVertsB2GPU.resize(vertexFaceCapacity*nPairs);
-        
-            
             
             {
 				B3_PROFILE("findClippingFacesKernel");
@@ -3491,13 +3742,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
             ///clip face B against face A, reduce contacts and append them to a global contact array
             if (1)
             {
-				B3_PROFILE("clipFacesAndContactReductionKernel");
+				B3_PROFILE("clipFacesAndFindContacts");
 				//nContacts = m_totalContactsOut.at(0);
 				//int h = m_hasSeparatingNormals.at(0);
 				//int4 p = clippingFacesOutGPU.at(0);
                 b3BufferInfoCL bInfo[] = {
-                    b3BufferInfoCL( pairs->getBufferCL(), true ),
-                    b3BufferInfoCL( bodyBuf->getBufferCL(),true),
                     b3BufferInfoCL( m_sepNormals.getBufferCL()),
                     b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()),
 					b3BufferInfoCL( contactOut->getBufferCL()),
@@ -3516,23 +3765,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 				launcher.setConst( nPairs  );
                 int debugMode = 0;
 				launcher.setConst( debugMode);
-
-				/*
-				int serializationBytes = launcher.getSerializationBufferSize();
-				unsigned char* buf = (unsigned char*)malloc(serializationBytes+1);
-				int actualWritten = launcher.serializeArguments(buf,serializationBytes+1);
-				FILE* f = fopen("clipFacesAndContactReductionKernel.bin","wb");
-				fwrite(buf,actualWritten,1,f);
-				fclose(f);
-				free(buf);
-				printf("serializationBytes=%d, actualWritten=%d\n",serializationBytes,actualWritten);
-				*/
-
-                int num = nPairs;
-
-                launcher.launch1D( num);
+				int num = nPairs;
+				launcher.launch1D( num);
                 clFinish(m_queue);
-                {
+               
+				{
 //                    nContacts = m_totalContactsOut.at(0);
   //                  printf("nContacts = %d\n",nContacts);
                     
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h
index 0e8a7d3d4..9d11b50ab 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h
@@ -52,6 +52,7 @@ struct GpuSatCollision
 	b3OpenCLArray<b3Vector3> m_sepNormals;
 	b3OpenCLArray<int>		m_hasSeparatingNormals;
 	b3OpenCLArray<b3Vector3> m_concaveSepNormals;
+	b3OpenCLArray<int>		m_concaveHasSeparatingNormals;
 	b3OpenCLArray<int>		m_numConcavePairsOut;
 	b3OpenCLArray<b3CompoundOverlappingPair> m_gpuCompoundPairs;
 	b3OpenCLArray<b3Vector3> m_gpuCompoundSepNormals;
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h
index 2292ee834..629a0fce7 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h
@@ -41,6 +41,9 @@ class b3Serializer;
 #define b3QuantizedBvhDataName "b3QuantizedBvhFloatData"
 #endif
 
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+
 
 
 //http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
@@ -55,16 +58,10 @@ class b3Serializer;
 
 ///b3QuantizedBvhNode is a compressed aabb node, 16 bytes.
 ///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
-B3_ATTRIBUTE_ALIGNED16	(struct) b3QuantizedBvhNode
+B3_ATTRIBUTE_ALIGNED16	(struct) b3QuantizedBvhNode : public b3QuantizedBvhNodeData
 {
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	//12 bytes
-	unsigned short int	m_quantizedAabbMin[3];
-	unsigned short int	m_quantizedAabbMax[3];
-	//4 bytes
-	int	m_escapeIndexOrTriangleIndex;
-
 	bool isLeafNode() const
 	{
 		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
@@ -116,20 +113,11 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3OptimizedBvhNode
 
 
 ///b3BvhSubtreeInfo provides info to gather a subtree of limited size
-B3_ATTRIBUTE_ALIGNED16(class) b3BvhSubtreeInfo
+B3_ATTRIBUTE_ALIGNED16(class) b3BvhSubtreeInfo : public b3BvhSubtreeInfoData
 {
 public:
 	B3_DECLARE_ALIGNED_ALLOCATOR();
 
-	//12 bytes
-	unsigned short int	m_quantizedAabbMin[3];
-	unsigned short int	m_quantizedAabbMax[3];
-	//4 bytes, points to the root of the subtree
-	int			m_rootNodeIndex;
-	//4 bytes
-	int			m_subtreeSize;
-	int			m_padding[3];
-
 	b3BvhSubtreeInfo()
 	{
 		//memset(&m_padding[0], 0, sizeof(m_padding));
@@ -501,14 +489,6 @@ private:
 ;
 
 
-struct	b3BvhSubtreeInfoData
-{
-	int			m_rootNodeIndex;
-	int			m_subtreeSize;
-	unsigned short m_quantizedAabbMin[3];
-	unsigned short m_quantizedAabbMax[3];
-};
-
 struct b3OptimizedBvhNodeFloatData
 {
 	b3Vector3FloatData	m_aabbMinOrg;
@@ -530,12 +510,6 @@ struct b3OptimizedBvhNodeDoubleData
 };
 
 
-struct b3QuantizedBvhNodeData
-{
-	unsigned short m_quantizedAabbMin[3];
-	unsigned short m_quantizedAabbMax[3];
-	int	m_escapeIndexOrTriangleIndex;
-};
 
 struct	b3QuantizedBvhFloatData
 {
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl
index adc2b5d8c..faa413441 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.cl
@@ -34,33 +34,6 @@ typedef struct
 
 } b3BvhInfo;
 
-/*
-	bool isLeafNode() const
-	{
-		//skipindex is negative (internal node), triangleindex >=0 (leafnode)
-		return (m_escapeIndexOrTriangleIndex >= 0);
-	}
-	int getEscapeIndex() const
-	{
-		btAssert(!isLeafNode());
-		return -m_escapeIndexOrTriangleIndex;
-	}
-	int	getTriangleIndex() const
-	{
-		btAssert(isLeafNode());
-		unsigned int x=0;
-		unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
-		// Get only the lower bits where the triangle index is stored
-		return (m_escapeIndexOrTriangleIndex&~(y));
-	}
-	int	getPartId() const
-	{
-		btAssert(isLeafNode());
-		// Get only the highest bits where the part index is stored
-		return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
-	}
-*/
-
 int	getTriangleIndex(const btQuantizedBvhNode* rootNode)
 {
 	unsigned int x=0;
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.h
index d51084e41..4b3b49eae 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/bvhTraversal.h
@@ -29,32 +29,6 @@ static const char* bvhTraversalKernelCL= \
 "	int			m_nodeOffset;\n"
 "	int			m_subTreeOffset;\n"
 "} b3BvhInfo;\n"
-"/*\n"
-"	bool isLeafNode() const\n"
-"	{\n"
-"		//skipindex is negative (internal node), triangleindex >=0 (leafnode)\n"
-"		return (m_escapeIndexOrTriangleIndex >= 0);\n"
-"	}\n"
-"	int getEscapeIndex() const\n"
-"	{\n"
-"		btAssert(!isLeafNode());\n"
-"		return -m_escapeIndexOrTriangleIndex;\n"
-"	}\n"
-"	int	getTriangleIndex() const\n"
-"	{\n"
-"		btAssert(isLeafNode());\n"
-"		unsigned int x=0;\n"
-"		unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);\n"
-"		// Get only the lower bits where the triangle index is stored\n"
-"		return (m_escapeIndexOrTriangleIndex&~(y));\n"
-"	}\n"
-"	int	getPartId() const\n"
-"	{\n"
-"		btAssert(isLeafNode());\n"
-"		// Get only the highest bits where the part index is stored\n"
-"		return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));\n"
-"	}\n"
-"*/\n"
 "int	getTriangleIndex(const btQuantizedBvhNode* rootNode)\n"
 "{\n"
 "	unsigned int x=0;\n"
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h
index 03f0480d1..a282d1eff 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h
@@ -13,6 +13,7 @@ static const char* primitiveContactsKernelsCL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -36,6 +37,9 @@ static const char* primitiveContactsKernelsCL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl
index df6938dd8..e94accf7c 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl
@@ -401,7 +401,7 @@ bool findSeparatingAxisLocalA(	const ConvexPolyhedronCL* hullA, __global const C
 	float4* sep,
 	float* dmin)
 {
-	int i = get_global_id(0);
+	
 
 	float4 posA = posA1;
 	posA.w = 0.f;
@@ -452,7 +452,7 @@ bool findSeparatingAxisLocalB(	__global const ConvexPolyhedronCL* hullA,  const
 	float4* sep,
 	float* dmin)
 {
-	int i = get_global_id(0);
+
 
 	float4 posA = posA1;
 	posA.w = 0.f;
@@ -505,7 +505,7 @@ bool findSeparatingAxisEdgeEdgeLocalA(	const ConvexPolyhedronCL* hullA, __global
 		float4* sep,
 	float* dmin)
 {
-	int i = get_global_id(0);
+
 
 	float4 posA = posA1;
 	posA.w = 0.f;
@@ -607,7 +607,7 @@ bool findSeparatingAxis(	__global const ConvexPolyhedronCL* hullA, __global cons
 	float4* sep,
 	float* dmin)
 {
-	int i = get_global_id(0);
+	
 
 	float4 posA = posA1;
 	posA.w = 0.f;
@@ -666,7 +666,7 @@ bool findSeparatingAxisEdgeEdge(	__global const ConvexPolyhedronCL* hullA, __glo
 	float4* sep,
 	float* dmin)
 {
-	int i = get_global_id(0);
+	
 
 	float4 posA = posA1;
 	posA.w = 0.f;
@@ -1353,6 +1353,97 @@ __kernel void   findSeparatingAxisKernel( __global const int4* pairs,
 
 
 
+
+int	findClippingFaces(const float4 separatingNormal,
+                      const ConvexPolyhedronCL* hullA, 
+					  __global const ConvexPolyhedronCL* hullB,
+                      const float4 posA, const Quaternion ornA,const float4 posB, const Quaternion ornB,
+                       __global float4* worldVertsA1,
+                      __global float4* worldNormalsA1,
+                      __global float4* worldVertsB1,
+                      int capacityWorldVerts,
+                      const float minDist, float maxDist,
+					  const float4* verticesA,
+                      const btGpuFace* facesA,
+                      const int* indicesA,
+					  __global const float4* verticesB,
+                      __global const btGpuFace* facesB,
+                      __global const int* indicesB,
+                      __global int4* clippingFaces, int pairIndex)
+{
+	int numContactsOut = 0;
+	int numWorldVertsB1= 0;
+    
+    
+	int closestFaceB=-1;
+	float dmax = -FLT_MAX;
+    
+	{
+		for(int face=0;face<hullB->m_numFaces;face++)
+		{
+			const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x,
+                                              facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);
+			const float4 WorldNormal = qtRotate(ornB, Normal);
+			float d = dot3F4(WorldNormal,separatingNormal);
+			if (d > dmax)
+			{
+				dmax = d;
+				closestFaceB = face;
+			}
+		}
+	}
+    
+	{
+		const btGpuFace polyB = facesB[hullB->m_faceOffset+closestFaceB];
+		const int numVertices = polyB.m_numIndices;
+		for(int e0=0;e0<numVertices;e0++)
+		{
+			const float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
+			worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB);
+		}
+	}
+    
+    int closestFaceA=-1;
+	{
+		float dmin = FLT_MAX;
+		for(int face=0;face<hullA->m_numFaces;face++)
+		{
+			const float4 Normal = make_float4(
+                                              facesA[hullA->m_faceOffset+face].m_plane.x,
+                                              facesA[hullA->m_faceOffset+face].m_plane.y,
+                                              facesA[hullA->m_faceOffset+face].m_plane.z,
+                                              0.f);
+			const float4 faceANormalWS = qtRotate(ornA,Normal);
+            
+			float d = dot3F4(faceANormalWS,separatingNormal);
+			if (d < dmin)
+			{
+				dmin = d;
+				closestFaceA = face;
+                worldNormalsA1[pairIndex] = faceANormalWS;
+			}
+		}
+	}
+    
+    int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;
+	for(int e0=0;e0<numVerticesA;e0++)
+	{
+        const float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];
+        worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA);
+    }
+    
+    clippingFaces[pairIndex].x = closestFaceA;
+    clippingFaces[pairIndex].y = closestFaceB;
+    clippingFaces[pairIndex].z = numVerticesA;
+    clippingFaces[pairIndex].w = numWorldVertsB1;
+    
+    
+	return numContactsOut;
+}
+
+
+
+
 // work-in-progress
 __kernel void   findConcaveSeparatingAxisKernel( __global int4* concavePairs,
 																					__global const BodyData* rigidBodies,
@@ -1365,6 +1456,12 @@ __kernel void   findConcaveSeparatingAxisKernel( __global int4* concavePairs,
 																					__global const btGpuChildShape* gpuChildShapes,
 																					__global btAabbCL* aabbs,
 																					__global float4* concaveSeparatingNormalsOut,
+																					__global int* concaveHasSeparatingNormals,
+																					__global int4* clippingFacesOut,
+																					__global float4* worldVertsA1GPU,
+																					__global float4*  worldNormalsAGPU,
+																					__global float4* worldVertsB1GPU,
+																					int vertexFaceCapacity,
 																					int numConcavePairs
 																					)
 {
@@ -1372,6 +1469,9 @@ __kernel void   findConcaveSeparatingAxisKernel( __global int4* concavePairs,
 	int i = get_global_id(0);
 	if (i>=numConcavePairs)
 		return;
+
+	concaveHasSeparatingNormals[i] = 0;
+
 	int pairIdx = i;
 
 	int bodyIndexA = concavePairs[i].x;
@@ -1604,6 +1704,33 @@ __kernel void   findConcaveSeparatingAxisKernel( __global int4* concavePairs,
 		{
 			sepAxis.w = dmin;
 			concaveSeparatingNormalsOut[pairIdx]=sepAxis;
+			concaveHasSeparatingNormals[i]=1;
+
+
+			float minDist = -1e30f;
+			float maxDist = 0.02f;
+
+		
+
+			findClippingFaces(sepAxis,
+                     &convexPolyhedronA,
+					 &convexShapes[shapeIndexB],
+					 posA,ornA,
+					 posB,ornB,
+                      worldVertsA1GPU,
+                      worldNormalsAGPU,
+                      worldVertsB1GPU,
+					  vertexFaceCapacity,
+                      minDist, maxDist,
+                      verticesA,
+                      facesA,
+                      indicesA,
+ 					  vertices,
+                      faces,
+                      indices,
+                      clippingFacesOut, pairIdx);
+
+
 		} else
 		{	
 			//mark this pair as in-active
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl
index aa4918b9a..98253c95c 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl
@@ -1669,9 +1669,7 @@ __kernel void   findClippingFacesKernel(  __global const int4* pairs,
 
 
 
-__kernel void   clipFacesAndFindContactsKernel( __global int4* pairs,
-                                                   __global const b3RigidBodyData_t* rigidBodies,
-                                                   __global const float4* separatingNormals,
+__kernel void   clipFacesAndFindContactsKernel(    __global const float4* separatingNormals,
                                                    __global const int* hasSeparatingAxis,
                                                      __global struct b3Contact4Data* globalContactsOut,
                                                    __global int4* clippingFacesOut,
@@ -1698,8 +1696,8 @@ __kernel void   clipFacesAndFindContactsKernel( __global int4* pairs,
 		if (hasSeparatingAxis[i])
 		{
             
-			int bodyIndexA = pairs[i].x;
-			int bodyIndexB = pairs[i].y;
+//			int bodyIndexA = pairs[i].x;
+	//		int bodyIndexB = pairs[i].y;
 		    
             int numLocalContactsOut = 0;
 
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h
index 83a24bdf7..cbf9bce24 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h
@@ -40,6 +40,7 @@ static const char* satClipKernelsCL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -63,6 +64,9 @@ static const char* satClipKernelsCL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
@@ -1859,9 +1863,7 @@ static const char* satClipKernelsCL= \
 "	}//	if (i<numPairs)\n"
 "    \n"
 "}\n"
-"__kernel void   clipFacesAndFindContactsKernel( __global int4* pairs,\n"
-"                                                   __global const b3RigidBodyData_t* rigidBodies,\n"
-"                                                   __global const float4* separatingNormals,\n"
+"__kernel void   clipFacesAndFindContactsKernel(    __global const float4* separatingNormals,\n"
 "                                                   __global const int* hasSeparatingAxis,\n"
 "                                                     __global struct b3Contact4Data* globalContactsOut,\n"
 "                                                   __global int4* clippingFacesOut,\n"
@@ -1888,8 +1890,8 @@ static const char* satClipKernelsCL= \
 "		if (hasSeparatingAxis[i])\n"
 "		{\n"
 "            \n"
-"			int bodyIndexA = pairs[i].x;\n"
-"			int bodyIndexB = pairs[i].y;\n"
+"//			int bodyIndexA = pairs[i].x;\n"
+"	//		int bodyIndexB = pairs[i].y;\n"
 "		    \n"
 "            int numLocalContactsOut = 0;\n"
 "            int capacityWorldVertsB2 = vertexFaceCapacity;\n"
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h
index 9ff2f67ea..16510a836 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/satKernels.h
@@ -154,6 +154,7 @@ static const char* satKernelsCL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -177,6 +178,9 @@ static const char* satKernelsCL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
@@ -633,7 +637,7 @@ static const char* satKernelsCL= \
 "	float4* sep,\n"
 "	float* dmin)\n"
 "{\n"
-"	int i = get_global_id(0);\n"
+"	\n"
 "	float4 posA = posA1;\n"
 "	posA.w = 0.f;\n"
 "	float4 posB = posB1;\n"
@@ -682,7 +686,6 @@ static const char* satKernelsCL= \
 "	float4* sep,\n"
 "	float* dmin)\n"
 "{\n"
-"	int i = get_global_id(0);\n"
 "	float4 posA = posA1;\n"
 "	posA.w = 0.f;\n"
 "	float4 posB = posB1;\n"
@@ -731,7 +734,6 @@ static const char* satKernelsCL= \
 "		float4* sep,\n"
 "	float* dmin)\n"
 "{\n"
-"	int i = get_global_id(0);\n"
 "	float4 posA = posA1;\n"
 "	posA.w = 0.f;\n"
 "	float4 posB = posB1;\n"
@@ -815,7 +817,7 @@ static const char* satKernelsCL= \
 "	float4* sep,\n"
 "	float* dmin)\n"
 "{\n"
-"	int i = get_global_id(0);\n"
+"	\n"
 "	float4 posA = posA1;\n"
 "	posA.w = 0.f;\n"
 "	float4 posB = posB1;\n"
@@ -866,7 +868,7 @@ static const char* satKernelsCL= \
 "	float4* sep,\n"
 "	float* dmin)\n"
 "{\n"
-"	int i = get_global_id(0);\n"
+"	\n"
 "	float4 posA = posA1;\n"
 "	posA.w = 0.f;\n"
 "	float4 posB = posB1;\n"
@@ -1470,6 +1472,92 @@ static const char* satKernelsCL= \
 "		\n"
 "	}\n"
 "}\n"
+"int	findClippingFaces(const float4 separatingNormal,\n"
+"                      const ConvexPolyhedronCL* hullA, \n"
+"					  __global const ConvexPolyhedronCL* hullB,\n"
+"                      const float4 posA, const Quaternion ornA,const float4 posB, const Quaternion ornB,\n"
+"                       __global float4* worldVertsA1,\n"
+"                      __global float4* worldNormalsA1,\n"
+"                      __global float4* worldVertsB1,\n"
+"                      int capacityWorldVerts,\n"
+"                      const float minDist, float maxDist,\n"
+"					  const float4* verticesA,\n"
+"                      const btGpuFace* facesA,\n"
+"                      const int* indicesA,\n"
+"					  __global const float4* verticesB,\n"
+"                      __global const btGpuFace* facesB,\n"
+"                      __global const int* indicesB,\n"
+"                      __global int4* clippingFaces, int pairIndex)\n"
+"{\n"
+"	int numContactsOut = 0;\n"
+"	int numWorldVertsB1= 0;\n"
+"    \n"
+"    \n"
+"	int closestFaceB=-1;\n"
+"	float dmax = -FLT_MAX;\n"
+"    \n"
+"	{\n"
+"		for(int face=0;face<hullB->m_numFaces;face++)\n"
+"		{\n"
+"			const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x,\n"
+"                                              facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);\n"
+"			const float4 WorldNormal = qtRotate(ornB, Normal);\n"
+"			float d = dot3F4(WorldNormal,separatingNormal);\n"
+"			if (d > dmax)\n"
+"			{\n"
+"				dmax = d;\n"
+"				closestFaceB = face;\n"
+"			}\n"
+"		}\n"
+"	}\n"
+"    \n"
+"	{\n"
+"		const btGpuFace polyB = facesB[hullB->m_faceOffset+closestFaceB];\n"
+"		const int numVertices = polyB.m_numIndices;\n"
+"		for(int e0=0;e0<numVertices;e0++)\n"
+"		{\n"
+"			const float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];\n"
+"			worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB);\n"
+"		}\n"
+"	}\n"
+"    \n"
+"    int closestFaceA=-1;\n"
+"	{\n"
+"		float dmin = FLT_MAX;\n"
+"		for(int face=0;face<hullA->m_numFaces;face++)\n"
+"		{\n"
+"			const float4 Normal = make_float4(\n"
+"                                              facesA[hullA->m_faceOffset+face].m_plane.x,\n"
+"                                              facesA[hullA->m_faceOffset+face].m_plane.y,\n"
+"                                              facesA[hullA->m_faceOffset+face].m_plane.z,\n"
+"                                              0.f);\n"
+"			const float4 faceANormalWS = qtRotate(ornA,Normal);\n"
+"            \n"
+"			float d = dot3F4(faceANormalWS,separatingNormal);\n"
+"			if (d < dmin)\n"
+"			{\n"
+"				dmin = d;\n"
+"				closestFaceA = face;\n"
+"                worldNormalsA1[pairIndex] = faceANormalWS;\n"
+"			}\n"
+"		}\n"
+"	}\n"
+"    \n"
+"    int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;\n"
+"	for(int e0=0;e0<numVerticesA;e0++)\n"
+"	{\n"
+"        const float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];\n"
+"        worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA);\n"
+"    }\n"
+"    \n"
+"    clippingFaces[pairIndex].x = closestFaceA;\n"
+"    clippingFaces[pairIndex].y = closestFaceB;\n"
+"    clippingFaces[pairIndex].z = numVerticesA;\n"
+"    clippingFaces[pairIndex].w = numWorldVertsB1;\n"
+"    \n"
+"    \n"
+"	return numContactsOut;\n"
+"}\n"
 "// work-in-progress\n"
 "__kernel void   findConcaveSeparatingAxisKernel( __global int4* concavePairs,\n"
 "																					__global const BodyData* rigidBodies,\n"
@@ -1482,12 +1570,19 @@ static const char* satKernelsCL= \
 "																					__global const btGpuChildShape* gpuChildShapes,\n"
 "																					__global btAabbCL* aabbs,\n"
 "																					__global float4* concaveSeparatingNormalsOut,\n"
+"																					__global int* concaveHasSeparatingNormals,\n"
+"																					__global int4* clippingFacesOut,\n"
+"																					__global float4* worldVertsA1GPU,\n"
+"																					__global float4*  worldNormalsAGPU,\n"
+"																					__global float4* worldVertsB1GPU,\n"
+"																					int vertexFaceCapacity,\n"
 "																					int numConcavePairs\n"
 "																					)\n"
 "{\n"
 "	int i = get_global_id(0);\n"
 "	if (i>=numConcavePairs)\n"
 "		return;\n"
+"	concaveHasSeparatingNormals[i] = 0;\n"
 "	int pairIdx = i;\n"
 "	int bodyIndexA = concavePairs[i].x;\n"
 "	int bodyIndexB = concavePairs[i].y;\n"
@@ -1691,6 +1786,27 @@ static const char* satKernelsCL= \
 "		{\n"
 "			sepAxis.w = dmin;\n"
 "			concaveSeparatingNormalsOut[pairIdx]=sepAxis;\n"
+"			concaveHasSeparatingNormals[i]=1;\n"
+"			float minDist = -1e30f;\n"
+"			float maxDist = 0.02f;\n"
+"		\n"
+"			findClippingFaces(sepAxis,\n"
+"                     &convexPolyhedronA,\n"
+"					 &convexShapes[shapeIndexB],\n"
+"					 posA,ornA,\n"
+"					 posB,ornB,\n"
+"                      worldVertsA1GPU,\n"
+"                      worldNormalsAGPU,\n"
+"                      worldVertsB1GPU,\n"
+"					  vertexFaceCapacity,\n"
+"                      minDist, maxDist,\n"
+"                      verticesA,\n"
+"                      facesA,\n"
+"                      indicesA,\n"
+" 					  vertices,\n"
+"                      faces,\n"
+"                      indices,\n"
+"                      clippingFacesOut, pairIdx);\n"
 "		} else\n"
 "		{	\n"
 "			//mark this pair as in-active\n"
diff --git a/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp b/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp
index ae53d25f7..5306a5ad9 100644
--- a/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp
+++ b/src/Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.cpp
@@ -27,7 +27,7 @@ b3LauncherCL::~b3LauncherCL()
 	  if (gDebugLauncherCL)
 	  {
 		static int counter = 0;
-		printf("[%d] Finished launching OpenCL kernel %s [%d]\n", counter++,m_name);
+		printf("[%d] Finished launching OpenCL kernel %s\n", counter++,m_name);
 	  }
   }
 
diff --git a/src/Bullet3OpenCL/RigidBody/b3Solver.cpp b/src/Bullet3OpenCL/RigidBody/b3Solver.cpp
index b2bb087b4..de4aa794a 100644
--- a/src/Bullet3OpenCL/RigidBody/b3Solver.cpp
+++ b/src/Bullet3OpenCL/RigidBody/b3Solver.cpp
@@ -124,7 +124,7 @@ b3Solver::b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue,
 	
 	{
 		
-		cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, 0, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH,false);
+		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);
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h b/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h
index 2b910591a..6c839074b 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h
@@ -25,6 +25,7 @@ static const char* batchingKernelsCL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -48,6 +49,9 @@ static const char* batchingKernelsCL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h b/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h
index 91c4d828b..4daf95380 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h
@@ -25,6 +25,7 @@ static const char* batchingKernelsNewCL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -48,6 +49,9 @@ static const char* batchingKernelsNewCL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h b/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h
index 98479bda9..1146f0e57 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/integrateKernel.h
@@ -25,6 +25,7 @@ static const char* integrateKernelCL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -48,6 +49,9 @@ static const char* integrateKernelCL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h
index d854dfe97..7f125298b 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h
@@ -25,6 +25,7 @@ static const char* solverSetupCL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -48,6 +49,9 @@ static const char* solverSetupCL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h
index c16c71685..d3c905995 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h
@@ -25,6 +25,7 @@ static const char* solverSetup2CL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -48,6 +49,9 @@ static const char* solverSetup2CL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h
index 2476d1cab..e70b44373 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h
@@ -25,6 +25,7 @@ static const char* solverUtilsCL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -48,6 +49,9 @@ static const char* solverUtilsCL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
diff --git a/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h b/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h
index 05a557f0e..01d6f8b45 100644
--- a/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h
+++ b/src/Bullet3OpenCL/RigidBody/kernels/updateAabbsKernel.h
@@ -15,6 +15,7 @@ static const char* updateAabbsKernelCL= \
 "#ifdef __cplusplus\n"
 "#else\n"
 "#define b3AtomicInc atomic_inc\n"
+"#define b3AtomicAdd atomic_add\n"
 "#define b3Fabs fabs\n"
 "#define b3Sqrt native_sqrt\n"
 "#define b3Sin native_sin\n"
@@ -38,6 +39,9 @@ static const char* updateAabbsKernelCL= \
 "		float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
 "		return cross(a1, b1);\n"
 "	}\n"
+"	#define b3MinFloat4 min\n"
+"	#define b3MaxFloat4 max\n"
+"	#define b3Normalized(a) normalize(a)\n"
 "#endif \n"
 "		\n"
 "inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"