diff --git a/Demos3/CpuDemos/main_opengl3core.cpp b/Demos3/CpuDemos/main_opengl3core.cpp
index 6541446a7..cbbd26cd8 100644
--- a/Demos3/CpuDemos/main_opengl3core.cpp
+++ b/Demos3/CpuDemos/main_opengl3core.cpp
@@ -99,11 +99,14 @@ struct EmptyDemo : public CpuDemo
 
 };
 #include "rendering/RenderDemo.h"
+#include "rigidbody/RigidBodyDemo.h"
+
 
 b3AlignedObjectArray<const char*> demoNames;
 int selectedDemo = 0;
 CpuDemo::CreateFunc* allDemos[]=
 {
+	RigidBodyDemo::MyCreateFunc,
 	RenderDemo::MyCreateFunc,
 	EmptyDemo::MyCreateFunc,
 };
@@ -623,7 +626,7 @@ int main(int argc, char* argv[])
 		bool useGpu = false;
 
 		
-		int maxObjectCapacity=128*1024;
+		int maxObjectCapacity=1024*1024;//128*1024;
 		int maxShapeCapacityInBytes=128*1024;
 
 		//maxObjectCapacity = b3Max(maxObjectCapacity,ci.arraySizeX*ci.arraySizeX*ci.arraySizeX+10);
diff --git a/Demos3/CpuDemos/rendering/RenderDemo.cpp b/Demos3/CpuDemos/rendering/RenderDemo.cpp
index adff69d0d..e28fa93b6 100644
--- a/Demos3/CpuDemos/rendering/RenderDemo.cpp
+++ b/Demos3/CpuDemos/rendering/RenderDemo.cpp
@@ -16,7 +16,9 @@ static b3Vector4 colors[4] =
 void    RenderDemo::initPhysics(const ConstructionInfo& ci)
 {
 	m_instancingRenderer = ci.m_instancingRenderer;
-
+	m_instancingRenderer ->setCameraDistance(10);
+	float target[4]={0,0,0,0};
+	m_instancingRenderer->setCameraTargetPosition(target);
 	int strideInBytes = 9*sizeof(float);
 	int numVertices = sizeof(cube_vertices)/strideInBytes;
 	int numIndices = sizeof(cube_indices)/sizeof(int);
diff --git a/Demos3/CpuDemos/rigidbody/RigidBodyDemo.cpp b/Demos3/CpuDemos/rigidbody/RigidBodyDemo.cpp
new file mode 100644
index 000000000..e41ee733a
--- /dev/null
+++ b/Demos3/CpuDemos/rigidbody/RigidBodyDemo.cpp
@@ -0,0 +1,154 @@
+#include "RigidBodyDemo.h"
+#include "OpenGLWindow/GLInstancingRenderer.h"
+#include "OpenGLWindow/ShapeData.h"
+#include "Bullet3Common/b3Quaternion.h"
+
+#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
+#include "Bullet3Dynamics/b3CpuRigidBodyPipeline.h"
+#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
+
+#include "Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.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),
+};
+
+
+
+void    RigidBodyDemo::initPhysics(const ConstructionInfo& ci)
+{
+	m_instancingRenderer = ci.m_instancingRenderer;
+
+	int x_dim=30;
+	int y_dim=30;
+	int z_dim=30;
+
+	int aabbCapacity = x_dim*y_dim*z_dim+1;
+
+	b3Config config;
+
+	m_bp = new b3DynamicBvhBroadphase(aabbCapacity);
+	m_np = new b3CpuNarrowPhase(config);
+	m_cd = new b3CpuCollisionWorld(m_bp,m_np);
+	m_rb = new b3CpuRigidBodyPipeline(m_np,m_bp, config);
+
+
+
+
+
+	m_instancingRenderer ->setCameraDistance(100);
+	float target[4]={0,0,0,0};
+	m_instancingRenderer->setCameraTargetPosition(target);
+	int strideInBytes = 9*sizeof(float);
+	int numVertices = sizeof(cube_vertices)/strideInBytes;
+	int numIndices = sizeof(cube_indices)/sizeof(int);
+	int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
+	
+	
+
+
+	
+
+	
+	{
+		static int curColor=0;
+		b3Vector4 color = colors[curColor];
+		curColor++;
+		curColor&=3;
+
+		b3Vector3 position = b3MakeVector3(0,0,0);//((j+1)&1)+i*2.2,1+j*2.,((j+1)&1)+k*2.2);
+		b3Quaternion orn(0,0,0,1);
+		b3Vector4 scaling=b3MakeVector4(100,1,100,1);
+		int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+		float mass=0.f;
+		int collidableIndex = m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
+		int bodyIndex = m_rb->getNumBodies();
+		m_cd->addCollidable(bodyIndex,collidableIndex,position,orn);
+		
+
+		int userData=-1;
+		int rbid = m_rb->registerPhysicsInstance(mass, position, orn, collidableIndex, userData);
+	}
+
+	ci.m_instancingRenderer->writeTransforms();
+
+	{
+		
+		b3Vector4 scaling=b3MakeVector4(1,1,1,1);
+		int collidableIndex = m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling);
+		for (int x=-x_dim/2;x<x_dim/2;x++)
+		{
+			//for (int y=-y_dim/2;y<y_dim/2;y++)
+			for (int y=1;y<y_dim;y++)
+			{
+				for (int z=-z_dim/2;z<z_dim/2;z++)
+				{
+					static int curColor=0;
+					b3Vector4 color = colors[curColor];
+					curColor++;
+					curColor&=3;
+
+					b3Vector3 position = b3MakeVector3(x*2,y*2,z*2);
+					b3Quaternion orn(0,0,0,1);
+					
+					int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+					float mass=1.f;
+					
+					int userData=-1;
+					int bodyIndex = m_rb->getNumBodies();
+					m_cd->addCollidable(bodyIndex,collidableIndex,position,orn);
+					int rbid = m_rb->registerPhysicsInstance(mass, position, orn, collidableIndex, userData);
+					
+				}
+			}
+		}
+	}
+
+	
+	printf("num objects = %d\n",m_rb->getNumBodies());
+
+	
+}
+	
+void    RigidBodyDemo::exitPhysics()
+{
+	delete m_rb;
+	m_rb=0;
+	delete m_np;
+	m_np=0;
+	delete m_bp;
+	m_bp=0;
+}
+	
+void RigidBodyDemo::renderScene()
+{
+	{
+		B3_PROFILE("writeSingleInstanceTransformToCPU");
+		const b3RigidBodyData* bodies = m_rb->getBodyBuffer();
+		//sync transforms
+		int numBodies = m_rb->getNumBodies();
+		for (int i=0;i<numBodies;i++)
+		{
+			m_instancingRenderer->writeSingleInstanceTransformToCPU(&bodies[i].m_pos.x,bodies[i].m_quat,i);
+		}
+	}
+	{
+		B3_PROFILE("writeTransforms");
+		m_instancingRenderer->writeTransforms();
+	}
+	{
+		B3_PROFILE("renderScene");
+		m_instancingRenderer->renderScene();
+	}
+}
+	
+void RigidBodyDemo::clientMoveAndDisplay()
+{
+	m_rb->stepSimulation(1.f/60.f);
+}
diff --git a/Demos3/CpuDemos/rigidbody/RigidBodyDemo.h b/Demos3/CpuDemos/rigidbody/RigidBodyDemo.h
new file mode 100644
index 000000000..d05b53f3c
--- /dev/null
+++ b/Demos3/CpuDemos/rigidbody/RigidBodyDemo.h
@@ -0,0 +1,47 @@
+#ifndef B3_RIGID_BODY_DEMO_H
+#define B3_RIGID_BODY_DEMO_H
+
+#include "../CpuDemo.h"
+
+struct RigidBodyDemo : public CpuDemo
+{
+
+	struct b3DynamicBvhBroadphase* m_bp;
+	struct b3CpuNarrowPhase* m_np;
+	class b3CpuCollisionWorld* m_cd;
+
+	struct b3CpuRigidBodyPipeline* m_rb;
+
+	GLInstancingRenderer*   m_instancingRenderer;
+			
+	virtual void    initPhysics(const ConstructionInfo& ci);
+	
+	virtual void    exitPhysics();
+	
+	virtual void renderScene();
+	
+	virtual void clientMoveAndDisplay();
+	virtual const char* getName() {
+		return "RigidBodyDemo";
+	}
+	virtual bool	mouseMoveCallback(float x,float y)
+	{
+		return false;
+	}
+	virtual bool	mouseButtonCallback(int button, int state, float x, float y)
+	{
+		return false;
+	}
+	virtual bool	keyboardCallback(int key, int state)
+	{
+		return false;
+	}
+
+	static CpuDemo* MyCreateFunc()
+	{
+		CpuDemo* demo = new RigidBodyDemo;
+		return demo;
+	}
+};
+#endif //B3_RIGID_BODY_DEMO_H
+
diff --git a/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp b/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp
index 75a7c3e70..19f6f6405 100644
--- a/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp
+++ b/Demos3/GpuDemos/constraints/ConstraintsDemo.cpp
@@ -27,7 +27,7 @@ subject to the following restrictions:
 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
