// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/R3Shapes/R3Grid.cpp
// Function: gaps::R3Grid::ClusterWithMeanShift()
void R3Grid::
ClusterWithMeanShift(void)
{
  // Determine maximum number of iterations
  int max_iterations = XResolution();
  if (max_iterations < YResolution()) max_iterations = YResolution();
  if (max_iterations < ZResolution()) max_iterations = ZResolution();

  // Create gaussian kernel
  static RNScalar kernel[5][5][5];
  const double sigma = 1;
  const double denom = -2 * sigma * sigma;
  for (int jz = -2; jz <=2; jz++) {
    for (int jy = -2; jy <= 2; jy++) {
      for (int jx = -2; jx <= 2; jx++) {
        double dd = jx*jx + jy*jy + jz*jz;
        kernel[jx+2][jy+2][jz+2] = exp(dd/denom);
      }
    }
  }

  // Create flow
  int index = 0;
  int *flow = new int [ NEntries() ];
  for (int iz = 0; iz < ZResolution(); iz++) {
    for (int iy = 0; iy < YResolution(); iy++) {
      for (int ix = 0; ix < XResolution(); ix++) {
        flow[index] = -1;
        if (GridValue(ix, iy, iz) > 0) {
          R3Point position(ix, iy, iz);
          for (int k = 0; k < max_iterations; k++) {
            // Compute movement vector
            RNScalar weight = 0;
            R3Vector movement(0, 0, 0);
            for (int jz = -2; jz <= 2; jz++) {
              double z = position[2] + jz;
              for (int jy = -2; jy <= 2; jy++) {
                double y = position[1] + jy;
                for (int jx = -2; jx <= 2; jx++) {
                  double x = position[0] + jx;
                  RNScalar neighbor_value = GridValue(x, y, z);
                  if (neighbor_value > 0) {
                    RNScalar w = kernel[jx+2][jy+2][jz+2] * neighbor_value;
                    movement += R3Vector(w*jx, w*jy, w*jz);
                    weight += w;
                  }
                }
              }
            }
            
            // Move to centroid of neighbors
            if (weight == 0) break;
            if (RNIsZero(movement.Dot(movement))) break;
            position += movement / weight;
          }
          
          // Assign flow
          int fx = (int) (position[0] + 0.5);
          int fy = (int) (position[1] + 0.5);
          int fz = (int) (position[2] + 0.5);
          if (fx < 0) fx = 0;
          if (fy < 0) fy = 0;
          if (fz < 0) fz = 0;
          if (fx >= XResolution()) fx = XResolution() - 1;
          if (fy >= YResolution()) fx = YResolution() - 1;
          if (fz >= ZResolution()) fx = ZResolution() - 1;
          IndicesToIndex(fx, fy, fz, flow[index]);
        }
        index++;
      }
    }
  }

  // Compute cluster IDs
  int nclusters = 0;
  for (int i = 0; i < NEntries(); i++) {
    if (flow[i] == i) {
      SetGridValue(i, nclusters + 1);
      nclusters++;
    }
  }

  // Assign grid cells to cluster IDs
  for (int i = 0; i < NEntries(); i++) {
    if (flow[i] >= 0) {
      int index = flow[i];
      for (int j = 0; j < NEntries(); j++) {
        if (index == flow[index]) break;
        index = flow[index];
      }
      
      // Set grid value
      RNScalar cluster = GridValue(index);
      SetGridValue(i, cluster);
    }
  }

  // Delete flow
  delete [] flow;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xd8, %rsp
movq %rdi, %rbx
movl 0x238(%rdi), %eax
movl %eax, 0x18(%rsp)
movl 0x23c(%rdi), %eax
movl %eax, 0xc(%rsp)
movl 0x240(%rdi), %eax
movl %eax, 0x10(%rsp)
movq $-0x2, %rax
leaq 0xb276a(%rip), %r13     # 0xe6260
movsd 0x6e762(%rip), %xmm1    # 0xa2260
movq %rax, 0x28(%rsp)
imulq %rax, %rax
movq %rax, 0x30(%rsp)
movq %r13, 0x20(%rsp)
movq $-0x2, %rbp
movq %rbp, %r14
imulq %rbp, %r14
addq 0x30(%rsp), %r14
movq $-0x2, %r15
movq %r13, %r12
movl %r15d, %eax
imull %r15d, %eax
addl %r14d, %eax
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
mulsd %xmm1, %xmm0
callq 0xe7f0
movsd 0x6e710(%rip), %xmm1    # 0xa2260
movsd %xmm0, (%r12)
incq %r15
addq $0xc8, %r12
cmpq $0x3, %r15
jne 0x33b2e
incq %rbp
addq $0x28, %r13
cmpq $0x3, %rbp
jne 0x33b18
movq 0x28(%rsp), %rax
incq %rax
movq 0x20(%rsp), %r13
addq $0x8, %r13
cmpq $0x3, %rax
jne 0x33afe
movl 0xc(%rsp), %eax
movl 0x18(%rsp), %ecx
cmpl %eax, %ecx
cmovgl %ecx, %eax
movl 0x10(%rsp), %ecx
cmpl %ecx, %eax
cmovlel %ecx, %eax
movl %eax, 0xc(%rsp)
movslq 0x24c(%rbx), %r14
leaq (,%r14,4), %rax
testq %r14, %r14
movq $-0x1, %rdi
cmovnsq %rax, %rdi
callq 0xe030
movl 0x240(%rbx), %ecx
testl %ecx, %ecx
jle 0x33fa3
movl 0x23c(%rbx), %r11d
leaq 0xc0(%rsp), %r14
xorl %r15d, %r15d
xorl %ebp, %ebp
movq %rbx, 0x48(%rsp)
movq %rax, 0x68(%rsp)
testl %r11d, %r11d
jle 0x33f92
xorps %xmm0, %xmm0
cvtsi2sd %ebp, %xmm0
movsd %xmm0, 0x70(%rsp)
movl 0x238(%rbx), %edx
xorl %r12d, %r12d
movl %ebp, 0x38(%rsp)
testl %edx, %edx
jle 0x33f86
xorps %xmm0, %xmm0
cvtsi2sd %r12d, %xmm0
movsd %xmm0, 0x78(%rsp)
movslq %r15d, %r15
movl 0x244(%rbx), %edi
movl 0x248(%rbx), %esi
xorl %r13d, %r13d
movl %r12d, 0x3c(%rsp)
movl $0xffffffff, (%rax,%r15,4) # imm = 0xFFFFFFFF
movq 0x230(%rbx), %r8
movl %esi, %r9d
imull %ebp, %r9d
movl %edi, %r10d
imull %r12d, %r10d
addl %r13d, %r9d
addl %r10d, %r9d
movslq %r9d, %r9
movsd (%r8,%r9,8), %xmm0
ucomisd 0x6e56e(%rip), %xmm0    # 0xa21e8
jbe 0x33f77
movq %r15, 0x80(%rsp)
movl %r13d, 0x40(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %r13d, %xmm0
leaq 0x50(%rsp), %rdi
movsd 0x78(%rsp), %xmm1
movsd 0x70(%rsp), %xmm2
callq 0x3c532
cmpl $0x0, 0xc(%rsp)
jle 0x33eab
xorl %ebp, %ebp
movl %ebp, 0x44(%rsp)
xorpd %xmm0, %xmm0
movsd %xmm0, 0x10(%rsp)
xorpd %xmm1, %xmm1
xorpd %xmm2, %xmm2
movq %r14, %r13
movq %r14, %rdi
callq 0x3cfc6
movq $-0x2, %rcx
leaq 0xb2579(%rip), %r15     # 0xe6260
movq %rcx, 0x90(%rsp)
xorps %xmm0, %xmm0
cvtsi2sd %ecx, %xmm0
movsd 0x60(%rsp), %xmm1
movsd %xmm0, 0x98(%rsp)
addsd %xmm0, %xmm1
movsd %xmm1, 0x20(%rsp)
movq %r15, 0x88(%rsp)
movq $-0x2, %rbp
xorps %xmm0, %xmm0
cvtsi2sd %ebp, %xmm0
movsd 0x58(%rsp), %xmm1
movsd %xmm0, 0xa0(%rsp)
addsd %xmm0, %xmm1
movsd %xmm1, 0x28(%rsp)
movq $-0x3e8, %r14           # imm = 0xFC18
movl $0xfffffffe, %r12d      # imm = 0xFFFFFFFE
xorps %xmm1, %xmm1
cvtsi2sd %r12d, %xmm1
movsd 0x50(%rsp), %xmm0
movsd %xmm1, 0x30(%rsp)
addsd %xmm1, %xmm0
movq %rbx, %rdi
movsd 0x28(%rsp), %xmm1
movsd 0x20(%rsp), %xmm2
callq 0x2f0d6
ucomisd 0x6e469(%rip), %xmm0    # 0xa21e8
jbe 0x33deb
mulsd 0x3e8(%r15,%r14), %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x30(%rsp), %xmm3
mulsd %xmm0, %xmm3
movapd %xmm0, %xmm1
mulsd 0xa0(%rsp), %xmm1
movapd %xmm0, %xmm2
mulsd 0x98(%rsp), %xmm2
leaq 0xa8(%rsp), %rbx
movq %rbx, %rdi
movapd %xmm3, %xmm0
callq 0x3cfc6
movq %r13, %rdi
movq %rbx, %rsi
movq 0x48(%rsp), %rbx
callq 0x3d984
movsd 0x10(%rsp), %xmm0
addsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x10(%rsp)
incl %r12d
addq $0xc8, %r14
jne 0x33d4b
incq %rbp
addq $0x28, %r15
cmpq $0x3, %rbp
jne 0x33d1e
movq 0x90(%rsp), %rcx
incq %rcx
movq 0x88(%rsp), %r15
addq $0x8, %r15
cmpq $0x3, %rcx
jne 0x33ce7
movsd 0x10(%rsp), %xmm0
ucomisd 0x6e3ad(%rip), %xmm0    # 0xa21e8
movq %r13, %r14
movl 0x44(%rsp), %ebp
jne 0x33e46
jnp 0x33eab
movq %r14, %rdi
movq %r14, %rsi
callq 0x3d252
leaq 0xb1d40(%rip), %rax     # 0xe5b98
movsd (%rax), %xmm1
movapd %xmm1, %xmm2
xorpd 0x6e1b8(%rip), %xmm2    # 0xa2020
ucomisd %xmm2, %xmm0
jb 0x33e74
ucomisd %xmm0, %xmm1
jae 0x33eab
leaq 0xa8(%rsp), %rbx
movq %rbx, %rdi
movq %r14, %rsi
movsd 0x10(%rsp), %xmm0
callq 0x3dcac
leaq 0x50(%rsp), %rdi
movq %rbx, %rsi
movq 0x48(%rsp), %rbx
callq 0x3c914
incl %ebp
cmpl 0xc(%rsp), %ebp
jne 0x33cb8
movsd 0x60(%rsp), %xmm0
addsd 0x6e177(%rip), %xmm0    # 0xa2030
cvttsd2si %xmm0, %r8d
testl %r8d, %r8d
movl $0x0, %eax
cmovlel %eax, %r8d
movapd 0x50(%rsp), %xmm0
addpd 0x6e138(%rip), %xmm0    # 0xa2010
cvttpd2dq %xmm0, %xmm1
movapd %xmm1, %xmm0
pcmpgtd 0x6eeb8(%rip), %xmm0    # 0xa2da0
pand %xmm1, %xmm0
movq 0x238(%rbx), %rdx
movq %rdx, %xmm1
leal -0x1(%rdx), %eax
pcmpgtd %xmm0, %xmm1
pshufd $0x50, %xmm1, %xmm1     # xmm1 = xmm1[0,0,1,1]
movmskpd %xmm1, %ecx
movd %xmm0, %esi
testb $0x1, %cl
cmovel %eax, %esi
movq %rdx, %r11
shrq $0x20, %r11
leal -0x1(%r11), %edi
testb $0x2, %cl
cmovnel %esi, %edi
movl 0x240(%rbx), %ecx
leal -0x1(%rcx), %r9d
cmpl %ecx, %r8d
cmovll %edi, %r9d
movl 0x248(%rbx), %esi
imull %esi, %r8d
movl 0x244(%rbx), %edi
pshufd $0x55, %xmm0, %xmm0     # xmm0 = xmm0[1,1,1,1]
movd %xmm0, %r10d
imull %edi, %r10d
addl %r8d, %r10d
addl %r9d, %r10d
movq 0x68(%rsp), %rax
movq 0x80(%rsp), %r15
movl %r10d, (%rax,%r15,4)
movl 0x38(%rsp), %ebp
movl 0x3c(%rsp), %r12d
movl 0x40(%rsp), %r13d
incq %r15
incl %r13d
cmpl %edx, %r13d
jl 0x33c46
incl %r12d
cmpl %r11d, %r12d
jl 0x33c19
incl %ebp
cmpl %ecx, %ebp
jl 0x33bf6
movl 0x24c(%rbx), %r14d
testl %r14d, %r14d
jle 0x34017
movq 0x230(%rbx), %rcx
movl %r14d, %r10d
xorl %edx, %edx
xorl %esi, %esi
movl (%rax,%rdx,4), %edi
cmpq %rdi, %rdx
jne 0x33fcc
incl %esi
xorps %xmm0, %xmm0
cvtsi2sd %esi, %xmm0
movsd %xmm0, (%rcx,%rdx,8)
incq %rdx
cmpq %rdx, %r10
jne 0x33fb6
testl %r14d, %r14d
jle 0x34017
movq 0x230(%rbx), %rcx
xorl %edx, %edx
movl (%rax,%rdx,4), %edi
movl %r14d, %esi
testl %edi, %edi
js 0x3400f
movl %edi, %r8d
movslq %edi, %r9
movl (%rax,%r9,4), %edi
cmpl %edi, %r9d
je 0x34002
movl %edi, %r8d
decl %esi
jne 0x33fec
movslq %r8d, %rsi
movsd (%rcx,%rsi,8), %xmm0
movsd %xmm0, (%rcx,%rdx,8)
incq %rdx
cmpq %r10, %rdx
jne 0x33fe2
movq %rax, %rdi
callq 0xe5b0
addq $0xd8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop