dali-physics/third-party/bullet3/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h

   1 #ifndef B3_CLIP_FACES_H
   2 #define B3_CLIP_FACES_H
   3
   4 #include "Bullet3Common/shared/b3Int4.h"
   5 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
   6 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
   7 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
   8 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
   9 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
  10 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
  11
  12 inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
  13 {
  14         return b3MakeFloat4(a.x + (b.x - a.x) * t,
  15                                                 a.y + (b.y - a.y) * t,
  16                                                 a.z + (b.z - a.z) * t,
  17                                                 0.f);
  18 }
  19
  20 // Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
  21 int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
  22 {
  23         int ve;
  24         float ds, de;
  25         int numVertsOut = 0;
  26         //double-check next test
  27         //      if (numVertsIn < 2)
  28         //              return 0;
  29
  30         b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
  31         b3Float4 endVertex = pVtxIn[0];
  32
  33         ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;
  34
  35         for (ve = 0; ve < numVertsIn; ve++)
  36         {
  37                 endVertex = pVtxIn[ve];
  38                 de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
  39                 if (ds < 0)
  40                 {
  41                         if (de < 0)
  42                         {
  43                                 // Start < 0, end < 0, so output endVertex
  44                                 ppVtxOut[numVertsOut++] = endVertex;
  45                         }
  46                         else
  47                         {
  48                                 // Start < 0, end >= 0, so output intersection
  49                                 ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
  50                         }
  51                 }
  52                 else
  53                 {
  54                         if (de < 0)
  55                         {
  56                                 // Start >= 0, end < 0 so output intersection and end
  57                                 ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
  58                                 ppVtxOut[numVertsOut++] = endVertex;
  59                         }
  60                 }
  61                 firstVertex = endVertex;
  62                 ds = de;
  63         }
  64         return numVertsOut;
  65 }
  66
  67 __kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
  68                                                                                          __global const int* hasSeparatingAxis,
  69                                                                                          __global b3Int4* clippingFacesOut,
  70                                                                                          __global b3Float4* worldVertsA1,
  71                                                                                          __global b3Float4* worldNormalsA1,
  72                                                                                          __global b3Float4* worldVertsB1,
  73                                                                                          __global b3Float4* worldVertsB2,
  74                                                                                          int vertexFaceCapacity,
  75                                                                                          int pairIndex)
  76 {
  77         //    int i = get_global_id(0);
  78         //int pairIndex = i;
  79         int i = pairIndex;
  80
  81         float minDist = -1e30f;
  82         float maxDist = 0.02f;
  83
  84         //      if (i<numPairs)
  85         {
  86                 if (hasSeparatingAxis[i])
  87                 {
  88                         //                      int bodyIndexA = pairs[i].x;
  89                         //              int bodyIndexB = pairs[i].y;
  90
  91                         int numLocalContactsOut = 0;
  92
  93                         int capacityWorldVertsB2 = vertexFaceCapacity;
  94
  95                         __global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
  96                         __global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
  97
  98                         {
  99                                 __global b3Int4* clippingFaces = clippingFacesOut;
 100
 101                                 int closestFaceA = clippingFaces[pairIndex].x;
 102                                 // int closestFaceB = clippingFaces[pairIndex].y;
 103                                 int numVertsInA = clippingFaces[pairIndex].z;
 104                                 int numVertsInB = clippingFaces[pairIndex].w;
 105
 106                                 int numVertsOut = 0;
 107
 108                                 if (closestFaceA >= 0)
 109                                 {
 110                                         // clip polygon to back of planes of all faces of hull A that are adjacent to witness face
 111
 112                                         for (int e0 = 0; e0 < numVertsInA; e0++)
 113                                         {
 114                                                 const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
 115                                                 const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
 116                                                 const b3Float4 WorldEdge0 = aw - bw;
 117                                                 b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
 118                                                 b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
 119                                                 b3Float4 worldA1 = aw;
 120                                                 float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
 121                                                 b3Float4 planeNormalWS = planeNormalWS1;
 122                                                 float planeEqWS = planeEqWS1;
 123                                                 numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
 124                                                 __global b3Float4* tmp = pVtxOut;
 125                                                 pVtxOut = pVtxIn;
 126                                                 pVtxIn = tmp;
 127                                                 numVertsInB = numVertsOut;
 128                                                 numVertsOut = 0;
 129                                         }
 130
 131                                         b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
 132                                         float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);
 133
 134                                         for (int i = 0; i < numVertsInB; i++)
 135                                         {
 136                                                 float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
 137                                                 if (depth <= minDist)
 138                                                 {
 139                                                         depth = minDist;
 140                                                 }
 141                                                 /*
 142                                                 static float maxDepth = 0.f;
 143                                                 if (depth < maxDepth)
 144                                                 {
 145                                                         maxDepth = depth;
 146                                                         if (maxDepth < -10)
 147                                                         {
 148                                                                 printf("error at framecount %d?\n",myframecount);
 149                                                         }
 150                                                         printf("maxDepth = %f\n", maxDepth);
 151
 152                                                 }
 153 */
 154                                                 if (depth <= maxDist)
 155                                                 {
 156                                                         b3Float4 pointInWorld = pVtxIn[i];
 157                                                         pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
 158                                                 }
 159                                         }
 160                                 }
 161                                 clippingFaces[pairIndex].w = numLocalContactsOut;
 162                         }
 163
 164                         for (int i = 0; i < numLocalContactsOut; i++)
 165                                 pVtxIn[i] = pVtxOut[i];
 166
 167                 }  //           if (hasSeparatingAxis[i])
 168         }      //       if (i<numPairs)
 169 }
 170
 171 #endif  //B3_CLIP_FACES_H