-#include "Bullet3OpenCL/RigidBody/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "../rigidbody/GpuRigidBodyDemoInternalData.h"
 #include "../gwenUserInterface.h"
 #include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h"
@@ -36,7 +36,7 @@ subject to the following restrictions:
 
 #include "OpenGLWindow/GLPrimitiveRenderer.h"
 #include "Bullet3OpenCL/Raycast/b3GpuRaycast.h"
-#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
 
 #include "OpenGLWindow/GLRenderToTexture.h"
 
diff --git a/Demos3/GpuDemos/main_opengl3core.cpp b/Demos3/GpuDemos/main_opengl3core.cpp
index 0805119c7..574e40839 100644
--- a/Demos3/GpuDemos/main_opengl3core.cpp
+++ b/Demos3/GpuDemos/main_opengl3core.cpp
@@ -84,7 +84,7 @@ enum
 };
 
 b3AlignedObjectArray<const char*> demoNames;
-int selectedDemo = 0;
+int selectedDemo = 1;
 GpuDemo::CreateFunc* allDemos[]=
 {
 		//ConcaveCompound2Scene::MyCreateFunc,
diff --git a/Demos3/GpuDemos/raytrace/RaytracedShadowDemo.cpp b/Demos3/GpuDemos/raytrace/RaytracedShadowDemo.cpp
index a21b84baf..b114b4511 100644
--- a/Demos3/GpuDemos/raytrace/RaytracedShadowDemo.cpp
+++ b/Demos3/GpuDemos/raytrace/RaytracedShadowDemo.cpp
@@ -17,13 +17,13 @@
 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
-#include "Bullet3OpenCL/RigidBody/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "../rigidbody/GpuRigidBodyDemoInternalData.h"
 #include "../gwenUserInterface.h"
 #include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h"
 #include "OpenGLWindow/GLPrimitiveRenderer.h"
 #include "Bullet3OpenCL/Raycast/b3GpuRaycast.h"
-#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
 
 #include "OpenGLWindow/GLRenderToTexture.h"
 
diff --git a/Demos3/GpuDemos/rigidbody/BulletDataExtractor.cpp b/Demos3/GpuDemos/rigidbody/BulletDataExtractor.cpp
index a28dd8d65..3d55dc89d 100644
--- a/Demos3/GpuDemos/rigidbody/BulletDataExtractor.cpp
+++ b/Demos3/GpuDemos/rigidbody/BulletDataExtractor.cpp
@@ -21,7 +21,7 @@ extern bool enableExperimentalCpuConcaveCollision;
 #include "OpenGLWindow/GLInstancingRenderer.h"
 #include "Bullet3Common/b3Quaternion.h"
 #include "Bullet3Common/b3Matrix3x3.h"
-#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
 #include "OpenGLWindow/ShapeData.h"
 
 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
diff --git a/Demos3/GpuDemos/rigidbody/ConcaveScene.cpp b/Demos3/GpuDemos/rigidbody/ConcaveScene.cpp
index 30f3dfe49..a12e8f3a1 100644
--- a/Demos3/GpuDemos/rigidbody/ConcaveScene.cpp
+++ b/Demos3/GpuDemos/rigidbody/ConcaveScene.cpp
@@ -13,11 +13,11 @@
 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
-#include "Bullet3OpenCL/RigidBody/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "GpuRigidBodyDemoInternalData.h"
 #include"../../Wavefront/tiny_obj_loader.h"
 #include "Bullet3Common/b3Transform.h"
-#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
 
 #include "../gwenUserInterface.h"
 #include "OpenGLWindow/GLInstanceGraphicsShape.h"
diff --git a/Demos3/GpuDemos/rigidbody/GpuCompoundScene.cpp b/Demos3/GpuDemos/rigidbody/GpuCompoundScene.cpp
index d1c556ed1..a7a9c88ed 100644
--- a/Demos3/GpuDemos/rigidbody/GpuCompoundScene.cpp
+++ b/Demos3/GpuDemos/rigidbody/GpuCompoundScene.cpp
@@ -13,7 +13,7 @@
 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
-#include "Bullet3OpenCL/RigidBody/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "GpuRigidBodyDemoInternalData.h"
 #include "Bullet3Common/b3Transform.h"
 
diff --git a/Demos3/GpuDemos/rigidbody/GpuConvexScene.cpp b/Demos3/GpuDemos/rigidbody/GpuConvexScene.cpp
index bac3ccf01..284fd31d2 100644
--- a/Demos3/GpuDemos/rigidbody/GpuConvexScene.cpp
+++ b/Demos3/GpuDemos/rigidbody/GpuConvexScene.cpp
@@ -13,13 +13,13 @@
 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
-#include "Bullet3OpenCL/RigidBody/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "GpuRigidBodyDemoInternalData.h"
 #include "../gwenUserInterface.h"
 #include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h"
 #include "OpenGLWindow/GLPrimitiveRenderer.h"
 #include "Bullet3OpenCL/Raycast/b3GpuRaycast.h"
-#include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
 #include "Bullet3Dynamics/ConstraintSolver/b3FixedConstraint.h"
 
 #include "OpenGLWindow/GLRenderToTexture.h"
diff --git a/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemo.cpp b/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemo.cpp
index 161e1c346..0521244ca 100644
--- a/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemo.cpp
+++ b/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemo.cpp
@@ -11,7 +11,7 @@
 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
-#include "Bullet3OpenCL/RigidBody/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "GpuRigidBodyDemoInternalData.h"
 #include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
 #include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
diff --git a/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemoInternalData.h b/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemoInternalData.h
index 648100fab..f0c99e979 100644
--- a/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemoInternalData.h
+++ b/Demos3/GpuDemos/rigidbody/GpuRigidBodyDemoInternalData.h
@@ -4,7 +4,7 @@
 #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
 #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
 #include "Bullet3Common/b3Vector3.h"
-#include "Bullet3OpenCL/RigidBody/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 
 struct	GpuRigidBodyDemoInternalData
 {
diff --git a/Demos3/GpuDemos/rigidbody/GpuSphereScene.cpp b/Demos3/GpuDemos/rigidbody/GpuSphereScene.cpp
index 1d4b93922..e3b77baf8 100644
--- a/Demos3/GpuDemos/rigidbody/GpuSphereScene.cpp
+++ b/Demos3/GpuDemos/rigidbody/GpuSphereScene.cpp
@@ -12,7 +12,7 @@
 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
-#include "Bullet3OpenCL/RigidBody/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "GpuRigidBodyDemoInternalData.h"
 #include "../gwenUserInterface.h"
 
diff --git a/Demos3/GpuDemos/softbody/GpuSoftBodyDemo.cpp b/Demos3/GpuDemos/softbody/GpuSoftBodyDemo.cpp
index 4b2df0948..a553c2e43 100644
--- a/Demos3/GpuDemos/softbody/GpuSoftBodyDemo.cpp
+++ b/Demos3/GpuDemos/softbody/GpuSoftBodyDemo.cpp
@@ -11,7 +11,7 @@
 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
 #include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
-#include "Bullet3OpenCL/RigidBody/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "GpuSoftBodyDemoInternalData.h"
 #include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
 #include "stb_image/stb_image.h"
diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.cpp b/btgui/OpenGLWindow/GLInstancingRenderer.cpp
index 6bbba8f4a..168aeb1d5 100644
--- a/btgui/OpenGLWindow/GLInstancingRenderer.cpp
+++ b/btgui/OpenGLWindow/GLInstancingRenderer.cpp
@@ -16,9 +16,9 @@ subject to the following restrictions:
 
 ///todo: make this configurable in the gui
 bool useShadowMap=true;
-float shadowMapWidth=4096;
-float shadowMapHeight=4096;
-float shadowMapWorldSize=300;
+float shadowMapWidth=8192;
+float shadowMapHeight=8192;
+float shadowMapWorldSize=100;
 float WHEEL_MULTIPLIER=3.f;
 float MOUSE_MOVE_MULTIPLIER = 0.4f;
 
@@ -388,7 +388,7 @@ GLInstancingRenderer::~GLInstancingRenderer()
 
 
 
-void GLInstancingRenderer::writeSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex)
+void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)
 {
 	m_data->m_instance_positions_ptr[srcIndex*4+0]=position[0];
 	m_data->m_instance_positions_ptr[srcIndex*4+1]=position[1];
diff --git a/btgui/OpenGLWindow/GLInstancingRenderer.h b/btgui/OpenGLWindow/GLInstancingRenderer.h
index 0497b14ab..c3f9d04f4 100644
--- a/btgui/OpenGLWindow/GLInstancingRenderer.h
+++ b/btgui/OpenGLWindow/GLInstancingRenderer.h
@@ -78,7 +78,7 @@ public:
 
 	void writeTransforms();
 
-	void writeSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex);
+	void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex);
 
 	void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex);
 
