From 9379c7fb8c7bffc4f63537a66cfdad5a773f73ad Mon Sep 17 00:00:00 2001
From: erwin coumans <erwin.coumans@gmail.com>
Date: Sat, 7 Dec 2013 11:19:57 -0800
Subject: [PATCH 1/4] enable mouse picking for BasicDemo (will move it to a
 shared code part soon)

---
 Demos3/bullet2/BasicDemo/main.cpp | 199 +++++++++++++++++++++++++++++-
 1 file changed, 197 insertions(+), 2 deletions(-)

diff --git a/Demos3/bullet2/BasicDemo/main.cpp b/Demos3/bullet2/BasicDemo/main.cpp
index 8d90b8416..c9265f958 100644
--- a/Demos3/bullet2/BasicDemo/main.cpp
+++ b/Demos3/bullet2/BasicDemo/main.cpp
@@ -66,9 +66,19 @@ class BasicDemo : public Bullet2RigidBodyDemo
 {
 	SimpleOpenGL3App* m_glApp;
 
+	btRigidBody*	m_pickedBody;
+	btTypedConstraint* m_pickedConstraint;
+	btVector3 m_oldPickingPos;
+	btVector3 m_hitPos;
+	btScalar m_oldPickingDist;
+
+
+
 public:
 	BasicDemo(SimpleOpenGL3App* app)
-	:m_glApp(app)
+	:m_glApp(app),
+	m_pickedBody(0),
+	m_pickedConstraint(0)
 	{
 	}
 	virtual ~BasicDemo()
@@ -141,7 +151,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 +189,193 @@ 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);
+}
+
+
 int main(int argc, char* argv[])
 {
 	
@@ -201,8 +392,12 @@ 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);
+
 	BasicDemo* demo = new BasicDemo(app);
 	demo->initPhysics();
+	sDemo = demo;
 
 	GLint err = glGetError();
     assert(err==GL_NO_ERROR);

From ba2ba28a891d49d58a3659a87e2e08fdead7e218 Mon Sep 17 00:00:00 2001
From: erwin coumans <erwin.coumans@gmail.com>
Date: Mon, 9 Dec 2013 14:16:51 -0800
Subject: [PATCH 2/4] add Bullet 2.x CPU FeatherstoneMultiBodyDemo

---
 .../FeatherstoneMultiBodyDemo/main.cpp        | 742 ++++++++++++++++++
 .../FeatherstoneMultiBodyDemo/premake4.lua    |  38 +
 btgui/OpenGLWindow/GLInstancingRenderer.cpp   |   4 +-
 build3/premake4.lua                           |   1 +
 4 files changed, 784 insertions(+), 1 deletion(-)
 create mode 100644 Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp
 create mode 100644 Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua

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"

From c155e126d0b647f9887ad9f9f389a3033957719d Mon Sep 17 00:00:00 2001
From: erwin coumans <erwin.coumans@gmail.com>
Date: Thu, 12 Dec 2013 11:03:55 -0800
Subject: [PATCH 3/4] move parts of collision pipeline to shared header files
 (work-in-progress)

