diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3Collidable.h b/src/Bullet3OpenCL/NarrowphaseCollision/b3Collidable.h index 5217a6c1e..a1c923655 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3Collidable.h +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3Collidable.h @@ -2,6 +2,9 @@ #ifndef B3_COLLIDABLE_H #define B3_COLLIDABLE_H +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3Quaternion.h" + enum b3ShapeTypes { SHAPE_HEIGHT_FIELD=1, @@ -34,8 +37,8 @@ struct b3CollidableNew struct b3GpuChildShape { - float m_childPosition[4]; - float m_childOrientation[4]; + b3Vector3 m_childPosition; + b3Quaternion m_childOrientation; int m_shapeIndex; int m_unused0; int m_unused1; diff --git a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp index b8a83b37a..6d1e43b5e 100644 --- a/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp +++ b/src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp @@ -21,6 +21,10 @@ subject to the following restrictions: //#define B3_DEBUG_SAT_FACE //#define CHECK_ON_HOST +#ifdef CHECK_ON_HOST +//#define PERSISTENT_CONTACTS_HOST +#endif + int b3g_actualSATPairTests=0; #include "b3ConvexHullContact.h" @@ -560,9 +564,11 @@ void computeContactPlaneCompound(int pairIndex, const b3RigidBodyCL* rigidBodies, const b3Collidable* collidables, const b3ConvexPolyhedronCL* convexShapes, + const b3GpuChildShape* cpuChildShapes, const b3Vector3* convexVertices, const int* convexIndices, const b3GpuFace* faces, + b3Contact4* globalContactsOut, int& nGlobalContactsOut, int maxContactCapacity) @@ -572,121 +578,133 @@ void computeContactPlaneCompound(int pairIndex, b3Assert(shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS); - - int shapeIndex = collidables[collidableIndexB].m_shapeIndex; - const b3ConvexPolyhedronCL* hullB = &convexShapes[shapeIndex]; - - b3Vector3 posB = rigidBodies[bodyIndexB].m_pos; - b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; - b3Vector3 posA = rigidBodies[bodyIndexA].m_pos; - b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; - - int numContactsOut = 0; - int numWorldVertsB1= 0; - - b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane; - b3Vector3 planeNormal(planeEq.x,planeEq.y,planeEq.z); - b3Vector3 planeNormalWorld = b3QuatRotate(ornA,planeNormal); - float planeConstant = planeEq.w; - b3Transform convexWorldTransform; - convexWorldTransform.setIdentity(); - convexWorldTransform.setOrigin(posB); - convexWorldTransform.setRotation(ornB); - b3Transform planeTransform; - planeTransform.setIdentity(); - planeTransform.setOrigin(posA); - planeTransform.setRotation(ornA); - - b3Transform planeInConvex; - planeInConvex= convexWorldTransform.inverse() * planeTransform; - b3Transform convexInPlane; - convexInPlane = planeTransform.inverse() * convexWorldTransform; - - b3Vector3 planeNormalInConvex = planeInConvex.getBasis()*-planeNormal; - float maxDot = -1e30; - int hitVertex=-1; - b3Vector3 hitVtx; - -#define MAX_PLANE_CONVEX_POINTS 64 - - b3Vector3 contactPoints[MAX_PLANE_CONVEX_POINTS]; - int numPoints = 0; - - b3Int4 contactIdx; - contactIdx.s[0] = 0; - contactIdx.s[1] = 1; - contactIdx.s[2] = 2; - contactIdx.s[3] = 3; - - for (int i=0;im_numVertices;i++) + int numChildrenB = collidables[collidableIndexB].m_numChildShapes; + for (int c=0;cm_vertexOffset+i]; - float curDot = vtx.dot(planeNormalInConvex); + int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+c; + int childColIndexB = cpuChildShapes[childShapeIndexB].m_shapeIndex; + float4 rootPosB = rigidBodies[bodyIndexB].m_pos; + b3Quaternion rootOrnB = rigidBodies[bodyIndexB].m_quat; + b3Vector3 childPosB = cpuChildShapes[childShapeIndexB].m_childPosition; + b3Quaternion childOrnB = cpuChildShapes[childShapeIndexB].m_childOrientation; + float4 posB = b3QuatRotate(rootOrnB,childPosB)+rootPosB; + b3Quaternion ornB = rootOrnB*childOrnB;//b3QuatMul(ornB,childOrnB); - if (curDot>maxDot) + int shapeIndexB = collidables[childColIndexB].m_shapeIndex; + + const b3ConvexPolyhedronCL* hullB = &convexShapes[shapeIndexB]; + + + b3Vector3 posA = rigidBodies[bodyIndexA].m_pos; + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + + int numContactsOut = 0; + int numWorldVertsB1= 0; + + b3Vector3 planeEq = faces[collidables[collidableIndexA].m_shapeIndex].m_plane; + b3Vector3 planeNormal(planeEq.x,planeEq.y,planeEq.z); + b3Vector3 planeNormalWorld = b3QuatRotate(ornA,planeNormal); + float planeConstant = planeEq.w; + b3Transform convexWorldTransform; + convexWorldTransform.setIdentity(); + convexWorldTransform.setOrigin(posB); + convexWorldTransform.setRotation(ornB); + b3Transform planeTransform; + planeTransform.setIdentity(); + planeTransform.setOrigin(posA); + planeTransform.setRotation(ornA); + + b3Transform planeInConvex; + planeInConvex= convexWorldTransform.inverse() * planeTransform; + b3Transform convexInPlane; + convexInPlane = planeTransform.inverse() * convexWorldTransform; + + b3Vector3 planeNormalInConvex = planeInConvex.getBasis()*-planeNormal; + float maxDot = -1e30; + int hitVertex=-1; + b3Vector3 hitVtx; + + #define MAX_PLANE_CONVEX_POINTS 64 + + b3Vector3 contactPoints[MAX_PLANE_CONVEX_POINTS]; + int numPoints = 0; + + b3Int4 contactIdx; + contactIdx.s[0] = 0; + contactIdx.s[1] = 1; + contactIdx.s[2] = 2; + contactIdx.s[3] = 3; + + for (int i=0;im_numVertices;i++) { - hitVertex=i; - maxDot=curDot; - hitVtx = vtx; - //make sure the deepest points is always included - if (numPoints==MAX_PLANE_CONVEX_POINTS) - numPoints--; + b3Vector3 vtx = convexVertices[hullB->m_vertexOffset+i]; + float curDot = vtx.dot(planeNormalInConvex); + + + if (curDot>maxDot) + { + hitVertex=i; + maxDot=curDot; + hitVtx = vtx; + //make sure the deepest points is always included + if (numPoints==MAX_PLANE_CONVEX_POINTS) + numPoints--; + } + + if (numPoints4) - { - numReducedPoints = extractManifoldSequentialGlobal( contactPoints, numPoints, planeNormalInConvex, &contactIdx); - } - int dstIdx; -// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx ); - - if (numReducedPoints>0) - { - if (nGlobalContactsOut < maxContactCapacity) + if (numPoints>4) { - dstIdx=nGlobalContactsOut; - nGlobalContactsOut++; - - b3Contact4* c = &globalContactsOut[dstIdx]; - c->m_worldNormalOnB = -planeNormalWorld; - 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; - for (int i=0;im_worldPosB[i] = pOnB1; - } - c->m_worldNormalOnB[3] = (b3Scalar)numReducedPoints; - }//if (dstIdx < numPairs) - } + numReducedPoints = extractManifoldSequentialGlobal( contactPoints, numPoints, planeNormalInConvex, &contactIdx); + } + int dstIdx; + // dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx ); + if (numReducedPoints>0) + { + if (nGlobalContactsOut < maxContactCapacity) + { + dstIdx=nGlobalContactsOut; + nGlobalContactsOut++; + b3Contact4* c = &globalContactsOut[dstIdx]; + c->m_worldNormalOnB = -planeNormalWorld; + c->setFrictionCoeff(0.7); + c->setRestituitionCoeff(0.f); -// printf("computeContactPlaneConvex\n"); + c->m_batchIdx = pairIndex; + c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass==0?-bodyIndexA:bodyIndexA; + c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass==0?-bodyIndexB:bodyIndexB; + for (int i=0;im_worldPosB[i] = pOnB1; + } + c->m_worldNormalOnB[3] = (b3Scalar)numReducedPoints; + }//if (dstIdx < numPairs) + } + + } + + } @@ -1647,9 +1665,9 @@ int computeContactConvexConvex( b3AlignedObjectArray& pairs, if (numOldPoints) { newContact = oldContacts[pairs[pairIndex].z]; -#ifdef CHECK_ON_HOST +#ifdef PERSISTENT_CONTACTS_HOST b3ContactCache::refreshContactPoints(transA,transB,newContact); -#endif //CHECK_ON_HOST +#endif //PERSISTENT_CONTACTS_HOST } numPoints = b3Contact4Data_getNumPoints(&newContact); @@ -1680,7 +1698,7 @@ int computeContactConvexConvex( b3AlignedObjectArray& pairs, resultPointOnBWorld.w = distance2; newContact.m_worldPosB[p] = resultPointOnBWorld; b3Vector3 resultPointOnAWorld = resultPointOnBWorld+distance2*sepAxis2; -#ifdef CHECK_ON_HOST +#ifdef PERSISTENT_CONTACTS_HOST newContact.m_localPosA[p] = transA.inverse()*resultPointOnAWorld; newContact.m_localPosB[p] = transB.inverse()*resultPointOnBWorld; #endif @@ -1881,7 +1899,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* } - hostContacts.resize(nPairs); + hostContacts.resize(maxContactCapacity); for (int i=0;i* hostCollidables[collidableIndexB].m_shapeType == SHAPE_PLANE) { computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&hostBodyBuf[0], - &hostCollidables[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); + &hostCollidables[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); // printf("convex-plane\n"); } @@ -1938,7 +1956,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* hostCollidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS) { computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&hostBodyBuf[0], - &hostCollidables[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); + &hostCollidables[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity); // printf("plane-convex\n"); } @@ -1970,9 +1988,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray* pairs->copyFromHost(hostPairs); } + hostContacts.resize(nContacts); + if (nContacts) { - hostContacts.resize(nContacts); + contactOut->copyFromHost(hostContacts); }