fix GPU solver (need to clear .w component because "m_linear" contains friction coefficient

added a mixed solver to find bugs like that

demo/gpudemo/GpuDemo.h

@@ -39,7 +39,7 @@ public:
                     preferredOpenCLPlatformIndex(-1),
                     preferredOpenCLDeviceIndex(-1),
 					arraySizeX(1),
-		arraySizeY(5),
+		arraySizeY(2),
 		arraySizeZ(1),
 		m_useConcaveMesh(false),
 		gapX(14.3),

demo/gpudemo/rigidbody/GpuSphereScene.cpp

@@ -123,7 +123,7 @@ void GpuSphereScene::setupScene(const ConstructionInfo& ci)
 						mass=0.f;
 
 					//btVector3 position((j&1)+i*2.2,2+j*2.,(j&1)+k*2.2);
-					btVector3 position(i*2.2,2+j*2.,k*2.2);
+					btVector3 position(i*2.2,2+j*4.,k*2.2);
 					
 					btQuaternion orn(0,0,0,1);
 				

opencl/gpu_rigidbody/host/btGpuBatchingPgsSolver.cpp

@@ -38,7 +38,7 @@ enum
 
 
 bool gpuBatchContacts = true;//true;
-bool gpuSolveConstraint = false;//true;//true;
+bool gpuSolveConstraint = true;//true;
 
 
 struct	btGpuBatchingPgsSolverInternalData

opencl/gpu_rigidbody/host/btGpuJacobiSolver.cpp

@@ -921,4 +921,438 @@ void  btGpuJacobiSolver::solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenC
 
 
 
+}
+
+
+void  btGpuJacobiSolver::solveGroupMixed(btOpenCLArray<btRigidBodyCL>* bodiesGPU,btOpenCLArray<btInertiaCL>* inertiasGPU,btOpenCLArray<btContact4>* manifoldPtrGPU,const btJacobiSolverInfo& solverInfo)
+{
+
+	btAlignedObjectArray<btRigidBodyCL> bodiesCPU;
+	bodiesGPU->copyToHost(bodiesCPU);
+	btAlignedObjectArray<btInertiaCL> inertiasCPU;
+	inertiasGPU->copyToHost(inertiasCPU);
+	btAlignedObjectArray<btContact4> manifoldPtrCPU;
+	manifoldPtrGPU->copyToHost(manifoldPtrCPU);
+	
+	int numBodiesCPU = bodiesGPU->size();
+	int numManifoldsCPU = manifoldPtrGPU->size();
+	BT_PROFILE("btGpuJacobiSolver::solveGroupMixed");
+
+	btAlignedObjectArray<unsigned int> bodyCount;
+	bodyCount.resize(numBodiesCPU);
+	for (int i=0;i<numBodiesCPU;i++)
+		bodyCount[i] = 0;
+
+	btAlignedObjectArray<btInt2> contactConstraintOffsets;
+	contactConstraintOffsets.resize(numManifoldsCPU);
+
+
+	for (int i=0;i<numManifoldsCPU;i++)
+	{
+		int pa = manifoldPtrCPU[i].m_bodyAPtrAndSignBit;
+		int pb = manifoldPtrCPU[i].m_bodyBPtrAndSignBit;
+
+		bool isFixedA = (pa <0) || (pa == solverInfo.m_fixedBodyIndex);
+		bool isFixedB = (pb <0) || (pb == solverInfo.m_fixedBodyIndex);
+
+		int bodyIndexA = manifoldPtrCPU[i].getBodyA();
+		int bodyIndexB = manifoldPtrCPU[i].getBodyB();
+
+		if (!isFixedA)
+		{
+			contactConstraintOffsets[i].x = bodyCount[bodyIndexA];
+			bodyCount[bodyIndexA]++;
+		}
+		if (!isFixedB)
+		{
+			contactConstraintOffsets[i].y = bodyCount[bodyIndexB];
+			bodyCount[bodyIndexB]++;
+		} 
+	}
+
+	btAlignedObjectArray<unsigned int> offsetSplitBodies;
+	offsetSplitBodies.resize(numBodiesCPU);
+	unsigned int totalNumSplitBodiesCPU;
+	m_data->m_scan->executeHost(bodyCount,offsetSplitBodies,numBodiesCPU,&totalNumSplitBodiesCPU);
+	int numlastBody = bodyCount[numBodiesCPU-1];
+	totalNumSplitBodiesCPU += numlastBody;
+
+		int numBodies = bodiesGPU->size();
+	int numManifolds = manifoldPtrGPU->size();
+
+	m_data->m_bodyCount->resize(numBodies);
+	
+	unsigned int val=0;
+	btInt2 val2;
+	val2.x=0;
+	val2.y=0;
+
+	 {
+		BT_PROFILE("m_filler");
+		m_data->m_contactConstraintOffsets->resize(numManifolds);
+		m_data->m_filler->execute(*m_data->m_bodyCount,val,numBodies);
+		
+	
+		m_data->m_filler->execute(*m_data->m_contactConstraintOffsets,val2,numManifolds);
+	}
+
+	{
+		BT_PROFILE("m_countBodiesKernel");
+		btLauncherCL launcher(this->m_queue,m_data->m_countBodiesKernel);
+		launcher.setBuffer(manifoldPtrGPU->getBufferCL());
+		launcher.setBuffer(m_data->m_bodyCount->getBufferCL());
+		launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL());
+		launcher.setConst(numManifolds);
+		launcher.setConst(solverInfo.m_fixedBodyIndex);
+		launcher.launch1D(numManifolds);
+	}
+
+	unsigned int totalNumSplitBodies=0;
+	m_data->m_offsetSplitBodies->resize(numBodies);
+	m_data->m_scan->execute(*m_data->m_bodyCount,*m_data->m_offsetSplitBodies,numBodies,&totalNumSplitBodies);
+	totalNumSplitBodies+=m_data->m_bodyCount->at(numBodies-1);
+
+	if (totalNumSplitBodies != totalNumSplitBodiesCPU)
+	{
+		printf("error in totalNumSplitBodies!\n");
+	}
+
+	int numContacts = manifoldPtrGPU->size();
+	m_data->m_contactConstraints->resize(numContacts);
+
+	
+	{
+		BT_PROFILE("contactToConstraintSplitKernel");
+		btLauncherCL launcher( m_queue, m_data->m_contactToConstraintSplitKernel);
+		launcher.setBuffer(manifoldPtrGPU->getBufferCL());
+		launcher.setBuffer(bodiesGPU->getBufferCL());
+		launcher.setBuffer(inertiasGPU->getBufferCL());
+		launcher.setBuffer(m_data->m_contactConstraints->getBufferCL());
+		launcher.setBuffer(m_data->m_bodyCount->getBufferCL());
+        launcher.setConst(numContacts);
+		launcher.setConst(solverInfo.m_deltaTime);
+		launcher.setConst(solverInfo.m_positionDrift);
+		launcher.setConst(solverInfo.m_positionConstraintCoeff);
+		launcher.launch1D( numContacts, 64 );
+		clFinish(m_queue);
+	}
+
+
+
+	btAlignedObjectArray<btGpuConstraint4> contactConstraints;
+	contactConstraints.resize(numManifoldsCPU);
+
+	for (int i=0;i<numManifoldsCPU;i++)
+	{
+		ContactToConstraintKernel(&manifoldPtrCPU[0],&bodiesCPU[0],&inertiasCPU[0],&contactConstraints[0],numManifoldsCPU,
+			solverInfo.m_deltaTime,
+			solverInfo.m_positionDrift,
+			solverInfo.m_positionConstraintCoeff,
+			i, bodyCount);
+	}
+	int maxIter = solverInfo.m_numIterations;
+
+
+	btAlignedObjectArray<btVector3> deltaLinearVelocities;
+	btAlignedObjectArray<btVector3> deltaAngularVelocities;
+	deltaLinearVelocities.resize(totalNumSplitBodiesCPU);
+	deltaAngularVelocities.resize(totalNumSplitBodiesCPU);
+	for (int i=0;i<totalNumSplitBodiesCPU;i++)
+	{
+		deltaLinearVelocities[i].setZero();
+		deltaAngularVelocities[i].setZero();
+	}
+
+	m_data->m_deltaLinearVelocities->resize(totalNumSplitBodies);
+	m_data->m_deltaAngularVelocities->resize(totalNumSplitBodies);
+
+
+	
+	{
+		BT_PROFILE("m_clearVelocitiesKernel");
+		btLauncherCL launch(m_queue,m_data->m_clearVelocitiesKernel);
+		launch.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL());
+		launch.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL());
+		launch.setConst(totalNumSplitBodies);
+		launch.launch1D(totalNumSplitBodies);
+	}
+	
+
+		///!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+
+
+	m_data->m_contactConstraints->copyToHost(contactConstraints);
+	m_data->m_offsetSplitBodies->copyToHost(offsetSplitBodies);
+	m_data->m_contactConstraintOffsets->copyToHost(contactConstraintOffsets);
+	m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities);
+	m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities);
+
+	for (int iter = 0;iter<maxIter;iter++)
+	{
+
+				{
+			BT_PROFILE("m_solveContactKernel");
+			btLauncherCL launcher( m_queue, m_data->m_solveContactKernel );
+			launcher.setBuffer(m_data->m_contactConstraints->getBufferCL());
+			launcher.setBuffer(bodiesGPU->getBufferCL());
+			launcher.setBuffer(inertiasGPU->getBufferCL());
+			launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL());
+			launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL());
+			launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL());
+			launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL());
+			launcher.setConst(solverInfo.m_deltaTime);
+			launcher.setConst(solverInfo.m_positionDrift);
+			launcher.setConst(solverInfo.m_positionConstraintCoeff);
+			launcher.setConst(solverInfo.m_fixedBodyIndex);
+			launcher.setConst(numManifolds);
+
+			launcher.launch1D(numManifolds);
+			clFinish(m_queue);
+		}
+
+
+		int i=0;
+		for( i=0; i<numManifoldsCPU; i++)
+		{
+
+			float frictionCoeff = contactConstraints[i].getFrictionCoeff();
+			int aIdx = (int)contactConstraints[i].m_bodyA;
+			int bIdx = (int)contactConstraints[i].m_bodyB;
+			btRigidBodyCL& bodyA = bodiesCPU[aIdx];
+			btRigidBodyCL& bodyB = bodiesCPU[bIdx];
+
+			btVector3 zero(0,0,0);
+			
+			btVector3* dlvAPtr=&zero;
+			btVector3* davAPtr=&zero;
+			btVector3* dlvBPtr=&zero;
+			btVector3* davBPtr=&zero;
+			
+			if (bodyA.getInvMass())
+			{
+				int bodyOffsetA = offsetSplitBodies[aIdx];
+				int constraintOffsetA = contactConstraintOffsets[i].x;
+				int splitIndexA = bodyOffsetA+constraintOffsetA;
+				dlvAPtr = &deltaLinearVelocities[splitIndexA];
+				davAPtr = &deltaAngularVelocities[splitIndexA];
+			}
+
+			if (bodyB.getInvMass())
+			{
+				int bodyOffsetB = offsetSplitBodies[bIdx];
+				int constraintOffsetB = contactConstraintOffsets[i].y;
+				int splitIndexB= bodyOffsetB+constraintOffsetB;
+				dlvBPtr =&deltaLinearVelocities[splitIndexB];
+				davBPtr = &deltaAngularVelocities[splitIndexB];
+			}
+
+
+
+			{
+				float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX};
+				float minRambdaDt[4] = {0.f,0.f,0.f,0.f};
+
+				solveContact( contactConstraints[i], (btVector3&)bodyA.m_pos, (btVector3&)bodyA.m_linVel, (btVector3&)bodyA.m_angVel, bodyA.m_invMass, inertiasCPU[aIdx].m_invInertiaWorld, 
+					(btVector3&)bodyB.m_pos, (btVector3&)bodyB.m_linVel, (btVector3&)bodyB.m_angVel, bodyB.m_invMass, inertiasCPU[bIdx].m_invInertiaWorld,
+					maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr		);
+
+
+			}
+		}
+
+		
+		{
+			BT_PROFILE("average velocities");
+			btLauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel);
+			launcher.setBuffer(bodiesGPU->getBufferCL());
+			launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL());
+			launcher.setBuffer(m_data->m_bodyCount->getBufferCL());
+			launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL());
+			launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL());
+			launcher.setConst(numBodies);
+			launcher.launch1D(numBodies);
+			clFinish(m_queue);
+		}
+
+		//easy
+		for (int i=0;i<numBodiesCPU;i++)
+		{
+			if (bodiesCPU[i].getInvMass())
+			{
+				int bodyOffset = offsetSplitBodies[i];
+				int count = bodyCount[i];
+				float factor = 1.f/float(count);
+				btVector3 averageLinVel;
+				averageLinVel.setZero();
+				btVector3 averageAngVel;
+				averageAngVel.setZero();
+				for (int j=0;j<count;j++)
+				{
+					averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;
+					averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;
+				}
+				for (int j=0;j<count;j++)
+				{
+					deltaLinearVelocities[bodyOffset+j] = averageLinVel;
+					deltaAngularVelocities[bodyOffset+j] = averageAngVel;
+				}
+			}
+		}
+//	m_data->m_deltaAngularVelocities->copyFromHost(deltaAngularVelocities);
+	//m_data->m_deltaLinearVelocities->copyFromHost(deltaLinearVelocities);
+	m_data->m_deltaAngularVelocities->copyToHost(deltaAngularVelocities);
+	m_data->m_deltaLinearVelocities->copyToHost(deltaLinearVelocities);
+
+#if 0
+
+		{
+			BT_PROFILE("m_solveFrictionKernel");
+			btLauncherCL launcher( m_queue, m_data->m_solveFrictionKernel);
+			launcher.setBuffer(m_data->m_contactConstraints->getBufferCL());
+			launcher.setBuffer(bodiesGPU->getBufferCL());
+			launcher.setBuffer(inertiasGPU->getBufferCL());
+			launcher.setBuffer(m_data->m_contactConstraintOffsets->getBufferCL());
+			launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL());
+			launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL());
+			launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL());
+			launcher.setConst(solverInfo.m_deltaTime);
+			launcher.setConst(solverInfo.m_positionDrift);
+			launcher.setConst(solverInfo.m_positionConstraintCoeff);
+			launcher.setConst(solverInfo.m_fixedBodyIndex);
+			launcher.setConst(numManifolds);
+
+			launcher.launch1D(numManifolds);
+			clFinish(m_queue);
+		}
+
+		//solve friction
+
+		for(int i=0; i<numManifoldsCPU; i++)
+		{
+			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 +=contactConstraints[i].m_appliedRambdaDt[j];
+			}
+			float frictionCoeff = contactConstraints[i].getFrictionCoeff();
+			int aIdx = (int)contactConstraints[i].m_bodyA;
+			int bIdx = (int)contactConstraints[i].m_bodyB;
+			btRigidBodyCL& bodyA = bodiesCPU[aIdx];
+			btRigidBodyCL& bodyB = bodiesCPU[bIdx];
+
+			btVector3 zero(0,0,0);
+			
+			btVector3* dlvAPtr=&zero;
+			btVector3* davAPtr=&zero;
+			btVector3* dlvBPtr=&zero;
+			btVector3* davBPtr=&zero;
+			
+			if (bodyA.getInvMass())
+			{
+				int bodyOffsetA = offsetSplitBodies[aIdx];
+				int constraintOffsetA = contactConstraintOffsets[i].x;
+				int splitIndexA = bodyOffsetA+constraintOffsetA;
+				dlvAPtr = &deltaLinearVelocities[splitIndexA];
+				davAPtr = &deltaAngularVelocities[splitIndexA];
+			}
+
+			if (bodyB.getInvMass())
+			{
+				int bodyOffsetB = offsetSplitBodies[bIdx];
+				int constraintOffsetB = contactConstraintOffsets[i].y;
+				int splitIndexB= bodyOffsetB+constraintOffsetB;
+				dlvBPtr =&deltaLinearVelocities[splitIndexB];
+				davBPtr = &deltaAngularVelocities[splitIndexB];
+			}
+
+			for(int j=0; j<4; j++)
+			{
+				maxRambdaDt[j] = frictionCoeff*sum;
+				minRambdaDt[j] = -maxRambdaDt[j];
+			}
+
+			solveFriction( contactConstraints[i], (btVector3&)bodyA.m_pos, (btVector3&)bodyA.m_linVel, (btVector3&)bodyA.m_angVel, bodyA.m_invMass,inertiasCPU[aIdx].m_invInertiaWorld, 
+				(btVector3&)bodyB.m_pos, (btVector3&)bodyB.m_linVel, (btVector3&)bodyB.m_angVel, bodyB.m_invMass, inertiasCPU[bIdx].m_invInertiaWorld,
+				maxRambdaDt, minRambdaDt , *dlvAPtr,*davAPtr,*dlvBPtr,*davBPtr);
+
+		}
+
+		{
+			BT_PROFILE("average velocities");
+			btLauncherCL launcher( m_queue, m_data->m_averageVelocitiesKernel);
+			launcher.setBuffer(bodiesGPU->getBufferCL());
+			launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL());
+			launcher.setBuffer(m_data->m_bodyCount->getBufferCL());
+			launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL());
+			launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL());
+			launcher.setConst(numBodies);
+			launcher.launch1D(numBodies);
+			clFinish(m_queue);
+		}
+
+		//easy
+		for (int i=0;i<numBodiesCPU;i++)
+		{
+			if (bodiesCPU[i].getInvMass())
+			{
+				int bodyOffset = offsetSplitBodies[i];
+				int count = bodyCount[i];
+				float factor = 1.f/float(count);
+				btVector3 averageLinVel;
+				averageLinVel.setZero();
+				btVector3 averageAngVel;
+				averageAngVel.setZero();
+				for (int j=0;j<count;j++)
+				{
+					averageLinVel += deltaLinearVelocities[bodyOffset+j]*factor;
+					averageAngVel += deltaAngularVelocities[bodyOffset+j]*factor;
+				}
+				for (int j=0;j<count;j++)
+				{
+					deltaLinearVelocities[bodyOffset+j] = averageLinVel;
+					deltaAngularVelocities[bodyOffset+j] = averageAngVel;
+				}
+			}
+		}
+
+#endif
+
+	}
+
+	{
+		BT_PROFILE("update body velocities");
+		btLauncherCL launcher( m_queue, m_data->m_updateBodyVelocitiesKernel);
+		launcher.setBuffer(bodiesGPU->getBufferCL());
+		launcher.setBuffer(m_data->m_offsetSplitBodies->getBufferCL());
+		launcher.setBuffer(m_data->m_bodyCount->getBufferCL());
+		launcher.setBuffer(m_data->m_deltaLinearVelocities->getBufferCL());
+		launcher.setBuffer(m_data->m_deltaAngularVelocities->getBufferCL());
+		launcher.setConst(numBodies);
+		launcher.launch1D(numBodies);
+		clFinish(m_queue);
+	}
+
+
+	//easy
+	for (int i=0;i<numBodiesCPU;i++)
+	{
+		if (bodiesCPU[i].getInvMass())
+		{
+			int bodyOffset = offsetSplitBodies[i];
+			int count = bodyCount[i];
+			if (count)
+			{
+				bodiesCPU[i].m_linVel += deltaLinearVelocities[bodyOffset];
+				bodiesCPU[i].m_angVel += deltaAngularVelocities[bodyOffset];
+			}
+		}
+	}
+
+
+//	bodiesGPU->copyFromHost(bodiesCPU);
+
+
 }

