more work towards shared CPU/OpenCL codebase

src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp

@@ -25,6 +25,12 @@ struct b3CpuNarrowPhaseInternalData
 	int	m_numAcceleratedShapes;
 };
 
+
+const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
+{
+	return m_data->m_contacts;
+}
+
 b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex)
 {
 	return m_data->m_collidablesCPU[collidableIndex];

src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h

@@ -7,6 +7,7 @@
 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
 #include "Bullet3Common/shared/b3Int4.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
 
 class b3CpuNarrowPhase
 {
@@ -72,11 +73,13 @@ public:
 	int		getNumCollidablesGpu() const;
 
 
-	const struct b3Contact4* getContactsCPU() const;
+	/*const struct b3Contact4* getContactsCPU() const;
 
 	
 	int	getNumContactsGpu() const;
+	*/
 
+	const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
 	
 	
 	int getNumRigidBodies() const;

src/Bullet3Dynamics/b3CpuRigidBodyPipeline.cpp

@@ -7,11 +7,15 @@
 #include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3CollidableData.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
+#include "Bullet3Dynamics/shared/b3Inertia.h"
 
 
 struct b3CpuRigidBodyPipelineInternalData
 {
 	b3AlignedObjectArray<b3RigidBodyData> m_rigidBodies;
+	b3AlignedObjectArray<b3Inertia> m_inertias;
 	b3AlignedObjectArray<b3Aabb> m_aabbWorldSpace;
 
 	b3DynamicBvhBroadphase* m_bp;
@@ -64,7 +68,7 @@ void	b3CpuRigidBodyPipeline::computeOverlappingPairs()
 	int numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
 	m_data->m_bp->calculateOverlappingPairs();
 	numPairs = m_data->m_bp->getOverlappingPairCache()->getNumOverlappingPairs();
-	//printf("numPairs=%d\n",numPairs);
+	printf("numPairs=%d\n",numPairs);
 }
 
 void b3CpuRigidBodyPipeline::computeContactPoints()
@@ -96,6 +100,322 @@ void	b3CpuRigidBodyPipeline::stepSimulation(float deltaTime)
 }
 
 
+static	inline	float b3CalcRelVel(const b3Vector3& l0, const b3Vector3& l1, const b3Vector3& a0, const b3Vector3& a1, 
+					 const b3Vector3& linVel0, const b3Vector3& angVel0, const b3Vector3& linVel1, const b3Vector3& angVel1)
+{
+	return b3Dot(l0, linVel0) + b3Dot(a0, angVel0) + b3Dot(l1, linVel1) + b3Dot(a1, angVel1);
+}
+
+
+static	inline	void b3SetLinearAndAngular(const b3Vector3& n, const b3Vector3& r0, const b3Vector3& r1,
+							 b3Vector3& linear, b3Vector3& angular0, b3Vector3& angular1)
+{
+	linear = -n;
+	angular0 = -b3Cross(r0, n);
+	angular1 = b3Cross(r1, n);
+}
+
+
+
+static inline void b3SolveContact(b3ContactConstraint4& cs, 
+	const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+	const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 
+	float maxRambdaDt[4], float minRambdaDt[4])
+{
+
+	b3Vector3 dLinVelA; dLinVelA.setZero();
+	b3Vector3 dAngVelA; dAngVelA.setZero();
+	b3Vector3 dLinVelB; dLinVelB.setZero();
+	b3Vector3 dAngVelB; dAngVelB.setZero();
+
+	for(int ic=0; ic<4; ic++)
+	{
+		//	dont necessary because this makes change to 0
+		if( cs.m_jacCoeffInv[ic] == 0.f ) continue;
+
+		{
+			b3Vector3 angular0, angular1, linear;
+			b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA;
+			b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB;
+			b3SetLinearAndAngular( (const b3Vector3 &)-cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, linear, angular0, angular1 );
+
+			float rambdaDt = b3CalcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1,
+				linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic];
+			rambdaDt *= cs.m_jacCoeffInv[ic];
+
+			{
+				float prevSum = cs.m_appliedRambdaDt[ic];
+				float updated = prevSum;
+				updated += rambdaDt;
+				updated = b3Max( updated, minRambdaDt[ic] );
+				updated = b3Min( updated, maxRambdaDt[ic] );
+				rambdaDt = updated - prevSum;
+				cs.m_appliedRambdaDt[ic] = updated;
+			}
+
+			b3Vector3 linImp0 = invMassA*linear*rambdaDt;
+			b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
+			b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
+			b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
+#ifdef _WIN32
+            b3Assert(_finite(linImp0.getX()));
+			b3Assert(_finite(linImp1.getX()));
+#endif
+			{
+				linVelA += linImp0;
+				angVelA += angImp0;
+				linVelB += linImp1;
+				angVelB += angImp1;
+			}
+		}
+	}
+
+
+}
+
+
+
+
+
+static inline void b3SolveFriction(b3ContactConstraint4& cs, 
+		const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
+		const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, 
+		float maxRambdaDt[4], float minRambdaDt[4])
+{
+
+	if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
+	const b3Vector3& center = (const b3Vector3&)cs.m_center;
+
+	b3Vector3 n = -(const b3Vector3&)cs.m_linear;
+
+	b3Vector3 tangent[2];
+
+	b3PlaneSpace1 (n, tangent[0],tangent[1]);
+
+	b3Vector3 angular0, angular1, linear;
+	b3Vector3 r0 = center - posA;
+	b3Vector3 r1 = center - posB;
+	for(int i=0; i<2; i++)
+	{
+		b3SetLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
+		float rambdaDt = b3CalcRelVel(linear, -linear, angular0, angular1,
+			linVelA, angVelA, linVelB, angVelB );
+		rambdaDt *= cs.m_fJacCoeffInv[i];
+
+			{
+				float prevSum = cs.m_fAppliedRambdaDt[i];
+				float updated = prevSum;
+				updated += rambdaDt;
+				updated = b3Max( updated, minRambdaDt[i] );
+				updated = b3Min( updated, maxRambdaDt[i] );
+				rambdaDt = updated - prevSum;
+				cs.m_fAppliedRambdaDt[i] = updated;
+			}
+
+		b3Vector3 linImp0 = invMassA*linear*rambdaDt;
+		b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
+		b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
+		b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
+#ifdef _WIN32
+		b3Assert(_finite(linImp0.getX()));
+		b3Assert(_finite(linImp1.getX()));
+#endif
+		linVelA += linImp0;
+		angVelA += angImp0;
+		linVelB += linImp1;
+		angVelB += angImp1;
+	}
+
+	{	//	angular damping for point constraint
+		b3Vector3 ab = ( posB - posA ).normalized();
+		b3Vector3 ac = ( center - posA ).normalized();
+		if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
+		{
+			float angNA = b3Dot( n, angVelA );
+			float angNB = b3Dot( n, angVelB );
+
+			angVelA -= (angNA*0.1f)*n;
+			angVelB -= (angNB*0.1f)*n;
+		}
+	}
+
+}
+
+
+
+
+
+struct b3SolveTask// : public ThreadPool::Task
+{
+	b3SolveTask(b3AlignedObjectArray<b3RigidBodyData>& bodies,  
+				b3AlignedObjectArray<b3Inertia>& shapes, 
+				b3AlignedObjectArray<b3ContactConstraint4>& constraints,
+				int start, int nConstraints,
+				int maxNumBatches,
+				b3AlignedObjectArray<int>* wgUsedBodies, int curWgidx
+				)
+		: m_bodies( bodies ), m_shapes( shapes ), m_constraints( constraints ), m_start( start ), m_nConstraints( nConstraints ),
+		m_solveFriction( true ),m_maxNumBatches(maxNumBatches),
+		m_wgUsedBodies(wgUsedBodies),m_curWgidx(curWgidx)
+	{}
+
+	unsigned short int getType(){ return 0; }
+
+	void run(int tIdx)
+	{
+		b3AlignedObjectArray<int> usedBodies;
+		//printf("run..............\n");
+
+		
+		for (int bb=0;bb<m_maxNumBatches;bb++)
+		{
+			usedBodies.resize(0);
+			for(int ic=m_nConstraints-1; ic>=0; ic--)
+			//for(int ic=0; ic<m_nConstraints; ic++)
+			{
+				
+				int i = m_start + ic;
+				if (m_constraints[i].m_batchIdx != bb)
+					continue;
+
+				float frictionCoeff = b3GetFrictionCoeff(&m_constraints[i]);
+				int aIdx = (int)m_constraints[i].m_bodyA;
+				int bIdx = (int)m_constraints[i].m_bodyB;
+				int localBatch = m_constraints[i].m_batchIdx;
+				b3RigidBodyData& bodyA = m_bodies[aIdx];
+				b3RigidBodyData& bodyB = m_bodies[bIdx];
+
+				if ((bodyA.m_invMass) && (bodyB.m_invMass))
+				{
+				//	printf("aIdx=%d, bIdx=%d\n", aIdx,bIdx);
+				}
+				if (bIdx==10)
+				{
+					//printf("ic(b)=%d, localBatch=%d\n",ic,localBatch);
+				}
+
+				if (aIdx==10)
+				{
+					//printf("ic(a)=%d, localBatch=%d\n",ic,localBatch);
+				}
+				if (usedBodies.size()<(aIdx+1))
+				{
+					usedBodies.resize(aIdx+1,0);
+				}
+		
+				if (usedBodies.size()<(bIdx+1))
+				{
+					usedBodies.resize(bIdx+1,0);
+				}
+
+				if (bodyA.m_invMass)
+				{
+					b3Assert(usedBodies[aIdx]==0);
+					usedBodies[aIdx]++;
+				}
+				
+				if (bodyB.m_invMass)
+				{
+					b3Assert(usedBodies[bIdx]==0);
+					usedBodies[bIdx]++;
+				}
+				
+
+				if( !m_solveFriction )
+				{
+					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
+					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
+
+					b3SolveContact( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, 
+							(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld,
+						maxRambdaDt, minRambdaDt );
+
+				}
+				else
+				{
+					float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
+					float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
+
+					float sum = 0;
+					for(int j=0; j<4; j++)
+					{
+						sum +=m_constraints[i].m_appliedRambdaDt[j];
+					}
+					frictionCoeff = 0.7f;
+					for(int j=0; j<4; j++)
+					{
+						maxRambdaDt[j] = frictionCoeff*sum;
+						minRambdaDt[j] = -maxRambdaDt[j];
+					}
+
+				b3SolveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, 
+						(b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld,
+						maxRambdaDt, minRambdaDt );
+			
+				}
+			}
+
+			if (m_wgUsedBodies)
+			{
+				if (m_wgUsedBodies[m_curWgidx].size()<usedBodies.size())
+				{
+					m_wgUsedBodies[m_curWgidx].resize(usedBodies.size());
+				}
+				for (int i=0;i<usedBodies.size();i++)
+				{
+					if (usedBodies[i])
+					{
+						//printf("cell %d uses body %d\n", m_curWgidx,i);
+						m_wgUsedBodies[m_curWgidx][i]=1;
+					}
+				}
+			}
+
+		}
+
+
+		
+	}
+
+	b3AlignedObjectArray<b3RigidBodyData>& m_bodies;
+	b3AlignedObjectArray<b3Inertia>& m_shapes;
+	b3AlignedObjectArray<b3ContactConstraint4>& m_constraints;
+	b3AlignedObjectArray<int>* m_wgUsedBodies;
+	int m_curWgidx;
+	int m_start;
+	int m_nConstraints;
+	bool m_solveFriction;
+	int m_maxNumBatches;
+};
+
+void b3CpuRigidBodyPipeline::solveContactConstraints()
+{
+	int m_nIterations = 4;
+
+	b3AlignedObjectArray<b3ContactConstraint4> contactConstraints;
+	const b3AlignedObjectArray<b3Contact4Data>& contacts = m_data->m_np->getContacts();
+	int n = contactConstraints.size();
+	//convert contacts...
+
+
+	
+	int maxNumBatches = 250;
+
+	for(int iter=0; iter<m_nIterations; iter++)
+	{
+		b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
+		task.m_solveFriction = false;
+		task.run(0);
+	}
+
+	for(int iter=0; iter<m_nIterations; iter++)
+	{
+		b3SolveTask task( m_data->m_rigidBodies, m_data->m_inertias, contactConstraints, 0, n ,maxNumBatches,0,0);
+		task.m_solveFriction = true;
+		task.run(0);
+	}
+}
+
 void b3CpuRigidBodyPipeline::integrate(float deltaTime)
 {
 	float angDamping=0.f;

src/Bullet3Dynamics/b3CpuRigidBodyPipeline.h

@@ -39,6 +39,7 @@ public:
 	virtual void	updateAabbWorldSpace();
 	virtual void	computeOverlappingPairs();
 	virtual void	computeContactPoints();
+	virtual void	solveContactConstraints();
 
 	int		registerConvexPolyhedron(class b3ConvexUtility* convex);
 

src/Bullet3Dynamics/shared/b3ContactConstraint4.h

@@ -0,0 +1,33 @@
+#ifndef B3_CONTACT_CONSTRAINT5_H
+#define B3_CONTACT_CONSTRAINT5_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+
+typedef struct b3ContactConstraint4 b3ContactConstraint4_t;
+
+struct b3ContactConstraint4
+{
+
+	b3Float4 m_linear;//normal?
+	b3Float4 m_worldPos[4];
+	b3Float4 m_center;	//	friction
+	float m_jacCoeffInv[4];
+	float m_b[4];
+	float m_appliedRambdaDt[4];
+	float m_fJacCoeffInv[2];	//	friction
+	float m_fAppliedRambdaDt[2];	//	friction
+
+	unsigned int m_bodyA;
+	unsigned int m_bodyB;
+	int			m_batchIdx;
+	unsigned int m_paddings;
+
+};
+
+//inline	void setFrictionCoeff(float value) { m_linear[3] = value; }
+inline	float b3GetFrictionCoeff(b3ContactConstraint4* constraint) 
+{
+	return constraint->m_linear[3]; 
+}
+
+#endif //B3_CONTACT_CONSTRAINT5_H

src/Bullet3Dynamics/shared/b3Inertia.h

@@ -0,0 +1,15 @@
+
+
+#ifndef B3_INERTIA_H
+#define B3_INERTIA_H
+
+#include "Bullet3Common/shared/b3Mat3x3.h"
+
+struct b3Inertia
+{
+	b3Mat3x3 m_invInertiaWorld;
+	b3Mat3x3 m_initInvInertia;
+};
+
+
+#endif //B3_INERTIA_H