contact normal should point from B to A (to be consistent with Bullet 2.x)

stringify: pre-allocate bigger buffer (10MB, workaround)

build3/stringifyKernel.lua

@@ -1,7 +1,7 @@
 
 
 function stringifyKernel(filenameIn, filenameOut, kernelMethod)
-  local BUFSIZE = 1024*1024     -- 1MB
+  local BUFSIZE = 10*1024*1024     -- 10MB
 	local f = io.open(filenameIn,"r");
    	local fw = io.open(filenameOut,"w");
    	fw:write("//this file is autogenerated using stringify.bat (premake --stringify) in the build folder of this project\n")

src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h

@@ -36,9 +36,9 @@ B3_ATTRIBUTE_ALIGNED16(struct) b3Contact4 : public b3Contact4Data
 	void setFrictionCoeff( float c ) { b3Assert( c >= 0.f && c <= 1.f ); m_frictionCoeffCmp = (unsigned short)(c*0xffff); }
 
 	//float& getNPoints() { return m_worldNormal[3]; }
-	int getNPoints() const { return (int) m_worldNormal[3]; }
+	int getNPoints() const { return (int) m_worldNormalOnB[3]; }
 
-	float getPenetration(int idx) const { return m_worldPos[idx][3]; }
+	float getPenetration(int idx) const { return m_worldPosB[idx][3]; }
 
 	bool isInvalid() const { return (getBodyA()==0 || getBodyB()==0); }
 };

src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h

@@ -7,9 +7,10 @@ typedef  struct b3Contact4Data b3Contact4Data_t;
 
 struct b3Contact4Data
 {
-	b3Float4	m_worldPos[4];
+	b3Float4	m_worldPosB[4];
+//	b3Float4	m_localPosA[4];
 //	b3Float4	m_localPosB[4];
-	b3Float4	m_worldNormal;	//	w: m_nPoints
+	b3Float4	m_worldNormalOnB;	//	w: m_nPoints
 	unsigned short  m_restituitionCoeffCmp;
 	unsigned short  m_frictionCoeffCmp;
 	int m_batchIdx;
@@ -21,17 +22,17 @@ struct b3Contact4Data
 	int m_unused1;
 	int m_unused2;
 
-//	b3Float4	m_localPosA;
+
 };
 
 inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
 {
-	return (int)contact->m_worldNormal.w;
+	return (int)contact->m_worldNormalOnB.w;
 };
 
 inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)
 {
-	contact->m_worldNormal.w = (float)numPoints;
+	contact->m_worldNormalOnB.w = (float)numPoints;
 };
 
 

src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp

@@ -118,9 +118,8 @@ void	getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& poin
 	pointOut.m_contactMotion1 = 0.f;
 	pointOut.m_contactMotion2 = 0.f;
 	pointOut.m_distance = contact->getPenetration(contactIndex);//??0.01f
-	b3Vector3 n = contact->m_worldNormal;
+	b3Vector3 normalOnB = contact->m_worldNormalOnB;
-	b3Vector3 normalOnB(-n);
+	normalOnB.normalize();//is this needed?
-	normalOnB.normalize();
 
 	b3Vector3 l1,l2;
 	b3PlaneSpace1(normalOnB,l1,l2);
@@ -132,7 +131,7 @@ void	getContactPoint(b3Contact4* contact, int contactIndex, b3ContactPoint& poin
 	pointOut.m_lateralFrictionInitialized = true;
 	
 	
-	b3Vector3 worldPosB = contact->m_worldPos[contactIndex];
+	b3Vector3 worldPosB = contact->m_worldPosB[contactIndex];
 	pointOut.m_positionWorldOnB = worldPosB;
 	pointOut.m_positionWorldOnA = worldPosB+normalOnB*pointOut.m_distance;
 }

src/Bullet3OpenCL/NarrowphaseCollision/b3ContactCache.cpp

@@ -194,12 +194,13 @@ void b3ContactCache::removeContactPoint(struct b3Contact4Data& newContactCache,i
 	{
 		b3Swap(newContactCache.m_localPosA[i],newContactCache.m_localPosA[numContacts-1]);
 		b3Swap(newContactCache.m_localPosB[i],newContactCache.m_localPosB[numContacts-1]);
-		b3Swap(newContactCache.m_worldPos[i],newContactCache.m_worldPos[numContacts-1]);
+		b3Swap(newContactCache.m_worldPosB[i],newContactCache.m_worldPosB[numContacts-1]);
 	}
 	b3Contact4Data_setNumPoints(&newContactCache,numContacts-1);
 
 }
 
