1
2#include "Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h"
3#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
4
5#define AppendInc(x, out) out = atomic_inc(x)
6#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
7#ifdef cl_ext_atomic_counters_32
8	#pragma OPENCL EXTENSION cl_ext_atomic_counters_32 : enable
9#else
10	#define counter32_t volatile __global int*
11#endif
12
13
14__kernel void   mprPenetrationKernel( __global int4* pairs,
15																					__global const b3RigidBodyData_t* rigidBodies,
16																					__global const b3Collidable_t* collidables,
17																					__global const b3ConvexPolyhedronData_t* convexShapes,
18																					__global const float4* vertices,
19																					__global float4* separatingNormals,
20																					__global int* hasSeparatingAxis,
21																					__global struct b3Contact4Data* restrict globalContactsOut,
22																					counter32_t nGlobalContactsOut,
23																					int contactCapacity,
24																					int numPairs)
25{
26	int i = get_global_id(0);
27	int pairIndex = i;
28	if (i<numPairs)
29	{
30		int bodyIndexA = pairs[i].x;
31		int bodyIndexB = pairs[i].y;
32
33		int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
34		int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
35
36		int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
37		int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
38
39
40		//once the broadphase avoids static-static pairs, we can remove this test
41		if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
42		{
43			return;
44		}
45
46
47		if ((collidables[collidableIndexA].m_shapeType!=SHAPE_CONVEX_HULL) ||(collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL))
48		{
49			return;
50		}
51
52		float depthOut;
53		b3Float4 dirOut;
54		b3Float4 posOut;
55
56
57		int res = b3MprPenetration(pairIndex, bodyIndexA, bodyIndexB,rigidBodies,convexShapes,collidables,vertices,separatingNormals,hasSeparatingAxis,&depthOut, &dirOut, &posOut);
58
59
60
61
62
63		if (res==0)
64		{
65			//add a contact
66
67			int dstIdx;
68			AppendInc( nGlobalContactsOut, dstIdx );
69			if (dstIdx<contactCapacity)
70			{
71				pairs[pairIndex].z = dstIdx;
72				__global struct b3Contact4Data* c = globalContactsOut + dstIdx;
73				c->m_worldNormalOnB = -dirOut;//normal;
74				c->m_restituitionCoeffCmp = (0.f*0xffff);c->m_frictionCoeffCmp = (0.7f*0xffff);
75				c->m_batchIdx = pairIndex;
76				int bodyA = pairs[pairIndex].x;
77				int bodyB = pairs[pairIndex].y;
78				c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0 ? -bodyA:bodyA;
79				c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0 ? -bodyB:bodyB;
80				c->m_childIndexA = -1;
81				c->m_childIndexB = -1;
82				//for (int i=0;i<nContacts;i++)
83				posOut.w = -depthOut;
84				c->m_worldPosB[0] = posOut;//localPoints[contactIdx[i]];
85				GET_NPOINTS(*c) = 1;//nContacts;
86			}
87		}
88
89	}
90}
91
92typedef float4 Quaternion;
93#define make_float4 (float4)
94
95__inline
96float dot3F4(float4 a, float4 b)
97{
98	float4 a1 = make_float4(a.xyz,0.f);
99	float4 b1 = make_float4(b.xyz,0.f);
100	return dot(a1, b1);
101}
102
103
104
105
106__inline
107float4 cross3(float4 a, float4 b)
108{
109	return cross(a,b);
110}
111__inline
112Quaternion qtMul(Quaternion a, Quaternion b)
113{
114	Quaternion ans;
115	ans = cross3( a, b );
116	ans += a.w*b+b.w*a;
117//	ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z);
118	ans.w = a.w*b.w - dot3F4(a, b);
119	return ans;
120}
121
122__inline
123Quaternion qtInvert(Quaternion q)
124{
125	return (Quaternion)(-q.xyz, q.w);
126}
127
128__inline
129float4 qtRotate(Quaternion q, float4 vec)
130{
131	Quaternion qInv = qtInvert( q );
132	float4 vcpy = vec;
133	vcpy.w = 0.f;
134	float4 out = qtMul(qtMul(q,vcpy),qInv);
135	return out;
136}
137
138__inline
139float4 transform(const float4* p, const float4* translation, const Quaternion* orientation)
140{
141	return qtRotate( *orientation, *p ) + (*translation);
142}
143
144
145__inline
146float4 qtInvRotate(const Quaternion q, float4 vec)
147{
148	return qtRotate( qtInvert( q ), vec );
149}
150
151
152inline void project(__global const b3ConvexPolyhedronData_t* hull,  const float4 pos, const float4 orn,
153const float4* dir, __global const float4* vertices, float* min, float* max)
154{
155	min[0] = FLT_MAX;
156	max[0] = -FLT_MAX;
157	int numVerts = hull->m_numVertices;
158
159	const float4 localDir = qtInvRotate(orn,*dir);
160	float offset = dot(pos,*dir);
161	for(int i=0;i<numVerts;i++)
162	{
163		float dp = dot(vertices[hull->m_vertexOffset+i],localDir);
164		if(dp < min[0])
165			min[0] = dp;
166		if(dp > max[0])
167			max[0] = dp;
168	}
169	if(min[0]>max[0])
170	{
171		float tmp = min[0];
172		min[0] = max[0];
173		max[0] = tmp;
174	}
175	min[0] += offset;
176	max[0] += offset;
177}
178
179
180bool findSeparatingAxisUnitSphere(	__global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
181	const float4 posA1,
182	const float4 ornA,
183	const float4 posB1,
184	const float4 ornB,
185	const float4 DeltaC2,
186	__global const float4* vertices,
187	__global const float4* unitSphereDirections,
188	int numUnitSphereDirections,
189	float4* sep,
190	float* dmin)
191{
192
193	float4 posA = posA1;
194	posA.w = 0.f;
195	float4 posB = posB1;
196	posB.w = 0.f;
197
198	int curPlaneTests=0;
199
200	int curEdgeEdge = 0;
201	// Test unit sphere directions
202	for (int i=0;i<numUnitSphereDirections;i++)
203	{
204
205		float4 crossje;
206		crossje = unitSphereDirections[i];
207
208		if (dot3F4(DeltaC2,crossje)>0)
209			crossje *= -1.f;
210		{
211			float dist;
212			bool result = true;
213			float Min0,Max0;
214			float Min1,Max1;
215			project(hullA,posA,ornA,&crossje,vertices, &Min0, &Max0);
216			project(hullB,posB,ornB,&crossje,vertices, &Min1, &Max1);
217
218			if(Max0<Min1 || Max1<Min0)
219				return false;
220
221			float d0 = Max0 - Min1;
222			float d1 = Max1 - Min0;
223			dist = d0<d1 ? d0:d1;
224			result = true;
225
226			if(dist<*dmin)
227			{
228				*dmin = dist;
229				*sep = crossje;
230			}
231		}
232	}
233
234
235	if((dot3F4(-DeltaC2,*sep))>0.0f)
236	{
237		*sep = -(*sep);
238	}
239	return true;
240}
241
242
243
244__kernel void   findSeparatingAxisUnitSphereKernel( __global const int4* pairs,
245																					__global const b3RigidBodyData_t* rigidBodies,
246																					__global const b3Collidable_t* collidables,
247																					__global const b3ConvexPolyhedronData_t* convexShapes,
248																					__global const float4* vertices,
249																					__global const float4* unitSphereDirections,
250																					__global  float4* separatingNormals,
251																					__global  int* hasSeparatingAxis,
252																					__global  float* dmins,
253																					int numUnitSphereDirections,
254																					int numPairs
255																					)
256{
257
258	int i = get_global_id(0);
259
260	if (i<numPairs)
261	{
262
263		if (hasSeparatingAxis[i])
264		{
265
266			int bodyIndexA = pairs[i].x;
267			int bodyIndexB = pairs[i].y;
268
269			int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
270			int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
271
272			int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
273			int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
274
275
276			int numFacesA = convexShapes[shapeIndexA].m_numFaces;
277
278			float dmin = dmins[i];
279
280			float4 posA = rigidBodies[bodyIndexA].m_pos;
281			posA.w = 0.f;
282			float4 posB = rigidBodies[bodyIndexB].m_pos;
283			posB.w = 0.f;
284			float4 c0local = convexShapes[shapeIndexA].m_localCenter;
285			float4 ornA = rigidBodies[bodyIndexA].m_quat;
286			float4 c0 = transform(&c0local, &posA, &ornA);
287			float4 c1local = convexShapes[shapeIndexB].m_localCenter;
288			float4 ornB =rigidBodies[bodyIndexB].m_quat;
289			float4 c1 = transform(&c1local,&posB,&ornB);
290			const float4 DeltaC2 = c0 - c1;
291			float4 sepNormal = separatingNormals[i];
292
293			int numEdgeEdgeDirections = convexShapes[shapeIndexA].m_numUniqueEdges*convexShapes[shapeIndexB].m_numUniqueEdges;
294			if (numEdgeEdgeDirections>numUnitSphereDirections)
295			{
296				bool sepEE = findSeparatingAxisUnitSphere(	&convexShapes[shapeIndexA], &convexShapes[shapeIndexB],posA,ornA,
297																										posB,ornB,
298																										DeltaC2,
299																										vertices,unitSphereDirections,numUnitSphereDirections,&sepNormal,&dmin);
300				if (!sepEE)
301				{
302					hasSeparatingAxis[i] = 0;
303				} else
304				{
305					hasSeparatingAxis[i] = 1;
306					separatingNormals[i] = sepNormal;
307				}
308			}
309		}		//if (hasSeparatingAxis[i])
310	}//(i<numPairs)
311}
312