more work towards CPU version

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
 {

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;

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"
 
 
 

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

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");
+	}
+}

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

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];
+}

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
+

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) 
 {

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();
+}

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

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;
+		
+	}
+}

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"

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"
 
 

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)

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:

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"

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"
 
 

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"

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"