diff --git a/build3/premake4.lua b/build3/premake4.lua
index 4e6218f28..0bd34d1d0 100644
--- a/build3/premake4.lua
+++ b/build3/premake4.lua
@@ -101,6 +101,7 @@
 	
 if not _OPTIONS["ios"] then
 	
+	include "../Demos3/CpuDemos"
 	include "../Demos3/GpuDemos"
 	include "../Demos3/Wavefront"
 	include "../btgui/MultiThreading"
diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
index dd0377a5c..97c5190d9 100644
--- a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
+++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
@@ -204,12 +204,21 @@ void							b3DynamicBvhBroadphase::destroyProxy(	b3BroadphaseProxy* absproxy,
 	m_needcleanup=true;
 }
 
+void	b3DynamicBvhBroadphase::getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
+{
+	const b3DbvtProxy*						proxy=&m_proxies[objectId];
+	aabbMin = proxy->m_aabbMin;
+	aabbMax = proxy->m_aabbMax;
+}
+/*
 void	b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
 {
 	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
 	aabbMin = proxy->m_aabbMin;
 	aabbMax = proxy->m_aabbMax;
 }
+*/
+
 
 struct	BroadphaseRayTester : b3DynamicBvh::ICollide
 {
diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
index df68f5d58..767ee72bf 100644
--- a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
+++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h
@@ -165,13 +165,14 @@ struct	b3DynamicBvhBroadphase
 	void							optimize();
 	
 	/* b3BroadphaseInterface Implementation	*/
-	b3BroadphaseProxy*				createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask);
+	b3BroadphaseProxy*				createProxy(const b3Vector3& aabbMin,const b3Vector3& aabbMax,int objectIndex,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask);
 	virtual void					destroyProxy(b3BroadphaseProxy* proxy,b3Dispatcher* dispatcher);
 	virtual void					setAabb(b3BroadphaseProxy* proxy,const b3Vector3& aabbMin,const b3Vector3& aabbMax,b3Dispatcher* dispatcher);
 	virtual void					rayTest(const b3Vector3& rayFrom,const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin=b3MakeVector3(0,0,0), const b3Vector3& aabbMax = b3MakeVector3(0,0,0));
 	virtual void					aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
 