opencl/gpu_rigidbody/host/btGpuJacobiSolver.h

@@ -46,6 +46,7 @@ public:
 
 	void  solveGroupHost(btRigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,btContact4* manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btJacobiSolverInfo& solverInfo);
 	void  solveGroup(btOpenCLArray<btRigidBodyCL>* bodies,btOpenCLArray<btInertiaCL>* inertias,btOpenCLArray<btContact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo);
+	void  solveGroupMixed(btOpenCLArray<btRigidBodyCL>* bodies,btOpenCLArray<btInertiaCL>* inertias,btOpenCLArray<btContact4>* manifoldPtr,const btJacobiSolverInfo& solverInfo);
 
 };
 #endif //BT_GPU_JACOBI_SOLVER_H

opencl/gpu_rigidbody/kernels/solveContact.cl

@@ -237,7 +237,7 @@ void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4
 
 void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
 {
-	*linear = -n;
+	*linear = mymake_float4(-n.xyz,0.f);
 	*angular0 = -cross3(r0, n);
 	*angular1 = cross3(r1, n);
 }

opencl/gpu_rigidbody/kernels/solveContact.h

@@ -239,7 +239,7 @@ static const char* solveContactCL= \
 "\n"
 "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
 "{\n"
-"	*linear = -n;\n"
+"	*linear = mymake_float4(-n.xyz,0.f);\n"
 "	*angular0 = -cross3(r0, n);\n"
 "	*angular1 = cross3(r1, n);\n"
 "}\n"

