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

   1 #ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
   2 #define B3_FIND_CONCAVE_SEPARATING_AXIS_H
   3
   4 #define B3_TRIANGLE_NUM_CONVEX_FACES 5
   5
   6 #include "Bullet3Common/shared/b3Int4.h"
   7 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
   8 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
   9 #include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
  10 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
  11 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
  12 #include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
  13
  14 inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
  15                                           const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
  16 {
  17         min[0] = FLT_MAX;
  18         max[0] = -FLT_MAX;
  19         int numVerts = hull->m_numVertices;
  20
  21         const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), *dir);
  22         float offset = b3Dot(pos, *dir);
  23         for (int i = 0; i < numVerts; i++)
  24         {
  25                 float dp = b3Dot(vertices[hull->m_vertexOffset + i], localDir);
  26                 if (dp < min[0])
  27                         min[0] = dp;
  28                 if (dp > max[0])
  29                         max[0] = dp;
  30         }
  31         if (min[0] > max[0])
  32         {
  33                 float tmp = min[0];
  34                 min[0] = max[0];
  35                 max[0] = tmp;
  36         }
  37         min[0] += offset;
  38         max[0] += offset;
  39 }
  40
  41 inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
  42                                                   b3Float4ConstArg posA, b3QuatConstArg ornA,
  43                                                   b3Float4ConstArg posB, b3QuatConstArg ornB,
  44                                                   b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB, float* depth)
  45 {
  46         float Min0, Max0;
  47         float Min1, Max1;
  48         b3Project(hullA, posA, ornA, sep_axis, verticesA, &Min0, &Max0);
  49         b3Project(hullB, posB, ornB, sep_axis, verticesB, &Min1, &Max1);
  50
  51         if (Max0 < Min1 || Max1 < Min0)
  52                 return false;
  53
  54         float d0 = Max0 - Min1;
  55         float d1 = Max1 - Min0;
  56         *depth = d0 < d1 ? d0 : d1;
  57         return true;
  58 }
  59
  60 bool b3FindSeparatingAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
  61                                                   b3Float4ConstArg posA1,
  62                                                   b3QuatConstArg ornA,
  63                                                   b3Float4ConstArg posB1,
  64                                                   b3QuatConstArg ornB,
  65                                                   b3Float4ConstArg DeltaC2,
  66
  67                                                   const b3Float4* verticesA,
  68                                                   const b3Float4* uniqueEdgesA,
  69                                                   const b3GpuFace* facesA,
  70                                                   const int* indicesA,
  71
  72                                                   __global const b3Float4* verticesB,
  73                                                   __global const b3Float4* uniqueEdgesB,
  74                                                   __global const b3GpuFace* facesB,
  75                                                   __global const int* indicesB,
  76                                                   b3Float4* sep,
  77                                                   float* dmin)
  78 {
  79         b3Float4 posA = posA1;
  80         posA.w = 0.f;
  81         b3Float4 posB = posB1;
  82         posB.w = 0.f;
  83         /*
  84         static int maxFaceVertex = 0;
  85
  86         int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices;
  87         curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices;
  88
  89         if (curFaceVertexAB>maxFaceVertex)
  90         {
  91                 maxFaceVertex = curFaceVertexAB;
  92                 printf("curFaceVertexAB = %d\n",curFaceVertexAB);
  93                 printf("hullA->m_numFaces = %d\n",hullA->m_numFaces);
  94                 printf("hullA->m_numVertices = %d\n",hullA->m_numVertices);
  95                 printf("hullB->m_numVertices = %d\n",hullB->m_numVertices);
  96         }
  97 */
  98
  99         int curPlaneTests = 0;
 100         {
 101                 int numFacesA = hullA->m_numFaces;
 102                 // Test normals from hullA
 103                 for (int i = 0; i < numFacesA; i++)
 104                 {
 105                         const b3Float4 normal = facesA[hullA->m_faceOffset + i].m_plane;
 106                         b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
 107                         if (b3Dot(DeltaC2, faceANormalWS) < 0)
 108                                 faceANormalWS *= -1.f;
 109                         curPlaneTests++;
 110                         float d;
 111                         if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, &faceANormalWS, verticesA, verticesB, &d))
 112                                 return false;
 113                         if (d < *dmin)
 114                         {
 115                                 *dmin = d;
 116                                 *sep = faceANormalWS;
 117                         }
 118                 }
 119         }
 120         if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
 121         {
 122                 *sep = -(*sep);
 123         }
 124         return true;
 125 }
 126
 127 b3Vector3 unitSphere162[] =
 128         {
 129                 b3MakeVector3(0.000000, -1.000000, 0.000000),
 130                 b3MakeVector3(0.203181, -0.967950, 0.147618),
 131                 b3MakeVector3(-0.077607, -0.967950, 0.238853),
 132                 b3MakeVector3(0.723607, -0.447220, 0.525725),
 133                 b3MakeVector3(0.609547, -0.657519, 0.442856),
 134                 b3MakeVector3(0.812729, -0.502301, 0.295238),
 135                 b3MakeVector3(-0.251147, -0.967949, 0.000000),
 136                 b3MakeVector3(-0.077607, -0.967950, -0.238853),
 137                 b3MakeVector3(0.203181, -0.967950, -0.147618),
 138                 b3MakeVector3(0.860698, -0.251151, 0.442858),
 139                 b3MakeVector3(-0.276388, -0.447220, 0.850649),
 140                 b3MakeVector3(-0.029639, -0.502302, 0.864184),
 141                 b3MakeVector3(-0.155215, -0.251152, 0.955422),
 142                 b3MakeVector3(-0.894426, -0.447216, 0.000000),
 143                 b3MakeVector3(-0.831051, -0.502299, 0.238853),
 144                 b3MakeVector3(-0.956626, -0.251149, 0.147618),
 145                 b3MakeVector3(-0.276388, -0.447220, -0.850649),
 146                 b3MakeVector3(-0.483971, -0.502302, -0.716565),
 147                 b3MakeVector3(-0.436007, -0.251152, -0.864188),
 148                 b3MakeVector3(0.723607, -0.447220, -0.525725),
 149                 b3MakeVector3(0.531941, -0.502302, -0.681712),
 150                 b3MakeVector3(0.687159, -0.251152, -0.681715),
 151                 b3MakeVector3(0.687159, -0.251152, 0.681715),
 152                 b3MakeVector3(-0.436007, -0.251152, 0.864188),
 153                 b3MakeVector3(-0.956626, -0.251149, -0.147618),
 154                 b3MakeVector3(-0.155215, -0.251152, -0.955422),
 155                 b3MakeVector3(0.860698, -0.251151, -0.442858),
 156                 b3MakeVector3(0.276388, 0.447220, 0.850649),
 157                 b3MakeVector3(0.483971, 0.502302, 0.716565),
 158                 b3MakeVector3(0.232822, 0.657519, 0.716563),
 159                 b3MakeVector3(-0.723607, 0.447220, 0.525725),
 160                 b3MakeVector3(-0.531941, 0.502302, 0.681712),
 161                 b3MakeVector3(-0.609547, 0.657519, 0.442856),
 162                 b3MakeVector3(-0.723607, 0.447220, -0.525725),
 163                 b3MakeVector3(-0.812729, 0.502301, -0.295238),
 164                 b3MakeVector3(-0.609547, 0.657519, -0.442856),
 165                 b3MakeVector3(0.276388, 0.447220, -0.850649),
 166                 b3MakeVector3(0.029639, 0.502302, -0.864184),
 167                 b3MakeVector3(0.232822, 0.657519, -0.716563),
 168                 b3MakeVector3(0.894426, 0.447216, 0.000000),
 169                 b3MakeVector3(0.831051, 0.502299, -0.238853),
 170                 b3MakeVector3(0.753442, 0.657515, 0.000000),
 171                 b3MakeVector3(-0.232822, -0.657519, 0.716563),
 172                 b3MakeVector3(-0.162456, -0.850654, 0.499995),
 173                 b3MakeVector3(0.052790, -0.723612, 0.688185),
 174                 b3MakeVector3(0.138199, -0.894429, 0.425321),
 175                 b3MakeVector3(0.262869, -0.525738, 0.809012),
 176                 b3MakeVector3(0.361805, -0.723611, 0.587779),
 177                 b3MakeVector3(0.531941, -0.502302, 0.681712),
 178                 b3MakeVector3(0.425323, -0.850654, 0.309011),
 179                 b3MakeVector3(0.812729, -0.502301, -0.295238),
 180                 b3MakeVector3(0.609547, -0.657519, -0.442856),
 181                 b3MakeVector3(0.850648, -0.525736, 0.000000),
 182                 b3MakeVector3(0.670817, -0.723611, -0.162457),
 183                 b3MakeVector3(0.670817, -0.723610, 0.162458),
 184                 b3MakeVector3(0.425323, -0.850654, -0.309011),
 185                 b3MakeVector3(0.447211, -0.894428, 0.000001),
 186                 b3MakeVector3(-0.753442, -0.657515, 0.000000),
 187                 b3MakeVector3(-0.525730, -0.850652, 0.000000),
 188                 b3MakeVector3(-0.638195, -0.723609, 0.262864),
 189                 b3MakeVector3(-0.361801, -0.894428, 0.262864),
 190                 b3MakeVector3(-0.688189, -0.525736, 0.499997),
 191                 b3MakeVector3(-0.447211, -0.723610, 0.525729),
 192                 b3MakeVector3(-0.483971, -0.502302, 0.716565),
 193                 b3MakeVector3(-0.232822, -0.657519, -0.716563),
 194                 b3MakeVector3(-0.162456, -0.850654, -0.499995),
 195                 b3MakeVector3(-0.447211, -0.723611, -0.525727),
 196                 b3MakeVector3(-0.361801, -0.894429, -0.262863),
 197                 b3MakeVector3(-0.688189, -0.525736, -0.499997),
 198                 b3MakeVector3(-0.638195, -0.723609, -0.262863),
 199                 b3MakeVector3(-0.831051, -0.502299, -0.238853),
 200                 b3MakeVector3(0.361804, -0.723612, -0.587779),
 201                 b3MakeVector3(0.138197, -0.894429, -0.425321),
 202                 b3MakeVector3(0.262869, -0.525738, -0.809012),
 203                 b3MakeVector3(0.052789, -0.723611, -0.688186),
 204                 b3MakeVector3(-0.029639, -0.502302, -0.864184),
 205                 b3MakeVector3(0.956626, 0.251149, 0.147618),
 206                 b3MakeVector3(0.956626, 0.251149, -0.147618),
 207                 b3MakeVector3(0.951058, -0.000000, 0.309013),
 208                 b3MakeVector3(1.000000, 0.000000, 0.000000),
 209                 b3MakeVector3(0.947213, -0.276396, 0.162458),
 210                 b3MakeVector3(0.951058, 0.000000, -0.309013),
 211                 b3MakeVector3(0.947213, -0.276396, -0.162458),
 212                 b3MakeVector3(0.155215, 0.251152, 0.955422),
 213                 b3MakeVector3(0.436007, 0.251152, 0.864188),
 214                 b3MakeVector3(-0.000000, -0.000000, 1.000000),
 215                 b3MakeVector3(0.309017, 0.000000, 0.951056),
 216                 b3MakeVector3(0.138199, -0.276398, 0.951055),
 217                 b3MakeVector3(0.587786, 0.000000, 0.809017),
 218                 b3MakeVector3(0.447216, -0.276398, 0.850648),
 219                 b3MakeVector3(-0.860698, 0.251151, 0.442858),
 220                 b3MakeVector3(-0.687159, 0.251152, 0.681715),
 221                 b3MakeVector3(-0.951058, -0.000000, 0.309013),
 222                 b3MakeVector3(-0.809018, 0.000000, 0.587783),
 223                 b3MakeVector3(-0.861803, -0.276396, 0.425324),
 224                 b3MakeVector3(-0.587786, 0.000000, 0.809017),
 225                 b3MakeVector3(-0.670819, -0.276397, 0.688191),
 226                 b3MakeVector3(-0.687159, 0.251152, -0.681715),
 227                 b3MakeVector3(-0.860698, 0.251151, -0.442858),
 228                 b3MakeVector3(-0.587786, -0.000000, -0.809017),
 229                 b3MakeVector3(-0.809018, -0.000000, -0.587783),
 230                 b3MakeVector3(-0.670819, -0.276397, -0.688191),
 231                 b3MakeVector3(-0.951058, 0.000000, -0.309013),
 232                 b3MakeVector3(-0.861803, -0.276396, -0.425324),
 233                 b3MakeVector3(0.436007, 0.251152, -0.864188),
 234                 b3MakeVector3(0.155215, 0.251152, -0.955422),
 235                 b3MakeVector3(0.587786, -0.000000, -0.809017),
 236                 b3MakeVector3(0.309017, -0.000000, -0.951056),
 237                 b3MakeVector3(0.447216, -0.276398, -0.850648),
 238                 b3MakeVector3(0.000000, 0.000000, -1.000000),
 239                 b3MakeVector3(0.138199, -0.276398, -0.951055),
 240                 b3MakeVector3(0.670820, 0.276396, 0.688190),
 241                 b3MakeVector3(0.809019, -0.000002, 0.587783),
 242                 b3MakeVector3(0.688189, 0.525736, 0.499997),
 243                 b3MakeVector3(0.861804, 0.276394, 0.425323),
 244                 b3MakeVector3(0.831051, 0.502299, 0.238853),
 245                 b3MakeVector3(-0.447216, 0.276397, 0.850649),
 246                 b3MakeVector3(-0.309017, -0.000001, 0.951056),
 247                 b3MakeVector3(-0.262869, 0.525738, 0.809012),
 248                 b3MakeVector3(-0.138199, 0.276397, 0.951055),
 249                 b3MakeVector3(0.029639, 0.502302, 0.864184),
 250                 b3MakeVector3(-0.947213, 0.276396, -0.162458),
 251                 b3MakeVector3(-1.000000, 0.000001, 0.000000),
 252                 b3MakeVector3(-0.850648, 0.525736, -0.000000),
 253                 b3MakeVector3(-0.947213, 0.276397, 0.162458),
 254                 b3MakeVector3(-0.812729, 0.502301, 0.295238),
 255                 b3MakeVector3(-0.138199, 0.276397, -0.951055),
 256                 b3MakeVector3(-0.309016, -0.000000, -0.951057),
 257                 b3MakeVector3(-0.262869, 0.525738, -0.809012),
 258                 b3MakeVector3(-0.447215, 0.276397, -0.850649),
 259                 b3MakeVector3(-0.531941, 0.502302, -0.681712),
 260                 b3MakeVector3(0.861804, 0.276396, -0.425322),
 261                 b3MakeVector3(0.809019, 0.000000, -0.587782),
 262                 b3MakeVector3(0.688189, 0.525736, -0.499997),
 263                 b3MakeVector3(0.670821, 0.276397, -0.688189),
 264                 b3MakeVector3(0.483971, 0.502302, -0.716565),
 265                 b3MakeVector3(0.077607, 0.967950, 0.238853),
 266                 b3MakeVector3(0.251147, 0.967949, 0.000000),
 267                 b3MakeVector3(0.000000, 1.000000, 0.000000),
 268                 b3MakeVector3(0.162456, 0.850654, 0.499995),
 269                 b3MakeVector3(0.361800, 0.894429, 0.262863),
 270                 b3MakeVector3(0.447209, 0.723612, 0.525728),
 271                 b3MakeVector3(0.525730, 0.850652, 0.000000),
 272                 b3MakeVector3(0.638194, 0.723610, 0.262864),
 273                 b3MakeVector3(-0.203181, 0.967950, 0.147618),
 274                 b3MakeVector3(-0.425323, 0.850654, 0.309011),
 275                 b3MakeVector3(-0.138197, 0.894430, 0.425320),
 276                 b3MakeVector3(-0.361804, 0.723612, 0.587778),
 277                 b3MakeVector3(-0.052790, 0.723612, 0.688185),
 278                 b3MakeVector3(-0.203181, 0.967950, -0.147618),
 279                 b3MakeVector3(-0.425323, 0.850654, -0.309011),
 280                 b3MakeVector3(-0.447210, 0.894429, 0.000000),
 281                 b3MakeVector3(-0.670817, 0.723611, -0.162457),
 282                 b3MakeVector3(-0.670817, 0.723611, 0.162457),
 283                 b3MakeVector3(0.077607, 0.967950, -0.238853),
 284                 b3MakeVector3(0.162456, 0.850654, -0.499995),
 285                 b3MakeVector3(-0.138197, 0.894430, -0.425320),
 286                 b3MakeVector3(-0.052790, 0.723612, -0.688185),
 287                 b3MakeVector3(-0.361804, 0.723612, -0.587778),
 288                 b3MakeVector3(0.361800, 0.894429, -0.262863),
 289                 b3MakeVector3(0.638194, 0.723610, -0.262864),
 290                 b3MakeVector3(0.447209, 0.723612, -0.525728)};
 291
 292 bool b3FindSeparatingAxisEdgeEdge(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
 293                                                                   b3Float4ConstArg posA1,
 294                                                                   b3QuatConstArg ornA,
 295                                                                   b3Float4ConstArg posB1,
 296                                                                   b3QuatConstArg ornB,
 297                                                                   b3Float4ConstArg DeltaC2,
 298                                                                   const b3Float4* verticesA,
 299                                                                   const b3Float4* uniqueEdgesA,
 300                                                                   const b3GpuFace* facesA,
 301                                                                   const int* indicesA,
 302                                                                   __global const b3Float4* verticesB,
 303                                                                   __global const b3Float4* uniqueEdgesB,
 304                                                                   __global const b3GpuFace* facesB,
 305                                                                   __global const int* indicesB,
 306                                                                   b3Float4* sep,
 307                                                                   float* dmin,
 308                                                                   bool searchAllEdgeEdge)
 309 {
 310         b3Float4 posA = posA1;
 311         posA.w = 0.f;
 312         b3Float4 posB = posB1;
 313         posB.w = 0.f;
 314
 315         //      int curPlaneTests=0;
 316
 317         int curEdgeEdge = 0;
 318         // Test edges
 319         static int maxEdgeTests = 0;
 320         int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
 321         if (curEdgeTests > maxEdgeTests)
 322         {
 323                 maxEdgeTests = curEdgeTests;
 324                 printf("maxEdgeTests = %d\n", maxEdgeTests);
 325                 printf("hullA->m_numUniqueEdges = %d\n", hullA->m_numUniqueEdges);
 326                 printf("hullB->m_numUniqueEdges = %d\n", hullB->m_numUniqueEdges);
 327         }
 328
 329         if (searchAllEdgeEdge)
 330         {
 331                 for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
 332                 {
 333                         const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset + e0];
 334                         b3Float4 edge0World = b3QuatRotate(ornA, edge0);
 335
 336                         for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
 337                         {
 338                                 const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset + e1];
 339                                 b3Float4 edge1World = b3QuatRotate(ornB, edge1);
 340
 341                                 b3Float4 crossje = b3Cross(edge0World, edge1World);
 342
 343                                 curEdgeEdge++;
 344                                 if (!b3IsAlmostZero(crossje))
 345                                 {
 346                                         crossje = b3Normalized(crossje);
 347                                         if (b3Dot(DeltaC2, crossje) < 0)
 348                                                 crossje *= -1.f;
 349
 350                                         float dist;
 351                                         bool result = true;
 352                                         {
 353                                                 float Min0, Max0;
 354                                                 float Min1, Max1;
 355                                                 b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
 356                                                 b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
 357
 358                                                 if (Max0 < Min1 || Max1 < Min0)
 359                                                         return false;
 360
 361                                                 float d0 = Max0 - Min1;
 362                                                 float d1 = Max1 - Min0;
 363                                                 dist = d0 < d1 ? d0 : d1;
 364                                                 result = true;
 365                                         }
 366
 367                                         if (dist < *dmin)
 368                                         {
 369                                                 *dmin = dist;
 370                                                 *sep = crossje;
 371                                         }
 372                                 }
 373                         }
 374                 }
 375         }
 376         else
 377         {
 378                 int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
 379                 //printf("numDirections =%d\n",numDirections );
 380
 381                 for (int i = 0; i < numDirections; i++)
 382                 {
 383                         b3Float4 crossje = unitSphere162[i];
 384                         {
 385                                 //if (b3Dot(DeltaC2,crossje)>0)
 386                                 {
 387                                         float dist;
 388                                         bool result = true;
 389                                         {
 390                                                 float Min0, Max0;
 391                                                 float Min1, Max1;
 392                                                 b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
 393                                                 b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
 394
 395                                                 if (Max0 < Min1 || Max1 < Min0)
 396                                                         return false;
 397
 398                                                 float d0 = Max0 - Min1;
 399                                                 float d1 = Max1 - Min0;
 400                                                 dist = d0 < d1 ? d0 : d1;
 401                                                 result = true;
 402                                         }
 403
 404                                         if (dist < *dmin)
 405                                         {
 406                                                 *dmin = dist;
 407                                                 *sep = crossje;
 408                                         }
 409                                 }
 410                         }
 411                 }
 412         }
 413
 414         if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
 415         {
 416                 *sep = -(*sep);
 417         }
 418         return true;
 419 }
 420
 421 inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
 422                                                            __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
 423                                                            b3Float4ConstArg posA, b3QuatConstArg ornA, b3Float4ConstArg posB, b3QuatConstArg ornB,
 424                                                            __global b3Float4* worldVertsA1,
 425                                                            __global b3Float4* worldNormalsA1,
 426                                                            __global b3Float4* worldVertsB1,
 427                                                            int capacityWorldVerts,
 428                                                            const float minDist, float maxDist,
 429                                                            __global const b3Float4* verticesA,
 430                                                            __global const b3GpuFace_t* facesA,
 431                                                            __global const int* indicesA,
 432                                                            __global const b3Float4* verticesB,
 433                                                            __global const b3GpuFace_t* facesB,
 434                                                            __global const int* indicesB,
 435
 436                                                            __global b3Int4* clippingFaces, int pairIndex)
 437 {
 438         int numContactsOut = 0;
 439         int numWorldVertsB1 = 0;
 440
 441         int closestFaceB = -1;
 442         float dmax = -FLT_MAX;
 443
 444         {
 445                 for (int face = 0; face < hullB->m_numFaces; face++)
 446                 {
 447                         const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset + face].m_plane.x,
 448                                                                                                  facesB[hullB->m_faceOffset + face].m_plane.y, facesB[hullB->m_faceOffset + face].m_plane.z, 0.f);
 449                         const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
 450                         float d = b3Dot(WorldNormal, separatingNormal);
 451                         if (d > dmax)
 452                         {
 453                                 dmax = d;
 454                                 closestFaceB = face;
 455                         }
 456                 }
 457         }
 458
 459         {
 460                 const b3GpuFace_t polyB = facesB[hullB->m_faceOffset + closestFaceB];
 461                 const int numVertices = polyB.m_numIndices;
 462                 for (int e0 = 0; e0 < numVertices; e0++)
 463                 {
 464                         const b3Float4 b = verticesB[hullB->m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
 465                         worldVertsB1[pairIndex * capacityWorldVerts + numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
 466                 }
 467         }
 468
 469         int closestFaceA = -1;
 470         {
 471                 float dmin = FLT_MAX;
 472                 for (int face = 0; face < hullA->m_numFaces; face++)
 473                 {
 474                         const b3Float4 Normal = b3MakeFloat4(
 475                                 facesA[hullA->m_faceOffset + face].m_plane.x,
 476                                 facesA[hullA->m_faceOffset + face].m_plane.y,
 477                                 facesA[hullA->m_faceOffset + face].m_plane.z,
 478                                 0.f);
 479                         const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
 480
 481                         float d = b3Dot(faceANormalWS, separatingNormal);
 482                         if (d < dmin)
 483                         {
 484                                 dmin = d;
 485                                 closestFaceA = face;
 486                                 worldNormalsA1[pairIndex] = faceANormalWS;
 487                         }
 488                 }
 489         }
 490
 491         int numVerticesA = facesA[hullA->m_faceOffset + closestFaceA].m_numIndices;
 492         for (int e0 = 0; e0 < numVerticesA; e0++)
 493         {
 494                 const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[facesA[hullA->m_faceOffset + closestFaceA].m_indexOffset + e0]];
 495                 worldVertsA1[pairIndex * capacityWorldVerts + e0] = b3TransformPoint(a, posA, ornA);
 496         }
 497
 498         clippingFaces[pairIndex].x = closestFaceA;
 499         clippingFaces[pairIndex].y = closestFaceB;
 500         clippingFaces[pairIndex].z = numVerticesA;
 501         clippingFaces[pairIndex].w = numWorldVertsB1;
 502
 503         return numContactsOut;
 504 }
 505
 506 __kernel void b3FindConcaveSeparatingAxisKernel(__global b3Int4* concavePairs,
 507                                                                                                 __global const b3RigidBodyData* rigidBodies,
 508                                                                                                 __global const b3Collidable* collidables,
 509                                                                                                 __global const b3ConvexPolyhedronData* convexShapes,
 510                                                                                                 __global const b3Float4* vertices,
 511                                                                                                 __global const b3Float4* uniqueEdges,
 512                                                                                                 __global const b3GpuFace* faces,
 513                                                                                                 __global const int* indices,
 514                                                                                                 __global const b3GpuChildShape* gpuChildShapes,
 515                                                                                                 __global b3Aabb* aabbs,
 516                                                                                                 __global b3Float4* concaveSeparatingNormalsOut,
 517                                                                                                 __global b3Int4* clippingFacesOut,
 518                                                                                                 __global b3Vector3* worldVertsA1Out,
 519                                                                                                 __global b3Vector3* worldNormalsA1Out,
 520                                                                                                 __global b3Vector3* worldVertsB1Out,
 521                                                                                                 __global int* hasSeparatingNormals,
 522                                                                                                 int vertexFaceCapacity,
 523                                                                                                 int numConcavePairs,
 524                                                                                                 int pairIdx)
 525 {
 526         int i = pairIdx;
 527         /*      int i = get_global_id(0);
 528         if (i>=numConcavePairs)
 529                 return;
 530         int pairIdx = i;
 531         */
 532
 533         int bodyIndexA = concavePairs[i].x;
 534         int bodyIndexB = concavePairs[i].y;
 535
 536         int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
 537         int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
 538
 539         int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
 540         int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
 541
 542         if (collidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL &&
 543                 collidables[collidableIndexB].m_shapeType != SHAPE_COMPOUND_OF_CONVEX_HULLS)
 544         {
 545                 concavePairs[pairIdx].w = -1;
 546                 return;
 547         }
 548
 549         hasSeparatingNormals[i] = 0;
 550
 551         //      int numFacesA = convexShapes[shapeIndexA].m_numFaces;
 552         int numActualConcaveConvexTests = 0;
 553
 554         int f = concavePairs[i].z;
 555
 556         bool overlap = false;
 557
 558         b3ConvexPolyhedronData convexPolyhedronA;
 559
 560         //add 3 vertices of the triangle
 561         convexPolyhedronA.m_numVertices = 3;
 562         convexPolyhedronA.m_vertexOffset = 0;
 563         b3Float4 localCenter = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
 564
 565         b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset + f];
 566         b3Aabb triAabb;
 567         triAabb.m_minVec = b3MakeFloat4(1e30f, 1e30f, 1e30f, 0.f);
 568         triAabb.m_maxVec = b3MakeFloat4(-1e30f, -1e30f, -1e30f, 0.f);
 569
 570         b3Float4 verticesA[3];
 571         for (int i = 0; i < 3; i++)
 572         {
 573                 int index = indices[face.m_indexOffset + i];
 574                 b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset + index];
 575                 verticesA[i] = vert;
 576                 localCenter += vert;
 577
 578                 triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec, vert);
 579                 triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec, vert);
 580         }
 581
 582         overlap = true;
 583         overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
 584         overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
 585         overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
 586
 587         if (overlap)
 588         {
 589                 float dmin = FLT_MAX;
 590                 int hasSeparatingAxis = 5;
 591                 b3Float4 sepAxis = b3MakeFloat4(1, 2, 3, 4);
 592
 593                 //      int localCC=0;
 594                 numActualConcaveConvexTests++;
 595
 596                 //a triangle has 3 unique edges
 597                 convexPolyhedronA.m_numUniqueEdges = 3;
 598                 convexPolyhedronA.m_uniqueEdgesOffset = 0;
 599                 b3Float4 uniqueEdgesA[3];
 600
 601                 uniqueEdgesA[0] = (verticesA[1] - verticesA[0]);
 602                 uniqueEdgesA[1] = (verticesA[2] - verticesA[1]);
 603                 uniqueEdgesA[2] = (verticesA[0] - verticesA[2]);
 604
 605                 convexPolyhedronA.m_faceOffset = 0;
 606
 607                 b3Float4 normal = b3MakeFloat4(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
 608
 609                 b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
 610                 int indicesA[3 + 3 + 2 + 2 + 2];
 611                 int curUsedIndices = 0;
 612                 int fidx = 0;
 613
 614                 //front size of triangle
 615                 {
 616                         facesA[fidx].m_indexOffset = curUsedIndices;
 617                         indicesA[0] = 0;
 618                         indicesA[1] = 1;
 619                         indicesA[2] = 2;
 620                         curUsedIndices += 3;
 621                         float c = face.m_plane.w;
 622                         facesA[fidx].m_plane.x = normal.x;
 623                         facesA[fidx].m_plane.y = normal.y;
 624                         facesA[fidx].m_plane.z = normal.z;
 625                         facesA[fidx].m_plane.w = c;
 626                         facesA[fidx].m_numIndices = 3;
 627                 }
 628                 fidx++;
 629                 //back size of triangle
 630                 {
 631                         facesA[fidx].m_indexOffset = curUsedIndices;
 632                         indicesA[3] = 2;
 633                         indicesA[4] = 1;
 634                         indicesA[5] = 0;
 635                         curUsedIndices += 3;
 636                         float c = b3Dot(normal, verticesA[0]);
 637                         //      float c1 = -face.m_plane.w;
 638                         facesA[fidx].m_plane.x = -normal.x;
 639                         facesA[fidx].m_plane.y = -normal.y;
 640                         facesA[fidx].m_plane.z = -normal.z;
 641                         facesA[fidx].m_plane.w = c;
 642                         facesA[fidx].m_numIndices = 3;
 643                 }
 644                 fidx++;
 645
 646                 bool addEdgePlanes = true;
 647                 if (addEdgePlanes)
 648                 {
 649                         int numVertices = 3;
 650                         int prevVertex = numVertices - 1;
 651                         for (int i = 0; i < numVertices; i++)
 652                         {
 653                                 b3Float4 v0 = verticesA[i];
 654                                 b3Float4 v1 = verticesA[prevVertex];
 655
 656                                 b3Float4 edgeNormal = b3Normalized(b3Cross(normal, v1 - v0));
 657                                 float c = -b3Dot(edgeNormal, v0);
 658
 659                                 facesA[fidx].m_numIndices = 2;
 660                                 facesA[fidx].m_indexOffset = curUsedIndices;
 661                                 indicesA[curUsedIndices++] = i;
 662                                 indicesA[curUsedIndices++] = prevVertex;
 663
 664                                 facesA[fidx].m_plane.x = edgeNormal.x;
 665                                 facesA[fidx].m_plane.y = edgeNormal.y;
 666                                 facesA[fidx].m_plane.z = edgeNormal.z;
 667                                 facesA[fidx].m_plane.w = c;
 668                                 fidx++;
 669                                 prevVertex = i;
 670                         }
 671                 }
 672                 convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
 673                 convexPolyhedronA.m_localCenter = localCenter * (1.f / 3.f);
 674
 675                 b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
 676                 posA.w = 0.f;
 677                 b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
 678                 posB.w = 0.f;
 679
 680                 b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
 681                 b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
 682
 683                 ///////////////////
 684                 ///compound shape support
 685
 686                 if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
 687                 {
 688                         int compoundChild = concavePairs[pairIdx].w;
 689                         int childShapeIndexB = compoundChild;  //collidables[collidableIndexB].m_shapeIndex+compoundChild;
 690                         int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
 691                         b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
 692                         b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
 693                         b3Float4 newPosB = b3TransformPoint(childPosB, posB, ornB);
 694                         b3Quaternion newOrnB = b3QuatMul(ornB, childOrnB);
 695                         posB = newPosB;
 696                         ornB = newOrnB;
 697                         shapeIndexB = collidables[childColIndexB].m_shapeIndex;
 698                 }
 699                 //////////////////
 700
 701                 b3Float4 c0local = convexPolyhedronA.m_localCenter;
 702                 b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
 703                 b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
 704                 b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
 705                 const b3Float4 DeltaC2 = c0 - c1;
 706
 707                 bool sepA = b3FindSeparatingAxis(&convexPolyhedronA, &convexShapes[shapeIndexB],
 708                                                                                  posA, ornA,
 709                                                                                  posB, ornB,
 710                                                                                  DeltaC2,
 711                                                                                  verticesA, uniqueEdgesA, facesA, indicesA,
 712                                                                                  vertices, uniqueEdges, faces, indices,
 713                                                                                  &sepAxis, &dmin);
 714                 hasSeparatingAxis = 4;
 715                 if (!sepA)
 716                 {
 717                         hasSeparatingAxis = 0;
 718                 }
 719                 else
 720                 {
 721                         bool sepB = b3FindSeparatingAxis(&convexShapes[shapeIndexB], &convexPolyhedronA,
 722                                                                                          posB, ornB,
 723                                                                                          posA, ornA,
 724                                                                                          DeltaC2,
 725                                                                                          vertices, uniqueEdges, faces, indices,
 726                                                                                          verticesA, uniqueEdgesA, facesA, indicesA,
 727                                                                                          &sepAxis, &dmin);
 728
 729                         if (!sepB)
 730                         {
 731                                 hasSeparatingAxis = 0;
 732                         }
 733                         else
 734                         {
 735                                 bool sepEE = b3FindSeparatingAxisEdgeEdge(&convexPolyhedronA, &convexShapes[shapeIndexB],
 736                                                                                                                   posA, ornA,
 737                                                                                                                   posB, ornB,
 738                                                                                                                   DeltaC2,
 739                                                                                                                   verticesA, uniqueEdgesA, facesA, indicesA,
 740                                                                                                                   vertices, uniqueEdges, faces, indices,
 741                                                                                                                   &sepAxis, &dmin, true);
 742
 743                                 if (!sepEE)
 744                                 {
 745                                         hasSeparatingAxis = 0;
 746                                 }
 747                                 else
 748                                 {
 749                                         hasSeparatingAxis = 1;
 750                                 }
 751                         }
 752                 }
 753
 754                 if (hasSeparatingAxis)
 755                 {
 756                         hasSeparatingNormals[i] = 1;
 757                         sepAxis.w = dmin;
 758                         concaveSeparatingNormalsOut[pairIdx] = sepAxis;
 759
 760                         //now compute clipping faces A and B, and world-space clipping vertices A and B...
 761
 762                         float minDist = -1e30f;
 763                         float maxDist = 0.02f;
 764
 765                         b3FindClippingFaces(sepAxis,
 766                                                                 &convexPolyhedronA,
 767                                                                 &convexShapes[shapeIndexB],
 768                                                                 posA, ornA,
 769                                                                 posB, ornB,
 770                                                                 worldVertsA1Out,
 771                                                                 worldNormalsA1Out,
 772                                                                 worldVertsB1Out,
 773                                                                 vertexFaceCapacity,
 774                                                                 minDist, maxDist,
 775                                                                 verticesA,
 776                                                                 facesA,
 777                                                                 indicesA,
 778
 779                                                                 vertices,
 780                                                                 faces,
 781                                                                 indices,
 782                                                                 clippingFacesOut, pairIdx);
 783                 }
 784                 else
 785                 {
 786                         //mark this pair as in-active
 787                         concavePairs[pairIdx].w = -1;
 788                 }
 789         }
 790         else
 791         {
 792                 //mark this pair as in-active
 793                 concavePairs[pairIdx].w = -1;
 794         }
 795 }
 796
 797 #endif  //B3_FIND_CONCAVE_SEPARATING_AXIS_H