-	virtual void					getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
+	//virtual void					getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
+	virtual void					getAabb(int objectId,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
 	virtual	void					calculateOverlappingPairs(b3Dispatcher* dispatcher=0);
 	virtual	b3OverlappingPairCache*	getOverlappingPairCache();
 	virtual	const b3OverlappingPairCache*	getOverlappingPairCache() const;
diff --git a/src/Bullet3OpenCL/RigidBody/b3Config.h b/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h
similarity index 100%
rename from src/Bullet3OpenCL/RigidBody/b3Config.h
rename to src/Bullet3Collision/NarrowPhaseCollision/b3Config.h
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.cpp b/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp
similarity index 99%
rename from src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.cpp
rename to src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp
index 4f565143c..87dfebddb 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.cpp
+++ b/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp
@@ -20,7 +20,7 @@ subject to the following restrictions:
 #include "Bullet3Common/b3Quaternion.h"
 #include "Bullet3Common/b3HashMap.h"
 
-#include "b3ConvexPolyhedronCL.h"
+//#include "b3ConvexPolyhedronCL.h"
 
 
 
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h b/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h
similarity index 97%
rename from src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h
rename to src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h
index 659a02693..88f433921 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexUtility.h
+++ b/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h
@@ -20,7 +20,7 @@ subject to the following restrictions:
 #include "Bullet3Common/b3AlignedObjectArray.h"
 #include "Bullet3Common/b3Transform.h"
 
-#include "b3ConvexPolyhedronCL.h"
+//#include "b3ConvexPolyhedronCL.h"
 
 
 struct b3MyFace
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.cpp b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.cpp
new file mode 100644
index 000000000..5eded6c6e
--- /dev/null
+++ b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.cpp
@@ -0,0 +1,46 @@
+
+#include "b3CpuCollisionWorld.h"
+#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
+
+
+b3CpuCollisionWorld::b3CpuCollisionWorld(b3DynamicBvhBroadphase* bp, b3CpuNarrowPhase* np)
+	:m_bp(bp),
+	m_np(np)
+{
+
+}
+
+b3CpuCollisionWorld::~b3CpuCollisionWorld()
+{
+
+}
+
+void	b3CpuCollisionWorld::addCollidable(int bodyIndex, int collidableIndex,const b3Vector3& position, const b3Quaternion& orientation)
+{
+	b3Vector3 aabbMinWorld, aabbMaxWorld;
+
+	if (collidableIndex>=0)
+	{
+		b3Aabb localAabb = m_np->getLocalSpaceAabb(collidableIndex);
+		b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
+		b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
+		
+		b3Scalar margin = 0.01f;
+		b3Transform t;
+		t.setIdentity();
+		t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
+		t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
+		b3TransformAabb(localAabbMin,localAabbMax, margin,t,aabbMinWorld,aabbMaxWorld);
+
+		m_bp->createProxy(aabbMinWorld,aabbMaxWorld,bodyIndex,0,1,1);
+		
+		b3Vector3 aabbMin,aabbMax;
+		m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
+		
+
+	} else
+	{
+		b3Error("registerPhysicsInstance using invalid collidableIndex\n");
+	}
+}
\ No newline at end of file
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.h b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.h
new file mode 100644
index 000000000..ad6e7c573
--- /dev/null
+++ b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuCollisionWorld.h
@@ -0,0 +1,28 @@
+
+#ifndef B3_CPU2_COLLISION_WORLD_H
+#define B3_CPU2_COLLISION_WORLD_H
+
+class b3CpuNarrowPhase;
+struct b3DynamicBvhBroadphase;
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3Vector3.h"
+
+class b3CpuCollisionWorld
+{
+protected:
+
+	b3DynamicBvhBroadphase* m_bp;
+	b3CpuNarrowPhase*		m_np;
+
+public:
+
+	b3CpuCollisionWorld(b3DynamicBvhBroadphase* bp, b3CpuNarrowPhase* np);
+
+	void	addCollidable(int bodyIndex, int collidableIndex,const b3Vector3& position, const b3Quaternion& orientation);
+
+	virtual ~b3CpuCollisionWorld();
+
+};
+
+
+#endif //B3_CPU_COLLISION_WORLD_H
\ No newline at end of file
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
new file mode 100644
index 000000000..89e2efd1c
--- /dev/null
+++ b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp
@@ -0,0 +1,208 @@
+#include "b3CpuNarrowPhase.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+
+struct b3CpuNarrowPhaseInternalData
+{
+	b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
+	b3AlignedObjectArray<b3Collidable>	m_collidablesCPU;
+	b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
+	b3Config m_config;
+
+
+	b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
+	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
+	b3AlignedObjectArray<b3Vector3> m_convexVertices;
+	b3AlignedObjectArray<int> m_convexIndices;
+	b3AlignedObjectArray<b3GpuFace> m_convexFaces;
+
+
+	int	m_numAcceleratedShapes;
+};
+
+
+b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
+{
+	m_data = new b3CpuNarrowPhaseInternalData;
+	m_data->m_config = config;
+	m_data->m_numAcceleratedShapes = 0;
+}
+
+b3CpuNarrowPhase::~b3CpuNarrowPhase()
+{
+	delete m_data;
+}
+
+void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>* broadphasePairs, b3AlignedObjectArray<b3Aabb>* aabbsWorldSpace)
+{
+
+}
+
+int	b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
+{
+	int collidableIndex = allocateCollidable();
+	if (collidableIndex<0)
+		return collidableIndex;
+
+	
+	b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
+	col.m_shapeType = SHAPE_CONVEX_HULL;
+	col.m_shapeIndex = -1;
+	
+	
+	{
+		b3Vector3 localCenter=b3MakeVector3(0,0,0);
+		for (int i=0;i<utilPtr->m_vertices.size();i++)
+			localCenter+=utilPtr->m_vertices[i];
+		localCenter*= (1.f/utilPtr->m_vertices.size());
+		utilPtr->m_localCenter = localCenter;
+
+		col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col);
+	}
+
+	if (col.m_shapeIndex>=0)
+	{
+		b3Aabb aabb;
+		
+		b3Vector3 myAabbMin=b3MakeVector3(1e30f,1e30f,1e30f);
+		b3Vector3 myAabbMax=b3MakeVector3(-1e30f,-1e30f,-1e30f);
+
+		for (int i=0;i<utilPtr->m_vertices.size();i++)
+		{
+			myAabbMin.setMin(utilPtr->m_vertices[i]);
+			myAabbMax.setMax(utilPtr->m_vertices[i]);
+		}
+		aabb.m_min[0] = myAabbMin[0];
+		aabb.m_min[1] = myAabbMin[1];
+		aabb.m_min[2] = myAabbMin[2];
+		aabb.m_minIndices[3] = 0;
+
+		aabb.m_max[0] = myAabbMax[0];
+		aabb.m_max[1] = myAabbMax[1];
+		aabb.m_max[2] = myAabbMax[2];
+		aabb.m_signedMaxIndices[3] = 0;
+
+		m_data->m_localShapeAABBCPU.push_back(aabb);
+
+	}
+	
+	return collidableIndex;
+}
+
+int	b3CpuNarrowPhase::allocateCollidable()
+{
+	int curSize = m_data->m_collidablesCPU.size();
+	if (curSize<m_data->m_config.m_maxConvexShapes)
+	{
+		m_data->m_collidablesCPU.expand();
+		return curSize;
+	}
+	else
+	{
+		b3Error("allocateCollidable out-of-range %d\n",m_data->m_config.m_maxConvexShapes);
+	}
+	return -1;
+
+}
+
+int	b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
+{
+	b3AlignedObjectArray<b3Vector3> verts;
+
+	unsigned char* vts = (unsigned char*) vertices;
+	for (int i=0;i<numVertices;i++)
+	{
+		float* vertex = (float*) &vts[i*strideInBytes];
+		verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2]));
+	}
+
+	b3ConvexUtility* utilPtr = new b3ConvexUtility();
+	bool merge = true;
+	if (numVertices)
+	{
+		utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge);
+	}
+
+	int collidableIndex = registerConvexHullShape(utilPtr);
+
+	delete utilPtr;
+	return collidableIndex;
+}
+
+
+int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col)
+{
+
+	m_data->m_convexData.resize(m_data->m_numAcceleratedShapes+1);
+	m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes+1);
+	
+    
+	b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size()-1);
+	convex.mC = convexPtr->mC;
+	convex.mE = convexPtr->mE;
+	convex.m_extents= convexPtr->m_extents;
+	convex.m_localCenter = convexPtr->m_localCenter;
+	convex.m_radius = convexPtr->m_radius;
+	
+	convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
+	int edgeOffset = m_data->m_uniqueEdges.size();
+	convex.m_uniqueEdgesOffset = edgeOffset;
+	
+	m_data->m_uniqueEdges.resize(edgeOffset+convex.m_numUniqueEdges);
+    
+	//convex data here
+	int i;
+	for ( i=0;i<convexPtr->m_uniqueEdges.size();i++)
+	{
+		m_data->m_uniqueEdges[edgeOffset+i] = convexPtr->m_uniqueEdges[i];
+	}
+    
+	int faceOffset = m_data->m_convexFaces.size();
+	convex.m_faceOffset = faceOffset;
+	convex.m_numFaces = convexPtr->m_faces.size();
+
+	m_data->m_convexFaces.resize(faceOffset+convex.m_numFaces);
+	
+
+	for (i=0;i<convexPtr->m_faces.size();i++)
+	{
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
+																			convexPtr->m_faces[i].m_plane[1],
+																			convexPtr->m_faces[i].m_plane[2],
+																			convexPtr->m_faces[i].m_plane[3]);
+
+		
+		int indexOffset = m_data->m_convexIndices.size();
+		int numIndices = convexPtr->m_faces[i].m_indices.size();
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_numIndices = numIndices;
+		m_data->m_convexFaces[convex.m_faceOffset+i].m_indexOffset = indexOffset;
+		m_data->m_convexIndices.resize(indexOffset+numIndices);
+		for (int p=0;p<numIndices;p++)
+		{
+			m_data->m_convexIndices[indexOffset+p] = convexPtr->m_faces[i].m_indices[p];
+		}
+	}
+    
+	convex.m_numVertices = convexPtr->m_vertices.size();
+	int vertexOffset = m_data->m_convexVertices.size();
+	convex.m_vertexOffset =vertexOffset;
+	
+	m_data->m_convexVertices.resize(vertexOffset+convex.m_numVertices);
+	for (int i=0;i<convexPtr->m_vertices.size();i++)
+	{
+		m_data->m_convexVertices[vertexOffset+i] = convexPtr->m_vertices[i];
+	}
+
+	(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
+	
+    
+    
+	return m_data->m_numAcceleratedShapes++;
+}
+
+const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
+{
+	return m_data->m_localShapeAABBCPU[collidableIndex];
+}
diff --git a/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h
new file mode 100644
index 000000000..fcff82613
--- /dev/null
+++ b/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h
@@ -0,0 +1,101 @@
+#ifndef B3_CPU_NARROWPHASE_H
+#define B3_CPU_NARROWPHASE_H
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Common/shared/b3Int4.h"
+
+class b3CpuNarrowPhase
+{
+protected:
+
+	struct b3CpuNarrowPhaseInternalData*	m_data;
+	int m_acceleratedCompanionShapeIndex;
+	int m_planeBodyIndex;
+	int	m_static0Index;
+
+	int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr,b3Collidable& col);
+	int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
+
+public:
+
+	
+
+
+	b3CpuNarrowPhase(const struct b3Config& config);
+
+	virtual ~b3CpuNarrowPhase(void);
+
+	int		registerSphereShape(float radius);
+	int		registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
+
+	int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
+	int registerFace(const b3Vector3& faceNormal, float faceConstant);
+	
+	int	registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices,const float* scaling);
+	
+	//do they need to be merged?
+	
+	int	registerConvexHullShape(b3ConvexUtility* utilPtr);
+	int	registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
+
+	//int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
+	void setObjectTransform(const float* position, const float* orientation , int bodyIndex);
+
+	void	writeAllBodiesToGpu();
+	void  reset();
+	void	readbackAllBodiesToCpu();
+	bool	getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const;
+
+	void setObjectTransformCpu(float* position, float* orientation , int bodyIndex);
+	void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
+
+	
+	//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
+	virtual void computeContacts(b3AlignedObjectArray<b3Int4>* broadphasePairs, b3AlignedObjectArray<b3Aabb>* aabbsWorldSpace);
+	
+
+	
+	const struct b3RigidBodyCL* getBodiesCpu() const;
+	//struct b3RigidBodyCL* getBodiesCpu();
+
+	int	getNumBodiesGpu() const;
+
+	
+	int	getNumBodyInertiasGpu() const;
+
+	
+	const struct b3Collidable* getCollidablesCpu() const;
+	int		getNumCollidablesGpu() const;
+
+
+	const struct b3Contact4* getContactsCPU() const;
+
+	
+	int	getNumContactsGpu() const;
+
+	
+	
+	int getNumRigidBodies() const;
+
+	int allocateCollidable();
+
+	int getStatic0Index() const
+	{
+		return m_static0Index;
+	}
+	b3Collidable& getCollidableCpu(int collidableIndex);
+	const b3Collidable& getCollidableCpu(int collidableIndex) const;
+
+	const b3CpuNarrowPhaseInternalData*	getInternalData() const
+	{
+			return m_data;
+	}
+
+	const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
+};
+
+#endif //B3_CPU_NARROWPHASE_H
+
diff --git a/src/Bullet3Common/b3Quaternion.h b/src/Bullet3Common/b3Quaternion.h
index 70c92b036..c89f2cf39 100644
--- a/src/Bullet3Common/b3Quaternion.h
+++ b/src/Bullet3Common/b3Quaternion.h
@@ -838,6 +838,14 @@ b3QuatMul(const b3Quaternion& rot0, const b3Quaternion& rot1)
 	return rot0*rot1;
 }
 