opencl/gpu_rigidbody/kernels/solveFriction.cl

@@ -237,7 +237,7 @@ void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4
 
 void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
 {
-	*linear = -n;
+	*linear = mymake_float4(-n.xyz,0.f);
 	*angular0 = -cross3(r0, n);
 	*angular1 = cross3(r1, n);
 }

opencl/gpu_rigidbody/kernels/solveFriction.h

@@ -239,7 +239,7 @@ static const char* solveFrictionCL= \
 "\n"
 "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
 "{\n"
-"	*linear = -n;\n"
+"	*linear = mymake_float4(-n.xyz,0.f);\n"
 "	*angular0 = -cross3(r0, n);\n"
 "	*angular1 = cross3(r1, n);\n"
 "}\n"

opencl/gpu_rigidbody/kernels/solverSetup.cl

@@ -435,7 +435,7 @@ typedef struct
 
 void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
 {
-	*linear = -n;
+	*linear = make_float4(-n.xyz,0.f);
 	*angular0 = -cross3(r0, n);
 	*angular1 = cross3(r1, n);
 }

opencl/gpu_rigidbody/kernels/solverSetup.h

@@ -437,7 +437,7 @@ static const char* solverSetupCL= \
 "\n"
 "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
 "{\n"
-"	*linear = -n;\n"
+"	*linear = make_float4(-n.xyz,0.f);\n"
 "	*angular0 = -cross3(r0, n);\n"
 "	*angular1 = cross3(r1, n);\n"
 "}\n"