---
 Demos3/GpuDemos/GpuDemo.h                     |   6 +-
 Demos3/bullet2/BasicDemo/main.cpp             |  27 +-
 .../shared/b3BvhSubtreeInfoData.h             |  20 +
 .../shared/b3BvhTraversal.h                   | 126 +++++
 .../shared/b3CollidableData.h                 |   0
 .../shared/b3FindConcaveSatAxis.h             | 474 ++++++++++++++++++
 .../shared/b3QuantizedBvhNodeData.h           |  90 ++++
 src/Bullet3Common/shared/b3Float4.h           |  23 +
 src/Bullet3Common/shared/b3Int4.h             |  13 +
 .../shared/b3PlatformDefinitions.h            |  10 +
 src/Bullet3Common/shared/b3Quat.h             |   4 +
 .../b3CpuRigidBodyPipeline.cpp                |   2 +-
 .../b3ConvexHullContact.cpp                   | 219 ++++++--
 .../NarrowphaseCollision/b3QuantizedBvh.h     |  36 +-
 .../kernels/bvhTraversal.cl                   |  27 -
 .../NarrowphaseCollision/kernels/sat.cl       |  10 +-
 .../ParallelPrimitives/b3LauncherCL.cpp       |   2 +-
 17 files changed, 969 insertions(+), 120 deletions(-)
 create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
 create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h
 delete mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3CollidableData.h
 create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
 create mode 100644 src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h

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/bullet2/BasicDemo/main.cpp b/Demos3/bullet2/BasicDemo/main.cpp
index c9265f958..3c9f77f45 100644
--- a/Demos3/bullet2/BasicDemo/main.cpp
+++ b/Demos3/bullet2/BasicDemo/main.cpp
@@ -64,7 +64,6 @@ public:
 
 class BasicDemo : public Bullet2RigidBodyDemo
 {
-	SimpleOpenGL3App* m_glApp;
 
 	btRigidBody*	m_pickedBody;
 	btTypedConstraint* m_pickedConstraint;
@@ -75,6 +74,9 @@ class BasicDemo : public Bullet2RigidBodyDemo
 
 
 public:
+	SimpleOpenGL3App* m_glApp;
+
+
 	BasicDemo(SimpleOpenGL3App* app)
 	:m_glApp(app),
 	m_pickedBody(0),
@@ -375,6 +377,28 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
 		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[])
 {
@@ -394,6 +418,7 @@ int main(int argc, char* argv[])
 
 	app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
 	app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
+	app->m_window->setKeyboardCallback(MyKeyboardCallback);
 
 	BasicDemo* demo = new BasicDemo(app);
 	demo->initPhysics();
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..88301aa03
--- /dev/null
+++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
@@ -0,0 +1,474 @@
+#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;
+}
+
+// work-in-progress
+__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,
+																					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;
+	}
+
+
+
+	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)
+		{
+			sepAxis.w = dmin;
+			concaveSeparatingNormalsOut[pairIdx]=sepAxis;
+		} 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..0fdc65fe3 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 = false;//true;
 
 ///This file was written by Erwin Coumans
 ///Separating axis rest based on work from Pierre Terdiman, see
@@ -20,7 +24,7 @@ subject to the following restrictions:
 
 //#define B3_DEBUG_SAT_FACE
 
-//#define CHECK_ON_HOST
+#define CHECK_ON_HOST
 
 #ifdef CHECK_ON_HOST
 //#define PERSISTENT_CONTACTS_HOST
@@ -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 )
@@ -1197,7 +1206,7 @@ int clipHullHullSingle(
 			int numPoints = 0;
 					
 			{
-				B3_PROFILE("extractManifold");
+			//	B3_PROFILE("extractManifold");
 				numPoints = extractManifold(contactsOut, numContactsOut, normalOnSurfaceB,  &contactIdx);
 			}
 					
@@ -2723,6 +2732,9 @@ int computeContactConvexConvex2(
 }
 
 
+
+																
+																
 void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>* pairs, int nPairs,
 			const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
 			b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
@@ -2898,15 +2910,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 +2942,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 		contactOut->resize(0);
 		}
 
-	return;
+		m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true);
+
 #else
 
 	{
@@ -2996,7 +3007,6 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 
 	int numCompoundPairs = 0;
 
-	bool findSeparatingAxisOnGpu = true;//false;
 	int numConcavePairs =0;
 
 	{
@@ -3038,65 +3048,172 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 				
 					if (treeNodesGPU->size() && treeNodesGPU->size())
 					{
-						B3_PROFILE("m_bvhTraversalKernel");
+						if (bvhTraversalKernelGPU)
+						{
+						
+							B3_PROFILE("m_bvhTraversalKernel");
 						
 						
-						numConcavePairs = m_numConcavePairsOut.at(0);
+							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());
+							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);
+							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("Rxceeded %d times the maxTriConvexPairCapacity (found %d but max is %d)\n", exceeded_maxTriConvexPairCapacity_count++,
-								numConcavePairs,maxTriConvexPairCapacity);
+							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)
 						{
-							//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())
-							};
+							if (findConcaveSeparatingAxisKernelGPU)
+							{
+								//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) );
+								b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisKernel,"m_findConcaveSeparatingAxisKernel");
+								launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
 
-							launcher.setConst( numConcavePairs  );
+								launcher.setConst( numConcavePairs  );
 
-							int num = numConcavePairs;
-							launcher.launch1D( num);
-							clFinish(m_queue);
+								int num = numConcavePairs;
+								launcher.launch1D( num);
+								clFinish(m_queue);
+							} else
+							{
 
+									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);
+
+
+									//numConcavePairs
+									//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())
+
+									b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
+									m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
+							}
 //							b3AlignedObjectArray<b3Vector3> cpuCompoundSepNormals;
 	//						m_concaveSepNormals.copyToHost(cpuCompoundSepNormals);
 		//					b3AlignedObjectArray<b3Int4> cpuConcavePairs;
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/sat.cl b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl
index df6938dd8..c9d00b5ad 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;
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);
 	  }
   }
 