+
 void b3ContactCache::refreshContactPoints(const b3Transform& trA,const b3Transform& trB, struct b3Contact4Data& contacts)
 {
 
@@ -212,9 +213,9 @@ void b3ContactCache::refreshContactPoints(const b3Transform& trA,const b3Transfo
 	{
 		b3Vector3 worldPosA = trA( contacts.m_localPosA[i]);
 		b3Vector3 worldPosB = trB( contacts.m_localPosB[i]);
-		contacts.m_worldPos[i] = worldPosB;
+		contacts.m_worldPosB[i] = worldPosB;
-		float distance = (worldPosA -  worldPosB).dot(contacts.m_worldNormal);
+		float distance = (worldPosA -  worldPosB).dot(contacts.m_worldNormalOnB);
-		contacts.m_worldPos[i].w = distance;
+		contacts.m_worldPosB[i].w = distance;
 	}
 
 	/// then 
@@ -224,7 +225,7 @@ void b3ContactCache::refreshContactPoints(const b3Transform& trA,const b3Transfo
 	{
 		b3Vector3 worldPosA = trA( contacts.m_localPosA[i]);
 		b3Vector3 worldPosB = trB( contacts.m_localPosB[i]);
-		b3Vector3&pt = contacts.m_worldPos[i];
+		b3Vector3&pt = contacts.m_worldPosB[i];
 		//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
 		if (!validContactDistance(pt))
 		{
@@ -232,8 +233,8 @@ void b3ContactCache::refreshContactPoints(const b3Transform& trA,const b3Transfo
 		} else
 		{
 			//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
-			projectedPoint = contacts.m_worldPos[i] - contacts.m_worldNormal * contacts.m_worldPos[i].w;
+			projectedPoint = worldPosA - contacts.m_worldNormalOnB * contacts.m_worldPosB[i].w;
-			projectedDifference = contacts.m_worldPos[i] - projectedPoint;
+			projectedDifference = contacts.m_worldPosB[i] - projectedPoint;
 			distance2d = projectedDifference.dot(projectedDifference);
 			if (distance2d  > gContactBreakingThreshold*gContactBreakingThreshold )
 			{

src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp

@@ -530,7 +530,7 @@ void computeContactPlaneConvex(int pairIndex,
 			nGlobalContactsOut++;
 
 			b3Contact4* c = &globalContactsOut[dstIdx];
-			c->m_worldNormal = planeNormalWorld;
+			c->m_worldNormalOnB = -planeNormalWorld;
 			c->setFrictionCoeff(0.7);
 			c->setRestituitionCoeff(0.f);
 
@@ -540,9 +540,9 @@ void computeContactPlaneConvex(int pairIndex,
 			for (int i=0;i<numReducedPoints;i++)
 			{
 				b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
-				c->m_worldPos[i] = pOnB1;
+				c->m_worldPosB[i] = pOnB1;
 			}
-			c->m_worldNormal[3] = (b3Scalar)numReducedPoints;
+			c->m_worldNormalOnB[3] = (b3Scalar)numReducedPoints;
 		}//if (dstIdx < numPairs)
 	}	
 		
@@ -668,7 +668,7 @@ void computeContactPlaneCompound(int pairIndex,
 			nGlobalContactsOut++;
 
 			b3Contact4* c = &globalContactsOut[dstIdx];
-			c->m_worldNormal = planeNormalWorld;
+			c->m_worldNormalOnB = -planeNormalWorld;
 			c->setFrictionCoeff(0.7);
 			c->setRestituitionCoeff(0.f);
 
@@ -678,9 +678,9 @@ void computeContactPlaneCompound(int pairIndex,
 			for (int i=0;i<numReducedPoints;i++)
 			{
 				b3Vector3 pOnB1 = contactPoints[contactIdx.s[i]];
-				c->m_worldPos[i] = pOnB1;
+				c->m_worldPosB[i] = pOnB1;
 			}
-			c->m_worldNormal[3] = (b3Scalar)numReducedPoints;
+			c->m_worldNormalOnB[3] = (b3Scalar)numReducedPoints;
 		}//if (dstIdx < numPairs)
 	}	
 		
@@ -811,7 +811,7 @@ void	computeContactSphereConvex(int pairIndex,
 	if (bCollide && minDist > -10000)
 	{
 		
-		float4 normalOnSurfaceB1 = tr.getBasis()*-localHitNormal;//-hitNormalWorld;
+		float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld;
 		float4 pOnB1 = tr(closestPnt);
 		//printf("dist ,%f,",minDist);
 		float actualDepth = minDist-radius;
@@ -831,16 +831,16 @@ void	computeContactSphereConvex(int pairIndex,
 			nGlobalContactsOut++;
 
 			b3Contact4* c = &globalContactsOut[dstIdx];
-			c->m_worldNormal = normalOnSurfaceB1;
+			c->m_worldNormalOnB = normalOnSurfaceB1;
 			c->setFrictionCoeff(0.7);
 			c->setRestituitionCoeff(0.f);
 
 			c->m_batchIdx = pairIndex;
 			c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
 			c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
-			c->m_worldPos[0] = pOnB1;
+			c->m_worldPosB[0] = pOnB1;
 			int numPoints = 1;
-			c->m_worldNormal[3] = (b3Scalar)numPoints;
+			c->m_worldNormalOnB[3] = (b3Scalar)numPoints;
 		}//if (dstIdx < numPairs)
 		}
 	}//if (hasCollision)
@@ -1520,7 +1520,7 @@ int clipHullHullSingle(
 		{
 			B3_PROFILE("overlap");
 
-			float4 normalOnSurfaceB = -(float4&)hostNormal;
+			float4 normalOnSurfaceB = (float4&)hostNormal;
 			float4 centerOut;
 			
 			b3Int4 contactIdx;
@@ -1553,11 +1553,11 @@ int clipHullHullSingle(
 				float distance = 0.f;
 				for (int p=0;p<numPoints;p++)
 				{
-					contact.m_worldPos[p] = contactsOut[contactIdx.s[p]];
+					contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];//check if it is actually on B
-					contact.m_worldNormal = normalOnSurfaceB; 
+					contact.m_worldNormalOnB = normalOnSurfaceB; 
 				}
 				//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
-				contact.m_worldNormal.w = (b3Scalar)numPoints;
+				contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
 				nContacts++;
 			} else
 			{
@@ -1638,7 +1638,7 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& pairs,
 					
 			
 			int numPoints = 0;
-			if (0)//pairs[pairIndex].z>=0)
+			if (pairs[pairIndex].z>=0)
 			{
 				//printf("add existing points?\n");
 				//refresh
@@ -1647,7 +1647,9 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& pairs,
 				if (numOldPoints)
 				{
 					newContact = oldContacts[pairs[pairIndex].z];
-					//b3ContactCache::refreshContactPoints(transA,transB,newContact);
+#ifdef CHECK_ON_HOST
+					b3ContactCache::refreshContactPoints(transA,transB,newContact);
+#endif //CHECK_ON_HOST
 				}
 				numPoints = b3Contact4Data_getNumPoints(&newContact);
 
@@ -1666,20 +1668,28 @@ int computeContactConvexConvex( b3AlignedObjectArray<b3Int4>& pairs,
 			*/
 			
 			int p=numPoints;
-			if (numPoints<3)
+			if (numPoints<4)
 			{
 				numPoints++;
+			} else
+			{
+				p=3;
 			}
+
 			{
 				resultPointOnBWorld.w = distance2;
-				newContact.m_worldPos[p] = resultPointOnBWorld;
+				newContact.m_worldPosB[p] = resultPointOnBWorld;
 				b3Vector3 resultPointOnAWorld = resultPointOnBWorld+distance2*sepAxis2;
-				//newContact.m_localPosA[p] = transA.inverse()*resultPointOnAWorld;
+#ifdef CHECK_ON_HOST
-			//	newContact.m_localPosB[p] = transB.inverse()*resultPointOnBWorld;
+				newContact.m_localPosA[p] = transA.inverse()*resultPointOnAWorld;
-				newContact.m_worldNormal = sepAxis2; 
+				newContact.m_localPosB[p] = transB.inverse()*resultPointOnBWorld;
+#endif
+				newContact.m_worldNormalOnB = sepAxis2; 
 			}
 			//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
-			newContact.m_worldNormal.w = (b3Scalar)numPoints;
+			newContact.m_worldNormalOnB.w = (b3Scalar)numPoints;
+
+			
 			nGlobalContactsOut++;
 		} else
 		{
@@ -1937,11 +1947,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
 			hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
 		{
 			//printf("hostPairs[i].z=%d\n",hostPairs[i].z);
-			int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
+			//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
-					hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
+			//		hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
-			//int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
+			int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
-			//		hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,
+					hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,
-			//		oldHostContacts);
+					oldHostContacts);
 
 
 			if (contactIndex>=0)

src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.cl

@@ -77,7 +77,7 @@ typedef struct
 	int m_unused2;
 } btGpuChildShape;
 
-#define GET_NPOINTS(x) (x).m_worldNormal.w
+#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
 
 typedef struct
 {
@@ -467,12 +467,12 @@ void	computeContactSphereConvex(int pairIndex,
 			if (1)//dstIdx < maxContactCapacity)
 			{
 				__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
-				c->m_worldNormal = normalOnSurfaceB1;
+				c->m_worldNormalOnB = -normalOnSurfaceB1;
 				c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 				c->m_batchIdx = pairIndex;
 				c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
 				c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
-				c->m_worldPos[0] = pOnB1;
+				c->m_worldPosB[0] = pOnB1;
 				c->m_childIndexA = -1;
 				c->m_childIndexB = -1;
 
@@ -693,7 +693,7 @@ int computeContactPlaneConvex(int pairIndex,
 		{
 			resultIndex = dstIdx;
 			__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
-			c->m_worldNormal = planeNormalWorld;
+			c->m_worldNormalOnB = -planeNormalWorld;
 			//c->setFrictionCoeff(0.7);
 			//c->setRestituitionCoeff(0.f);
 			c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
@@ -706,13 +706,13 @@ int computeContactPlaneConvex(int pairIndex,
 			switch (numReducedPoints)
             {
                 case 4:
-                    c->m_worldPos[3] = contactPoints[contactIdx.w];
+                    c->m_worldPosB[3] = contactPoints[contactIdx.w];
                 case 3:
-                    c->m_worldPos[2] = contactPoints[contactIdx.z];
+                    c->m_worldPosB[2] = contactPoints[contactIdx.z];
                 case 2:
-                    c->m_worldPos[1] = contactPoints[contactIdx.y];
+                    c->m_worldPosB[1] = contactPoints[contactIdx.y];
                 case 1:
-                    c->m_worldPos[0] = contactPoints[contactIdx.x];
+                    c->m_worldPosB[0] = contactPoints[contactIdx.x];
                 default:
                 {
                 }
@@ -776,12 +776,12 @@ void	computeContactPlaneSphere(int pairIndex,
 		if (dstIdx < maxContactCapacity)
 		{
 			__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
-			c->m_worldNormal = normalOnSurfaceB1;
+			c->m_worldNormalOnB = -normalOnSurfaceB1;
 			c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 			c->m_batchIdx = pairIndex;
 			c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
 			c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
-			c->m_worldPos[0] = pOnB1;
+			c->m_worldPosB[0] = pOnB1;
 			c->m_childIndexA = -1;
 			c->m_childIndexB = -1;
 			GET_NPOINTS(*c) = 1;
@@ -954,14 +954,14 @@ __kernel void   primitiveContactsKernel( __global int4* pairs,
 				if (dstIdx < maxContactCapacity)
 				{
 					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
-					c->m_worldNormal = -normalOnSurfaceB;
+					c->m_worldNormalOnB = normalOnSurfaceB;
 					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 					c->m_batchIdx = pairIndex;
 					int bodyA = pairs[pairIndex].x;
 					int bodyB = pairs[pairIndex].y;
 					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;
 					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;
-					c->m_worldPos[0] = contactPosB;
+					c->m_worldPosB[0] = contactPosB;
 					c->m_childIndexA = -1;
 					c->m_childIndexB = -1;
 					GET_NPOINTS(*c) = 1;
@@ -1294,12 +1294,12 @@ void	computeContactSphereTriangle(int pairIndex,
 				if (dstIdx < maxContactCapacity)
 				{
 					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
-					c->m_worldNormal = normalOnSurfaceB1;
+					c->m_worldNormalOnB = -normalOnSurfaceB1;
 					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 					c->m_batchIdx = pairIndex;
 					c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;
 					c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;
-					c->m_worldPos[0] = pOnB1;
+					c->m_worldPosB[0] = pOnB1;
 
 					c->m_childIndexA = -1;
 					c->m_childIndexB = faceIndex;

src/Bullet3OpenCL/NarrowphaseCollision/kernels/primitiveContacts.h

@@ -19,9 +19,10 @@ static const char* primitiveContactsKernelsCL= \
 "typedef  struct b3Contact4Data b3Contact4Data_t;\n"
 "struct b3Contact4Data\n"
 "{\n"
-"	b3Float4	m_worldPos[4];\n"
+"	b3Float4	m_worldPosB[4];\n"
+"//	b3Float4	m_localPosA[4];\n"
 "//	b3Float4	m_localPosB[4];\n"
-"	b3Float4	m_worldNormal;	//	w: m_nPoints\n"
+"	b3Float4	m_worldNormalOnB;	//	w: m_nPoints\n"
 "	unsigned short  m_restituitionCoeffCmp;\n"
 "	unsigned short  m_frictionCoeffCmp;\n"
 "	int m_batchIdx;\n"
@@ -31,15 +32,14 @@ static const char* primitiveContactsKernelsCL= \
 "	int	m_childIndexB;\n"
 "	int m_unused1;\n"
 "	int m_unused2;\n"
-"//	b3Float4	m_localPosA;\n"
 "};\n"
 "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n"
 "{\n"
-"	return (int)contact->m_worldNormal.w;\n"
+"	return (int)contact->m_worldNormalOnB.w;\n"
 "};\n"
 "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n"
 "{\n"
-"	contact->m_worldNormal.w = (float)numPoints;\n"
+"	contact->m_worldNormalOnB.w = (float)numPoints;\n"
 "};\n"
 "#endif //B3_CONTACT4DATA_H\n"
 "#define SHAPE_CONVEX_HULL 3\n"
@@ -106,7 +106,7 @@ static const char* primitiveContactsKernelsCL= \
 "	int m_unused1;\n"
 "	int m_unused2;\n"
 "} btGpuChildShape;\n"
-"#define GET_NPOINTS(x) (x).m_worldNormal.w\n"
+"#define GET_NPOINTS(x) (x).m_worldNormalOnB.w\n"
 "typedef struct\n"
 "{\n"
 "	float4 m_pos;\n"
@@ -431,12 +431,12 @@ static const char* primitiveContactsKernelsCL= \
 "			if (1)//dstIdx < maxContactCapacity)\n"
 "			{\n"
 "				__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n"
-"				c->m_worldNormal = normalOnSurfaceB1;\n"
+"				c->m_worldNormalOnB = -normalOnSurfaceB1;\n"
 "				c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "				c->m_batchIdx = pairIndex;\n"
 "				c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;\n"
 "				c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n"
-"				c->m_worldPos[0] = pOnB1;\n"
+"				c->m_worldPosB[0] = pOnB1;\n"
 "				c->m_childIndexA = -1;\n"
 "				c->m_childIndexB = -1;\n"
 "				GET_NPOINTS(*c) = 1;\n"
@@ -635,7 +635,7 @@ static const char* primitiveContactsKernelsCL= \
 "		{\n"
 "			resultIndex = dstIdx;\n"
 "			__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n"
-"			c->m_worldNormal = planeNormalWorld;\n"
+"			c->m_worldNormalOnB = -planeNormalWorld;\n"
 "			//c->setFrictionCoeff(0.7);\n"
 "			//c->setRestituitionCoeff(0.f);\n"
 "			c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
@@ -647,13 +647,13 @@ static const char* primitiveContactsKernelsCL= \
 "			switch (numReducedPoints)\n"
 "            {\n"
 "                case 4:\n"
-"                    c->m_worldPos[3] = contactPoints[contactIdx.w];\n"
+"                    c->m_worldPosB[3] = contactPoints[contactIdx.w];\n"
 "                case 3:\n"
-"                    c->m_worldPos[2] = contactPoints[contactIdx.z];\n"
+"                    c->m_worldPosB[2] = contactPoints[contactIdx.z];\n"
 "                case 2:\n"
-"                    c->m_worldPos[1] = contactPoints[contactIdx.y];\n"
+"                    c->m_worldPosB[1] = contactPoints[contactIdx.y];\n"
 "                case 1:\n"
-"                    c->m_worldPos[0] = contactPoints[contactIdx.x];\n"
+"                    c->m_worldPosB[0] = contactPoints[contactIdx.x];\n"
 "                default:\n"
 "                {\n"
 "                }\n"
@@ -713,12 +713,12 @@ static const char* primitiveContactsKernelsCL= \
 "		if (dstIdx < maxContactCapacity)\n"
 "		{\n"
 "			__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n"
-"			c->m_worldNormal = normalOnSurfaceB1;\n"
+"			c->m_worldNormalOnB = -normalOnSurfaceB1;\n"
 "			c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "			c->m_batchIdx = pairIndex;\n"
 "			c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;\n"
 "			c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n"
-"			c->m_worldPos[0] = pOnB1;\n"
+"			c->m_worldPosB[0] = pOnB1;\n"
 "			c->m_childIndexA = -1;\n"
 "			c->m_childIndexB = -1;\n"
 "			GET_NPOINTS(*c) = 1;\n"
@@ -863,14 +863,14 @@ static const char* primitiveContactsKernelsCL= \
 "				if (dstIdx < maxContactCapacity)\n"
 "				{\n"
 "					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n"
-"					c->m_worldNormal = -normalOnSurfaceB;\n"
+"					c->m_worldNormalOnB = normalOnSurfaceB;\n"
 "					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "					c->m_batchIdx = pairIndex;\n"
 "					int bodyA = pairs[pairIndex].x;\n"
 "					int bodyB = pairs[pairIndex].y;\n"
 "					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n"
 "					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n"
-"					c->m_worldPos[0] = contactPosB;\n"
+"					c->m_worldPosB[0] = contactPosB;\n"
 "					c->m_childIndexA = -1;\n"
 "					c->m_childIndexB = -1;\n"
 "					GET_NPOINTS(*c) = 1;\n"
@@ -1161,12 +1161,12 @@ static const char* primitiveContactsKernelsCL= \
 "				if (dstIdx < maxContactCapacity)\n"
 "				{\n"
 "					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n"
-"					c->m_worldNormal = normalOnSurfaceB1;\n"
+"					c->m_worldNormalOnB = -normalOnSurfaceB1;\n"
 "					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "					c->m_batchIdx = pairIndex;\n"
 "					c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA;\n"
 "					c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB;\n"
-"					c->m_worldPos[0] = pOnB1;\n"
+"					c->m_worldPosB[0] = pOnB1;\n"
 "					c->m_childIndexA = -1;\n"
 "					c->m_childIndexB = faceIndex;\n"
 "					GET_NPOINTS(*c) = 1;\n"

src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.cl

@@ -64,7 +64,7 @@ typedef struct
 	int m_unused2;
 } btGpuChildShape;
 
-#define GET_NPOINTS(x) (x).m_worldNormal.w
+#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
 
 typedef struct
 {
@@ -908,7 +908,7 @@ __kernel void   extractManifoldAndAddContactKernel(__global const int4* pairs,
 		//if ((dstIdx+nContacts) < capacity)
 		{
 			__global struct b3Contact4Data* c = contactsOut + dstIdx;
-			c->m_worldNormal = normal;
+			c->m_worldNormalOnB = -normal;
 			c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 			c->m_batchIdx = idx;
 			int bodyA = pairs[pairIndex].x;
@@ -919,7 +919,7 @@ __kernel void   extractManifoldAndAddContactKernel(__global const int4* pairs,
 			c->m_childIndexB = -1;
 			for (int i=0;i<nContacts;i++)
 			{
-				c->m_worldPos[i] = localPoints[contactIdx[i]];
+				c->m_worldPosB[i] = localPoints[contactIdx[i]];
 			}
 			GET_NPOINTS(*c) = nContacts;
 		}
@@ -1023,7 +1023,7 @@ __kernel void   clipHullHullKernel( __global int4* pairs,
 					pairs[pairIndex].z = dstIdx;
 
 					__global struct b3Contact4Data* c = globalContactsOut+ dstIdx;
-					c->m_worldNormal = normal;
+					c->m_worldNormalOnB = -normal;
 					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 					c->m_batchIdx = pairIndex;
 					int bodyA = pairs[pairIndex].x;
@@ -1035,7 +1035,7 @@ __kernel void   clipHullHullKernel( __global int4* pairs,
 
 					for (int i=0;i<nReducedContacts;i++)
 					{
-						c->m_worldPos[i] = pointsIn[contactIdx[i]];
+						c->m_worldPosB[i] = pointsIn[contactIdx[i]];
 					}
 					GET_NPOINTS(*c) = nReducedContacts;
 				}
@@ -1156,7 +1156,7 @@ __kernel void   clipCompoundsHullHullKernel( __global const int4* gpuCompoundPai
 				if ((dstIdx+nReducedContacts) < maxContactCapacity)
 				{
 					__global struct b3Contact4Data* c = globalContactsOut+ dstIdx;
-					c->m_worldNormal = normal;
+					c->m_worldNormalOnB = -normal;
 					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 					c->m_batchIdx = pairIndex;
 					int bodyA = gpuCompoundPairs[pairIndex].x;
@@ -1167,7 +1167,7 @@ __kernel void   clipCompoundsHullHullKernel( __global const int4* gpuCompoundPai
 					c->m_childIndexB = childShapeIndexB;
 					for (int i=0;i<nReducedContacts;i++)
 					{
-						c->m_worldPos[i] = pointsIn[contactIdx[i]];
+						c->m_worldPosB[i] = pointsIn[contactIdx[i]];
 					}
 					GET_NPOINTS(*c) = nReducedContacts;
 				}
@@ -1232,14 +1232,14 @@ __kernel void   sphereSphereCollisionKernel( __global const int4* pairs,
 				if (dstIdx < numPairs)
 				{
 					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
-					c->m_worldNormal = normalOnSurfaceB;
+					c->m_worldNormalOnB = -normalOnSurfaceB;
 					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 					c->m_batchIdx = pairIndex;
 					int bodyA = pairs[pairIndex].x;
 					int bodyB = pairs[pairIndex].y;
 					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;
 					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;
-					c->m_worldPos[0] = contactPosB;
+					c->m_worldPosB[0] = contactPosB;
 					c->m_childIndexA = -1;
 					c->m_childIndexB = -1;
 
@@ -1465,7 +1465,7 @@ __kernel void   clipHullHullConcaveConvexKernel( __global int4* concavePairsIn,
 			//if ((dstIdx+nReducedContacts) < capacity)
 			{
 				__global struct b3Contact4Data* c = globalContactsOut+ dstIdx;
-				c->m_worldNormal = normal;
+				c->m_worldNormalOnB = -normal;
 				c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 				c->m_batchIdx = pairIndex;
 				int bodyA = concavePairsIn[pairIndex].x;
@@ -1476,7 +1476,7 @@ __kernel void   clipHullHullConcaveConvexKernel( __global int4* concavePairsIn,
 				c->m_childIndexB = childShapeIndexB;
 				for (int i=0;i<nReducedContacts;i++)
 				{
-					c->m_worldPos[i] = pointsIn[contactIdx[i]];
+					c->m_worldPosB[i] = pointsIn[contactIdx[i]];
 				}
 				GET_NPOINTS(*c) = nReducedContacts;
 			}
@@ -1887,7 +1887,7 @@ __kernel void   newContactReductionKernel( __global int4* pairs,
 				{
 
 					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
-					c->m_worldNormal = normal;
+					c->m_worldNormalOnB = -normal;
 					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
 					c->m_batchIdx = pairIndex;
 					int bodyA = pairs[pairIndex].x;
@@ -1903,13 +1903,13 @@ __kernel void   newContactReductionKernel( __global int4* pairs,
                     switch (nReducedContacts)
                     {
                         case 4:
-                            c->m_worldPos[3] = pointsIn[contactIdx.w];
+                            c->m_worldPosB[3] = pointsIn[contactIdx.w];
                         case 3:
-                            c->m_worldPos[2] = pointsIn[contactIdx.z];
+                            c->m_worldPosB[2] = pointsIn[contactIdx.z];
                         case 2:
-                            c->m_worldPos[1] = pointsIn[contactIdx.y];
+                            c->m_worldPosB[1] = pointsIn[contactIdx.y];
                         case 1:
-                            c->m_worldPos[0] = pointsIn[contactIdx.x];
+                            c->m_worldPosB[0] = pointsIn[contactIdx.x];
                         default:
                         {
                         }

src/Bullet3OpenCL/NarrowphaseCollision/kernels/satClipHullContacts.h

@@ -51,9 +51,10 @@ static const char* satClipKernelsCL= \
 "typedef  struct b3Contact4Data b3Contact4Data_t;\n"
 "struct b3Contact4Data\n"
 "{\n"
-"	b3Float4	m_worldPos[4];\n"
+"	b3Float4	m_worldPosB[4];\n"
+"//	b3Float4	m_localPosA[4];\n"
 "//	b3Float4	m_localPosB[4];\n"
-"	b3Float4	m_worldNormal;	//	w: m_nPoints\n"
+"	b3Float4	m_worldNormalOnB;	//	w: m_nPoints\n"
 "	unsigned short  m_restituitionCoeffCmp;\n"
 "	unsigned short  m_frictionCoeffCmp;\n"
 "	int m_batchIdx;\n"
@@ -63,15 +64,14 @@ static const char* satClipKernelsCL= \
 "	int	m_childIndexB;\n"
 "	int m_unused1;\n"
 "	int m_unused2;\n"
-"//	b3Float4	m_localPosA;\n"
 "};\n"
 "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n"
 "{\n"
-"	return (int)contact->m_worldNormal.w;\n"
+"	return (int)contact->m_worldNormalOnB.w;\n"
 "};\n"
 "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n"
 "{\n"
-"	contact->m_worldNormal.w = (float)numPoints;\n"
+"	contact->m_worldNormalOnB.w = (float)numPoints;\n"
 "};\n"
 "#endif //B3_CONTACT4DATA_H\n"
 "///keep this in sync with btCollidable.h\n"
@@ -92,7 +92,7 @@ static const char* satClipKernelsCL= \
 "	int m_unused1;\n"
 "	int m_unused2;\n"
 "} btGpuChildShape;\n"
-"#define GET_NPOINTS(x) (x).m_worldNormal.w\n"
+"#define GET_NPOINTS(x) (x).m_worldNormalOnB.w\n"
 "typedef struct\n"
 "{\n"
 "	float4 m_pos;\n"
@@ -821,7 +821,7 @@ static const char* satClipKernelsCL= \
 "		//if ((dstIdx+nContacts) < capacity)\n"
 "		{\n"
 "			__global struct b3Contact4Data* c = contactsOut + dstIdx;\n"
-"			c->m_worldNormal = normal;\n"
+"			c->m_worldNormalOnB = -normal;\n"
 "			c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "			c->m_batchIdx = idx;\n"
 "			int bodyA = pairs[pairIndex].x;\n"
@@ -832,7 +832,7 @@ static const char* satClipKernelsCL= \
 "			c->m_childIndexB = -1;\n"
 "			for (int i=0;i<nContacts;i++)\n"
 "			{\n"
-"				c->m_worldPos[i] = localPoints[contactIdx[i]];\n"
+"				c->m_worldPosB[i] = localPoints[contactIdx[i]];\n"
 "			}\n"
 "			GET_NPOINTS(*c) = nContacts;\n"
 "		}\n"
@@ -919,7 +919,7 @@ static const char* satClipKernelsCL= \
 "				{\n"
 "					pairs[pairIndex].z = dstIdx;\n"
 "					__global struct b3Contact4Data* c = globalContactsOut+ dstIdx;\n"
-"					c->m_worldNormal = normal;\n"
+"					c->m_worldNormalOnB = -normal;\n"
 "					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "					c->m_batchIdx = pairIndex;\n"
 "					int bodyA = pairs[pairIndex].x;\n"
@@ -930,7 +930,7 @@ static const char* satClipKernelsCL= \
 "					c->m_childIndexB = -1;\n"
 "					for (int i=0;i<nReducedContacts;i++)\n"
 "					{\n"
-"						c->m_worldPos[i] = pointsIn[contactIdx[i]];\n"
+"						c->m_worldPosB[i] = pointsIn[contactIdx[i]];\n"
 "					}\n"
 "					GET_NPOINTS(*c) = nReducedContacts;\n"
 "				}\n"
@@ -1042,7 +1042,7 @@ static const char* satClipKernelsCL= \
 "				if ((dstIdx+nReducedContacts) < maxContactCapacity)\n"
 "				{\n"
 "					__global struct b3Contact4Data* c = globalContactsOut+ dstIdx;\n"
-"					c->m_worldNormal = normal;\n"
+"					c->m_worldNormalOnB = -normal;\n"
 "					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "					c->m_batchIdx = pairIndex;\n"
 "					int bodyA = gpuCompoundPairs[pairIndex].x;\n"
@@ -1053,7 +1053,7 @@ static const char* satClipKernelsCL= \
 "					c->m_childIndexB = childShapeIndexB;\n"
 "					for (int i=0;i<nReducedContacts;i++)\n"
 "					{\n"
-"						c->m_worldPos[i] = pointsIn[contactIdx[i]];\n"
+"						c->m_worldPosB[i] = pointsIn[contactIdx[i]];\n"
 "					}\n"
 "					GET_NPOINTS(*c) = nReducedContacts;\n"
 "				}\n"
@@ -1111,14 +1111,14 @@ static const char* satClipKernelsCL= \
 "				if (dstIdx < numPairs)\n"
 "				{\n"
 "					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n"
-"					c->m_worldNormal = normalOnSurfaceB;\n"
+"					c->m_worldNormalOnB = -normalOnSurfaceB;\n"
 "					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "					c->m_batchIdx = pairIndex;\n"
 "					int bodyA = pairs[pairIndex].x;\n"
 "					int bodyB = pairs[pairIndex].y;\n"
 "					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA;\n"
 "					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB;\n"
-"					c->m_worldPos[0] = contactPosB;\n"
+"					c->m_worldPosB[0] = contactPosB;\n"
 "					c->m_childIndexA = -1;\n"
 "					c->m_childIndexB = -1;\n"
 "					GET_NPOINTS(*c) = 1;\n"
@@ -1323,7 +1323,7 @@ static const char* satClipKernelsCL= \
 "			//if ((dstIdx+nReducedContacts) < capacity)\n"
 "			{\n"
 "				__global struct b3Contact4Data* c = globalContactsOut+ dstIdx;\n"
-"				c->m_worldNormal = normal;\n"
+"				c->m_worldNormalOnB = -normal;\n"
 "				c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "				c->m_batchIdx = pairIndex;\n"
 "				int bodyA = concavePairsIn[pairIndex].x;\n"
@@ -1334,7 +1334,7 @@ static const char* satClipKernelsCL= \
 "				c->m_childIndexB = childShapeIndexB;\n"
 "				for (int i=0;i<nReducedContacts;i++)\n"
 "				{\n"
-"					c->m_worldPos[i] = pointsIn[contactIdx[i]];\n"
+"					c->m_worldPosB[i] = pointsIn[contactIdx[i]];\n"
 "				}\n"
 "				GET_NPOINTS(*c) = nReducedContacts;\n"
 "			}\n"
@@ -1714,7 +1714,7 @@ static const char* satClipKernelsCL= \
 "				if (dstIdx < numPairs)\n"
 "				{\n"
 "					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];\n"
-"					c->m_worldNormal = normal;\n"
+"					c->m_worldNormalOnB = -normal;\n"
 "					c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);\n"
 "					c->m_batchIdx = pairIndex;\n"
 "					int bodyA = pairs[pairIndex].x;\n"
@@ -1727,13 +1727,13 @@ static const char* satClipKernelsCL= \
 "                    switch (nReducedContacts)\n"
 "                    {\n"
 "                        case 4:\n"
-"                            c->m_worldPos[3] = pointsIn[contactIdx.w];\n"
+"                            c->m_worldPosB[3] = pointsIn[contactIdx.w];\n"
 "                        case 3:\n"
-"                            c->m_worldPos[2] = pointsIn[contactIdx.z];\n"
+"                            c->m_worldPosB[2] = pointsIn[contactIdx.z];\n"
 "                        case 2:\n"
-"                            c->m_worldPos[1] = pointsIn[contactIdx.y];\n"
+"                            c->m_worldPosB[1] = pointsIn[contactIdx.y];\n"
 "                        case 1:\n"
-"                            c->m_worldPos[0] = pointsIn[contactIdx.x];\n"
+"                            c->m_worldPosB[0] = pointsIn[contactIdx.x];\n"
 "                        default:\n"
 "                        {\n"
 "                        }\n"

src/Bullet3OpenCL/RigidBody/kernels/batchingKernels.h

@@ -31,9 +31,10 @@ static const char* batchingKernelsCL= \
 "typedef  struct b3Contact4Data b3Contact4Data_t;\n"
 "struct b3Contact4Data\n"
 "{\n"
-"	b3Float4	m_worldPos[4];\n"
+"	b3Float4	m_worldPosB[4];\n"
+"//	b3Float4	m_localPosA[4];\n"
 "//	b3Float4	m_localPosB[4];\n"
-"	b3Float4	m_worldNormal;	//	w: m_nPoints\n"
+"	b3Float4	m_worldNormalOnB;	//	w: m_nPoints\n"
 "	unsigned short  m_restituitionCoeffCmp;\n"
 "	unsigned short  m_frictionCoeffCmp;\n"
 "	int m_batchIdx;\n"
@@ -43,15 +44,14 @@ static const char* batchingKernelsCL= \
 "	int	m_childIndexB;\n"
 "	int m_unused1;\n"
 "	int m_unused2;\n"
-"//	b3Float4	m_localPosA;\n"
 "};\n"
 "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n"
 "{\n"
-"	return (int)contact->m_worldNormal.w;\n"
+"	return (int)contact->m_worldNormalOnB.w;\n"
 "};\n"
 "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n"
 "{\n"
-"	contact->m_worldNormal.w = (float)numPoints;\n"
+"	contact->m_worldNormalOnB.w = (float)numPoints;\n"
 "};\n"
 "#endif //B3_CONTACT4DATA_H\n"
 "#pragma OPENCL EXTENSION cl_amd_printf : enable\n"

src/Bullet3OpenCL/RigidBody/kernels/batchingKernelsNew.h

@@ -31,9 +31,10 @@ static const char* batchingKernelsNewCL= \
 "typedef  struct b3Contact4Data b3Contact4Data_t;\n"
 "struct b3Contact4Data\n"
 "{\n"
-"	b3Float4	m_worldPos[4];\n"
+"	b3Float4	m_worldPosB[4];\n"
+"//	b3Float4	m_localPosA[4];\n"
 "//	b3Float4	m_localPosB[4];\n"
-"	b3Float4	m_worldNormal;	//	w: m_nPoints\n"
+"	b3Float4	m_worldNormalOnB;	//	w: m_nPoints\n"
 "	unsigned short  m_restituitionCoeffCmp;\n"
 "	unsigned short  m_frictionCoeffCmp;\n"
 "	int m_batchIdx;\n"
@@ -43,15 +44,14 @@ static const char* batchingKernelsNewCL= \
 "	int	m_childIndexB;\n"
 "	int m_unused1;\n"
 "	int m_unused2;\n"
-"//	b3Float4	m_localPosA;\n"
 "};\n"
 "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n"
 "{\n"
-"	return (int)contact->m_worldNormal.w;\n"
+"	return (int)contact->m_worldNormalOnB.w;\n"
 "};\n"
 "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n"
 "{\n"
-"	contact->m_worldNormal.w = (float)numPoints;\n"
+"	contact->m_worldNormalOnB.w = (float)numPoints;\n"
 "};\n"
 "#endif //B3_CONTACT4DATA_H\n"
 "#pragma OPENCL EXTENSION cl_amd_printf : enable\n"

src/Bullet3OpenCL/RigidBody/kernels/solverSetup.cl

@@ -427,9 +427,9 @@ typedef struct
 
 void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
 {
-	*linear = make_float4(-n.xyz,0.f);
+	*linear = make_float4(n.xyz,0.f);
-	*angular0 = -cross3(r0, n);
+	*angular0 = cross3(r0, n);
-	*angular1 = cross3(r1, n);
+	*angular1 = -cross3(r1, n);
 }
 
 
@@ -525,14 +525,14 @@ void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVe
 	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
 
 
-	dstC->m_linear = -src->m_worldNormal;
+	dstC->m_linear = src->m_worldNormalOnB;
 	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );
 	for(int ic=0; ic<4; ic++)
 	{
-		float4 r0 = src->m_worldPos[ic] - posA;
+		float4 r0 = src->m_worldPosB[ic] - posA;
-		float4 r1 = src->m_worldPos[ic] - posB;
+		float4 r1 = src->m_worldPosB[ic] - posB;
 
-		if( ic >= src->m_worldNormal.w )//npoints
+		if( ic >= src->m_worldNormalOnB.w )//npoints
 		{
 			dstC->m_jacCoeffInv[ic] = 0.f;
 			continue;
@@ -541,7 +541,7 @@ void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVe
 		float relVelN;
 		{
 			float4 linear, angular0, angular1;
-			setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1);
+			setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
 
 			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
 				invMassA, &invInertiaA, invMassB, &invInertiaB );
@@ -553,21 +553,21 @@ void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVe
 			if( relVelN*relVelN < 0.004f ) e = 0.f;
 
 			dstC->m_b[ic] = e*relVelN;
-			//float penetration = src->m_worldPos[ic].w;
+			//float penetration = src->m_worldPosB[ic].w;
-			dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
+			dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
 			dstC->m_appliedRambdaDt[ic] = 0.f;
 		}
 	}
 
-	if( src->m_worldNormal.w > 0 )//npoints
+	if( src->m_worldNormalOnB.w > 0 )//npoints
 	{	//	prepare friction
 		float4 center = make_float4(0.f);
-		for(int i=0; i<src->m_worldNormal.w; i++) 
+		for(int i=0; i<src->m_worldNormalOnB.w; i++) 
-			center += src->m_worldPos[i];
+			center += src->m_worldPosB[i];
-		center /= (float)src->m_worldNormal.w;
+		center /= (float)src->m_worldNormalOnB.w;
 
 		float4 tangent[2];
-		btPlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]);
+		btPlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);
 		
 		float4 r[2];
 		r[0] = center - posA;
@@ -587,9 +587,9 @@ void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVe
 
 	for(int i=0; i<4; i++)
 	{
-		if( i<src->m_worldNormal.w )
+		if( i<src->m_worldNormalOnB.w )
 		{
-			dstC->m_worldPos[i] = src->m_worldPos[i];
+			dstC->m_worldPos[i] = src->m_worldPosB[i];
 		}
 		else
 		{

src/Bullet3OpenCL/RigidBody/kernels/solverSetup.h

@@ -31,9 +31,10 @@ static const char* solverSetupCL= \
 "typedef  struct b3Contact4Data b3Contact4Data_t;\n"
 "struct b3Contact4Data\n"
 "{\n"
-"	b3Float4	m_worldPos[4];\n"
+"	b3Float4	m_worldPosB[4];\n"
+"//	b3Float4	m_localPosA[4];\n"
 "//	b3Float4	m_localPosB[4];\n"
-"	b3Float4	m_worldNormal;	//	w: m_nPoints\n"
+"	b3Float4	m_worldNormalOnB;	//	w: m_nPoints\n"
 "	unsigned short  m_restituitionCoeffCmp;\n"
 "	unsigned short  m_frictionCoeffCmp;\n"
 "	int m_batchIdx;\n"
@@ -43,15 +44,14 @@ static const char* solverSetupCL= \
 "	int	m_childIndexB;\n"
 "	int m_unused1;\n"
 "	int m_unused2;\n"
-"//	b3Float4	m_localPosA;\n"
 "};\n"
 "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n"
 "{\n"
-"	return (int)contact->m_worldNormal.w;\n"
+"	return (int)contact->m_worldNormalOnB.w;\n"
 "};\n"
 "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n"
 "{\n"
-"	contact->m_worldNormal.w = (float)numPoints;\n"
+"	contact->m_worldNormalOnB.w = (float)numPoints;\n"
 "};\n"
 "#endif //B3_CONTACT4DATA_H\n"
 "#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
@@ -389,9 +389,9 @@ static const char* solverSetupCL= \
 "} ConstBufferBatchSolve;\n"
 "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
 "{\n"
-"	*linear = make_float4(-n.xyz,0.f);\n"
+"	*linear = make_float4(n.xyz,0.f);\n"
-"	*angular0 = -cross3(r0, n);\n"
+"	*angular0 = cross3(r0, n);\n"
-"	*angular1 = cross3(r1, n);\n"
+"	*angular1 = -cross3(r1, n);\n"
 "}\n"
 "float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n"
 "{\n"
@@ -467,13 +467,13 @@ static const char* solverSetupCL= \
 "		dstC->m_appliedRambdaDt[ic] = 0.f;\n"
 "	}\n"
 "	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n"
-"	dstC->m_linear = -src->m_worldNormal;\n"
+"	dstC->m_linear = src->m_worldNormalOnB;\n"
 "	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n"
 "	for(int ic=0; ic<4; ic++)\n"
 "	{\n"
-"		float4 r0 = src->m_worldPos[ic] - posA;\n"
+"		float4 r0 = src->m_worldPosB[ic] - posA;\n"
-"		float4 r1 = src->m_worldPos[ic] - posB;\n"
+"		float4 r1 = src->m_worldPosB[ic] - posB;\n"
-"		if( ic >= src->m_worldNormal.w )//npoints\n"
+"		if( ic >= src->m_worldNormalOnB.w )//npoints\n"
 "		{\n"
 "			dstC->m_jacCoeffInv[ic] = 0.f;\n"
 "			continue;\n"
@@ -481,7 +481,7 @@ static const char* solverSetupCL= \
 "		float relVelN;\n"
 "		{\n"
 "			float4 linear, angular0, angular1;\n"
-"			setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1);\n"
+"			setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);\n"
 "			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n"
 "				invMassA, &invInertiaA, invMassB, &invInertiaB );\n"
 "			relVelN = calcRelVel(linear, -linear, angular0, angular1,\n"
@@ -489,19 +489,19 @@ static const char* solverSetupCL= \
 "			float e = 0.f;//src->getRestituitionCoeff();\n"
 "			if( relVelN*relVelN < 0.004f ) e = 0.f;\n"
 "			dstC->m_b[ic] = e*relVelN;\n"
-"			//float penetration = src->m_worldPos[ic].w;\n"
+"			//float penetration = src->m_worldPosB[ic].w;\n"
-"			dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n"
+"			dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n"
 "			dstC->m_appliedRambdaDt[ic] = 0.f;\n"
 "		}\n"
 "	}\n"
-"	if( src->m_worldNormal.w > 0 )//npoints\n"
+"	if( src->m_worldNormalOnB.w > 0 )//npoints\n"
 "	{	//	prepare friction\n"
 "		float4 center = make_float4(0.f);\n"
-"		for(int i=0; i<src->m_worldNormal.w; i++) \n"
+"		for(int i=0; i<src->m_worldNormalOnB.w; i++) \n"
-"			center += src->m_worldPos[i];\n"
+"			center += src->m_worldPosB[i];\n"
-"		center /= (float)src->m_worldNormal.w;\n"
+"		center /= (float)src->m_worldNormalOnB.w;\n"
 "		float4 tangent[2];\n"
-"		btPlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]);\n"
+"		btPlaneSpace1(src->m_worldNormalOnB,&tangent[0],&tangent[1]);\n"
 "		\n"
 "		float4 r[2];\n"
 "		r[0] = center - posA;\n"
@@ -518,9 +518,9 @@ static const char* solverSetupCL= \
 "	}\n"
 "	for(int i=0; i<4; i++)\n"
 "	{\n"
-"		if( i<src->m_worldNormal.w )\n"
+"		if( i<src->m_worldNormalOnB.w )\n"
 "		{\n"
-"			dstC->m_worldPos[i] = src->m_worldPos[i];\n"
+"			dstC->m_worldPos[i] = src->m_worldPosB[i];\n"
 "		}\n"
 "		else\n"
 "		{\n"

src/Bullet3OpenCL/RigidBody/kernels/solverSetup2.h

@@ -31,9 +31,10 @@ static const char* solverSetup2CL= \
 "typedef  struct b3Contact4Data b3Contact4Data_t;\n"
 "struct b3Contact4Data\n"
 "{\n"
-"	b3Float4	m_worldPos[4];\n"
+"	b3Float4	m_worldPosB[4];\n"
+"//	b3Float4	m_localPosA[4];\n"
 "//	b3Float4	m_localPosB[4];\n"
-"	b3Float4	m_worldNormal;	//	w: m_nPoints\n"
+"	b3Float4	m_worldNormalOnB;	//	w: m_nPoints\n"
 "	unsigned short  m_restituitionCoeffCmp;\n"
 "	unsigned short  m_frictionCoeffCmp;\n"
 "	int m_batchIdx;\n"
@@ -43,15 +44,14 @@ static const char* solverSetup2CL= \
 "	int	m_childIndexB;\n"
 "	int m_unused1;\n"
 "	int m_unused2;\n"
-"//	b3Float4	m_localPosA;\n"
 "};\n"
 "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n"
 "{\n"
-"	return (int)contact->m_worldNormal.w;\n"
+"	return (int)contact->m_worldNormalOnB.w;\n"
 "};\n"
 "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n"
 "{\n"
-"	contact->m_worldNormal.w = (float)numPoints;\n"
+"	contact->m_worldNormalOnB.w = (float)numPoints;\n"
 "};\n"
 "#endif //B3_CONTACT4DATA_H\n"
 "#pragma OPENCL EXTENSION cl_amd_printf : enable\n"

src/Bullet3OpenCL/RigidBody/kernels/solverUtils.cl

@@ -455,9 +455,9 @@ __global float4* deltaLinearVelocities, __global float4* deltaAngularVelocities,
 
 void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)
 {
-	*linear = make_float4(-n.xyz,0.f);
+	*linear = make_float4(n.xyz,0.f);
-	*angular0 = -cross3(r0, n);
+	*angular0 = cross3(r0, n);
-	*angular1 = cross3(r1, n);
+	*angular1 = -cross3(r1, n);
 }
 
 
@@ -845,14 +845,14 @@ void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVe
 	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;
 
 
-	dstC->m_linear = -src->m_worldNormal;
+	dstC->m_linear = src->m_worldNormalOnB;
 	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );
 	for(int ic=0; ic<4; ic++)
 	{
-		float4 r0 = src->m_worldPos[ic] - posA;
+		float4 r0 = src->m_worldPosB[ic] - posA;
-		float4 r1 = src->m_worldPos[ic] - posB;
+		float4 r1 = src->m_worldPosB[ic] - posB;
 
-		if( ic >= src->m_worldNormal.w )//npoints
+		if( ic >= src->m_worldNormalOnB.w )//npoints
 		{
 			dstC->m_jacCoeffInv[ic] = 0.f;
 			continue;
@@ -861,7 +861,7 @@ void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVe
 		float relVelN;
 		{
 			float4 linear, angular0, angular1;
-			setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1);
+			setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);
 
 			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,
 				invMassA, &invInertiaA, invMassB, &invInertiaB , countA, countB);
@@ -873,21 +873,21 @@ void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVe
 			if( relVelN*relVelN < 0.004f ) e = 0.f;
 
 			dstC->m_b[ic] = e*relVelN;
-			//float penetration = src->m_worldPos[ic].w;
+			//float penetration = src->m_worldPosB[ic].w;
-			dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
+			dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;
 			dstC->m_appliedRambdaDt[ic] = 0.f;
 		}
 	}
 
-	if( src->m_worldNormal.w > 0 )//npoints
+	if( src->m_worldNormalOnB.w > 0 )//npoints
 	{	//	prepare friction
 		float4 center = make_float4(0.f);
-		for(int i=0; i<src->m_worldNormal.w; i++) 
+		for(int i=0; i<src->m_worldNormalOnB.w; i++) 
-			center += src->m_worldPos[i];
+			center += src->m_worldPosB[i];
-		center /= (float)src->m_worldNormal.w;
+		center /= (float)src->m_worldNormalOnB.w;
 
 		float4 tangent[2];
-		btPlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]);
+		btPlaneSpace1(-src->m_worldNormalOnB,&tangent[0],&tangent[1]);
 		
 		float4 r[2];
 		r[0] = center - posA;
@@ -907,9 +907,9 @@ void setConstraint4( const float4 posA, const float4 linVelA, const float4 angVe
 
 	for(int i=0; i<4; i++)
 	{
-		if( i<src->m_worldNormal.w )
+		if( i<src->m_worldNormalOnB.w )
 		{
-			dstC->m_worldPos[i] = src->m_worldPos[i];
+			dstC->m_worldPos[i] = src->m_worldPosB[i];
 		}
 		else
 		{

src/Bullet3OpenCL/RigidBody/kernels/solverUtils.h

@@ -31,9 +31,10 @@ static const char* solverUtilsCL= \
 "typedef  struct b3Contact4Data b3Contact4Data_t;\n"
 "struct b3Contact4Data\n"
 "{\n"
-"	b3Float4	m_worldPos[4];\n"
+"	b3Float4	m_worldPosB[4];\n"
+"//	b3Float4	m_localPosA[4];\n"
 "//	b3Float4	m_localPosB[4];\n"
-"	b3Float4	m_worldNormal;	//	w: m_nPoints\n"
+"	b3Float4	m_worldNormalOnB;	//	w: m_nPoints\n"
 "	unsigned short  m_restituitionCoeffCmp;\n"
 "	unsigned short  m_frictionCoeffCmp;\n"
 "	int m_batchIdx;\n"
@@ -43,15 +44,14 @@ static const char* solverUtilsCL= \
 "	int	m_childIndexB;\n"
 "	int m_unused1;\n"
 "	int m_unused2;\n"
-"//	b3Float4	m_localPosA;\n"
 "};\n"
 "inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)\n"
 "{\n"
-"	return (int)contact->m_worldNormal.w;\n"
+"	return (int)contact->m_worldNormalOnB.w;\n"
 "};\n"
 "inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)\n"
 "{\n"
-"	contact->m_worldNormal.w = (float)numPoints;\n"
+"	contact->m_worldNormalOnB.w = (float)numPoints;\n"
 "};\n"
 "#endif //B3_CONTACT4DATA_H\n"
 "#pragma OPENCL EXTENSION cl_amd_printf : enable\n"
@@ -418,9 +418,9 @@ static const char* solverUtilsCL= \
 "}\n"
 "void setLinearAndAngular( float4 n, float4 r0, float4 r1, float4* linear, float4* angular0, float4* angular1)\n"
 "{\n"
-"	*linear = make_float4(-n.xyz,0.f);\n"
+"	*linear = make_float4(n.xyz,0.f);\n"
-"	*angular0 = -cross3(r0, n);\n"
+"	*angular0 = cross3(r0, n);\n"
-"	*angular1 = cross3(r1, n);\n"
+"	*angular1 = -cross3(r1, n);\n"
 "}\n"
 "float calcRelVel( float4 l0, float4 l1, float4 a0, float4 a1, float4 linVel0, float4 angVel0, float4 linVel1, float4 angVel1 )\n"
 "{\n"
@@ -746,13 +746,13 @@ static const char* solverUtilsCL= \
 "		dstC->m_appliedRambdaDt[ic] = 0.f;\n"
 "	}\n"
 "	dstC->m_fJacCoeffInv[0] = dstC->m_fJacCoeffInv[1] = 0.f;\n"
-"	dstC->m_linear = -src->m_worldNormal;\n"
+"	dstC->m_linear = src->m_worldNormalOnB;\n"
 "	dstC->m_linear.w = 0.7f ;//src->getFrictionCoeff() );\n"
 "	for(int ic=0; ic<4; ic++)\n"
 "	{\n"
-"		float4 r0 = src->m_worldPos[ic] - posA;\n"
+"		float4 r0 = src->m_worldPosB[ic] - posA;\n"
-"		float4 r1 = src->m_worldPos[ic] - posB;\n"
+"		float4 r1 = src->m_worldPosB[ic] - posB;\n"
-"		if( ic >= src->m_worldNormal.w )//npoints\n"
+"		if( ic >= src->m_worldNormalOnB.w )//npoints\n"
 "		{\n"
 "			dstC->m_jacCoeffInv[ic] = 0.f;\n"
 "			continue;\n"
@@ -760,7 +760,7 @@ static const char* solverUtilsCL= \
 "		float relVelN;\n"
 "		{\n"
 "			float4 linear, angular0, angular1;\n"
-"			setLinearAndAngular(src->m_worldNormal, r0, r1, &linear, &angular0, &angular1);\n"
+"			setLinearAndAngular(src->m_worldNormalOnB, r0, r1, &linear, &angular0, &angular1);\n"
 "			dstC->m_jacCoeffInv[ic] = calcJacCoeff(linear, -linear, angular0, angular1,\n"
 "				invMassA, &invInertiaA, invMassB, &invInertiaB , countA, countB);\n"
 "			relVelN = calcRelVel(linear, -linear, angular0, angular1,\n"
@@ -768,19 +768,19 @@ static const char* solverUtilsCL= \
 "			float e = 0.f;//src->getRestituitionCoeff();\n"
 "			if( relVelN*relVelN < 0.004f ) e = 0.f;\n"
 "			dstC->m_b[ic] = e*relVelN;\n"
-"			//float penetration = src->m_worldPos[ic].w;\n"
+"			//float penetration = src->m_worldPosB[ic].w;\n"
-"			dstC->m_b[ic] += (src->m_worldPos[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n"
+"			dstC->m_b[ic] += (src->m_worldPosB[ic].w + positionDrift)*positionConstraintCoeff*dtInv;\n"
 "			dstC->m_appliedRambdaDt[ic] = 0.f;\n"
 "		}\n"
 "	}\n"
-"	if( src->m_worldNormal.w > 0 )//npoints\n"
+"	if( src->m_worldNormalOnB.w > 0 )//npoints\n"
 "	{	//	prepare friction\n"
 "		float4 center = make_float4(0.f);\n"
-"		for(int i=0; i<src->m_worldNormal.w; i++) \n"
+"		for(int i=0; i<src->m_worldNormalOnB.w; i++) \n"
-"			center += src->m_worldPos[i];\n"
+"			center += src->m_worldPosB[i];\n"
-"		center /= (float)src->m_worldNormal.w;\n"
+"		center /= (float)src->m_worldNormalOnB.w;\n"
 "		float4 tangent[2];\n"
-"		btPlaneSpace1(src->m_worldNormal,&tangent[0],&tangent[1]);\n"
+"		btPlaneSpace1(-src->m_worldNormalOnB,&tangent[0],&tangent[1]);\n"
 "		\n"
 "		float4 r[2];\n"
 "		r[0] = center - posA;\n"
@@ -797,9 +797,9 @@ static const char* solverUtilsCL= \
 "	}\n"
 "	for(int i=0; i<4; i++)\n"
 "	{\n"
-"		if( i<src->m_worldNormal.w )\n"
+"		if( i<src->m_worldNormalOnB.w )\n"
 "		{\n"
-"			dstC->m_worldPos[i] = src->m_worldPos[i];\n"
+"			dstC->m_worldPos[i] = src->m_worldPosB[i];\n"
 "		}\n"
 "		else\n"
 "		{\n"