dali-physics/third-party/bullet3/src/Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.cpp

   1
   2 bool searchIncremental3dSapOnGpu = true;
   3 #include <limits.h>
   4 #include "b3GpuSapBroadphase.h"
   5 #include "Bullet3Common/b3Vector3.h"
   6 #include "Bullet3OpenCL/ParallelPrimitives/b3LauncherCL.h"
   7 #include "Bullet3OpenCL/ParallelPrimitives/b3PrefixScanFloat4CL.h"
   8
   9 #include "Bullet3OpenCL/Initialize/b3OpenCLUtils.h"
  10 #include "kernels/sapKernels.h"
  11
  12 #include "Bullet3Common/b3MinMax.h"
  13
  14 #define B3_BROADPHASE_SAP_PATH "src/Bullet3OpenCL/BroadphaseCollision/kernels/sap.cl"
  15
  16 /*
  17
  18
  19
  20
  21
  22
  23         b3OpenCLArray<int> m_pairCount;
  24
  25
  26         b3OpenCLArray<b3SapAabb>        m_allAabbsGPU;
  27         b3AlignedObjectArray<b3SapAabb> m_allAabbsCPU;
  28
  29         virtual b3OpenCLArray<b3SapAabb>&       getAllAabbsGPU()
  30         {
  31  return m_allAabbsGPU;
  32         }
  33         virtual b3AlignedObjectArray<b3SapAabb>&        getAllAabbsCPU()
  34         {
  35  return m_allAabbsCPU;
  36         }
  37
  38         b3OpenCLArray<b3Vector3>        m_sum;
  39         b3OpenCLArray<b3Vector3>        m_sum2;
  40         b3OpenCLArray<b3Vector3>        m_dst;
  41
  42         b3OpenCLArray<int>      m_smallAabbsMappingGPU;
  43         b3AlignedObjectArray<int> m_smallAabbsMappingCPU;
  44
  45         b3OpenCLArray<int>      m_largeAabbsMappingGPU;
  46         b3AlignedObjectArray<int> m_largeAabbsMappingCPU;
  47
  48
  49         b3OpenCLArray<b3Int4>           m_overlappingPairs;
  50
  51         //temporary gpu work memory
  52         b3OpenCLArray<b3SortData>       m_gpuSmallSortData;
  53         b3OpenCLArray<b3SapAabb>        m_gpuSmallSortedAabbs;
  54
  55         class b3PrefixScanFloat4CL*             m_prefixScanFloat4;
  56  */
  57
  58 b3GpuSapBroadphase::b3GpuSapBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q, b3GpuSapKernelType kernelType)
  59         : m_context(ctx),
  60           m_device(device),
  61           m_queue(q),
  62
  63           m_objectMinMaxIndexGPUaxis0(ctx, q),
  64           m_objectMinMaxIndexGPUaxis1(ctx, q),
  65           m_objectMinMaxIndexGPUaxis2(ctx, q),
  66           m_objectMinMaxIndexGPUaxis0prev(ctx, q),
  67           m_objectMinMaxIndexGPUaxis1prev(ctx, q),
  68           m_objectMinMaxIndexGPUaxis2prev(ctx, q),
  69           m_sortedAxisGPU0(ctx, q),
  70           m_sortedAxisGPU1(ctx, q),
  71           m_sortedAxisGPU2(ctx, q),
  72           m_sortedAxisGPU0prev(ctx, q),
  73           m_sortedAxisGPU1prev(ctx, q),
  74           m_sortedAxisGPU2prev(ctx, q),
  75           m_addedHostPairsGPU(ctx, q),
  76           m_removedHostPairsGPU(ctx, q),
  77           m_addedCountGPU(ctx, q),
  78           m_removedCountGPU(ctx, q),
  79           m_currentBuffer(-1),
  80           m_pairCount(ctx, q),
  81           m_allAabbsGPU(ctx, q),
  82           m_sum(ctx, q),
  83           m_sum2(ctx, q),
  84           m_dst(ctx, q),
  85           m_smallAabbsMappingGPU(ctx, q),
  86           m_largeAabbsMappingGPU(ctx, q),
  87           m_overlappingPairs(ctx, q),
  88           m_gpuSmallSortData(ctx, q),
  89           m_gpuSmallSortedAabbs(ctx, q)
  90 {
  91         const char* sapSrc = sapCL;
  92
  93         cl_int errNum = 0;
  94
  95         b3Assert(m_context);
  96         b3Assert(m_device);
  97         cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, sapSrc, &errNum, "", B3_BROADPHASE_SAP_PATH);
  98         b3Assert(errNum == CL_SUCCESS);
  99
 100         b3Assert(errNum == CL_SUCCESS);
 101 #ifndef __APPLE__
 102         m_prefixScanFloat4 = new b3PrefixScanFloat4CL(m_context, m_device, m_queue);
 103 #else
 104         m_prefixScanFloat4 = 0;
 105 #endif
 106         m_sapKernel = 0;
 107
 108         switch (kernelType)
 109         {
 110                 case B3_GPU_SAP_KERNEL_BRUTE_FORCE_CPU:
 111                 {
 112                         m_sapKernel = 0;
 113                         break;
 114                 }
 115                 case B3_GPU_SAP_KERNEL_BRUTE_FORCE_GPU:
 116                 {
 117                         m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelBruteForce", &errNum, sapProg);
 118                         break;
 119                 }
 120
 121                 case B3_GPU_SAP_KERNEL_ORIGINAL:
 122                 {
 123                         m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelOriginal", &errNum, sapProg);
 124                         break;
 125                 }
 126                 case B3_GPU_SAP_KERNEL_BARRIER:
 127                 {
 128                         m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelBarrier", &errNum, sapProg);
 129                         break;
 130                 }
 131                 case B3_GPU_SAP_KERNEL_LOCAL_SHARED_MEMORY:
 132                 {
 133                         m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelLocalSharedMemory", &errNum, sapProg);
 134                         break;
 135                 }
 136
 137                 default:
 138                 {
 139                         m_sapKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelLocalSharedMemory", &errNum, sapProg);
 140                         b3Error("Unknown 3D GPU SAP provided, fallback to computePairsKernelLocalSharedMemory");
 141                 }
 142         };
 143
 144         m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelTwoArrays", &errNum, sapProg);
 145         b3Assert(errNum == CL_SUCCESS);
 146
 147         m_prepareSumVarianceKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "prepareSumVarianceKernel", &errNum, sapProg);
 148         b3Assert(errNum == CL_SUCCESS);
 149
 150         m_flipFloatKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "flipFloatKernel", &errNum, sapProg);
 151
 152         m_copyAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "copyAabbsKernel", &errNum, sapProg);
 153
 154         m_scatterKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "scatterKernel", &errNum, sapProg);
 155
 156         m_sorter = new b3RadixSort32CL(m_context, m_device, m_queue);
 157 }
 158
 159 b3GpuSapBroadphase::~b3GpuSapBroadphase()
 160 {
 161         delete m_sorter;
 162         delete m_prefixScanFloat4;
 163
 164         clReleaseKernel(m_scatterKernel);
 165         clReleaseKernel(m_flipFloatKernel);
 166         clReleaseKernel(m_copyAabbsKernel);
 167         clReleaseKernel(m_sapKernel);
 168         clReleaseKernel(m_sap2Kernel);
 169         clReleaseKernel(m_prepareSumVarianceKernel);
 170 }
 171
 172 /// conservative test for overlap between two aabbs
 173 static bool TestAabbAgainstAabb2(const b3Vector3& aabbMin1, const b3Vector3& aabbMax1,
 174                                                                  const b3Vector3& aabbMin2, const b3Vector3& aabbMax2)
 175 {
 176         bool overlap = true;
 177         overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
 178         overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
 179         overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
 180         return overlap;
 181 }
 182
 183 //http://stereopsis.com/radix.html
 184 static unsigned int FloatFlip(float fl)
 185 {
 186         unsigned int f = *(unsigned int*)&fl;
 187         unsigned int mask = -(int)(f >> 31) | 0x80000000;
 188         return f ^ mask;
 189 };
 190
 191 void b3GpuSapBroadphase::init3dSap()
 192 {
 193         if (m_currentBuffer < 0)
 194         {
 195                 m_allAabbsGPU.copyToHost(m_allAabbsCPU);
 196
 197                 m_currentBuffer = 0;
 198                 for (int axis = 0; axis < 3; axis++)
 199                 {
 200                         for (int buf = 0; buf < 2; buf++)
 201                         {
 202                                 int totalNumAabbs = m_allAabbsCPU.size();
 203                                 int numEndPoints = 2 * totalNumAabbs;
 204                                 m_sortedAxisCPU[axis][buf].resize(numEndPoints);
 205
 206                                 if (buf == m_currentBuffer)
 207                                 {
 208                                         for (int i = 0; i < totalNumAabbs; i++)
 209                                         {
 210                                                 m_sortedAxisCPU[axis][buf][i * 2].m_key = FloatFlip(m_allAabbsCPU[i].m_min[axis]) - 1;
 211                                                 m_sortedAxisCPU[axis][buf][i * 2].m_value = i * 2;
 212                                                 m_sortedAxisCPU[axis][buf][i * 2 + 1].m_key = FloatFlip(m_allAabbsCPU[i].m_max[axis]) + 1;
 213                                                 m_sortedAxisCPU[axis][buf][i * 2 + 1].m_value = i * 2 + 1;
 214                                         }
 215                                 }
 216                         }
 217                 }
 218
 219                 for (int axis = 0; axis < 3; axis++)
 220                 {
 221                         m_sorter->executeHost(m_sortedAxisCPU[axis][m_currentBuffer]);
 222                 }
 223
 224                 for (int axis = 0; axis < 3; axis++)
 225                 {
 226                         //int totalNumAabbs = m_allAabbsCPU.size();
 227                         int numEndPoints = m_sortedAxisCPU[axis][m_currentBuffer].size();
 228                         m_objectMinMaxIndexCPU[axis][m_currentBuffer].resize(numEndPoints);
 229                         for (int i = 0; i < numEndPoints; i++)
 230                         {
 231                                 int destIndex = m_sortedAxisCPU[axis][m_currentBuffer][i].m_value;
 232                                 int newDest = destIndex / 2;
 233                                 if (destIndex & 1)
 234                                 {
 235                                         m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].y = i;
 236                                 }
 237                                 else
 238                                 {
 239                                         m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].x = i;
 240                                 }
 241                         }
 242                 }
 243         }
 244 }
 245
 246 static bool b3PairCmp(const b3Int4& p, const b3Int4& q)
 247 {
 248         return ((p.x < q.x) || ((p.x == q.x) && (p.y < q.y)));
 249 }
 250
 251 static bool operator==(const b3Int4& a, const b3Int4& b)
 252 {
 253         return a.x == b.x && a.y == b.y;
 254 };
 255
 256 static bool operator<(const b3Int4& a, const b3Int4& b)
 257 {
 258         return a.x < b.x || (a.x == b.x && a.y < b.y);
 259 };
 260
 261 static bool operator>(const b3Int4& a, const b3Int4& b)
 262 {
 263         return a.x > b.x || (a.x == b.x && a.y > b.y);
 264 };
 265
 266 b3AlignedObjectArray<b3Int4> addedHostPairs;
 267 b3AlignedObjectArray<b3Int4> removedHostPairs;
 268
 269 b3AlignedObjectArray<b3SapAabb> preAabbs;
 270
 271 void b3GpuSapBroadphase::calculateOverlappingPairsHostIncremental3Sap()
 272 {
 273         //static int framepje = 0;
 274         //printf("framepje=%d\n",framepje++);
 275
 276         B3_PROFILE("calculateOverlappingPairsHostIncremental3Sap");
 277
 278         addedHostPairs.resize(0);
 279         removedHostPairs.resize(0);
 280
 281         b3Assert(m_currentBuffer >= 0);
 282
 283         {
 284                 preAabbs.resize(m_allAabbsCPU.size());
 285                 for (int i = 0; i < preAabbs.size(); i++)
 286                 {
 287                         preAabbs[i] = m_allAabbsCPU[i];
 288                 }
 289         }
 290
 291         if (m_currentBuffer < 0)
 292                 return;
 293         {
 294                 B3_PROFILE("m_allAabbsGPU.copyToHost");
 295                 m_allAabbsGPU.copyToHost(m_allAabbsCPU);
 296         }
 297
 298         b3AlignedObjectArray<b3Int4> allPairs;
 299         {
 300                 B3_PROFILE("m_overlappingPairs.copyToHost");
 301                 m_overlappingPairs.copyToHost(allPairs);
 302         }
 303         if (0)
 304         {
 305                 {
 306                         printf("ab[40].min=%f,%f,%f,ab[40].max=%f,%f,%f\n",
 307                                    m_allAabbsCPU[40].m_min[0], m_allAabbsCPU[40].m_min[1], m_allAabbsCPU[40].m_min[2],
 308                                    m_allAabbsCPU[40].m_max[0], m_allAabbsCPU[40].m_max[1], m_allAabbsCPU[40].m_max[2]);
 309                 }
 310
 311                 {
 312                         printf("ab[53].min=%f,%f,%f,ab[53].max=%f,%f,%f\n",
 313                                    m_allAabbsCPU[53].m_min[0], m_allAabbsCPU[53].m_min[1], m_allAabbsCPU[53].m_min[2],
 314                                    m_allAabbsCPU[53].m_max[0], m_allAabbsCPU[53].m_max[1], m_allAabbsCPU[53].m_max[2]);
 315                 }
 316
 317                 {
 318                         b3Int4 newPair;
 319                         newPair.x = 40;
 320                         newPair.y = 53;
 321                         int index = allPairs.findBinarySearch(newPair);
 322                         printf("hasPair(40,53)=%d out of %d\n", index, allPairs.size());
 323
 324                         {
 325                                 int overlap = TestAabbAgainstAabb2((const b3Vector3&)m_allAabbsCPU[40].m_min, (const b3Vector3&)m_allAabbsCPU[40].m_max, (const b3Vector3&)m_allAabbsCPU[53].m_min, (const b3Vector3&)m_allAabbsCPU[53].m_max);
 326                                 printf("overlap=%d\n", overlap);
 327                         }
 328
 329                         if (preAabbs.size())
 330                         {
 331                                 int prevOverlap = TestAabbAgainstAabb2((const b3Vector3&)preAabbs[40].m_min, (const b3Vector3&)preAabbs[40].m_max, (const b3Vector3&)preAabbs[53].m_min, (const b3Vector3&)preAabbs[53].m_max);
 332                                 printf("prevoverlap=%d\n", prevOverlap);
 333                         }
 334                         else
 335                         {
 336                                 printf("unknown prevoverlap\n");
 337                         }
 338                 }
 339         }
 340
 341         if (0)
 342         {
 343                 for (int i = 0; i < m_allAabbsCPU.size(); i++)
 344                 {
 345                         //printf("aabb[%d] min=%f,%f,%f max=%f,%f,%f\n",i,m_allAabbsCPU[i].m_min[0],m_allAabbsCPU[i].m_min[1],m_allAabbsCPU[i].m_min[2],                        m_allAabbsCPU[i].m_max[0],m_allAabbsCPU[i].m_max[1],m_allAabbsCPU[i].m_max[2]);
 346                 }
 347
 348                 for (int axis = 0; axis < 3; axis++)
 349                 {
 350                         for (int buf = 0; buf < 2; buf++)
 351                         {
 352                                 b3Assert(m_sortedAxisCPU[axis][buf].size() == m_allAabbsCPU.size() * 2);
 353                         }
 354                 }
 355         }
 356
 357         m_currentBuffer = 1 - m_currentBuffer;
 358
 359         int totalNumAabbs = m_allAabbsCPU.size();
 360
 361         {
 362                 B3_PROFILE("assign m_sortedAxisCPU(FloatFlip)");
 363                 for (int i = 0; i < totalNumAabbs; i++)
 364                 {
 365                         unsigned int keyMin[3];
 366                         unsigned int keyMax[3];
 367                         for (int axis = 0; axis < 3; axis++)
 368                         {
 369                                 float vmin = m_allAabbsCPU[i].m_min[axis];
 370                                 float vmax = m_allAabbsCPU[i].m_max[axis];
 371                                 keyMin[axis] = FloatFlip(vmin);
 372                                 keyMax[axis] = FloatFlip(vmax);
 373
 374                                 m_sortedAxisCPU[axis][m_currentBuffer][i * 2].m_key = keyMin[axis] - 1;
 375                                 m_sortedAxisCPU[axis][m_currentBuffer][i * 2].m_value = i * 2;
 376                                 m_sortedAxisCPU[axis][m_currentBuffer][i * 2 + 1].m_key = keyMax[axis] + 1;
 377                                 m_sortedAxisCPU[axis][m_currentBuffer][i * 2 + 1].m_value = i * 2 + 1;
 378                         }
 379                         //printf("aabb[%d] min=%u,%u,%u max %u,%u,%u\n", i,keyMin[0],keyMin[1],keyMin[2],keyMax[0],keyMax[1],keyMax[2]);
 380                 }
 381         }
 382
 383         {
 384                 B3_PROFILE("sort m_sortedAxisCPU");
 385                 for (int axis = 0; axis < 3; axis++)
 386                         m_sorter->executeHost(m_sortedAxisCPU[axis][m_currentBuffer]);
 387         }
 388
 389 #if 0
 390         if (0)
 391         {
 392                 for (int axis=0;axis<3;axis++)
 393                 {
 394                         //printf("axis %d\n",axis);
 395                         for (int i=0;i<m_sortedAxisCPU[axis][m_currentBuffer].size();i++)
 396                         {
 397                                 //int key = m_sortedAxisCPU[axis][m_currentBuffer][i].m_key;
 398                                 //int value = m_sortedAxisCPU[axis][m_currentBuffer][i].m_value;
 399                                 //printf("[%d]=%d\n",i,value);
 400                         }
 401
 402                 }
 403         }
 404 #endif
 405
 406         {
 407                 B3_PROFILE("assign m_objectMinMaxIndexCPU");
 408                 for (int axis = 0; axis < 3; axis++)
 409                 {
 410                         int totalNumAabbs = m_allAabbsCPU.size();
 411                         int numEndPoints = m_sortedAxisCPU[axis][m_currentBuffer].size();
 412                         m_objectMinMaxIndexCPU[axis][m_currentBuffer].resize(totalNumAabbs);
 413                         for (int i = 0; i < numEndPoints; i++)
 414                         {
 415                                 int destIndex = m_sortedAxisCPU[axis][m_currentBuffer][i].m_value;
 416                                 int newDest = destIndex / 2;
 417                                 if (destIndex & 1)
 418                                 {
 419                                         m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].y = i;
 420                                 }
 421                                 else
 422                                 {
 423                                         m_objectMinMaxIndexCPU[axis][m_currentBuffer][newDest].x = i;
 424                                 }
 425                         }
 426                 }
 427         }
 428
 429 #if 0
 430         if (0)
 431         {
 432                 printf("==========================\n");
 433                 for (int axis=0;axis<3;axis++)
 434                 {
 435                         unsigned int curMinIndex40 = m_objectMinMaxIndexCPU[axis][m_currentBuffer][40].x;
 436                         unsigned int curMaxIndex40 = m_objectMinMaxIndexCPU[axis][m_currentBuffer][40].y;
 437                         unsigned int prevMaxIndex40 = m_objectMinMaxIndexCPU[axis][1-m_currentBuffer][40].y;
 438                         unsigned int prevMinIndex40 = m_objectMinMaxIndexCPU[axis][1-m_currentBuffer][40].x;
 439
 440                         int dmin40 = curMinIndex40 - prevMinIndex40;
 441                         int dmax40 = curMinIndex40 - prevMinIndex40;
 442                         printf("axis %d curMinIndex40=%d prevMinIndex40=%d\n",axis,curMinIndex40, prevMinIndex40);
 443                         printf("axis %d curMaxIndex40=%d prevMaxIndex40=%d\n",axis,curMaxIndex40, prevMaxIndex40);
 444                 }
 445                 printf(".........................\n");
 446                 for (int axis=0;axis<3;axis++)
 447                 {
 448                         unsigned int curMinIndex53 = m_objectMinMaxIndexCPU[axis][m_currentBuffer][53].x;
 449                         unsigned int curMaxIndex53 = m_objectMinMaxIndexCPU[axis][m_currentBuffer][53].y;
 450                         unsigned int prevMaxIndex53 = m_objectMinMaxIndexCPU[axis][1-m_currentBuffer][53].y;
 451                         unsigned int prevMinIndex53 = m_objectMinMaxIndexCPU[axis][1-m_currentBuffer][53].x;
 452
 453                         int dmin40 = curMinIndex53 - prevMinIndex53;
 454                         int dmax40 = curMinIndex53 - prevMinIndex53;
 455                         printf("axis %d curMinIndex53=%d prevMinIndex53=%d\n",axis,curMinIndex53, prevMinIndex53);
 456                         printf("axis %d curMaxIndex53=%d prevMaxIndex53=%d\n",axis,curMaxIndex53, prevMaxIndex53);
 457                 }
 458
 459         }
 460 #endif
 461
 462         int a = m_objectMinMaxIndexCPU[0][m_currentBuffer].size();
 463         int b = m_objectMinMaxIndexCPU[1][m_currentBuffer].size();
 464         int c = m_objectMinMaxIndexCPU[2][m_currentBuffer].size();
 465         b3Assert(a == b);
 466         b3Assert(b == c);
 467         /*
 468         if (searchIncremental3dSapOnGpu)
 469         {
 470                 B3_PROFILE("computePairsIncremental3dSapKernelGPU");
 471                 int numObjects = m_objectMinMaxIndexCPU[0][m_currentBuffer].size();
 472                 int maxCapacity = 1024*1024;
 473                 {
 474                         B3_PROFILE("copy from host");
 475                         m_objectMinMaxIndexGPUaxis0.copyFromHost(m_objectMinMaxIndexCPU[0][m_currentBuffer]);
 476                         m_objectMinMaxIndexGPUaxis1.copyFromHost(m_objectMinMaxIndexCPU[1][m_currentBuffer]);
 477                         m_objectMinMaxIndexGPUaxis2.copyFromHost(m_objectMinMaxIndexCPU[2][m_currentBuffer]);
 478                         m_objectMinMaxIndexGPUaxis0prev.copyFromHost(m_objectMinMaxIndexCPU[0][1-m_currentBuffer]);
 479                         m_objectMinMaxIndexGPUaxis1prev.copyFromHost(m_objectMinMaxIndexCPU[1][1-m_currentBuffer]);
 480                         m_objectMinMaxIndexGPUaxis2prev.copyFromHost(m_objectMinMaxIndexCPU[2][1-m_currentBuffer]);
 481
 482                         m_sortedAxisGPU0.copyFromHost(m_sortedAxisCPU[0][m_currentBuffer]);
 483                         m_sortedAxisGPU1.copyFromHost(m_sortedAxisCPU[1][m_currentBuffer]);
 484                         m_sortedAxisGPU2.copyFromHost(m_sortedAxisCPU[2][m_currentBuffer]);
 485                         m_sortedAxisGPU0prev.copyFromHost(m_sortedAxisCPU[0][1-m_currentBuffer]);
 486                         m_sortedAxisGPU1prev.copyFromHost(m_sortedAxisCPU[1][1-m_currentBuffer]);
 487                         m_sortedAxisGPU2prev.copyFromHost(m_sortedAxisCPU[2][1-m_currentBuffer]);
 488
 489
 490                         m_addedHostPairsGPU.resize(maxCapacity);
 491                         m_removedHostPairsGPU.resize(maxCapacity);
 492
 493                         m_addedCountGPU.resize(0);
 494                         m_addedCountGPU.push_back(0);
 495                         m_removedCountGPU.resize(0);
 496                         m_removedCountGPU.push_back(0);
 497                 }
 498
 499                 {
 500                         B3_PROFILE("launch1D");
 501                         b3LauncherCL launcher(m_queue,  m_computePairsIncremental3dSapKernel,"m_computePairsIncremental3dSapKernel");
 502                         launcher.setBuffer(m_objectMinMaxIndexGPUaxis0.getBufferCL());
 503                         launcher.setBuffer(m_objectMinMaxIndexGPUaxis1.getBufferCL());
 504                         launcher.setBuffer(m_objectMinMaxIndexGPUaxis2.getBufferCL());
 505                         launcher.setBuffer(m_objectMinMaxIndexGPUaxis0prev.getBufferCL());
 506                         launcher.setBuffer(m_objectMinMaxIndexGPUaxis1prev.getBufferCL());
 507                         launcher.setBuffer(m_objectMinMaxIndexGPUaxis2prev.getBufferCL());
 508
 509                         launcher.setBuffer(m_sortedAxisGPU0.getBufferCL());
 510                         launcher.setBuffer(m_sortedAxisGPU1.getBufferCL());
 511                         launcher.setBuffer(m_sortedAxisGPU2.getBufferCL());
 512                         launcher.setBuffer(m_sortedAxisGPU0prev.getBufferCL());
 513                         launcher.setBuffer(m_sortedAxisGPU1prev.getBufferCL());
 514                         launcher.setBuffer(m_sortedAxisGPU2prev.getBufferCL());
 515
 516
 517                         launcher.setBuffer(m_addedHostPairsGPU.getBufferCL());
 518                         launcher.setBuffer(m_removedHostPairsGPU.getBufferCL());
 519                         launcher.setBuffer(m_addedCountGPU.getBufferCL());
 520                         launcher.setBuffer(m_removedCountGPU.getBufferCL());
 521                         launcher.setConst(maxCapacity);
 522                         launcher.setConst( numObjects);
 523                         launcher.launch1D( numObjects);
 524                         clFinish(m_queue);
 525                 }
 526
 527                 {
 528                         B3_PROFILE("copy to host");
 529                         int addedCountGPU = m_addedCountGPU.at(0);
 530                         m_addedHostPairsGPU.resize(addedCountGPU);
 531                         m_addedHostPairsGPU.copyToHost(addedHostPairs);
 532
 533                         //printf("addedCountGPU=%d\n",addedCountGPU);
 534                         int removedCountGPU = m_removedCountGPU.at(0);
 535                         m_removedHostPairsGPU.resize(removedCountGPU);
 536                         m_removedHostPairsGPU.copyToHost(removedHostPairs);
 537                         //printf("removedCountGPU=%d\n",removedCountGPU);
 538
 539                 }
 540
 541
 542
 543         }
 544         else
 545         */
 546         {
 547                 int numObjects = m_objectMinMaxIndexCPU[0][m_currentBuffer].size();
 548
 549                 B3_PROFILE("actual search");
 550                 for (int i = 0; i < numObjects; i++)
 551                 {
 552                         //int numObjects = m_objectMinMaxIndexCPU[axis][m_currentBuffer].size();
 553                         //int checkObjects[]={40,53};
 554                         //int numCheckObjects = sizeof(checkObjects)/sizeof(int);
 555
 556                         //for (int a=0;a<numCheckObjects ;a++)
 557
 558                         for (int axis = 0; axis < 3; axis++)
 559                         {
 560                                 //int i = checkObjects[a];
 561
 562                                 unsigned int curMinIndex = m_objectMinMaxIndexCPU[axis][m_currentBuffer][i].x;
 563                                 unsigned int curMaxIndex = m_objectMinMaxIndexCPU[axis][m_currentBuffer][i].y;
 564                                 unsigned int prevMinIndex = m_objectMinMaxIndexCPU[axis][1 - m_currentBuffer][i].x;
 565                                 int dmin = curMinIndex - prevMinIndex;
 566
 567                                 unsigned int prevMaxIndex = m_objectMinMaxIndexCPU[axis][1 - m_currentBuffer][i].y;
 568
 569                                 int dmax = curMaxIndex - prevMaxIndex;
 570                                 if (dmin != 0)
 571                                 {
 572                                         //printf("for object %d, dmin=%d\n",i,dmin);
 573                                 }
 574                                 if (dmax != 0)
 575                                 {
 576                                         //printf("for object %d, dmax=%d\n",i,dmax);
 577                                 }
 578                                 for (int otherbuffer = 0; otherbuffer < 2; otherbuffer++)
 579                                 {
 580                                         if (dmin != 0)
 581                                         {
 582                                                 int stepMin = dmin < 0 ? -1 : 1;
 583                                                 for (int j = prevMinIndex; j != curMinIndex; j += stepMin)
 584                                                 {
 585                                                         int otherIndex2 = m_sortedAxisCPU[axis][otherbuffer][j].y;
 586                                                         int otherIndex = otherIndex2 / 2;
 587                                                         if (otherIndex != i)
 588                                                         {
 589                                                                 bool otherIsMax = ((otherIndex2 & 1) != 0);
 590
 591                                                                 if (otherIsMax)
 592                                                                 {
 593                                                                         //bool overlap = TestAabbAgainstAabb2((const b3Vector3&)m_allAabbsCPU[i].m_min, (const b3Vector3&)m_allAabbsCPU[i].m_max,(const b3Vector3&)m_allAabbsCPU[otherIndex].m_min,(const b3Vector3&)m_allAabbsCPU[otherIndex].m_max);
 594                                                                         //bool prevOverlap = TestAabbAgainstAabb2((const b3Vector3&)preAabbs[i].m_min, (const b3Vector3&)preAabbs[i].m_max,(const b3Vector3&)preAabbs[otherIndex].m_min,(const b3Vector3&)preAabbs[otherIndex].m_max);
 595
 596                                                                         bool overlap = true;
 597
 598                                                                         for (int ax = 0; ax < 3; ax++)
 599                                                                         {
 600                                                                                 if ((m_objectMinMaxIndexCPU[ax][m_currentBuffer][i].x > m_objectMinMaxIndexCPU[ax][m_currentBuffer][otherIndex].y) ||
 601                                                                                         (m_objectMinMaxIndexCPU[ax][m_currentBuffer][i].y < m_objectMinMaxIndexCPU[ax][m_currentBuffer][otherIndex].x))
 602                                                                                         overlap = false;
 603                                                                         }
 604
 605                                                                         //      b3Assert(overlap2==overlap);
 606
 607                                                                         bool prevOverlap = true;
 608
 609                                                                         for (int ax = 0; ax < 3; ax++)
 610                                                                         {
 611                                                                                 if ((m_objectMinMaxIndexCPU[ax][1 - m_currentBuffer][i].x > m_objectMinMaxIndexCPU[ax][1 - m_currentBuffer][otherIndex].y) ||
 612                                                                                         (m_objectMinMaxIndexCPU[ax][1 - m_currentBuffer][i].y < m_objectMinMaxIndexCPU[ax][1 - m_currentBuffer][otherIndex].x))
 613                                                                                         prevOverlap = false;
 614                                                                         }
 615
 616                                                                         //b3Assert(overlap==overlap2);
 617
 618                                                                         if (dmin < 0)
 619                                                                         {
 620                                                                                 if (overlap && !prevOverlap)
 621                                                                                 {
 622                                                                                         //add a pair
 623                                                                                         b3Int4 newPair;
 624                                                                                         if (i <= otherIndex)
 625                                                                                         {
 626                                                                                                 newPair.x = i;
 627                                                                                                 newPair.y = otherIndex;
 628                                                                                         }
 629                                                                                         else
 630                                                                                         {
 631                                                                                                 newPair.x = otherIndex;
 632                                                                                                 newPair.y = i;
 633                                                                                         }
 634                                                                                         addedHostPairs.push_back(newPair);
 635                                                                                 }
 636                                                                         }
 637                                                                         else
 638                                                                         {
 639                                                                                 if (!overlap && prevOverlap)
 640                                                                                 {
 641                                                                                         //remove a pair
 642                                                                                         b3Int4 removedPair;
 643                                                                                         if (i <= otherIndex)
 644                                                                                         {
 645                                                                                                 removedPair.x = i;
 646                                                                                                 removedPair.y = otherIndex;
 647                                                                                         }
 648                                                                                         else
 649                                                                                         {
 650                                                                                                 removedPair.x = otherIndex;
 651                                                                                                 removedPair.y = i;
 652                                                                                         }
 653                                                                                         removedHostPairs.push_back(removedPair);
 654                                                                                 }
 655                                                                         }  //otherisMax
 656                                                                 }      //if (dmin<0)
 657                                                         }          //if (otherIndex!=i)
 658                                                 }              //for (int j=
 659                                         }
 660
 661                                         if (dmax != 0)
 662                                         {
 663                                                 int stepMax = dmax < 0 ? -1 : 1;
 664                                                 for (int j = prevMaxIndex; j != curMaxIndex; j += stepMax)
 665                                                 {
 666                                                         int otherIndex2 = m_sortedAxisCPU[axis][otherbuffer][j].y;
 667                                                         int otherIndex = otherIndex2 / 2;
 668                                                         if (otherIndex != i)
 669                                                         {
 670                                                                 //bool otherIsMin = ((otherIndex2&1)==0);
 671                                                                 //if (otherIsMin)
 672                                                                 {
 673                                                                         //bool overlap = TestAabbAgainstAabb2((const b3Vector3&)m_allAabbsCPU[i].m_min, (const b3Vector3&)m_allAabbsCPU[i].m_max,(const b3Vector3&)m_allAabbsCPU[otherIndex].m_min,(const b3Vector3&)m_allAabbsCPU[otherIndex].m_max);
 674                                                                         //bool prevOverlap = TestAabbAgainstAabb2((const b3Vector3&)preAabbs[i].m_min, (const b3Vector3&)preAabbs[i].m_max,(const b3Vector3&)preAabbs[otherIndex].m_min,(const b3Vector3&)preAabbs[otherIndex].m_max);
 675
 676                                                                         bool overlap = true;
 677
 678                                                                         for (int ax = 0; ax < 3; ax++)
 679                                                                         {
 680                                                                                 if ((m_objectMinMaxIndexCPU[ax][m_currentBuffer][i].x > m_objectMinMaxIndexCPU[ax][m_currentBuffer][otherIndex].y) ||
 681                                                                                         (m_objectMinMaxIndexCPU[ax][m_currentBuffer][i].y < m_objectMinMaxIndexCPU[ax][m_currentBuffer][otherIndex].x))
 682                                                                                         overlap = false;
 683                                                                         }
 684                                                                         //b3Assert(overlap2==overlap);
 685
 686                                                                         bool prevOverlap = true;
 687
 688                                                                         for (int ax = 0; ax < 3; ax++)
 689                                                                         {
 690                                                                                 if ((m_objectMinMaxIndexCPU[ax][1 - m_currentBuffer][i].x > m_objectMinMaxIndexCPU[ax][1 - m_currentBuffer][otherIndex].y) ||
 691                                                                                         (m_objectMinMaxIndexCPU[ax][1 - m_currentBuffer][i].y < m_objectMinMaxIndexCPU[ax][1 - m_currentBuffer][otherIndex].x))
 692                                                                                         prevOverlap = false;
 693                                                                         }
 694
 695                                                                         if (dmax > 0)
 696                                                                         {
 697                                                                                 if (overlap && !prevOverlap)
 698                                                                                 {
 699                                                                                         //add a pair
 700                                                                                         b3Int4 newPair;
 701                                                                                         if (i <= otherIndex)
 702                                                                                         {
 703                                                                                                 newPair.x = i;
 704                                                                                                 newPair.y = otherIndex;
 705                                                                                         }
 706                                                                                         else
 707                                                                                         {
 708                                                                                                 newPair.x = otherIndex;
 709                                                                                                 newPair.y = i;
 710                                                                                         }
 711                                                                                         addedHostPairs.push_back(newPair);
 712                                                                                 }
 713                                                                         }
 714                                                                         else
 715                                                                         {
 716                                                                                 if (!overlap && prevOverlap)
 717                                                                                 {
 718                                                                                         //if (otherIndex2&1==0) -> min?
 719                                                                                         //remove a pair
 720                                                                                         b3Int4 removedPair;
 721                                                                                         if (i <= otherIndex)
 722                                                                                         {
 723                                                                                                 removedPair.x = i;
 724                                                                                                 removedPair.y = otherIndex;
 725                                                                                         }
 726                                                                                         else
 727                                                                                         {
 728                                                                                                 removedPair.x = otherIndex;
 729                                                                                                 removedPair.y = i;
 730                                                                                         }
 731                                                                                         removedHostPairs.push_back(removedPair);
 732                                                                                 }
 733                                                                         }
 734
 735                                                                 }  //if (dmin<0)
 736                                                         }      //if (otherIndex!=i)
 737                                                 }          //for (int j=
 738                                         }
 739                                 }  //for (int otherbuffer
 740                         }      //for (int axis=0;
 741                 }          //for (int i=0;i<numObjects
 742         }
 743
 744         //remove duplicates and add/remove then to existing m_overlappingPairs
 745
 746         {
 747                 {
 748                         B3_PROFILE("sort allPairs");
 749                         allPairs.quickSort(b3PairCmp);
 750                 }
 751                 {
 752                         B3_PROFILE("sort addedHostPairs");
 753                         addedHostPairs.quickSort(b3PairCmp);
 754                 }
 755                 {
 756                         B3_PROFILE("sort removedHostPairs");
 757                         removedHostPairs.quickSort(b3PairCmp);
 758                 }
 759         }
 760
 761         b3Int4 prevPair;
 762         prevPair.x = -1;
 763         prevPair.y = -1;
 764
 765         int uniqueRemovedPairs = 0;
 766
 767         b3AlignedObjectArray<int> removedPositions;
 768
 769         {
 770                 B3_PROFILE("actual removing");
 771                 for (int i = 0; i < removedHostPairs.size(); i++)
 772                 {
 773                         b3Int4 removedPair = removedHostPairs[i];
 774                         if ((removedPair.x != prevPair.x) || (removedPair.y != prevPair.y))
 775                         {
 776                                 int index1 = allPairs.findBinarySearch(removedPair);
 777
 778                                 //#ifdef _DEBUG
 779
 780                                 int index2 = allPairs.findLinearSearch(removedPair);
 781                                 b3Assert(index1 == index2);
 782
 783                                 //b3Assert(index1!=allPairs.size());
 784                                 if (index1 < allPairs.size())
 785                                 //#endif//_DEBUG
 786                                 {
 787                                         uniqueRemovedPairs++;
 788                                         removedPositions.push_back(index1);
 789                                         {
 790                                                 //printf("framepje(%d) remove pair(%d):%d,%d\n",framepje,i,removedPair.x,removedPair.y);
 791                                         }
 792                                 }
 793                         }
 794                         prevPair = removedPair;
 795                 }
 796
 797                 if (uniqueRemovedPairs)
 798                 {
 799                         for (int i = 0; i < removedPositions.size(); i++)
 800                         {
 801                                 allPairs[removedPositions[i]].x = INT_MAX;
 802                                 allPairs[removedPositions[i]].y = INT_MAX;
 803                         }
 804                         allPairs.quickSort(b3PairCmp);
 805                         allPairs.resize(allPairs.size() - uniqueRemovedPairs);
 806                 }
 807         }
 808         //if (uniqueRemovedPairs)
 809         //      printf("uniqueRemovedPairs=%d\n",uniqueRemovedPairs);
 810         //printf("removedHostPairs.size = %d\n",removedHostPairs.size());
 811
 812         prevPair.x = -1;
 813         prevPair.y = -1;
 814
 815         int uniqueAddedPairs = 0;
 816         b3AlignedObjectArray<b3Int4> actualAddedPairs;
 817
 818         {
 819                 B3_PROFILE("actual adding");
 820                 for (int i = 0; i < addedHostPairs.size(); i++)
 821                 {
 822                         b3Int4 newPair = addedHostPairs[i];
 823                         if ((newPair.x != prevPair.x) || (newPair.y != prevPair.y))
 824                         {
 825                                 //#ifdef _DEBUG
 826                                 int index1 = allPairs.findBinarySearch(newPair);
 827
 828                                 int index2 = allPairs.findLinearSearch(newPair);
 829                                 b3Assert(index1 == index2);
 830
 831                                 b3Assert(index1 == allPairs.size());
 832                                 if (index1 != allPairs.size())
 833                                 {
 834                                         printf("??\n");
 835                                 }
 836
 837                                 if (index1 == allPairs.size())
 838                                 //#endif //_DEBUG
 839                                 {
 840                                         uniqueAddedPairs++;
 841                                         actualAddedPairs.push_back(newPair);
 842                                 }
 843                         }
 844                         prevPair = newPair;
 845                 }
 846                 for (int i = 0; i < actualAddedPairs.size(); i++)
 847                 {
 848                         //printf("framepje (%d), new pair(%d):%d,%d\n",framepje,i,actualAddedPairs[i].x,actualAddedPairs[i].y);
 849                         allPairs.push_back(actualAddedPairs[i]);
 850                 }
 851         }
 852
 853         //if (uniqueAddedPairs)
 854         //      printf("uniqueAddedPairs=%d\n", uniqueAddedPairs);
 855
 856         {
 857                 B3_PROFILE("m_overlappingPairs.copyFromHost");
 858                 m_overlappingPairs.copyFromHost(allPairs);
 859         }
 860 }
 861
 862 void b3GpuSapBroadphase::calculateOverlappingPairsHost(int maxPairs)
 863 {
 864         //test
 865         //      if (m_currentBuffer>=0)
 866         //      return calculateOverlappingPairsHostIncremental3Sap();
 867
 868         b3Assert(m_allAabbsCPU.size() == m_allAabbsGPU.size());
 869         m_allAabbsGPU.copyToHost(m_allAabbsCPU);
 870
 871         int axis = 0;
 872         {
 873                 B3_PROFILE("CPU compute best variance axis");
 874                 b3Vector3 s = b3MakeVector3(0, 0, 0), s2 = b3MakeVector3(0, 0, 0);
 875                 int numRigidBodies = m_smallAabbsMappingCPU.size();
 876
 877                 for (int i = 0; i < numRigidBodies; i++)
 878                 {
 879                         b3SapAabb aabb = this->m_allAabbsCPU[m_smallAabbsMappingCPU[i]];
 880
 881                         b3Vector3 maxAabb = b3MakeVector3(aabb.m_max[0], aabb.m_max[1], aabb.m_max[2]);
 882                         b3Vector3 minAabb = b3MakeVector3(aabb.m_min[0], aabb.m_min[1], aabb.m_min[2]);
 883                         b3Vector3 centerAabb = (maxAabb + minAabb) * 0.5f;
 884
 885                         s += centerAabb;
 886                         s2 += centerAabb * centerAabb;
 887                 }
 888                 b3Vector3 v = s2 - (s * s) / (float)numRigidBodies;
 889
 890                 if (v[1] > v[0])
 891                         axis = 1;
 892                 if (v[2] > v[axis])
 893                         axis = 2;
 894         }
 895
 896         b3AlignedObjectArray<b3Int4> hostPairs;
 897
 898         {
 899                 int numSmallAabbs = m_smallAabbsMappingCPU.size();
 900                 for (int i = 0; i < numSmallAabbs; i++)
 901                 {
 902                         b3SapAabb smallAabbi = m_allAabbsCPU[m_smallAabbsMappingCPU[i]];
 903                         //float reference = smallAabbi.m_max[axis];
 904
 905                         for (int j = i + 1; j < numSmallAabbs; j++)
 906                         {
 907                                 b3SapAabb smallAabbj = m_allAabbsCPU[m_smallAabbsMappingCPU[j]];
 908
 909                                 if (TestAabbAgainstAabb2((b3Vector3&)smallAabbi.m_min, (b3Vector3&)smallAabbi.m_max,
 910                                                                                  (b3Vector3&)smallAabbj.m_min, (b3Vector3&)smallAabbj.m_max))
 911                                 {
 912                                         b3Int4 pair;
 913                                         int a = smallAabbi.m_minIndices[3];
 914                                         int b = smallAabbj.m_minIndices[3];
 915                                         if (a <= b)
 916                                         {
 917                                                 pair.x = a;  //store the original index in the unsorted aabb array
 918                                                 pair.y = b;
 919                                         }
 920                                         else
 921                                         {
 922                                                 pair.x = b;  //store the original index in the unsorted aabb array
 923                                                 pair.y = a;
 924                                         }
 925                                         hostPairs.push_back(pair);
 926                                 }
 927                         }
 928                 }
 929         }
 930
 931         {
 932                 int numSmallAabbs = m_smallAabbsMappingCPU.size();
 933                 for (int i = 0; i < numSmallAabbs; i++)
 934                 {
 935                         b3SapAabb smallAabbi = m_allAabbsCPU[m_smallAabbsMappingCPU[i]];
 936
 937                         //float reference = smallAabbi.m_max[axis];
 938                         int numLargeAabbs = m_largeAabbsMappingCPU.size();
 939
 940                         for (int j = 0; j < numLargeAabbs; j++)
 941                         {
 942                                 b3SapAabb largeAabbj = m_allAabbsCPU[m_largeAabbsMappingCPU[j]];
 943                                 if (TestAabbAgainstAabb2((b3Vector3&)smallAabbi.m_min, (b3Vector3&)smallAabbi.m_max,
 944                                                                                  (b3Vector3&)largeAabbj.m_min, (b3Vector3&)largeAabbj.m_max))
 945                                 {
 946                                         b3Int4 pair;
 947                                         int a = largeAabbj.m_minIndices[3];
 948                                         int b = smallAabbi.m_minIndices[3];
 949                                         if (a <= b)
 950                                         {
 951                                                 pair.x = a;
 952                                                 pair.y = b;  //store the original index in the unsorted aabb array
 953                                         }
 954                                         else
 955                                         {
 956                                                 pair.x = b;
 957                                                 pair.y = a;  //store the original index in the unsorted aabb array
 958                                         }
 959
 960                                         hostPairs.push_back(pair);
 961                                 }
 962                         }
 963                 }
 964         }
 965
 966         if (hostPairs.size() > maxPairs)
 967         {
 968                 hostPairs.resize(maxPairs);
 969         }
 970
 971         if (hostPairs.size())
 972         {
 973                 m_overlappingPairs.copyFromHost(hostPairs);
 974         }
 975         else
 976         {
 977                 m_overlappingPairs.resize(0);
 978         }
 979
 980         //init3dSap();
 981 }
 982
 983 void b3GpuSapBroadphase::reset()
 984 {
 985         m_allAabbsGPU.resize(0);
 986         m_allAabbsCPU.resize(0);
 987
 988         m_smallAabbsMappingGPU.resize(0);
 989         m_smallAabbsMappingCPU.resize(0);
 990
 991         m_pairCount.resize(0);
 992
 993         m_largeAabbsMappingGPU.resize(0);
 994         m_largeAabbsMappingCPU.resize(0);
 995 }
 996
 997 void b3GpuSapBroadphase::calculateOverlappingPairs(int maxPairs)
 998 {
 999         if (m_sapKernel == 0)
1000         {
1001                 calculateOverlappingPairsHost(maxPairs);
1002                 return;
1003         }
1004
1005         //if (m_currentBuffer>=0)
1006         //      return calculateOverlappingPairsHostIncremental3Sap();
1007
1008         //calculateOverlappingPairsHost(maxPairs);
1009
1010         B3_PROFILE("GPU 1-axis SAP calculateOverlappingPairs");
1011
1012         int axis = 0;
1013
1014         {
1015                 //bool syncOnHost = false;
1016
1017                 int numSmallAabbs = m_smallAabbsMappingCPU.size();
1018                 if (m_prefixScanFloat4 && numSmallAabbs)
1019                 {
1020                         B3_PROFILE("GPU compute best variance axis");
1021
1022                         if (m_dst.size() != (numSmallAabbs + 1))
1023                         {
1024                                 m_dst.resize(numSmallAabbs + 128);
1025                                 m_sum.resize(numSmallAabbs + 128);
1026                                 m_sum2.resize(numSmallAabbs + 128);
1027                                 m_sum.at(numSmallAabbs) = b3MakeVector3(0, 0, 0);   //slow?
1028                                 m_sum2.at(numSmallAabbs) = b3MakeVector3(0, 0, 0);  //slow?
1029                         }
1030
1031                         b3LauncherCL launcher(m_queue, m_prepareSumVarianceKernel, "m_prepareSumVarianceKernel");
1032                         launcher.setBuffer(m_allAabbsGPU.getBufferCL());
1033
1034                         launcher.setBuffer(m_smallAabbsMappingGPU.getBufferCL());
1035                         launcher.setBuffer(m_sum.getBufferCL());
1036                         launcher.setBuffer(m_sum2.getBufferCL());
1037                         launcher.setConst(numSmallAabbs);
1038                         int num = numSmallAabbs;
1039                         launcher.launch1D(num);
1040
1041                         b3Vector3 s;
1042                         b3Vector3 s2;
1043                         m_prefixScanFloat4->execute(m_sum, m_dst, numSmallAabbs + 1, &s);
1044                         m_prefixScanFloat4->execute(m_sum2, m_dst, numSmallAabbs + 1, &s2);
1045
1046                         b3Vector3 v = s2 - (s * s) / (float)numSmallAabbs;
1047
1048                         if (v[1] > v[0])
1049                                 axis = 1;
1050                         if (v[2] > v[axis])
1051                                 axis = 2;
1052                 }
1053
1054                 m_gpuSmallSortData.resize(numSmallAabbs);
1055
1056 #if 1
1057                 if (m_smallAabbsMappingGPU.size())
1058                 {
1059                         B3_PROFILE("flipFloatKernel");
1060                         b3BufferInfoCL bInfo[] = {
1061                                 b3BufferInfoCL(m_allAabbsGPU.getBufferCL(), true),
1062                                 b3BufferInfoCL(m_smallAabbsMappingGPU.getBufferCL(), true),
1063                                 b3BufferInfoCL(m_gpuSmallSortData.getBufferCL())};
1064                         b3LauncherCL launcher(m_queue, m_flipFloatKernel, "m_flipFloatKernel");
1065                         launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
1066                         launcher.setConst(numSmallAabbs);
1067                         launcher.setConst(axis);
1068
1069                         int num = numSmallAabbs;
1070                         launcher.launch1D(num);
1071                         clFinish(m_queue);
1072                 }
1073
1074                 if (m_gpuSmallSortData.size())
1075                 {
1076                         B3_PROFILE("gpu radix sort");
1077                         m_sorter->execute(m_gpuSmallSortData);
1078                         clFinish(m_queue);
1079                 }
1080
1081                 m_gpuSmallSortedAabbs.resize(numSmallAabbs);
1082                 if (numSmallAabbs)
1083                 {
1084                         B3_PROFILE("scatterKernel");
1085
1086                         b3BufferInfoCL bInfo[] = {
1087                                 b3BufferInfoCL(m_allAabbsGPU.getBufferCL(), true),
1088                                 b3BufferInfoCL(m_smallAabbsMappingGPU.getBufferCL(), true),
1089                                 b3BufferInfoCL(m_gpuSmallSortData.getBufferCL(), true),
1090                                 b3BufferInfoCL(m_gpuSmallSortedAabbs.getBufferCL())};
1091                         b3LauncherCL launcher(m_queue, m_scatterKernel, "m_scatterKernel ");
1092                         launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
1093                         launcher.setConst(numSmallAabbs);
1094                         int num = numSmallAabbs;
1095                         launcher.launch1D(num);
1096                         clFinish(m_queue);
1097                 }
1098
1099                 m_overlappingPairs.resize(maxPairs);
1100
1101                 m_pairCount.resize(0);
1102                 m_pairCount.push_back(0);
1103                 int numPairs = 0;
1104
1105                 {
1106                         int numLargeAabbs = m_largeAabbsMappingGPU.size();
1107                         if (numLargeAabbs && numSmallAabbs)
1108                         {
1109                                 //@todo
1110                                 B3_PROFILE("sap2Kernel");
1111                                 b3BufferInfoCL bInfo[] = {
1112                                         b3BufferInfoCL(m_allAabbsGPU.getBufferCL()),
1113                                         b3BufferInfoCL(m_largeAabbsMappingGPU.getBufferCL()),
1114                                         b3BufferInfoCL(m_smallAabbsMappingGPU.getBufferCL()),
1115                                         b3BufferInfoCL(m_overlappingPairs.getBufferCL()),
1116                                         b3BufferInfoCL(m_pairCount.getBufferCL())};
1117                                 b3LauncherCL launcher(m_queue, m_sap2Kernel, "m_sap2Kernel");
1118                                 launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
1119                                 launcher.setConst(numLargeAabbs);
1120                                 launcher.setConst(numSmallAabbs);
1121                                 launcher.setConst(axis);
1122                                 launcher.setConst(maxPairs);
1123                                 //@todo: use actual maximum work item sizes of the device instead of hardcoded values
1124                                 launcher.launch2D(numLargeAabbs, numSmallAabbs, 4, 64);
1125
1126                                 numPairs = m_pairCount.at(0);
1127                                 if (numPairs > maxPairs)
1128                                 {
1129                                         b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
1130                                         numPairs = maxPairs;
1131                                 }
1132                         }
1133                 }
1134                 if (m_gpuSmallSortedAabbs.size())
1135                 {
1136                         B3_PROFILE("sapKernel");
1137                         b3BufferInfoCL bInfo[] = {b3BufferInfoCL(m_gpuSmallSortedAabbs.getBufferCL()), b3BufferInfoCL(m_overlappingPairs.getBufferCL()), b3BufferInfoCL(m_pairCount.getBufferCL())};
1138                         b3LauncherCL launcher(m_queue, m_sapKernel, "m_sapKernel");
1139                         launcher.setBuffers(bInfo, sizeof(bInfo) / sizeof(b3BufferInfoCL));
1140                         launcher.setConst(numSmallAabbs);
1141                         launcher.setConst(axis);
1142                         launcher.setConst(maxPairs);
1143
1144                         int num = numSmallAabbs;
1145 #if 0
1146                 int buffSize = launcher.getSerializationBufferSize();
1147                 unsigned char* buf = new unsigned char[buffSize+sizeof(int)];
1148                 for (int i=0;i<buffSize+1;i++)
1149                 {
1150                     unsigned char* ptr = (unsigned char*)&buf[i];
1151                     *ptr = 0xff;
1152                 }
1153                 int actualWrite = launcher.serializeArguments(buf,buffSize);
1154
1155                 unsigned char* cptr = (unsigned char*)&buf[buffSize];
1156     //            printf("buf[buffSize] = %d\n",*cptr);
1157
1158                 assert(buf[buffSize]==0xff);//check for buffer overrun
1159                 int* ptr = (int*)&buf[buffSize];
1160
1161                 *ptr = num;
1162
1163                 FILE* f = fopen("m_sapKernelArgs.bin","wb");
1164                 fwrite(buf,buffSize+sizeof(int),1,f);
1165                 fclose(f);
1166 #endif  //
1167
1168                         launcher.launch1D(num);
1169                         clFinish(m_queue);
1170
1171                         numPairs = m_pairCount.at(0);
1172                         if (numPairs > maxPairs)
1173                         {
1174                                 b3Error("Error running out of pairs: numPairs = %d, maxPairs = %d.\n", numPairs, maxPairs);
1175                                 numPairs = maxPairs;
1176                                 m_pairCount.resize(0);
1177                                 m_pairCount.push_back(maxPairs);
1178                         }
1179                 }
1180
1181 #else
1182                 int numPairs = 0;
1183
1184                 b3LauncherCL launcher(m_queue, m_sapKernel);
1185
1186                 const char* fileName = "m_sapKernelArgs.bin";
1187                 FILE* f = fopen(fileName, "rb");
1188                 if (f)
1189                 {
1190                         int sizeInBytes = 0;
1191                         if (fseek(f, 0, SEEK_END) || (sizeInBytes = ftell(f)) == EOF || fseek(f, 0, SEEK_SET))
1192                         {
1193                                 printf("error, cannot get file size\n");
1194                                 exit(0);
1195                         }
1196
1197                         unsigned char* buf = (unsigned char*)malloc(sizeInBytes);
1198                         fread(buf, sizeInBytes, 1, f);
1199                         int serializedBytes = launcher.deserializeArgs(buf, sizeInBytes, m_context);
1200                         int num = *(int*)&buf[serializedBytes];
1201                         launcher.launch1D(num);
1202
1203                         b3OpenCLArray<int> pairCount(m_context, m_queue);
1204                         int numElements = launcher.m_arrays[2]->size() / sizeof(int);
1205                         pairCount.setFromOpenCLBuffer(launcher.m_arrays[2]->getBufferCL(), numElements);
1206                         numPairs = pairCount.at(0);
1207                         //printf("overlapping pairs = %d\n",numPairs);
1208                         b3AlignedObjectArray<b3Int4> hostOoverlappingPairs;
1209                         b3OpenCLArray<b3Int4> tmpGpuPairs(m_context, m_queue);
1210                         tmpGpuPairs.setFromOpenCLBuffer(launcher.m_arrays[1]->getBufferCL(), numPairs);
1211
1212                         tmpGpuPairs.copyToHost(hostOoverlappingPairs);
1213                         m_overlappingPairs.copyFromHost(hostOoverlappingPairs);
1214                         //printf("hello %d\n", m_overlappingPairs.size());
1215                         free(buf);
1216                         fclose(f);
1217                 }
1218                 else
1219                 {
1220                         printf("error: cannot find file %s\n", fileName);
1221                 }
1222
1223                 clFinish(m_queue);
1224
1225 #endif
1226
1227                 m_overlappingPairs.resize(numPairs);
1228
1229         }  //B3_PROFILE("GPU_RADIX SORT");
1230            //init3dSap();
1231 }
1232
1233 void b3GpuSapBroadphase::writeAabbsToGpu()
1234 {
1235         m_smallAabbsMappingGPU.copyFromHost(m_smallAabbsMappingCPU);
1236         m_largeAabbsMappingGPU.copyFromHost(m_largeAabbsMappingCPU);
1237
1238         m_allAabbsGPU.copyFromHost(m_allAabbsCPU);  //might not be necessary, the 'setupGpuAabbsFull' already takes care of this
1239 }
1240
1241 void b3GpuSapBroadphase::createLargeProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
1242 {
1243         int index = userPtr;
1244         b3SapAabb aabb;
1245         for (int i = 0; i < 4; i++)
1246         {
1247                 aabb.m_min[i] = aabbMin[i];
1248                 aabb.m_max[i] = aabbMax[i];
1249         }
1250         aabb.m_minIndices[3] = index;
1251         aabb.m_signedMaxIndices[3] = m_allAabbsCPU.size();
1252         m_largeAabbsMappingCPU.push_back(m_allAabbsCPU.size());
1253
1254         m_allAabbsCPU.push_back(aabb);
1255 }
1256
1257 void b3GpuSapBroadphase::createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int userPtr, int collisionFilterGroup, int collisionFilterMask)
1258 {
1259         int index = userPtr;
1260         b3SapAabb aabb;
1261         for (int i = 0; i < 4; i++)
1262         {
1263                 aabb.m_min[i] = aabbMin[i];
1264                 aabb.m_max[i] = aabbMax[i];
1265         }
1266         aabb.m_minIndices[3] = index;
1267         aabb.m_signedMaxIndices[3] = m_allAabbsCPU.size();
1268         m_smallAabbsMappingCPU.push_back(m_allAabbsCPU.size());
1269
1270         m_allAabbsCPU.push_back(aabb);
1271 }
1272
1273 cl_mem b3GpuSapBroadphase::getAabbBufferWS()
1274 {
1275         return m_allAabbsGPU.getBufferCL();
1276 }
1277
1278 int b3GpuSapBroadphase::getNumOverlap()
1279 {
1280         return m_overlappingPairs.size();
1281 }
1282 cl_mem b3GpuSapBroadphase::getOverlappingPairBuffer()
1283 {
1284         return m_overlappingPairs.getBufferCL();
1285 }
1286
1287 b3OpenCLArray<b3Int4>& b3GpuSapBroadphase::getOverlappingPairsGPU()
1288 {
1289         return m_overlappingPairs;
1290 }
1291 b3OpenCLArray<int>& b3GpuSapBroadphase::getSmallAabbIndicesGPU()
1292 {
1293         return m_smallAabbsMappingGPU;
1294 }
1295 b3OpenCLArray<int>& b3GpuSapBroadphase::getLargeAabbIndicesGPU()
1296 {
1297         return m_largeAabbsMappingGPU;
1298 }