#ifndef B3_NEW_CONTACT_REDUCTION_H #define B3_NEW_CONTACT_REDUCTION_H #include "Bullet3Common/shared/b3Float4.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" #define GET_NPOINTS(x) (x).m_worldNormalOnB.w int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx) { if( nPoints == 0 ) return 0; if (nPoints <=4) return nPoints; if (nPoints >64) nPoints = 64; b3Float4 center = b3MakeFloat4(0,0,0,0); { for (int i=0;i<nPoints;i++) center += p[i]; center /= (float)nPoints; } // sample 4 directions b3Float4 aVector = p[0] - center; b3Float4 u = b3Cross( nearNormal, aVector ); b3Float4 v = b3Cross( nearNormal, u ); u = b3Normalized( u ); v = b3Normalized( v ); //keep point with deepest penetration float minW= FLT_MAX; int minIndex=-1; b3Float4 maxDots; maxDots.x = FLT_MIN; maxDots.y = FLT_MIN; maxDots.z = FLT_MIN; maxDots.w = FLT_MIN; // idx, distance for(int ie = 0; ie<nPoints; ie++ ) { if (p[ie].w<minW) { minW = p[ie].w; minIndex=ie; } float f; b3Float4 r = p[ie]-center; f = b3Dot( u, r ); if (f<maxDots.x) { maxDots.x = f; contactIdx[0].x = ie; } f = b3Dot( -u, r ); if (f<maxDots.y) { maxDots.y = f; contactIdx[0].y = ie; } f = b3Dot( v, r ); if (f<maxDots.z) { maxDots.z = f; contactIdx[0].z = ie; } f = b3Dot( -v, r ); if (f<maxDots.w) { maxDots.w = f; contactIdx[0].w = ie; } } if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex) { //replace the first contact with minimum (todo: replace contact with least penetration) contactIdx[0].x = minIndex; } return 4; } __kernel void b3NewContactReductionKernel( __global b3Int4* pairs, __global const b3RigidBodyData_t* rigidBodies, __global const b3Float4* separatingNormals, __global const int* hasSeparatingAxis, __global struct b3Contact4Data* globalContactsOut, __global b3Int4* clippingFaces, __global b3Float4* worldVertsB2, volatile __global int* nGlobalContactsOut, int vertexFaceCapacity, int contactCapacity, int numPairs, int pairIndex ) { // int i = get_global_id(0); //int pairIndex = i; int i = pairIndex; b3Int4 contactIdx; contactIdx=b3MakeInt4(0,1,2,3); if (i<numPairs) { if (hasSeparatingAxis[i]) { int nPoints = clippingFaces[pairIndex].w; if (nPoints>0) { __global b3Float4* pointsIn = &worldVertsB2[pairIndex*vertexFaceCapacity]; b3Float4 normal = -separatingNormals[i]; int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx); int dstIdx; dstIdx = b3AtomicInc( nGlobalContactsOut); //#if 0 b3Assert(dstIdx < contactCapacity); if (dstIdx < contactCapacity) { __global struct b3Contact4Data* c = &globalContactsOut[dstIdx]; 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; int bodyB = pairs[pairIndex].y; pairs[pairIndex].w = dstIdx; c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass==0?-bodyA:bodyA; c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass==0?-bodyB:bodyB; c->m_childIndexA =-1; c->m_childIndexB =-1; switch (nReducedContacts) { case 4: c->m_worldPosB[3] = pointsIn[contactIdx.w]; case 3: c->m_worldPosB[2] = pointsIn[contactIdx.z]; case 2: c->m_worldPosB[1] = pointsIn[contactIdx.y]; case 1: c->m_worldPosB[0] = pointsIn[contactIdx.x]; default: { } }; GET_NPOINTS(*c) = nReducedContacts; } //#endif }// if (numContactsOut>0) }// if (hasSeparatingAxis[i]) }// if (i<numPairs) } #endif