diff options
Diffstat (limited to 'thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared')
16 files changed, 3587 insertions, 0 deletions
diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h new file mode 100644 index 0000000000..8788ccbb47 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h @@ -0,0 +1,20 @@ + +#ifndef B3_BVH_SUBTREE_INFO_DATA_H +#define B3_BVH_SUBTREE_INFO_DATA_H + +typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t; + +struct b3BvhSubtreeInfoData +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes, points to the root of the subtree + int m_rootNodeIndex; + //4 bytes + int m_subtreeSize; + int m_padding[3]; +}; + +#endif //B3_BVH_SUBTREE_INFO_DATA_H + diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h new file mode 100644 index 0000000000..2618da24bc --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h @@ -0,0 +1,126 @@ + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" + + + +// work-in-progress +void b3BvhTraversal( __global const b3Int4* pairs, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global b3Aabb* aabbs, + __global b3Int4* concavePairsOut, + __global volatile int* numConcavePairsOut, + __global const b3BvhSubtreeInfo* subtreeHeadersRoot, + __global const b3QuantizedBvhNode* quantizedNodesRoot, + __global const b3BvhInfo* bvhInfos, + int numPairs, + int maxNumConcavePairsCapacity, + int id) +{ + + int bodyIndexA = pairs[id].x; + int bodyIndexB = pairs[id].y; + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + //once the broadphase avoids static-static pairs, we can remove this test + if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0)) + { + return; + } + + if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH) + return; + + int shapeTypeB = collidables[collidableIndexB].m_shapeType; + + if (shapeTypeB!=SHAPE_CONVEX_HULL && + shapeTypeB!=SHAPE_SPHERE && + shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS + ) + return; + + b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes]; + + b3Float4 bvhAabbMin = bvhInfo.m_aabbMin; + b3Float4 bvhAabbMax = bvhInfo.m_aabbMax; + b3Float4 bvhQuantization = bvhInfo.m_quantization; + int numSubtreeHeaders = bvhInfo.m_numSubTrees; + __global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset]; + __global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset]; + + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization); + b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization); + + for (int i=0;i<numSubtreeHeaders;i++) + { + b3BvhSubtreeInfoData subtree = subtreeHeaders[i]; + + int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); + if (overlap != 0) + { + int startNodeIndex = subtree.m_rootNodeIndex; + int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize; + int curIndex = startNodeIndex; + int escapeIndex; + int isLeafNode; + int aabbOverlap; + while (curIndex < endNodeIndex) + { + b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex]; + aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax); + isLeafNode = b3IsLeaf(&rootNode); + if (aabbOverlap) + { + if (isLeafNode) + { + int triangleIndex = b3GetTriangleIndex(&rootNode); + if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + int numChildrenB = collidables[collidableIndexB].m_numChildShapes; + int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB); + for (int b=0;b<numChildrenB;b++) + { + if ((pairIdx+b)<maxNumConcavePairsCapacity) + { + int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b; + b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB); + concavePairsOut[pairIdx+b] = newPair; + } + } + } else + { + int pairIdx = b3AtomicInc(numConcavePairsOut); + if (pairIdx<maxNumConcavePairsCapacity) + { + b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0); + concavePairsOut[pairIdx] = newPair; + } + } + } + curIndex++; + } else + { + if (isLeafNode) + { + curIndex++; + } else + { + escapeIndex = b3GetEscapeIndex(&rootNode); + curIndex += escapeIndex; + } + } + } + } + } + +}
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h new file mode 100644 index 0000000000..8009e7d6e0 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h @@ -0,0 +1,188 @@ +#ifndef B3_CLIP_FACES_H +#define B3_CLIP_FACES_H + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + + +inline b3Float4 b3Lerp3(b3Float4ConstArg a,b3Float4ConstArg b, float t) +{ + return b3MakeFloat4( a.x + (b.x - a.x) * t, + a.y + (b.y - a.y) * t, + a.z + (b.z - a.z) * t, + 0.f); +} + +// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut +int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS,float planeEqWS, __global b3Float4* ppVtxOut) +{ + + int ve; + float ds, de; + int numVertsOut = 0; + //double-check next test + // if (numVertsIn < 2) + // return 0; + + b3Float4 firstVertex=pVtxIn[numVertsIn-1]; + b3Float4 endVertex = pVtxIn[0]; + + ds = b3Dot(planeNormalWS,firstVertex)+planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex=pVtxIn[ve]; + de = b3Dot(planeNormalWS,endVertex)+planeEqWS; + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut[numVertsOut++] = endVertex; + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +__kernel void clipFacesAndFindContactsKernel( __global const b3Float4* separatingNormals, + __global const int* hasSeparatingAxis, + __global b3Int4* clippingFacesOut, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + __global b3Float4* worldVertsB2, + int vertexFaceCapacity, + int pairIndex + ) +{ +// int i = get_global_id(0); + //int pairIndex = i; + int i = pairIndex; + + float minDist = -1e30f; + float maxDist = 0.02f; + +// if (i<numPairs) + { + + if (hasSeparatingAxis[i]) + { + +// int bodyIndexA = pairs[i].x; + // int bodyIndexB = pairs[i].y; + + int numLocalContactsOut = 0; + + int capacityWorldVertsB2 = vertexFaceCapacity; + + __global b3Float4* pVtxIn = &worldVertsB1[pairIndex*capacityWorldVertsB2]; + __global b3Float4* pVtxOut = &worldVertsB2[pairIndex*capacityWorldVertsB2]; + + + { + __global b3Int4* clippingFaces = clippingFacesOut; + + + int closestFaceA = clippingFaces[pairIndex].x; + // int closestFaceB = clippingFaces[pairIndex].y; + int numVertsInA = clippingFaces[pairIndex].z; + int numVertsInB = clippingFaces[pairIndex].w; + + int numVertsOut = 0; + + if (closestFaceA>=0) + { + + + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + + for(int e0=0;e0<numVertsInA;e0++) + { + const b3Float4 aw = worldVertsA1[pairIndex*capacityWorldVertsB2+e0]; + const b3Float4 bw = worldVertsA1[pairIndex*capacityWorldVertsB2+((e0+1)%numVertsInA)]; + const b3Float4 WorldEdge0 = aw - bw; + b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex]; + b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0,worldPlaneAnormal1); + b3Float4 worldA1 = aw; + float planeEqWS1 = -b3Dot(worldA1,planeNormalWS1); + b3Float4 planeNormalWS = planeNormalWS1; + float planeEqWS=planeEqWS1; + numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS,planeEqWS, pVtxOut); + __global b3Float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsInB = numVertsOut; + numVertsOut = 0; + } + + b3Float4 planeNormalWS = worldNormalsA1[pairIndex]; + float planeEqWS=-b3Dot(planeNormalWS,worldVertsA1[pairIndex*capacityWorldVertsB2]); + + for (int i=0;i<numVertsInB;i++) + { + float depth = b3Dot(planeNormalWS,pVtxIn[i])+planeEqWS; + if (depth <=minDist) + { + depth = minDist; + } +/* + static float maxDepth = 0.f; + if (depth < maxDepth) + { + maxDepth = depth; + if (maxDepth < -10) + { + printf("error at framecount %d?\n",myframecount); + } + printf("maxDepth = %f\n", maxDepth); + + } +*/ + if (depth <=maxDist) + { + b3Float4 pointInWorld = pVtxIn[i]; + pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth); + } + } + + } + clippingFaces[pairIndex].w =numLocalContactsOut; + + + } + + for (int i=0;i<numLocalContactsOut;i++) + pVtxIn[i] = pVtxOut[i]; + + }// if (hasSeparatingAxis[i]) + }// if (i<numPairs) + +} + +#endif //B3_CLIP_FACES_H + diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h new file mode 100644 index 0000000000..77cdc7b7a9 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h @@ -0,0 +1,76 @@ + +#ifndef B3_COLLIDABLE_H +#define B3_COLLIDABLE_H + + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Quat.h" + +enum b3ShapeTypes +{ + SHAPE_HEIGHT_FIELD=1, + + SHAPE_CONVEX_HULL=3, + SHAPE_PLANE=4, + SHAPE_CONCAVE_TRIMESH=5, + SHAPE_COMPOUND_OF_CONVEX_HULLS=6, + SHAPE_SPHERE=7, + MAX_NUM_SHAPE_TYPES, +}; + +typedef struct b3Collidable b3Collidable_t; + + +struct b3Collidable +{ + union { + int m_numChildShapes; + int m_bvhIndex; + }; + union + { + float m_radius; + int m_compoundBvhIndex; + }; + + int m_shapeType; + union + { + int m_shapeIndex; + float m_height; + }; +}; + +typedef struct b3GpuChildShape b3GpuChildShape_t; +struct b3GpuChildShape +{ + b3Float4 m_childPosition; + b3Quat m_childOrientation; + union + { + int m_shapeIndex;//used for SHAPE_COMPOUND_OF_CONVEX_HULLS + int m_capsuleAxis; + }; + union + { + float m_radius;//used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES + int m_numChildShapes;//used for compound shape + }; + union + { + float m_height;//used for childshape of SHAPE_COMPOUND_OF_CAPSULES + int m_collidableShapeIndex; + }; + int m_shapeType; +}; + +struct b3CompoundOverlappingPair +{ + int m_bodyIndexA; + int m_bodyIndexB; +// int m_pairType; + int m_childShapeIndexA; + int m_childShapeIndexB; +}; + +#endif //B3_COLLIDABLE_H diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h new file mode 100644 index 0000000000..dfd45cc566 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h @@ -0,0 +1,40 @@ +#ifndef B3_CONTACT4DATA_H +#define B3_CONTACT4DATA_H + +#include "Bullet3Common/shared/b3Float4.h" + +typedef struct b3Contact4Data b3Contact4Data_t; + +struct b3Contact4Data +{ + b3Float4 m_worldPosB[4]; +// b3Float4 m_localPosA[4]; +// b3Float4 m_localPosB[4]; + b3Float4 m_worldNormalOnB; // w: m_nPoints + unsigned short m_restituitionCoeffCmp; + unsigned short m_frictionCoeffCmp; + int m_batchIdx; + int m_bodyAPtrAndSignBit;//x:m_bodyAPtr, y:m_bodyBPtr + int m_bodyBPtrAndSignBit; + + int m_childIndexA; + int m_childIndexB; + int m_unused1; + int m_unused2; + + +}; + +inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact) +{ + return (int)contact->m_worldNormalOnB.w; +}; + +inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints) +{ + contact->m_worldNormalOnB.w = (float)numPoints; +}; + + + +#endif //B3_CONTACT4DATA_H
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h new file mode 100644 index 0000000000..f295f01a6c --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h @@ -0,0 +1,520 @@ + +#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H +#define B3_CONTACT_CONVEX_CONVEX_SAT_H + + +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h" + +#define B3_MAX_VERTS 1024 + + + +inline b3Float4 b3Lerp3(const b3Float4& a,const b3Float4& b, float t) +{ + return b3MakeVector3( a.x + (b.x - a.x) * t, + a.y + (b.y - a.y) * t, + a.z + (b.z - a.z) * t, + 0.f); +} + + +// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut +inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS,float planeEqWS, b3Float4* ppVtxOut) +{ + + int ve; + float ds, de; + int numVertsOut = 0; + if (numVertsIn < 2) + return 0; + + b3Float4 firstVertex=pVtxIn[numVertsIn-1]; + b3Float4 endVertex = pVtxIn[0]; + + ds = b3Dot3F4(planeNormalWS,firstVertex)+planeEqWS; + + for (ve = 0; ve < numVertsIn; ve++) + { + endVertex=pVtxIn[ve]; + + de = b3Dot3F4(planeNormalWS,endVertex)+planeEqWS; + + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut[numVertsOut++] = endVertex; + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex,(ds * 1.f/(ds - de)) ); + ppVtxOut[numVertsOut++] = endVertex; + } + } + firstVertex = endVertex; + ds = de; + } + return numVertsOut; +} + + +inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA, + const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1, + b3Float4* worldVertsB2, int capacityWorldVertsB2, + const float minDist, float maxDist, + const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA, + //const b3Float4* verticesB, const b3GpuFace* facesB, const int* indicesB, + b3Float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + + b3Float4* pVtxIn = worldVertsB1; + b3Float4* pVtxOut = worldVertsB2; + + int numVertsIn = numWorldVertsB1; + int numVertsOut = 0; + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;face<hullA->m_numFaces;face++) + { + const b3Float4 Normal = b3MakeVector3( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z,0.f); + const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal); + + float d = b3Dot3F4(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + } + } + } + if (closestFaceA<0) + return numContactsOut; + + b3GpuFace polyA = facesA[hullA->m_faceOffset+closestFaceA]; + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + //int numContacts = numWorldVertsB1; + int numVerticesA = polyA.m_numIndices; + for(int e0=0;e0<numVerticesA;e0++) + { + const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+e0]]; + const b3Float4 b = verticesA[hullA->m_vertexOffset+indicesA[polyA.m_indexOffset+((e0+1)%numVerticesA)]]; + const b3Float4 edge0 = a - b; + const b3Float4 WorldEdge0 = b3QuatRotate(ornA,edge0); + b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA,planeNormalA); + + b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0,worldPlaneAnormal1); + b3Float4 worldA1 = b3TransformPoint(a,posA,ornA); + float planeEqWS1 = -b3Dot3F4(worldA1,planeNormalWS1); + + b3Float4 planeNormalWS = planeNormalWS1; + float planeEqWS=planeEqWS1; + + //clip face + //clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); + numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS,planeEqWS, pVtxOut); + + //btSwap(pVtxIn,pVtxOut); + b3Float4* tmp = pVtxOut; + pVtxOut = pVtxIn; + pVtxIn = tmp; + numVertsIn = numVertsOut; + numVertsOut = 0; + } + + + // only keep points that are behind the witness face + { + b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x,polyA.m_plane.y,polyA.m_plane.z,0.f); + float localPlaneEq = polyA.m_plane.w; + b3Float4 planeNormalWS = b3QuatRotate(ornA,localPlaneNormal); + float planeEqWS=localPlaneEq-b3Dot3F4(planeNormalWS,posA); + for (int i=0;i<numVertsIn;i++) + { + float depth = b3Dot3F4(planeNormalWS,pVtxIn[i])+planeEqWS; + if (depth <=minDist) + { + depth = minDist; + } + if (numContactsOut<contactCapacity) + { + if (depth <=maxDist) + { + b3Float4 pointInWorld = pVtxIn[i]; + //resultOut.addContactPoint(separatingNormal,point,depth); + contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x,pointInWorld.y,pointInWorld.z,depth); + //printf("depth=%f\n",depth); + } + } else + { + b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut,contactCapacity); + } + } + } + + return numContactsOut; +} + + + +inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal, + const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA, const b3Quaternion& ornA,const b3Float4& posB, const b3Quaternion& ornB, + b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts, + const float minDist, float maxDist, + const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA, + const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB, + + b3Float4* contactsOut, + int contactCapacity) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + B3_PROFILE("clipHullAgainstHull"); + + //float curMaxDist=maxDist; + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + //B3_PROFILE("closestFaceB"); + if (hullB.m_numFaces!=1) + { + //printf("wtf\n"); + } + static bool once = true; + //printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z); + + for(int face=0;face<hullB.m_numFaces;face++) + { +#ifdef BT_DEBUG_SAT_FACE + if (once) + printf("face %d\n",face); + const b3GpuFace* faceB = &facesB[hullB.m_faceOffset+face]; + if (once) + { + for (int i=0;i<faceB->m_numIndices;i++) + { + b3Float4 vert = verticesB[hullB.m_vertexOffset+indicesB[faceB->m_indexOffset+i]]; + printf("vert[%d] = %f,%f,%f\n",i,vert.x,vert.y,vert.z); + } + } +#endif //BT_DEBUG_SAT_FACE + //if (facesB[hullB.m_faceOffset+face].m_numIndices>2) + { + const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset+face].m_plane.x, + facesB[hullB.m_faceOffset+face].m_plane.y, facesB[hullB.m_faceOffset+face].m_plane.z,0.f); + const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal); +#ifdef BT_DEBUG_SAT_FACE + if (once) + printf("faceNormal = %f,%f,%f\n",Normal.x,Normal.y,Normal.z); +#endif + float d = b3Dot3F4(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + once = false; + } + + + b3Assert(closestFaceB>=0); + { + //B3_PROFILE("worldVertsB1"); + const b3GpuFace& polyB = facesB[hullB.m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0<numVertices;e0++) + { + const b3Float4& b = verticesB[hullB.m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; + worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b,posB,ornB); + } + } + + if (closestFaceB>=0) + { + //B3_PROFILE("clipFaceAgainstHull"); + numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA, + posA,ornA, + worldVertsB1,numWorldVertsB1,worldVertsB2,capacityWorldVerts, minDist, maxDist, + verticesA, facesA, indicesA, + contactsOut,contactCapacity); + } + + return numContactsOut; +} + + + + +inline int b3ClipHullHullSingle( + int bodyIndexA, int bodyIndexB, + const b3Float4& posA, + const b3Quaternion& ornA, + const b3Float4& posB, + const b3Quaternion& ornB, + + int collidableIndexA, int collidableIndexB, + + const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf, + b3AlignedObjectArray<b3Contact4Data>* globalContactOut, + int& nContacts, + + const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA, + const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB, + + const b3AlignedObjectArray<b3Vector3>& verticesA, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA, + const b3AlignedObjectArray<b3GpuFace>& facesA, + const b3AlignedObjectArray<int>& indicesA, + + const b3AlignedObjectArray<b3Vector3>& verticesB, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB, + const b3AlignedObjectArray<b3GpuFace>& facesB, + const b3AlignedObjectArray<int>& indicesB, + + const b3AlignedObjectArray<b3Collidable>& hostCollidablesA, + const b3AlignedObjectArray<b3Collidable>& hostCollidablesB, + const b3Vector3& sepNormalWorldSpace, + int maxContactCapacity ) +{ + int contactIndex = -1; + b3ConvexPolyhedronData hullA, hullB; + + b3Collidable colA = hostCollidablesA[collidableIndexA]; + hullA = hostConvexDataA[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = hostCollidablesB[collidableIndexB]; + hullB = hostConvexDataB[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + + + b3Float4 contactsOut[B3_MAX_VERTS]; + int localContactCapacity = B3_MAX_VERTS; + +#ifdef _WIN32 + b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x)); + b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x)); +#endif + + + { + + b3Float4 worldVertsB1[B3_MAX_VERTS]; + b3Float4 worldVertsB2[B3_MAX_VERTS]; + int capacityWorldVerts = B3_MAX_VERTS; + + b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x,sepNormalWorldSpace.y,sepNormalWorldSpace.z,0.f); + int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex; + int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex; + + b3Scalar minDist = -1; + b3Scalar maxDist = 0.; + + + + b3Transform trA,trB; + { + //B3_PROFILE("b3TransformPoint computation"); + //trA.setIdentity(); + trA.setOrigin(b3MakeVector3(posA.x,posA.y,posA.z)); + trA.setRotation(b3Quaternion(ornA.x,ornA.y,ornA.z,ornA.w)); + + //trB.setIdentity(); + trB.setOrigin(b3MakeVector3(posB.x,posB.y,posB.z)); + trB.setRotation(b3Quaternion(ornB.x,ornB.y,ornB.z,ornB.w)); + } + + b3Quaternion trAorn = trA.getRotation(); + b3Quaternion trBorn = trB.getRotation(); + + int numContactsOut = b3ClipHullAgainstHull(hostNormal, + hostConvexDataA.at(shapeA), + hostConvexDataB.at(shapeB), + (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn, + (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn, + worldVertsB1,worldVertsB2,capacityWorldVerts, + minDist, maxDist, + verticesA, facesA,indicesA, + verticesB, facesB,indicesB, + + contactsOut,localContactCapacity); + + if (numContactsOut>0) + { + B3_PROFILE("overlap"); + + b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal; +// b3Float4 centerOut; + + b3Int4 contactIdx; + contactIdx.x = 0; + contactIdx.y = 1; + contactIdx.z = 2; + contactIdx.w = 3; + + int numPoints = 0; + + { + B3_PROFILE("extractManifold"); + numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx); + } + + b3Assert(numPoints); + + if (nContacts<maxContactCapacity) + { + contactIndex = nContacts; + globalContactOut->expand(); + b3Contact4Data& contact = globalContactOut->at(nContacts); + contact.m_batchIdx = 0;//i; + contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass==0)? -bodyIndexA:bodyIndexA; + contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass==0)? -bodyIndexB:bodyIndexB; + + contact.m_frictionCoeffCmp = 45874; + contact.m_restituitionCoeffCmp = 0; + + // float distance = 0.f; + for (int p=0;p<numPoints;p++) + { + contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];//check if it is actually on B + 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_worldNormalOnB.w = (b3Scalar)numPoints; + nContacts++; + } else + { + b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts,maxContactCapacity); + } + } + } + return contactIndex; +} + + + + + +inline int b3ContactConvexConvexSAT( + int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies, + const b3AlignedObjectArray<b3Collidable>& collidables, + const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes, + const b3AlignedObjectArray<b3Float4>& convexVertices, + const b3AlignedObjectArray<b3Float4>& uniqueEdges, + const b3AlignedObjectArray<int>& convexIndices, + const b3AlignedObjectArray<b3GpuFace>& faces, + b3AlignedObjectArray<b3Contact4Data>& globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity) +{ + int contactIndex = -1; + + + b3Float4 posA = rigidBodies[bodyIndexA].m_pos; + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + b3Float4 posB = rigidBodies[bodyIndexB].m_pos; + b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat; + + + b3ConvexPolyhedronData hullA, hullB; + + b3Float4 sepNormalWorldSpace; + + + + b3Collidable colA = collidables[collidableIndexA]; + hullA = convexShapes[colA.m_shapeIndex]; + //printf("numvertsA = %d\n",hullA.m_numVertices); + + + b3Collidable colB = collidables[collidableIndexB]; + hullB = convexShapes[colB.m_shapeIndex]; + //printf("numvertsB = %d\n",hullB.m_numVertices); + + + + +#ifdef _WIN32 + b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x)); + b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x)); +#endif + + bool foundSepAxis = b3FindSeparatingAxis(hullA,hullB, + posA, + ornA, + posB, + ornB, + + convexVertices,uniqueEdges,faces,convexIndices, + convexVertices,uniqueEdges,faces,convexIndices, + + sepNormalWorldSpace + ); + + + if (foundSepAxis) + { + + + contactIndex = b3ClipHullHullSingle( + bodyIndexA, bodyIndexB, + posA,ornA, + posB,ornB, + collidableIndexA, collidableIndexB, + &rigidBodies, + &globalContactsOut, + nGlobalContactsOut, + + convexShapes, + convexShapes, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + convexVertices, + uniqueEdges, + faces, + convexIndices, + + collidables, + collidables, + sepNormalWorldSpace, + maxContactCapacity); + + } + + return contactIndex; +} + +#endif //B3_CONTACT_CONVEX_CONVEX_SAT_H diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h new file mode 100644 index 0000000000..a3fa82287b --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h @@ -0,0 +1,162 @@ + +#ifndef B3_CONTACT_SPHERE_SPHERE_H +#define B3_CONTACT_SPHERE_SPHERE_H + + + + + +void computeContactSphereConvex(int pairIndex, + int bodyIndexA, int bodyIndexB, + int collidableIndexA, int collidableIndexB, + const b3RigidBodyData* rigidBodies, + const b3Collidable* collidables, + const b3ConvexPolyhedronData* convexShapes, + const b3Vector3* convexVertices, + const int* convexIndices, + const b3GpuFace* faces, + b3Contact4* globalContactsOut, + int& nGlobalContactsOut, + int maxContactCapacity) +{ + + float radius = collidables[collidableIndexA].m_radius; + float4 spherePos1 = rigidBodies[bodyIndexA].m_pos; + b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat; + + + + float4 pos = rigidBodies[bodyIndexB].m_pos; + + + b3Quaternion quat = rigidBodies[bodyIndexB].m_quat; + + b3Transform tr; + tr.setIdentity(); + tr.setOrigin(pos); + tr.setRotation(quat); + b3Transform trInv = tr.inverse(); + + float4 spherePos = trInv(spherePos1); + + int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx; + int shapeIndex = collidables[collidableIndex].m_shapeIndex; + int numFaces = convexShapes[shapeIndex].m_numFaces; + float4 closestPnt = b3MakeVector3(0, 0, 0, 0); + float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0); + float minDist = -1000000.f; // TODO: What is the largest/smallest float? + bool bCollide = true; + int region = -1; + float4 localHitNormal; + for ( int f = 0; f < numFaces; f++ ) + { + b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset+f]; + float4 planeEqn; + float4 localPlaneNormal = b3MakeVector3(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + float4 n1 = localPlaneNormal;//quatRotate(quat,localPlaneNormal); + planeEqn = n1; + planeEqn[3] = face.m_plane.w; + + float4 pntReturn; + float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn); + + if ( dist > radius) + { + bCollide = false; + break; + } + + if ( dist > 0 ) + { + //might hit an edge or vertex + b3Vector3 out; + + bool isInPoly = IsPointInPolygon(spherePos, + &face, + &convexVertices[convexShapes[shapeIndex].m_vertexOffset], + convexIndices, + &out); + if (isInPoly) + { + if (dist>minDist) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=1; + } + } else + { + b3Vector3 tmp = spherePos-out; + b3Scalar l2 = tmp.length2(); + if (l2<radius*radius) + { + dist = b3Sqrt(l2); + if (dist>minDist) + { + minDist = dist; + closestPnt = out; + localHitNormal = tmp/dist; + region=2; + } + + } else + { + bCollide = false; + break; + } + } + } + else + { + if ( dist > minDist ) + { + minDist = dist; + closestPnt = pntReturn; + localHitNormal = planeEqn; + region=3; + } + } + } + static int numChecks = 0; + numChecks++; + + if (bCollide && minDist > -10000) + { + + float4 normalOnSurfaceB1 = tr.getBasis()*localHitNormal;//-hitNormalWorld; + float4 pOnB1 = tr(closestPnt); + //printf("dist ,%f,",minDist); + float actualDepth = minDist-radius; + if (actualDepth<0) + { + //printf("actualDepth = ,%f,", actualDepth); + //printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z); + //printf("region=,%d,\n", region); + pOnB1[3] = actualDepth; + + int dstIdx; +// dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx ); + + if (nGlobalContactsOut < maxContactCapacity) + { + dstIdx=nGlobalContactsOut; + nGlobalContactsOut++; + + b3Contact4* c = &globalContactsOut[dstIdx]; + 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_worldPosB[0] = pOnB1; + int numPoints = 1; + c->m_worldNormalOnB.w = (b3Scalar)numPoints; + }//if (dstIdx < numPairs) + } + }//if (hasCollision) + +} +#endif //B3_CONTACT_SPHERE_SPHERE_H diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h new file mode 100644 index 0000000000..5c5f4e297f --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h @@ -0,0 +1,40 @@ + +#ifndef B3_CONVEX_POLYHEDRON_DATA_H +#define B3_CONVEX_POLYHEDRON_DATA_H + + + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Quat.h" + +typedef struct b3GpuFace b3GpuFace_t; +struct b3GpuFace +{ + b3Float4 m_plane; + int m_indexOffset; + int m_numIndices; + int m_unusedPadding1; + int m_unusedPadding2; +}; + +typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t; + +struct b3ConvexPolyhedronData +{ + b3Float4 m_localCenter; + b3Float4 m_extents; + b3Float4 mC; + b3Float4 mE; + + float m_radius; + int m_faceOffset; + int m_numFaces; + int m_numVertices; + + int m_vertexOffset; + int m_uniqueEdgesOffset; + int m_numUniqueEdges; + int m_unused; +}; + +#endif //B3_CONVEX_POLYHEDRON_DATA_H diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h new file mode 100644 index 0000000000..89993f3565 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h @@ -0,0 +1,832 @@ +#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H +#define B3_FIND_CONCAVE_SEPARATING_AXIS_H + +#define B3_TRIANGLE_NUM_CONVEX_FACES 5 + + +#include "Bullet3Common/shared/b3Int4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" + + +inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn, +const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max) +{ + min[0] = FLT_MAX; + max[0] = -FLT_MAX; + int numVerts = hull->m_numVertices; + + const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),*dir); + float offset = b3Dot(pos,*dir); + for(int i=0;i<numVerts;i++) + { + float dp = b3Dot(vertices[hull->m_vertexOffset+i],localDir); + if(dp < min[0]) + min[0] = dp; + if(dp > max[0]) + max[0] = dp; + } + if(min[0]>max[0]) + { + float tmp = min[0]; + min[0] = max[0]; + max[0] = tmp; + } + min[0] += offset; + max[0] += offset; +} + + +inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA,b3QuatConstArg ornA, + b3Float4ConstArg posB,b3QuatConstArg ornB, + b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB,float* depth) +{ + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + *depth = d0<d1 ? d0:d1; + return true; +} + + +bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA1, + b3QuatConstArg ornA, + b3Float4ConstArg posB1, + b3QuatConstArg ornB, + b3Float4ConstArg DeltaC2, + + const b3Float4* verticesA, + const b3Float4* uniqueEdgesA, + const b3GpuFace* facesA, + const int* indicesA, + + __global const b3Float4* verticesB, + __global const b3Float4* uniqueEdgesB, + __global const b3GpuFace* facesB, + __global const int* indicesB, + b3Float4* sep, + float* dmin) +{ + + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; +/* + static int maxFaceVertex = 0; + + int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices; + curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices; + + if (curFaceVertexAB>maxFaceVertex) + { + maxFaceVertex = curFaceVertexAB; + printf("curFaceVertexAB = %d\n",curFaceVertexAB); + printf("hullA->m_numFaces = %d\n",hullA->m_numFaces); + printf("hullA->m_numVertices = %d\n",hullA->m_numVertices); + printf("hullB->m_numVertices = %d\n",hullB->m_numVertices); + } +*/ + + int curPlaneTests=0; + { + int numFacesA = hullA->m_numFaces; + // Test normals from hullA + for(int i=0;i<numFacesA;i++) + { + const b3Float4 normal = facesA[hullA->m_faceOffset+i].m_plane; + b3Float4 faceANormalWS = b3QuatRotate(ornA,normal); + if (b3Dot(DeltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + curPlaneTests++; + float d; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d)) + return false; + if(d<*dmin) + { + *dmin = d; + *sep = faceANormalWS; + } + } + } + if((b3Dot(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + + +b3Vector3 unitSphere162[]= +{ + b3MakeVector3(0.000000,-1.000000,0.000000), +b3MakeVector3(0.203181,-0.967950,0.147618), +b3MakeVector3(-0.077607,-0.967950,0.238853), +b3MakeVector3(0.723607,-0.447220,0.525725), +b3MakeVector3(0.609547,-0.657519,0.442856), +b3MakeVector3(0.812729,-0.502301,0.295238), +b3MakeVector3(-0.251147,-0.967949,0.000000), +b3MakeVector3(-0.077607,-0.967950,-0.238853), +b3MakeVector3(0.203181,-0.967950,-0.147618), +b3MakeVector3(0.860698,-0.251151,0.442858), +b3MakeVector3(-0.276388,-0.447220,0.850649), +b3MakeVector3(-0.029639,-0.502302,0.864184), +b3MakeVector3(-0.155215,-0.251152,0.955422), +b3MakeVector3(-0.894426,-0.447216,0.000000), +b3MakeVector3(-0.831051,-0.502299,0.238853), +b3MakeVector3(-0.956626,-0.251149,0.147618), +b3MakeVector3(-0.276388,-0.447220,-0.850649), +b3MakeVector3(-0.483971,-0.502302,-0.716565), +b3MakeVector3(-0.436007,-0.251152,-0.864188), +b3MakeVector3(0.723607,-0.447220,-0.525725), +b3MakeVector3(0.531941,-0.502302,-0.681712), +b3MakeVector3(0.687159,-0.251152,-0.681715), +b3MakeVector3(0.687159,-0.251152,0.681715), +b3MakeVector3(-0.436007,-0.251152,0.864188), +b3MakeVector3(-0.956626,-0.251149,-0.147618), +b3MakeVector3(-0.155215,-0.251152,-0.955422), +b3MakeVector3(0.860698,-0.251151,-0.442858), +b3MakeVector3(0.276388,0.447220,0.850649), +b3MakeVector3(0.483971,0.502302,0.716565), +b3MakeVector3(0.232822,0.657519,0.716563), +b3MakeVector3(-0.723607,0.447220,0.525725), +b3MakeVector3(-0.531941,0.502302,0.681712), +b3MakeVector3(-0.609547,0.657519,0.442856), +b3MakeVector3(-0.723607,0.447220,-0.525725), +b3MakeVector3(-0.812729,0.502301,-0.295238), +b3MakeVector3(-0.609547,0.657519,-0.442856), +b3MakeVector3(0.276388,0.447220,-0.850649), +b3MakeVector3(0.029639,0.502302,-0.864184), +b3MakeVector3(0.232822,0.657519,-0.716563), +b3MakeVector3(0.894426,0.447216,0.000000), +b3MakeVector3(0.831051,0.502299,-0.238853), +b3MakeVector3(0.753442,0.657515,0.000000), +b3MakeVector3(-0.232822,-0.657519,0.716563), +b3MakeVector3(-0.162456,-0.850654,0.499995), +b3MakeVector3(0.052790,-0.723612,0.688185), +b3MakeVector3(0.138199,-0.894429,0.425321), +b3MakeVector3(0.262869,-0.525738,0.809012), +b3MakeVector3(0.361805,-0.723611,0.587779), +b3MakeVector3(0.531941,-0.502302,0.681712), +b3MakeVector3(0.425323,-0.850654,0.309011), +b3MakeVector3(0.812729,-0.502301,-0.295238), +b3MakeVector3(0.609547,-0.657519,-0.442856), +b3MakeVector3(0.850648,-0.525736,0.000000), +b3MakeVector3(0.670817,-0.723611,-0.162457), +b3MakeVector3(0.670817,-0.723610,0.162458), +b3MakeVector3(0.425323,-0.850654,-0.309011), +b3MakeVector3(0.447211,-0.894428,0.000001), +b3MakeVector3(-0.753442,-0.657515,0.000000), +b3MakeVector3(-0.525730,-0.850652,0.000000), +b3MakeVector3(-0.638195,-0.723609,0.262864), +b3MakeVector3(-0.361801,-0.894428,0.262864), +b3MakeVector3(-0.688189,-0.525736,0.499997), +b3MakeVector3(-0.447211,-0.723610,0.525729), +b3MakeVector3(-0.483971,-0.502302,0.716565), +b3MakeVector3(-0.232822,-0.657519,-0.716563), +b3MakeVector3(-0.162456,-0.850654,-0.499995), +b3MakeVector3(-0.447211,-0.723611,-0.525727), +b3MakeVector3(-0.361801,-0.894429,-0.262863), +b3MakeVector3(-0.688189,-0.525736,-0.499997), +b3MakeVector3(-0.638195,-0.723609,-0.262863), +b3MakeVector3(-0.831051,-0.502299,-0.238853), +b3MakeVector3(0.361804,-0.723612,-0.587779), +b3MakeVector3(0.138197,-0.894429,-0.425321), +b3MakeVector3(0.262869,-0.525738,-0.809012), +b3MakeVector3(0.052789,-0.723611,-0.688186), +b3MakeVector3(-0.029639,-0.502302,-0.864184), +b3MakeVector3(0.956626,0.251149,0.147618), +b3MakeVector3(0.956626,0.251149,-0.147618), +b3MakeVector3(0.951058,-0.000000,0.309013), +b3MakeVector3(1.000000,0.000000,0.000000), +b3MakeVector3(0.947213,-0.276396,0.162458), +b3MakeVector3(0.951058,0.000000,-0.309013), +b3MakeVector3(0.947213,-0.276396,-0.162458), +b3MakeVector3(0.155215,0.251152,0.955422), +b3MakeVector3(0.436007,0.251152,0.864188), +b3MakeVector3(-0.000000,-0.000000,1.000000), +b3MakeVector3(0.309017,0.000000,0.951056), +b3MakeVector3(0.138199,-0.276398,0.951055), +b3MakeVector3(0.587786,0.000000,0.809017), +b3MakeVector3(0.447216,-0.276398,0.850648), +b3MakeVector3(-0.860698,0.251151,0.442858), +b3MakeVector3(-0.687159,0.251152,0.681715), +b3MakeVector3(-0.951058,-0.000000,0.309013), +b3MakeVector3(-0.809018,0.000000,0.587783), +b3MakeVector3(-0.861803,-0.276396,0.425324), +b3MakeVector3(-0.587786,0.000000,0.809017), +b3MakeVector3(-0.670819,-0.276397,0.688191), +b3MakeVector3(-0.687159,0.251152,-0.681715), +b3MakeVector3(-0.860698,0.251151,-0.442858), +b3MakeVector3(-0.587786,-0.000000,-0.809017), +b3MakeVector3(-0.809018,-0.000000,-0.587783), +b3MakeVector3(-0.670819,-0.276397,-0.688191), +b3MakeVector3(-0.951058,0.000000,-0.309013), +b3MakeVector3(-0.861803,-0.276396,-0.425324), +b3MakeVector3(0.436007,0.251152,-0.864188), +b3MakeVector3(0.155215,0.251152,-0.955422), +b3MakeVector3(0.587786,-0.000000,-0.809017), +b3MakeVector3(0.309017,-0.000000,-0.951056), +b3MakeVector3(0.447216,-0.276398,-0.850648), +b3MakeVector3(0.000000,0.000000,-1.000000), +b3MakeVector3(0.138199,-0.276398,-0.951055), +b3MakeVector3(0.670820,0.276396,0.688190), +b3MakeVector3(0.809019,-0.000002,0.587783), +b3MakeVector3(0.688189,0.525736,0.499997), +b3MakeVector3(0.861804,0.276394,0.425323), +b3MakeVector3(0.831051,0.502299,0.238853), +b3MakeVector3(-0.447216,0.276397,0.850649), +b3MakeVector3(-0.309017,-0.000001,0.951056), +b3MakeVector3(-0.262869,0.525738,0.809012), +b3MakeVector3(-0.138199,0.276397,0.951055), +b3MakeVector3(0.029639,0.502302,0.864184), +b3MakeVector3(-0.947213,0.276396,-0.162458), +b3MakeVector3(-1.000000,0.000001,0.000000), +b3MakeVector3(-0.850648,0.525736,-0.000000), +b3MakeVector3(-0.947213,0.276397,0.162458), +b3MakeVector3(-0.812729,0.502301,0.295238), +b3MakeVector3(-0.138199,0.276397,-0.951055), +b3MakeVector3(-0.309016,-0.000000,-0.951057), +b3MakeVector3(-0.262869,0.525738,-0.809012), +b3MakeVector3(-0.447215,0.276397,-0.850649), +b3MakeVector3(-0.531941,0.502302,-0.681712), +b3MakeVector3(0.861804,0.276396,-0.425322), +b3MakeVector3(0.809019,0.000000,-0.587782), +b3MakeVector3(0.688189,0.525736,-0.499997), +b3MakeVector3(0.670821,0.276397,-0.688189), +b3MakeVector3(0.483971,0.502302,-0.716565), +b3MakeVector3(0.077607,0.967950,0.238853), +b3MakeVector3(0.251147,0.967949,0.000000), +b3MakeVector3(0.000000,1.000000,0.000000), +b3MakeVector3(0.162456,0.850654,0.499995), +b3MakeVector3(0.361800,0.894429,0.262863), +b3MakeVector3(0.447209,0.723612,0.525728), +b3MakeVector3(0.525730,0.850652,0.000000), +b3MakeVector3(0.638194,0.723610,0.262864), +b3MakeVector3(-0.203181,0.967950,0.147618), +b3MakeVector3(-0.425323,0.850654,0.309011), +b3MakeVector3(-0.138197,0.894430,0.425320), +b3MakeVector3(-0.361804,0.723612,0.587778), +b3MakeVector3(-0.052790,0.723612,0.688185), +b3MakeVector3(-0.203181,0.967950,-0.147618), +b3MakeVector3(-0.425323,0.850654,-0.309011), +b3MakeVector3(-0.447210,0.894429,0.000000), +b3MakeVector3(-0.670817,0.723611,-0.162457), +b3MakeVector3(-0.670817,0.723611,0.162457), +b3MakeVector3(0.077607,0.967950,-0.238853), +b3MakeVector3(0.162456,0.850654,-0.499995), +b3MakeVector3(-0.138197,0.894430,-0.425320), +b3MakeVector3(-0.052790,0.723612,-0.688185), +b3MakeVector3(-0.361804,0.723612,-0.587778), +b3MakeVector3(0.361800,0.894429,-0.262863), +b3MakeVector3(0.638194,0.723610,-0.262864), +b3MakeVector3(0.447209,0.723612,-0.525728) +}; + + +bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB, + b3Float4ConstArg posA1, + b3QuatConstArg ornA, + b3Float4ConstArg posB1, + b3QuatConstArg ornB, + b3Float4ConstArg DeltaC2, + const b3Float4* verticesA, + const b3Float4* uniqueEdgesA, + const b3GpuFace* facesA, + const int* indicesA, + __global const b3Float4* verticesB, + __global const b3Float4* uniqueEdgesB, + __global const b3GpuFace* facesB, + __global const int* indicesB, + b3Float4* sep, + float* dmin, + bool searchAllEdgeEdge) +{ + + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; + +// int curPlaneTests=0; + + int curEdgeEdge = 0; + // Test edges + static int maxEdgeTests = 0; + int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges; + if (curEdgeTests >maxEdgeTests ) + { + maxEdgeTests = curEdgeTests ; + printf("maxEdgeTests = %d\n",maxEdgeTests ); + printf("hullA->m_numUniqueEdges = %d\n",hullA->m_numUniqueEdges); + printf("hullB->m_numUniqueEdges = %d\n",hullB->m_numUniqueEdges); + + } + + + if (searchAllEdgeEdge) + { + for(int e0=0;e0<hullA->m_numUniqueEdges;e0++) + { + const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0]; + b3Float4 edge0World = b3QuatRotate(ornA,edge0); + + for(int e1=0;e1<hullB->m_numUniqueEdges;e1++) + { + const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1]; + b3Float4 edge1World = b3QuatRotate(ornB,edge1); + + + b3Float4 crossje = b3Cross(edge0World,edge1World); + + curEdgeEdge++; + if(!b3IsAlmostZero(crossje)) + { + crossje = b3Normalized(crossje); + if (b3Dot(DeltaC2,crossje)<0) + crossje *= -1.f; + + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + dist = d0<d1 ? d0:d1; + result = true; + + } + + + if(dist<*dmin) + { + *dmin = dist; + *sep = crossje; + } + } + } + + } + } else + { + int numDirections = sizeof(unitSphere162)/sizeof(b3Vector3); + //printf("numDirections =%d\n",numDirections ); + + + for(int i=0;i<numDirections;i++) + { + b3Float4 crossje = unitSphere162[i]; + { + //if (b3Dot(DeltaC2,crossje)>0) + { + float dist; + bool result = true; + { + float Min0,Max0; + float Min1,Max1; + b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); + b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + float d0 = Max0 - Min1; + float d1 = Max1 - Min0; + dist = d0<d1 ? d0:d1; + result = true; + + } + + + if(dist<*dmin) + { + *dmin = dist; + *sep = crossje; + } + } + } + } + + } + + + if((b3Dot(-DeltaC2,*sep))>0.0f) + { + *sep = -(*sep); + } + return true; +} + + + +inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal, + __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB, + b3Float4ConstArg posA, b3QuatConstArg ornA,b3Float4ConstArg posB, b3QuatConstArg ornB, + __global b3Float4* worldVertsA1, + __global b3Float4* worldNormalsA1, + __global b3Float4* worldVertsB1, + int capacityWorldVerts, + const float minDist, float maxDist, + __global const b3Float4* verticesA, + __global const b3GpuFace_t* facesA, + __global const int* indicesA, + __global const b3Float4* verticesB, + __global const b3GpuFace_t* facesB, + __global const int* indicesB, + + __global b3Int4* clippingFaces, int pairIndex) +{ + int numContactsOut = 0; + int numWorldVertsB1= 0; + + + int closestFaceB=-1; + float dmax = -FLT_MAX; + + { + for(int face=0;face<hullB->m_numFaces;face++) + { + const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset+face].m_plane.x, + facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f); + const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal); + float d = b3Dot(WorldNormal,separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + + { + const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB]; + const int numVertices = polyB.m_numIndices; + for(int e0=0;e0<numVertices;e0++) + { + const b3Float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; + worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = b3TransformPoint(b,posB,ornB); + } + } + + int closestFaceA=-1; + { + float dmin = FLT_MAX; + for(int face=0;face<hullA->m_numFaces;face++) + { + const b3Float4 Normal = b3MakeFloat4( + facesA[hullA->m_faceOffset+face].m_plane.x, + facesA[hullA->m_faceOffset+face].m_plane.y, + facesA[hullA->m_faceOffset+face].m_plane.z, + 0.f); + const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal); + + float d = b3Dot(faceANormalWS,separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + worldNormalsA1[pairIndex] = faceANormalWS; + } + } + } + + int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices; + for(int e0=0;e0<numVerticesA;e0++) + { + const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]]; + worldVertsA1[pairIndex*capacityWorldVerts+e0] = b3TransformPoint(a, posA,ornA); + } + + clippingFaces[pairIndex].x = closestFaceA; + clippingFaces[pairIndex].y = closestFaceB; + clippingFaces[pairIndex].z = numVerticesA; + clippingFaces[pairIndex].w = numWorldVertsB1; + + + return numContactsOut; +} + + + + +__kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs, + __global const b3RigidBodyData* rigidBodies, + __global const b3Collidable* collidables, + __global const b3ConvexPolyhedronData* convexShapes, + __global const b3Float4* vertices, + __global const b3Float4* uniqueEdges, + __global const b3GpuFace* faces, + __global const int* indices, + __global const b3GpuChildShape* gpuChildShapes, + __global b3Aabb* aabbs, + __global b3Float4* concaveSeparatingNormalsOut, + __global b3Int4* clippingFacesOut, + __global b3Vector3* worldVertsA1Out, + __global b3Vector3* worldNormalsA1Out, + __global b3Vector3* worldVertsB1Out, + __global int* hasSeparatingNormals, + int vertexFaceCapacity, + int numConcavePairs, + int pairIdx + ) +{ + int i = pairIdx; +/* int i = get_global_id(0); + if (i>=numConcavePairs) + return; + int pairIdx = i; + */ + + int bodyIndexA = concavePairs[i].x; + int bodyIndexB = concavePairs[i].y; + + int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; + int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; + + int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; + int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; + + if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&& + collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + concavePairs[pairIdx].w = -1; + return; + } + + hasSeparatingNormals[i] = 0; + +// int numFacesA = convexShapes[shapeIndexA].m_numFaces; + int numActualConcaveConvexTests = 0; + + int f = concavePairs[i].z; + + bool overlap = false; + + b3ConvexPolyhedronData convexPolyhedronA; + + //add 3 vertices of the triangle + convexPolyhedronA.m_numVertices = 3; + convexPolyhedronA.m_vertexOffset = 0; + b3Float4 localCenter = b3MakeFloat4(0.f,0.f,0.f,0.f); + + b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; + b3Aabb triAabb; + triAabb.m_minVec = b3MakeFloat4(1e30f,1e30f,1e30f,0.f); + triAabb.m_maxVec = b3MakeFloat4(-1e30f,-1e30f,-1e30f,0.f); + + b3Float4 verticesA[3]; + for (int i=0;i<3;i++) + { + int index = indices[face.m_indexOffset+i]; + b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; + verticesA[i] = vert; + localCenter += vert; + + triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec,vert); + triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec,vert); + + } + + overlap = true; + overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap; + overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap; + overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap; + + if (overlap) + { + float dmin = FLT_MAX; + int hasSeparatingAxis=5; + b3Float4 sepAxis=b3MakeFloat4(1,2,3,4); + + // int localCC=0; + numActualConcaveConvexTests++; + + //a triangle has 3 unique edges + convexPolyhedronA.m_numUniqueEdges = 3; + convexPolyhedronA.m_uniqueEdgesOffset = 0; + b3Float4 uniqueEdgesA[3]; + + uniqueEdgesA[0] = (verticesA[1]-verticesA[0]); + uniqueEdgesA[1] = (verticesA[2]-verticesA[1]); + uniqueEdgesA[2] = (verticesA[0]-verticesA[2]); + + + convexPolyhedronA.m_faceOffset = 0; + + b3Float4 normal = b3MakeFloat4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); + + b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES]; + int indicesA[3+3+2+2+2]; + int curUsedIndices=0; + int fidx=0; + + //front size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[0] = 0; + indicesA[1] = 1; + indicesA[2] = 2; + curUsedIndices+=3; + float c = face.m_plane.w; + facesA[fidx].m_plane.x = normal.x; + facesA[fidx].m_plane.y = normal.y; + facesA[fidx].m_plane.z = normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + //back size of triangle + { + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[3]=2; + indicesA[4]=1; + indicesA[5]=0; + curUsedIndices+=3; + float c = b3Dot(normal,verticesA[0]); + // float c1 = -face.m_plane.w; + facesA[fidx].m_plane.x = -normal.x; + facesA[fidx].m_plane.y = -normal.y; + facesA[fidx].m_plane.z = -normal.z; + facesA[fidx].m_plane.w = c; + facesA[fidx].m_numIndices=3; + } + fidx++; + + bool addEdgePlanes = true; + if (addEdgePlanes) + { + int numVertices=3; + int prevVertex = numVertices-1; + for (int i=0;i<numVertices;i++) + { + b3Float4 v0 = verticesA[i]; + b3Float4 v1 = verticesA[prevVertex]; + + b3Float4 edgeNormal = b3Normalized(b3Cross(normal,v1-v0)); + float c = -b3Dot(edgeNormal,v0); + + facesA[fidx].m_numIndices = 2; + facesA[fidx].m_indexOffset=curUsedIndices; + indicesA[curUsedIndices++]=i; + indicesA[curUsedIndices++]=prevVertex; + + facesA[fidx].m_plane.x = edgeNormal.x; + facesA[fidx].m_plane.y = edgeNormal.y; + facesA[fidx].m_plane.z = edgeNormal.z; + facesA[fidx].m_plane.w = c; + fidx++; + prevVertex = i; + } + } + convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES; + convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f); + + + b3Float4 posA = rigidBodies[bodyIndexA].m_pos; + posA.w = 0.f; + b3Float4 posB = rigidBodies[bodyIndexB].m_pos; + posB.w = 0.f; + + b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat; + b3Quaternion ornB =rigidBodies[bodyIndexB].m_quat; + + + + + /////////////////// + ///compound shape support + + if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) + { + int compoundChild = concavePairs[pairIdx].w; + int childShapeIndexB = compoundChild;//collidables[collidableIndexB].m_shapeIndex+compoundChild; + int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; + b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; + b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; + b3Float4 newPosB = b3TransformPoint(childPosB,posB,ornB); + b3Quaternion newOrnB = b3QuatMul(ornB,childOrnB); + posB = newPosB; + ornB = newOrnB; + shapeIndexB = collidables[childColIndexB].m_shapeIndex; + } + ////////////////// + + b3Float4 c0local = convexPolyhedronA.m_localCenter; + b3Float4 c0 = b3TransformPoint(c0local, posA, ornA); + b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter; + b3Float4 c1 = b3TransformPoint(c1local,posB,ornB); + const b3Float4 DeltaC2 = c0 - c1; + + + bool sepA = b3FindSeparatingAxis( &convexPolyhedronA, &convexShapes[shapeIndexB], + posA,ornA, + posB,ornB, + DeltaC2, + verticesA,uniqueEdgesA,facesA,indicesA, + vertices,uniqueEdges,faces,indices, + &sepAxis,&dmin); + hasSeparatingAxis = 4; + if (!sepA) + { + hasSeparatingAxis = 0; + } else + { + bool sepB = b3FindSeparatingAxis( &convexShapes[shapeIndexB],&convexPolyhedronA, + posB,ornB, + posA,ornA, + DeltaC2, + vertices,uniqueEdges,faces,indices, + verticesA,uniqueEdgesA,facesA,indicesA, + &sepAxis,&dmin); + + if (!sepB) + { + hasSeparatingAxis = 0; + } else + { + bool sepEE = b3FindSeparatingAxisEdgeEdge( &convexPolyhedronA, &convexShapes[shapeIndexB], + posA,ornA, + posB,ornB, + DeltaC2, + verticesA,uniqueEdgesA,facesA,indicesA, + vertices,uniqueEdges,faces,indices, + &sepAxis,&dmin,true); + + if (!sepEE) + { + hasSeparatingAxis = 0; + } else + { + hasSeparatingAxis = 1; + } + } + } + + if (hasSeparatingAxis) + { + hasSeparatingNormals[i]=1; + sepAxis.w = dmin; + concaveSeparatingNormalsOut[pairIdx]=sepAxis; + + //now compute clipping faces A and B, and world-space clipping vertices A and B... + + float minDist = -1e30f; + float maxDist = 0.02f; + + b3FindClippingFaces(sepAxis, + &convexPolyhedronA, + &convexShapes[shapeIndexB], + posA,ornA, + posB,ornB, + worldVertsA1Out, + worldNormalsA1Out, + worldVertsB1Out, + vertexFaceCapacity, + minDist, maxDist, + verticesA, + facesA, + indicesA, + + vertices, + faces, + indices, + clippingFacesOut, pairIdx); + + } else + { + //mark this pair as in-active + concavePairs[pairIdx].w = -1; + } + } + else + { + //mark this pair as in-active + concavePairs[pairIdx].w = -1; + } +} + + +#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H + diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h new file mode 100644 index 0000000000..332dbc278c --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h @@ -0,0 +1,206 @@ +#ifndef B3_FIND_SEPARATING_AXIS_H +#define B3_FIND_SEPARATING_AXIS_H + + +inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max) +{ + min = FLT_MAX; + max = -FLT_MAX; + int numVerts = hull.m_numVertices; + + const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir); + + b3Scalar offset = b3Dot3F4(pos,dir); + + for(int i=0;i<numVerts;i++) + { + //b3Vector3 pt = trans * vertices[m_vertexOffset+i]; + //b3Scalar dp = pt.dot(dir); + //b3Vector3 vertex = vertices[hull.m_vertexOffset+i]; + b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset+i],localDir); + //b3Assert(dp==dpL); + if(dp < min) min = dp; + if(dp > max) max = dp; + } + if(min>max) + { + b3Scalar tmp = min; + min = max; + max = tmp; + } + min += offset; + max += offset; +} + + +inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA,const b3Quaternion& ornA, + const b3Float4& posB,const b3Quaternion& ornB, + const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth) +{ + b3Scalar Min0,Max0; + b3Scalar Min1,Max1; + b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0); + b3ProjectAxis(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + b3Scalar d0 = Max0 - Min1; + b3Assert(d0>=0.0f); + b3Scalar d1 = Max1 - Min0; + b3Assert(d1>=0.0f); + depth = d0<d1 ? d0:d1; + return true; +} + + +inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA1, + const b3Quaternion& ornA, + const b3Float4& posB1, + const b3Quaternion& ornB, + const b3AlignedObjectArray<b3Vector3>& verticesA, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA, + const b3AlignedObjectArray<b3GpuFace>& facesA, + const b3AlignedObjectArray<int>& indicesA, + const b3AlignedObjectArray<b3Vector3>& verticesB, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB, + const b3AlignedObjectArray<b3GpuFace>& facesB, + const b3AlignedObjectArray<int>& indicesB, + + b3Vector3& sep) +{ + B3_PROFILE("findSeparatingAxis"); + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; +//#ifdef TEST_INTERNAL_OBJECTS + b3Float4 c0local = (b3Float4&)hullA.m_localCenter; + + b3Float4 c0 = b3TransformPoint(c0local, posA, ornA); + b3Float4 c1local = (b3Float4&)hullB.m_localCenter; + b3Float4 c1 = b3TransformPoint(c1local,posB,ornB); + const b3Float4 deltaC2 = c0 - c1; +//#endif + + b3Scalar dmin = FLT_MAX; + int curPlaneTests=0; + + int numFacesA = hullA.m_numFaces; + // Test normals from hullA + for(int i=0;i<numFacesA;i++) + { + const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset+i].m_plane; + b3Float4 faceANormalWS = b3QuatRotate(ornA,normal); + + if (b3Dot3F4(deltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + + b3Scalar d; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,faceANormalWS, verticesA, verticesB,d)) + return false; + + if(d<dmin) + { + dmin = d; + sep = (b3Vector3&)faceANormalWS; + } + } + + int numFacesB = hullB.m_numFaces; + // Test normals from hullB + for(int i=0;i<numFacesB;i++) + { + b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset+i].m_plane; + b3Float4 WorldNormal = b3QuatRotate(ornB, normal); + + if (b3Dot3F4(deltaC2,WorldNormal)<0) + { + WorldNormal*=-1.f; + } + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar d; + if(!b3TestSepAxis(hullA, hullB,posA,ornA,posB,ornB,WorldNormal,verticesA,verticesB,d)) + return false; + + if(d<dmin) + { + dmin = d; + sep = (b3Vector3&)WorldNormal; + } + } + +// b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend; + + int curEdgeEdge = 0; + // Test edges + for(int e0=0;e0<hullA.m_numUniqueEdges;e0++) + { + const b3Float4& edge0 = (b3Float4&) uniqueEdgesA[hullA.m_uniqueEdgesOffset+e0]; + b3Float4 edge0World = b3QuatRotate(ornA,(b3Float4&)edge0); + + for(int e1=0;e1<hullB.m_numUniqueEdges;e1++) + { + const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset+e1]; + b3Float4 edge1World = b3QuatRotate(ornB,(b3Float4&)edge1); + + + b3Float4 crossje = b3Cross3(edge0World,edge1World); + + curEdgeEdge++; + if(!b3IsAlmostZero((b3Vector3&)crossje)) + { + crossje = b3FastNormalized3(crossje); + if (b3Dot3F4(deltaC2,crossje)<0) + crossje*=-1.f; + + +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar dist; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,crossje, verticesA,verticesB,dist)) + return false; + + if(dist<dmin) + { + dmin = dist; + sep = (b3Vector3&)crossje; + } + } + } + + } + + + if((b3Dot3F4(-deltaC2,(b3Float4&)sep))>0.0f) + sep = -sep; + + return true; +} + +#endif //B3_FIND_SEPARATING_AXIS_H + diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h new file mode 100644 index 0000000000..6c3ad7c9dd --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h @@ -0,0 +1,920 @@ + +/*** + * --------------------------------- + * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz> + * + * This file was ported from mpr.c file, part of libccd. + * The Minkoski Portal Refinement implementation was ported + * to OpenCL by Erwin Coumans for the Bullet 3 Physics library. + * at http://github.com/erwincoumans/bullet3 + * + * Distributed under the OSI-approved BSD License (the "License"); + * see <http://www.opensource.org/licenses/bsd-license.php>. + * This software is distributed WITHOUT ANY WARRANTY; without even the + * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the License for more information. + */ + + + + +#ifndef B3_MPR_PENETRATION_H +#define B3_MPR_PENETRATION_H + +#include "Bullet3Common/shared/b3PlatformDefinitions.h" +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" + + + + +#ifdef __cplusplus +#define B3_MPR_SQRT sqrtf +#else +#define B3_MPR_SQRT sqrt +#endif +#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y)) +#define B3_MPR_FABS fabs + +#define B3_MPR_TOLERANCE 1E-6f +#define B3_MPR_MAX_ITERATIONS 1000 + +struct _b3MprSupport_t +{ + b3Float4 v; //!< Support point in minkowski sum + b3Float4 v1; //!< Support point in obj1 + b3Float4 v2; //!< Support point in obj2 +}; +typedef struct _b3MprSupport_t b3MprSupport_t; + +struct _b3MprSimplex_t +{ + b3MprSupport_t ps[4]; + int last; //!< index of last added point +}; +typedef struct _b3MprSimplex_t b3MprSimplex_t; + +inline b3MprSupport_t* b3MprSimplexPointW(b3MprSimplex_t *s, int idx) +{ + return &s->ps[idx]; +} + +inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size) +{ + s->last = size - 1; +} + + +inline int b3MprSimplexSize(const b3MprSimplex_t *s) +{ + return s->last + 1; +} + + +inline const b3MprSupport_t* b3MprSimplexPoint(const b3MprSimplex_t* s, int idx) +{ + // here is no check on boundaries + return &s->ps[idx]; +} + +inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s) +{ + *d = *s; +} + +inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a) +{ + b3MprSupportCopy(s->ps + pos, a); +} + + +inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2) +{ + b3MprSupport_t supp; + + b3MprSupportCopy(&supp, &s->ps[pos1]); + b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]); + b3MprSupportCopy(&s->ps[pos2], &supp); +} + + +inline int b3MprIsZero(float val) +{ + return B3_MPR_FABS(val) < FLT_EPSILON; +} + + + +inline int b3MprEq(float _a, float _b) +{ + float ab; + float a, b; + + ab = B3_MPR_FABS(_a - _b); + if (B3_MPR_FABS(ab) < FLT_EPSILON) + return 1; + + a = B3_MPR_FABS(_a); + b = B3_MPR_FABS(_b); + if (b > a){ + return ab < FLT_EPSILON * b; + }else{ + return ab < FLT_EPSILON * a; + } +} + + +inline int b3MprVec3Eq(const b3Float4* a, const b3Float4 *b) +{ + return b3MprEq((*a).x, (*b).x) + && b3MprEq((*a).y, (*b).y) + && b3MprEq((*a).z, (*b).z); +} + + + +inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec,__global const b3ConvexPolyhedronData_t* hull, b3ConstArray(b3Float4) verticesA) +{ + b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = supportVec; + int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + return verticesA[hull->m_vertexOffset+index]; + } + + return supVec; + +} + + +B3_STATIC void b3MprConvexSupport(int pairIndex,int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3Float4* outp, int logme) +{ + //dir is in worldspace, move to local space + + b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos; + b3Quat orn = cpuBodyBuf[bodyIndex].m_quat; + + b3Float4 dir = b3MakeFloat4((*_dir).x,(*_dir).y,(*_dir).z,0.f); + + const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),dir); + + + //find local support vertex + int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx; + + b3Assert(cpuCollidables[colIndex].m_shapeType==SHAPE_CONVEX_HULL); + __global const b3ConvexPolyhedronData_t* hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex]; + + b3Float4 pInA; + if (logme) + { + + + // b3Float4 supVec = b3MakeFloat4(0,0,0,0); + float maxDot = -B3_LARGE_FLOAT; + + if( 0 < hull->m_numVertices ) + { + const b3Float4 scaled = localDir; + int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot); + pInA = cpuVertices[hull->m_vertexOffset+index]; + + } + + + } else + { + pInA = b3LocalGetSupportVertex(localDir,hull,cpuVertices); + } + + //move vertex to world space + *outp = b3TransformPoint(pInA,pos,orn); + +} + +inline void b3MprSupport(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + const b3Float4* _dir, b3MprSupport_t *supp) +{ + b3Float4 dir; + dir = *_dir; + b3MprConvexSupport(pairIndex,bodyIndexA,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v1,0); + dir = *_dir*-1.f; + b3MprConvexSupport(pairIndex,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,&dir, &supp->v2,0); + supp->v = supp->v1 - supp->v2; +} + + + + + + + + + +inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center) +{ + + center->v1 = cpuBodyBuf[bodyIndexA].m_pos; + center->v2 = cpuBodyBuf[bodyIndexB].m_pos; + center->v = center->v1 - center->v2; +} + +inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z) +{ + (*v).x = x; + (*v).y = y; + (*v).z = z; + (*v).w = 0.f; +} + +inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w) +{ + (*v).x += (*w).x; + (*v).y += (*w).y; + (*v).z += (*w).z; +} + +inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w) +{ + *v = *w; +} + +inline void b3MprVec3Scale(b3Float4 *d, float k) +{ + *d *= k; +} + +inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b) +{ + float dot; + + dot = b3Dot3F4(*a,*b); + return dot; +} + + +inline float b3MprVec3Len2(const b3Float4 *v) +{ + return b3MprVec3Dot(v, v); +} + +inline void b3MprVec3Normalize(b3Float4 *d) +{ + float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d)); + b3MprVec3Scale(d, k); +} + +inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b) +{ + *d = b3Cross3(*a,*b); + +} + + +inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w) +{ + *d = *v - *w; +} + +inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir) +{ + b3Float4 v2v1, v3v1; + + b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b3MprVec3Cross(dir, &v2v1, &v3v1); + b3MprVec3Normalize(dir); +} + + +inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline int portalReachTolerance(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dv1, dv2, dv3, dv4; + float dot1, dot2, dot3; + + // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4} + + dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir); + dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir); + dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir); + dv4 = b3MprVec3Dot(&v4->v, dir); + + dot1 = dv4 - dv1; + dot2 = dv4 - dv2; + dot3 = dv4 - dv3; + + dot1 = B3_MPR_FMIN(dot1, dot2); + dot1 = B3_MPR_FMIN(dot1, dot3); + + return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE; +} + +inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal, + const b3MprSupport_t *v4, + const b3Float4 *dir) +{ + float dot; + dot = b3MprVec3Dot(&v4->v, dir); + return b3MprIsZero(dot) || dot > 0.f; +} + +inline void b3ExpandPortal(b3MprSimplex_t *portal, + const b3MprSupport_t *v4) +{ + float dot; + b3Float4 v4v0; + + b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v); + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0); + if (dot > 0.f){ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 1, v4); + }else{ + b3MprSimplexSet(portal, 3, v4); + } + }else{ + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0); + if (dot > 0.f){ + b3MprSimplexSet(portal, 2, v4); + }else{ + b3MprSimplexSet(portal, 1, v4); + } + } +} + + + +B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir, va, vb; + float dot; + int cont; + + + + // vertex 0 is center of portal + b3FindOrigin(bodyIndexA,bodyIndexB,cpuBodyBuf, b3MprSimplexPointW(portal, 0)); + // vertex 0 is center of portal + b3MprSimplexSetSize(portal, 1); + + + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin)){ + // Portal's center lies on origin (0,0,0) => we know that objects + // intersect but we would need to know penetration info. + // So move center little bit... + b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f); + b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va); + } + + + // vertex 1 = support in direction of origin + b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Scale(&dir, -1.f); + b3MprVec3Normalize(&dir); + + + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 1)); + + b3MprSimplexSetSize(portal, 2); + + // test if origin isn't outside of v1 + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir); + + + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + + // vertex 2 + b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + if (b3MprIsZero(b3MprVec3Len2(&dir))){ + if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin)){ + // origin lies on v1 + return 1; + }else{ + // origin lies on v0-v1 segment + return 2; + } + } + + b3MprVec3Normalize(&dir); + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 2)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + b3MprSimplexSetSize(portal, 3); + + // vertex 3 direction + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + + // it is better to form portal faces to be oriented "outside" origin + dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v); + if (dot > 0.f){ + b3MprSimplexSwap(portal, 1, 2); + b3MprVec3Scale(&dir, -1.f); + } + + while (b3MprSimplexSize(portal) < 4){ + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, b3MprSimplexPointW(portal, 3)); + + dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir); + if (b3MprIsZero(dot) || dot < 0.f) + return -1; + + cont = 0; + + // test if origin is outside (v1, v0, v3) - set v2 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 3)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + + if (!cont){ + // test if origin is outside (v3, v0, v2) - set v1 as v3 and + // continue + b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v); + if (dot < 0.f && !b3MprIsZero(dot)){ + b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3)); + cont = 1; + } + } + + if (cont){ + b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 0)->v); + b3MprVec3Cross(&dir, &va, &vb); + b3MprVec3Normalize(&dir); + }else{ + b3MprSimplexSetSize(portal, 4); + } + } + + return 0; +} + + +B3_STATIC int b3RefinePortal(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal) +{ + b3Float4 dir; + b3MprSupport_t v4; + + for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++) + //while (1) + { + // compute direction outside the portal (from v0 throught v1,v2,v3 + // face) + b3PortalDir(portal, &dir); + + // test if origin is inside the portal + if (portalEncapsulesOrigin(portal, &dir)) + return 0; + + // get next support point + + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4); + + + // test if v4 can expand portal to contain origin and if portal + // expanding doesn't reach given tolerance + if (!portalCanEncapsuleOrigin(portal, &v4, &dir) + || portalReachTolerance(portal, &v4, &dir)) + { + return -1; + } + + // v1-v2-v3 triangle must be rearranged to face outside Minkowski + // difference (direction from v0). + b3ExpandPortal(portal, &v4); + } + + return -1; +} + +B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos) +{ + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + b3Float4 dir; + size_t i; + float b[4], sum, inv; + b3Float4 vec, p1, p2; + + b3PortalDir(portal, &dir); + + // use barycentric coordinates of tetrahedron to find origin + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v); + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v); + + sum = b[0] + b[1] + b[2] + b[3]; + + if (b3MprIsZero(sum) || sum < 0.f){ + b[0] = 0.f; + + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v, + &b3MprSimplexPoint(portal, 3)->v); + b[1] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v, + &b3MprSimplexPoint(portal, 1)->v); + b[2] = b3MprVec3Dot(&vec, &dir); + b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v, + &b3MprSimplexPoint(portal, 2)->v); + b[3] = b3MprVec3Dot(&vec, &dir); + + sum = b[1] + b[2] + b[3]; + } + + inv = 1.f / sum; + + b3MprVec3Copy(&p1, b3mpr_vec3_origin); + b3MprVec3Copy(&p2, b3mpr_vec3_origin); + for (i = 0; i < 4; i++){ + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p1, &vec); + + b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2); + b3MprVec3Scale(&vec, b[i]); + b3MprVec3Add(&p2, &vec); + } + b3MprVec3Scale(&p1, inv); + b3MprVec3Scale(&p2, inv); + + b3MprVec3Copy(pos, &p1); + b3MprVec3Add(pos, &p2); + b3MprVec3Scale(pos, 0.5); +} + +inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b) +{ + b3Float4 ab; + b3MprVec3Sub2(&ab, a, b); + return b3MprVec3Len2(&ab); +} + +inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P, + const b3Float4 *x0, + const b3Float4 *b, + b3Float4 *witness) +{ + // The computation comes from solving equation of segment: + // S(t) = x0 + t.d + // where - x0 is initial point of segment + // - d is direction of segment from x0 (|d| > 0) + // - t belongs to <0, 1> interval + // + // Than, distance from a segment to some point P can be expressed: + // D(t) = |x0 + t.d - P|^2 + // which is distance from any point on segment. Minimization + // of this function brings distance from P to segment. + // Minimization of D(t) leads to simple quadratic equation that's + // solving is straightforward. + // + // Bonus of this method is witness point for free. + + float dist, t; + b3Float4 d, a; + + // direction of segment + b3MprVec3Sub2(&d, b, x0); + + // precompute vector from P to x0 + b3MprVec3Sub2(&a, x0, P); + + t = -1.f * b3MprVec3Dot(&a, &d); + t /= b3MprVec3Len2(&d); + + if (t < 0.f || b3MprIsZero(t)){ + dist = b3MprVec3Dist2(x0, P); + if (witness) + b3MprVec3Copy(witness, x0); + }else if (t > 1.f || b3MprEq(t, 1.f)){ + dist = b3MprVec3Dist2(b, P); + if (witness) + b3MprVec3Copy(witness, b); + }else{ + if (witness){ + b3MprVec3Copy(witness, &d); + b3MprVec3Scale(witness, t); + b3MprVec3Add(witness, x0); + dist = b3MprVec3Dist2(witness, P); + }else{ + // recycling variables + b3MprVec3Scale(&d, t); + b3MprVec3Add(&d, &a); + dist = b3MprVec3Len2(&d); + } + } + + return dist; +} + + +inline float b3MprVec3PointTriDist2(const b3Float4 *P, + const b3Float4 *x0, const b3Float4 *B, + const b3Float4 *C, + b3Float4 *witness) +{ + // Computation comes from analytic expression for triangle (x0, B, C) + // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and + // Then equation for distance is: + // D(s, t) = | T(s, t) - P |^2 + // This leads to minimization of quadratic function of two variables. + // The solution from is taken only if s is between 0 and 1, t is + // between 0 and 1 and t + s < 1, otherwise distance from segment is + // computed. + + b3Float4 d1, d2, a; + float u, v, w, p, q, r; + float s, t, dist, dist2; + b3Float4 witness2; + + b3MprVec3Sub2(&d1, B, x0); + b3MprVec3Sub2(&d2, C, x0); + b3MprVec3Sub2(&a, x0, P); + + u = b3MprVec3Dot(&a, &a); + v = b3MprVec3Dot(&d1, &d1); + w = b3MprVec3Dot(&d2, &d2); + p = b3MprVec3Dot(&a, &d1); + q = b3MprVec3Dot(&a, &d2); + r = b3MprVec3Dot(&d1, &d2); + + s = (q * r - w * p) / (w * v - r * r); + t = (-s * r - q) / w; + + if ((b3MprIsZero(s) || s > 0.f) + && (b3MprEq(s, 1.f) || s < 1.f) + && (b3MprIsZero(t) || t > 0.f) + && (b3MprEq(t, 1.f) || t < 1.f) + && (b3MprEq(t + s, 1.f) || t + s < 1.f)){ + + if (witness){ + b3MprVec3Scale(&d1, s); + b3MprVec3Scale(&d2, t); + b3MprVec3Copy(witness, x0); + b3MprVec3Add(witness, &d1); + b3MprVec3Add(witness, &d2); + + dist = b3MprVec3Dist2(witness, P); + }else{ + dist = s * s * v; + dist += t * t * w; + dist += 2.f * s * t * r; + dist += 2.f * s * p; + dist += 2.f * t * q; + dist += u; + } + }else{ + dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness); + + dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + + dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2); + if (dist2 < dist){ + dist = dist2; + if (witness) + b3MprVec3Copy(witness, &witness2); + } + } + + return dist; +} + + +B3_STATIC void b3FindPenetr(int pairIndex,int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + b3MprSimplex_t *portal, + float *depth, b3Float4 *pdir, b3Float4 *pos) +{ + b3Float4 dir; + b3MprSupport_t v4; + unsigned long iterations; + + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + iterations = 1UL; + for (int i=0;i<B3_MPR_MAX_ITERATIONS;i++) + //while (1) + { + // compute portal direction and obtain next support point + b3PortalDir(portal, &dir); + + b3MprSupport(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&dir, &v4); + + + // reached tolerance -> find penetration info + if (portalReachTolerance(portal, &v4, &dir) + || iterations ==B3_MPR_MAX_ITERATIONS) + { + *depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin,&b3MprSimplexPoint(portal, 1)->v,&b3MprSimplexPoint(portal, 2)->v,&b3MprSimplexPoint(portal, 3)->v,pdir); + *depth = B3_MPR_SQRT(*depth); + + if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z)) + { + + *pdir = dir; + } + b3MprVec3Normalize(pdir); + + // barycentric coordinates: + b3FindPos(portal, pos); + + + return; + } + + b3ExpandPortal(portal, &v4); + + iterations++; + } +} + +B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal,float *depth, b3Float4 *dir, b3Float4 *pos) +{ + // Touching contact on portal's v1 - so depth is zero and direction + // is unimportant and pos can be guessed + *depth = 0.f; + b3Float4 zero = b3MakeFloat4(0,0,0,0); + b3Float4* b3mpr_vec3_origin = &zero; + + + b3MprVec3Copy(dir, b3mpr_vec3_origin); + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5); +} + +B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal, + float *depth, b3Float4 *dir, b3Float4 *pos) +{ + + // Origin lies on v0-v1 segment. + // Depth is distance to v1, direction also and position must be + // computed + + b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1); + b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2); + b3MprVec3Scale(pos, 0.5f); + + + b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v); + *depth = B3_MPR_SQRT(b3MprVec3Len2(dir)); + b3MprVec3Normalize(dir); +} + + + +inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB, + b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, + b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData, + b3ConstArray(b3Collidable_t) cpuCollidables, + b3ConstArray(b3Float4) cpuVertices, + __global b3Float4* sepAxis, + __global int* hasSepAxis, + float *depthOut, b3Float4* dirOut, b3Float4* posOut) +{ + + b3MprSimplex_t portal; + + +// if (!hasSepAxis[pairIndex]) + // return -1; + + hasSepAxis[pairIndex] = 0; + int res; + + // Phase 1: Portal discovery + res = b3DiscoverPortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices,sepAxis,hasSepAxis, &portal); + + + //sepAxis[pairIndex] = *pdir;//or -dir? + + switch (res) + { + case 0: + { + // Phase 2: Portal refinement + + res = b3RefinePortal(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal); + if (res < 0) + return -1; + + // Phase 3. Penetration info + b3FindPenetr(pairIndex,bodyIndexA,bodyIndexB,cpuBodyBuf,cpuConvexData,cpuCollidables,cpuVertices, sepAxis,&portal, depthOut, dirOut, posOut); + hasSepAxis[pairIndex] = 1; + sepAxis[pairIndex] = -*dirOut; + break; + } + case 1: + { + // Touching contact on portal's v1. + b3FindPenetrTouch(&portal, depthOut, dirOut, posOut); + break; + } + case 2: + { + + b3FindPenetrSegment( &portal, depthOut, dirOut, posOut); + break; + } + default: + { + hasSepAxis[pairIndex]=0; + //if (res < 0) + //{ + // Origin isn't inside portal - no collision. + return -1; + //} + } + }; + + return 0; +}; + + + +#endif //B3_MPR_PENETRATION_H diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h new file mode 100644 index 0000000000..718222ebca --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h @@ -0,0 +1,196 @@ + +#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 diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h new file mode 100644 index 0000000000..3661e43cf1 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h @@ -0,0 +1,90 @@ + + +#ifndef B3_QUANTIZED_BVH_NODE_H +#define B3_QUANTIZED_BVH_NODE_H + +#include "Bullet3Common/shared/b3Float4.h" + +#define B3_MAX_NUM_PARTS_IN_BITS 10 + +///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes. +///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). +typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t; + +struct b3QuantizedBvhNodeData +{ + //12 bytes + unsigned short int m_quantizedAabbMin[3]; + unsigned short int m_quantizedAabbMax[3]; + //4 bytes + int m_escapeIndexOrTriangleIndex; +}; + +inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode) +{ + unsigned int x=0; + unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS); + // Get only the lower bits where the triangle index is stored + return (rootNode->m_escapeIndexOrTriangleIndex&~(y)); +} + +inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode) +{ + //skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; +} + +inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode) +{ + return -rootNode->m_escapeIndexOrTriangleIndex; +} + +inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization) +{ + b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin); + clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax); + + b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization; + if (isMax) + { + out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1)); + out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1)); + out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1)); + } else + { + out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe)); + out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe)); + out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe)); + } + +} + + +inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow( + const unsigned short int* aabbMin1, + const unsigned short int* aabbMax1, + const unsigned short int* aabbMin2, + const unsigned short int* aabbMax2) +{ + //int overlap = 1; + if (aabbMin1[0] > aabbMax2[0]) + return 0; + if (aabbMax1[0] < aabbMin2[0]) + return 0; + if (aabbMin1[1] > aabbMax2[1]) + return 0; + if (aabbMax1[1] < aabbMin2[1]) + return 0; + if (aabbMin1[2] > aabbMax2[2]) + return 0; + if (aabbMax1[2] < aabbMin2[2]) + return 0; + return 1; + //overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap; + //overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap; + //overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap; + //return overlap; +} + + +#endif //B3_QUANTIZED_BVH_NODE_H diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h new file mode 100644 index 0000000000..35b5197006 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h @@ -0,0 +1,97 @@ +#ifndef B3_REDUCE_CONTACTS_H +#define B3_REDUCE_CONTACTS_H + +inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& 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 = b3Cross3( nearNormal, aVector ); + b3Float4 v = b3Cross3( nearNormal, u ); + u = b3FastNormalized3( u ); + v = b3FastNormalized3( 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 = b3Dot3F4( u, r ); + if (f<maxDots.x) + { + maxDots.x = f; + contactIdx[0].x = ie; + } + + f = b3Dot3F4( -u, r ); + if (f<maxDots.y) + { + maxDots.y = f; + contactIdx[0].y = ie; + } + + + f = b3Dot3F4( v, r ); + if (f<maxDots.z) + { + maxDots.z = f; + contactIdx[0].z = ie; + } + + f = b3Dot3F4( -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; + +} + +#endif //B3_REDUCE_CONTACTS_H diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h new file mode 100644 index 0000000000..50632c871f --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h @@ -0,0 +1,34 @@ +#ifndef B3_RIGIDBODY_DATA_H +#define B3_RIGIDBODY_DATA_H + +#include "Bullet3Common/shared/b3Float4.h" +#include "Bullet3Common/shared/b3Quat.h" +#include "Bullet3Common/shared/b3Mat3x3.h" + +typedef struct b3RigidBodyData b3RigidBodyData_t; + + +struct b3RigidBodyData +{ + b3Float4 m_pos; + b3Quat m_quat; + b3Float4 m_linVel; + b3Float4 m_angVel; + + int m_collidableIdx; + float m_invMass; + float m_restituitionCoeff; + float m_frictionCoeff; +}; + +typedef struct b3InertiaData b3InertiaData_t; + +struct b3InertiaData +{ + b3Mat3x3 m_invInertiaWorld; + b3Mat3x3 m_initInvInertia; +}; + + +#endif //B3_RIGIDBODY_DATA_H +
\ No newline at end of file diff --git a/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h new file mode 100644 index 0000000000..8d40d19a03 --- /dev/null +++ b/thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h @@ -0,0 +1,40 @@ +#ifndef B3_UPDATE_AABBS_H +#define B3_UPDATE_AABBS_H + + + +#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h" +#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h" + + + +void b3ComputeWorldAabb( int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs) +{ + __global const b3RigidBodyData_t* body = &bodies[bodyId]; + + b3Float4 position = body->m_pos; + b3Quat orientation = body->m_quat; + + int collidableIndex = body->m_collidableIdx; + int shapeIndex = collidables[collidableIndex].m_shapeIndex; + + if (shapeIndex>=0) + { + + b3Aabb_t localAabb = localShapeAABB[collidableIndex]; + b3Aabb_t worldAabb; + + b3Float4 aabbAMinOut,aabbAMaxOut; + float margin = 0.f; + b3TransformAabb2(localAabb.m_minVec,localAabb.m_maxVec,margin,position,orientation,&aabbAMinOut,&aabbAMaxOut); + + worldAabb.m_minVec =aabbAMinOut; + worldAabb.m_minIndices[3] = bodyId; + worldAabb.m_maxVec = aabbAMaxOut; + worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass==0.f? 0 : 1; + worldAabbs[bodyId] = worldAabb; + } +} + +#endif //B3_UPDATE_AABBS_H |