opencl/gpu_rigidbody/kernels/solverUtils.cl

@@ -462,7 +462,7 @@ __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,
 
 void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
 {
-	*linear = -n;
+	*linear = make_float4(-n.xyz,0.f);
 	*angular0 = -cross3(r0, n);
 	*angular1 = cross3(r1, n);
 }
@@ -537,10 +537,12 @@ void solveContact(__global Constraint4* cs,
 		setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );
 	
 
+
 		float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, 
 			*linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];
 		rambdaDt *= cs->m_jacCoeffInv[ic];
 
+		
 		{
 			float prevSum = cs->m_appliedRambdaDt[ic];
 			float updated = prevSum;
@@ -550,12 +552,14 @@ void solveContact(__global Constraint4* cs,
 			rambdaDt = updated - prevSum;
 			cs->m_appliedRambdaDt[ic] = updated;
 		}
-		
+
+			
 		float4 linImp0 = invMassA*linear*rambdaDt;
 		float4 linImp1 = invMassB*(-linear)*rambdaDt;
 		float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
 		float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
 
+		
 		if (invMassA)
 		{
 			*dLinVelA += linImp0;

opencl/gpu_rigidbody/kernels/solverUtils.h

@@ -464,7 +464,7 @@ static const char* solverUtilsCL= \
 "\n"
 "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
 "{\n"
-"	*linear = -n;\n"
+"	*linear = make_float4(-n.xyz,0.f);\n"
 "	*angular0 = -cross3(r0, n);\n"
 "	*angular1 = cross3(r1, n);\n"
 "}\n"
@@ -539,10 +539,12 @@ static const char* solverUtilsCL= \
 "		setLinearAndAngular( -cs->m_linear, r0, r1, &linear, &angular0, &angular1 );\n"
 "	\n"
 "\n"
+"\n"
 "		float rambdaDt = calcRelVel( cs->m_linear, -cs->m_linear, angular0, angular1, \n"
 "			*linVelA+*dLinVelA, *angVelA+*dAngVelA, *linVelB+*dLinVelB, *angVelB+*dAngVelB ) + cs->m_b[ic];\n"
 "		rambdaDt *= cs->m_jacCoeffInv[ic];\n"
 "\n"
+"		\n"
 "		{\n"
 "			float prevSum = cs->m_appliedRambdaDt[ic];\n"
 "			float updated = prevSum;\n"
@@ -552,12 +554,14 @@ static const char* solverUtilsCL= \
 "			rambdaDt = updated - prevSum;\n"
 "			cs->m_appliedRambdaDt[ic] = updated;\n"
 "		}\n"
-"		\n"
+"\n"
+"			\n"
 "		float4 linImp0 = invMassA*linear*rambdaDt;\n"
 "		float4 linImp1 = invMassB*(-linear)*rambdaDt;\n"
 "		float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;\n"
 "		float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;\n"
 "\n"
+"		\n"
 "		if (invMassA)\n"
 "		{\n"
 "			*dLinVelA += linImp0;\n"