From 3fe969c4ee17f59d8c8bcc2201fd3637bb052e92 Mon Sep 17 00:00:00 2001
From: erwin coumans <erwin.coumans@gmail.com>
Date: Fri, 13 Dec 2013 07:52:41 -0800
Subject: [PATCH 4/4] b3Solver -> pass pointer to source instead of 0 (was left
 over from a debugging session), thanks to David for the report Break up
 clipHullHullConcaveConvexKernel into multiple stages, so it might 'fit' in
 Apple's OpenCL implementation Implemented bvhTraversalKernel and
 findConcaveSeparatingAxis on CPU (debugging, possible future CPU version)

---
 Demos3/GpuDemos/main_opengl3core.cpp          |  16 +-
 .../shared/b3FindConcaveSatAxis.h             | 127 +++-
 .../b3ConvexHullContact.cpp                   | 618 +++++++++++-------
 .../b3ConvexHullContact.h                     |   1 +
 .../kernels/bvhTraversal.h                    |  26 -
 .../kernels/primitiveContacts.h               |   4 +
 .../NarrowphaseCollision/kernels/sat.cl       | 127 ++++
 .../kernels/satClipHullContacts.cl            |   8 +-
 .../kernels/satClipHullContacts.h             |  12 +-
 .../NarrowphaseCollision/kernels/satKernels.h | 126 +++-
 src/Bullet3OpenCL/RigidBody/b3Solver.cpp      |   2 +-
 .../RigidBody/kernels/batchingKernels.h       |   4 +
 .../RigidBody/kernels/batchingKernelsNew.h    |   4 +
 .../RigidBody/kernels/integrateKernel.h       |   4 +
 .../RigidBody/kernels/solverSetup.h           |   4 +
 .../RigidBody/kernels/solverSetup2.h          |   4 +
 .../RigidBody/kernels/solverUtils.h           |   4 +
 .../RigidBody/kernels/updateAabbsKernel.h     |   4 +
 18 files changed, 800 insertions(+), 295 deletions(-)

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/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
index 88301aa03..bc7bdaa85 100644
--- a/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
+++ b/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
@@ -203,7 +203,98 @@ bool b3FindSeparatingAxisEdgeEdge(	const b3ConvexPolyhedronData* hullA, __global
 	return true;
 }
 
-// work-in-progress
+
+
+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,
@@ -215,6 +306,12 @@ __kernel void   b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
 																					__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
 																					)
@@ -242,7 +339,7 @@ __kernel void   b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
 		return;
 	}
 
-
+	hasSeparatingNormals[i] = 0;
 
 	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
 	int numActualConcaveConvexTests = 0;
@@ -454,8 +551,34 @@ __kernel void   b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs
 		
 		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
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
index 0fdc65fe3..9b7a109ff 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp
@@ -16,7 +16,7 @@ subject to the following restrictions:
 bool findSeparatingAxisOnGpu = true;
 
 bool bvhTraversalKernelGPU = true;
-bool findConcaveSeparatingAxisKernelGPU = false;//true;
+bool findConcaveSeparatingAxisKernelGPU = true;
 
 ///This file was written by Erwin Coumans
 ///Separating axis rest based on work from Pierre Terdiman, see
@@ -24,7 +24,7 @@ bool findConcaveSeparatingAxisKernelGPU = false;//true;
 
 //#define B3_DEBUG_SAT_FACE
 
-#define CHECK_ON_HOST
+//#define CHECK_ON_HOST
 
 #ifdef CHECK_ON_HOST
 //#define PERSISTENT_CONTACTS_HOST
@@ -85,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),
@@ -2990,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);
 
@@ -3039,191 +3040,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 				clFinish(m_queue);
 			}
 
-			//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)
-						{
-							if (findConcaveSeparatingAxisKernelGPU)
-							{
-								//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);
-							} else
-							{
-
-									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);
-
-
-									//numConcavePairs
-									//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())
-
-									b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
-									m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
-							}
-//							b3AlignedObjectArray<b3Vector3> cpuCompoundSepNormals;
-	//						m_concaveSepNormals.copyToHost(cpuCompoundSepNormals);
-		//					b3AlignedObjectArray<b3Int4> cpuConcavePairs;
-			//				triangleConvexPairsOut.copyToHost(cpuConcavePairs);
-
-
-						}
-					}
-				}
-			}
+			
 			
 			numCompoundPairs = m_numCompoundPairsOut.at(0);
 			bool useGpuFindCompoundPairs=true;
@@ -3442,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)
 	{
@@ -3494,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);
 		}
 
@@ -3553,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");
@@ -3608,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()),
@@ -3633,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/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 c9d00b5ad..e94accf7c 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/kernels/sat.cl
@@ -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/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"