+B3_FORCE_INLINE b3Quaternion
+b3QuatNormalized(const b3Quaternion& orn)
+{
+	return orn.normalized();
+}
+
+
+
 B3_FORCE_INLINE b3Vector3 
 b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v) 
 {
diff --git a/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp b/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp
new file mode 100644
index 000000000..a0ddc7e56
--- /dev/null
+++ b/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp
@@ -0,0 +1,122 @@
+#include "b3CpuRigidBodyPipeline.h"
+
+#include "Bullet3Dynamics/shared/b3IntegrateTransforms.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
+
+
+struct b3CpuRigidBodyPipelineInternalData
+{
+	b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
+	b3DynamicBvhBroadphase* m_bp;
+	b3CpuNarrowPhase* m_np;
+	b3Config m_config;
+};
+	
+
+b3CpuRigidBodyPipeline::b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const b3Config& config)
+{
+	m_data = new b3CpuRigidBodyPipelineInternalData;
+	m_data->m_np = narrowphase;
+	m_data->m_bp = broadphaseDbvt;
+	m_data->m_config = config;
+}
+
+b3CpuRigidBodyPipeline::~b3CpuRigidBodyPipeline()
+{
+	delete m_data;
+}
+
+void b3CpuRigidBodyPipeline::updateAabbWorldSpace()
+{
+
+}
+
+void	b3CpuRigidBodyPipeline::computeOverlappingPairs()
+{
+	int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
+	m_data->m_bp->calculateOverlappingPairs();
+	numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
+}
+
+void b3CpuRigidBodyPipeline::computeContactPoints()
+{
+	b3AlignedObjectArray<b3Aabb> aabbWorldSpace;
+	b3AlignedObjectArray<b3Int4> pairs;
+	
+	
+	m_data->m_np->computeContacts(&pairs,&aabbWorldSpace);
+
+}
+void	b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
+{
+	
+	//update world space aabb's
+	updateAabbWorldSpace();
+
+	//compute overlapping pairs
+	computeOverlappingPairs();
+
+	//compute contacts
+	computeContactPoints();
+
+	//solve contacts
+	
+	//update transforms
+	integrate(deltaTime);
+	
+	
+}
+
+
+void b3CpuRigidBodyPipeline::integrate(float deltaTime)
+{
+	float angDamping=0.f;
+	b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0);
+
+	//integrate transforms (external forces/gravity should be moved into constraint solver)
+	for (int i=0;i<m_data->m_rigidBodies.size();i++)
+	{
+		b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration);
+	}
+
+}
+
+int		b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
+{
+	b3RigidBodyData body;
+	int index = m_data->m_rigidBodies.size();
+	body.m_invMass = mass ? 1.f/mass : 0.f;
+	body.m_angVel.setValue(0,0,0);
+	body.m_collidableIdx = collidableIndex;
+	body.m_frictionCoeff = 0.3f;
+	body.m_linVel.setValue(0,0,0);
+	body.m_pos.setValue(position[0],position[1],position[2]);
+	body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
+	body.m_restituitionCoeff = 0.f;
+
+	m_data->m_rigidBodies.push_back(body);
+
+
+
+
+
+
+
+
+
+	return index;
+}
+
+
+const struct b3RigidBodyData* b3CpuRigidBodyPipeline::getBodyBuffer() const
+{
+	return m_data->m_rigidBodies.size() ? &m_data->m_rigidBodies[0] : 0;
+}
+
+int	b3CpuRigidBodyPipeline::getNumBodies() const
+{
+	return m_data->m_rigidBodies.size();
+}
diff --git a/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h b/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h
new file mode 100644
index 000000000..463530601
--- /dev/null
+++ b/src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h
@@ -0,0 +1,66 @@
+/*
+Copyright (c) 2013 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Erwin Coumans
+
+#ifndef B3_CPU_RIGIDBODY_PIPELINE_H
+#define B3_CPU_RIGIDBODY_PIPELINE_H
+
+
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3OpenCL/Raycast/b3RaycastInfo.h"
+
+class b3CpuRigidBodyPipeline
+{
+protected:
+	struct b3CpuRigidBodyPipelineInternalData*	m_data;
+
+	int allocateCollidable();
+
+public:
+
+
+	b3CpuRigidBodyPipeline(class b3CpuNarrowPhase* narrowphase, struct b3DynamicBvhBroadphase* broadphaseDbvt, const struct b3Config& config);
+	virtual ~b3CpuRigidBodyPipeline();
+
+	virtual void	stepSimulation(float deltaTime);
+	virtual void	integrate(float timeStep);
+	virtual void	updateAabbWorldSpace();
+	virtual void	computeOverlappingPairs();
+	virtual void	computeContactPoints();
+
+	int		registerConvexPolyhedron(class b3ConvexUtility* convex);
+
+	int		registerPhysicsInstance(float mass, const float* position, const float* orientation, int collisionShapeIndex, int userData);
+	void	writeAllInstancesToGpu();
+	void	copyConstraintsToHost();
+	void	setGravity(const float* grav);
+	void	reset();
+	
+	int createPoint2PointConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB,float breakingThreshold);
+	int createFixedConstraint(int bodyA, int bodyB, const float* pivotInA, const float* pivotInB, const float* relTargetAB, float breakingThreshold);
+	void removeConstraintByUid(int uid);
+
+	void	addConstraint(class b3TypedConstraint* constraint);
+	void	removeConstraint(b3TypedConstraint* constraint);
+
+	void	castRays(const b3AlignedObjectArray<b3RayInfo>& rays,	b3AlignedObjectArray<b3RayHit>& hitResults);
+
+	const struct b3RigidBodyData* getBodyBuffer() const;
+
+	int	getNumBodies() const;
+
+};
+
+#endif //B3_CPU_RIGIDBODY_PIPELINE_H
\ No newline at end of file
diff --git a/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h b/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h
new file mode 100644
index 000000000..6eeec3964
--- /dev/null
+++ b/src/Bullet3Dynamics/shared/b3IntegrateTransforms.h
@@ -0,0 +1,56 @@
+
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+
+inline void b3IntegrateTransform( b3RigidBodyData* body, float timeStep, float angularDamping, b3Float4ConstArg gravityAcceleration)
+{
+	float BT_GPU_ANGULAR_MOTION_THRESHOLD = (0.25f * 3.14159254f);
+	
+	if( (body->m_invMass != 0.f))
+	{
+		//angular velocity
+		{
+			b3Float4 axis;
+			//add some hardcoded angular damping
+			body->m_angVel.x *= angularDamping;
+			body->m_angVel.y *= angularDamping;
+			body->m_angVel.z *= angularDamping;
+			
+			b3Float4 angvel = body->m_angVel;
+			float fAngle = b3Sqrt(b3Dot(angvel, angvel));
+			//limit the angular motion
+			if(fAngle*timeStep > BT_GPU_ANGULAR_MOTION_THRESHOLD)
+			{
+				fAngle = BT_GPU_ANGULAR_MOTION_THRESHOLD / timeStep;
+			}
+			if(fAngle < 0.001f)
+			{
+				// use Taylor's expansions of sync function
+				axis = angvel * (0.5f*timeStep-(timeStep*timeStep*timeStep)*0.020833333333f * fAngle * fAngle);
+			}
+			else
+			{
+				// sync(fAngle) = sin(c*fAngle)/t
+				axis = angvel * ( b3Sin(0.5f * fAngle * timeStep) / fAngle);
+			}
+			b3Quat dorn;
+			dorn.x = axis.x;
+			dorn.y = axis.y;
+			dorn.z = axis.z;
+			dorn.w = b3Cos(fAngle * timeStep * 0.5f);
+			b3Quat orn0 = body->m_quat;
+
+			b3Quat predictedOrn = b3QuatMul(dorn, orn0);
+			predictedOrn = b3QuatNormalized(predictedOrn);
+			body->m_quat=predictedOrn;
+		}
+
+		//apply gravity
+		body->m_linVel += gravityAcceleration * timeStep;
+
+		//linear velocity		
+		body->m_pos +=  body->m_linVel * timeStep;
+		
+	}
+}
diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h
index f28027768..d37c327cd 100644
--- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h
+++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h
@@ -5,7 +5,7 @@
 #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
 #include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
 #include "Bullet3Common/b3AlignedObjectArray.h"
-#include "b3ConvexUtility.h"
+
 #include "b3ConvexPolyhedronCL.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
 #include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuBatchingPgsSolver.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuBatchingPgsSolver.cpp
index 03c1c2af8..72ba25012 100644
--- a/src/Bullet3OpenCL/RigidBody/b3GpuBatchingPgsSolver.cpp
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuBatchingPgsSolver.cpp
@@ -15,7 +15,7 @@ bool gpuSortContactsDeterminism = true;
 #include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanCL.h"
 #include <string.h>
 #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
-#include "b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "b3Solver.h"
 
 
diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp
index 1b676492c..0aa1daebb 100644
--- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.cpp
@@ -6,7 +6,7 @@
 #include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.h"
 #include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h"
 #include <string.h>
-#include "b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "Bullet3OpenCL/NarrowphaseCollision/b3OptimizedBvh.h"
 #include "Bullet3OpenCL/NarrowphaseCollision/b3TriangleIndexVertexArray.h"
 #include "Bullet3Geometry/b3AabbUtil.h"
@@ -14,7 +14,7 @@
 
 #include "b3GpuNarrowPhaseInternalData.h"
 #include "Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.h"
-
+#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
 
 
 
@@ -264,7 +264,7 @@ int		b3GpuNarrowPhase::registerPlaneShape(const b3Vector3& planeNormal, float pl
 }
 
 
-int b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* convexPtr,b3Collidable& col)
+int b3GpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr,b3Collidable& col)
 {
 
 	m_data->m_convexData->resize(m_data->m_numAcceleratedShapes+1);
@@ -376,7 +376,7 @@ int		b3GpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
 		localCenter*= (1.f/utilPtr->m_vertices.size());
 		utilPtr->m_localCenter = localCenter;
 
-		col.m_shapeIndex = registerConvexHullShape(utilPtr,col);
+		col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr,col);
 	}
 
 	if (col.m_shapeIndex>=0)
diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h
index 3f7cd2e7e..5d0f9f5ff 100644
--- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h
@@ -19,7 +19,7 @@ protected:
 	cl_device_id m_device;
 	cl_command_queue m_queue;
 
-	int registerConvexHullShape(class b3ConvexUtility* convexPtr, b3Collidable& col);
+	int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr, b3Collidable& col);
 	int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
 
 public:
diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h
index 14772f20d..7b0dfe11b 100644
--- a/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuNarrowPhaseInternalData.h
@@ -4,7 +4,7 @@
 
 #include "Bullet3OpenCL/ParallelPrimitives/b3OpenCLArray.h"
 #include "Bullet3OpenCL/NarrowphaseCollision/b3ConvexPolyhedronCL.h"
-#include "b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
 
 #include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
index e241f63a9..4f916cc93 100644
--- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.cpp
@@ -48,7 +48,7 @@ bool dumpContactStats = false;
 #include "b3GpuBatchingPgsSolver.h"
 #include "b3Solver.h"
 
-#include "b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 #include "Bullet3OpenCL/Raycast/b3GpuRaycast.h"
 
 
diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h
index e9d9cca97..2cc92294a 100644
--- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h
@@ -17,7 +17,7 @@ subject to the following restrictions:
 #define B3_GPU_RIGIDBODY_PIPELINE_H
 
 #include "Bullet3OpenCL/Initialize/b3OpenCLInclude.h"
-#include "b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
 
 #include "Bullet3Common/b3AlignedObjectArray.h"
 #include "Bullet3OpenCL/Raycast/b3RaycastInfo.h"
diff --git a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h
index eafdadb0e..e798b1cf6 100644
--- a/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h
+++ b/src/Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipelineInternalData.h
@@ -25,7 +25,7 @@ subject to the following restrictions:
 
 #include "Bullet3OpenCL/BroadphaseCollision/b3SapAabb.h"
 #include "Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h"
-#include "b3Config.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"