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