// File: 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:
subq $0x178, %rsp            # imm = 0x178
movq %rdi, 0x170(%rsp)
movq 0x170(%rsp), %rdi
movq %rdi, 0x38(%rsp)
callq 0x138c0
movq 0x38(%rsp), %rdi
movl %eax, 0x16c(%rsp)
movl 0x16c(%rsp), %eax
movl %eax, 0x44(%rsp)
callq 0x138e0
movl %eax, %ecx
movl 0x44(%rsp), %eax
cmpl %ecx, %eax
jge 0x5ef48
movq 0x38(%rsp), %rdi
callq 0x138e0
movl %eax, 0x16c(%rsp)
movq 0x38(%rsp), %rdi
movl 0x16c(%rsp), %eax
movl %eax, 0x34(%rsp)
callq 0x13900
movl %eax, %ecx
movl 0x34(%rsp), %eax
cmpl %ecx, %eax
jge 0x5ef78
movq 0x38(%rsp), %rdi
callq 0x13900
movl %eax, 0x16c(%rsp)
movsd 0xd4088(%rip), %xmm0    # 0x133008
movsd %xmm0, 0x160(%rsp)
movsd 0xd4277(%rip), %xmm0    # 0x133208
movsd %xmm0, 0x158(%rsp)
movl $0xfffffffe, 0x154(%rsp) # imm = 0xFFFFFFFE
cmpl $0x2, 0x154(%rsp)
jg 0x5f0c7
movl $0xfffffffe, 0x150(%rsp) # imm = 0xFFFFFFFE
cmpl $0x2, 0x150(%rsp)
jg 0x5f0af
movl $0xfffffffe, 0x14c(%rsp) # imm = 0xFFFFFFFE
cmpl $0x2, 0x14c(%rsp)
jg 0x5f097
movl 0x14c(%rsp), %eax
imull 0x14c(%rsp), %eax
movl 0x150(%rsp), %ecx
imull 0x150(%rsp), %ecx
addl %ecx, %eax
movl 0x154(%rsp), %ecx
imull 0x154(%rsp), %ecx
addl %ecx, %eax
cvtsi2sd %eax, %xmm0
movsd %xmm0, 0x140(%rsp)
movsd 0x140(%rsp), %xmm0
movsd 0xd41d4(%rip), %xmm1    # 0x133208
divsd %xmm1, %xmm0
callq 0xf8c0
movl 0x14c(%rsp), %eax
addl $0x2, %eax
movslq %eax, %rcx
leaq 0x119c5f(%rip), %rax    # 0x178cb0
imulq $0xc8, %rcx, %rcx
addq %rcx, %rax
movl 0x150(%rsp), %ecx
addl $0x2, %ecx
movslq %ecx, %rcx
imulq $0x28, %rcx, %rcx
addq %rcx, %rax
movl 0x154(%rsp), %ecx
addl $0x2, %ecx
movslq %ecx, %rcx
movsd %xmm0, (%rax,%rcx,8)
movl 0x14c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x14c(%rsp)
jmp 0x5efd7
jmp 0x5f099
movl 0x150(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x150(%rsp)
jmp 0x5efbe
jmp 0x5f0b1
movl 0x154(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x154(%rsp)
jmp 0x5efa5
movq 0x38(%rsp), %rdi
movl $0x0, 0x13c(%rsp)
callq 0x137e0
cltq
movl $0x4, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
callq 0xf030
movq %rax, 0x130(%rsp)
movl $0x0, 0x12c(%rsp)
movq 0x38(%rsp), %rdi
movl 0x12c(%rsp), %eax
movl %eax, 0x30(%rsp)
callq 0x13900
movl %eax, %ecx
movl 0x30(%rsp), %eax
cmpl %ecx, %eax
jge 0x5f691
movl $0x0, 0x128(%rsp)
movq 0x38(%rsp), %rdi
movl 0x128(%rsp), %eax
movl %eax, 0x2c(%rsp)
callq 0x138e0
movl %eax, %ecx
movl 0x2c(%rsp), %eax
cmpl %ecx, %eax
jge 0x5f679
movl $0x0, 0x124(%rsp)
movq 0x38(%rsp), %rdi
movl 0x124(%rsp), %eax
movl %eax, 0x28(%rsp)
callq 0x138c0
movl %eax, %ecx
movl 0x28(%rsp), %eax
cmpl %ecx, %eax
jge 0x5f661
movq 0x38(%rsp), %rdi
movq 0x130(%rsp), %rax
movslq 0x13c(%rsp), %rcx
movl $0xffffffff, (%rax,%rcx,4) # imm = 0xFFFFFFFF
movl 0x124(%rsp), %esi
movl 0x128(%rsp), %edx
movl 0x12c(%rsp), %ecx
callq 0x6bad0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x5f63a
cvtsi2sdl 0x124(%rsp), %xmm0
cvtsi2sdl 0x128(%rsp), %xmm1
cvtsi2sdl 0x12c(%rsp), %xmm2
leaq 0x108(%rsp), %rdi
callq 0x6eb50
movl $0x0, 0x104(%rsp)
movl 0x104(%rsp), %eax
cmpl 0x16c(%rsp), %eax
jge 0x5f4ea
xorps %xmm0, %xmm0
movsd %xmm0, 0xf8(%rsp)
leaq 0xe0(%rsp), %rdi
xorps %xmm2, %xmm2
movaps %xmm2, %xmm0
movaps %xmm2, %xmm1
callq 0x6fcb0
movl $0xfffffffe, 0xdc(%rsp) # imm = 0xFFFFFFFE
cmpl $0x2, 0xdc(%rsp)
jg 0x5f46a
leaq 0x108(%rsp), %rdi
movl $0x2, %esi
callq 0x36be0
movsd (%rax), %xmm0
cvtsi2sdl 0xdc(%rsp), %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, 0xd0(%rsp)
movl $0xfffffffe, 0xcc(%rsp) # imm = 0xFFFFFFFE
cmpl $0x2, 0xcc(%rsp)
jg 0x5f452
leaq 0x108(%rsp), %rdi
movl $0x1, %esi
callq 0x36be0
movsd (%rax), %xmm0
cvtsi2sdl 0xcc(%rsp), %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, 0xc0(%rsp)
movl $0xfffffffe, 0xbc(%rsp) # imm = 0xFFFFFFFE
cmpl $0x2, 0xbc(%rsp)
jg 0x5f43a
leaq 0x108(%rsp), %rdi
xorl %esi, %esi
callq 0x36be0
movq 0x38(%rsp), %rdi
movsd (%rax), %xmm0
cvtsi2sdl 0xbc(%rsp), %xmm1
addsd %xmm1, %xmm0
movsd %xmm0, 0xb0(%rsp)
movsd 0xb0(%rsp), %xmm0
movsd 0xc0(%rsp), %xmm1
movsd 0xd0(%rsp), %xmm2
callq 0x56e50
movsd %xmm0, 0xa8(%rsp)
movsd 0xa8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x5f422
movl 0xbc(%rsp), %eax
addl $0x2, %eax
movslq %eax, %rcx
leaq 0x11994f(%rip), %rax    # 0x178cb0
imulq $0xc8, %rcx, %rcx
addq %rcx, %rax
movl 0xcc(%rsp), %ecx
addl $0x2, %ecx
movslq %ecx, %rcx
imulq $0x28, %rcx, %rcx
addq %rcx, %rax
movl 0xdc(%rsp), %ecx
addl $0x2, %ecx
movslq %ecx, %rcx
movsd (%rax,%rcx,8), %xmm0
mulsd 0xa8(%rsp), %xmm0
movsd %xmm0, 0xa0(%rsp)
movsd 0xa0(%rsp), %xmm0
cvtsi2sdl 0xbc(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd 0xa0(%rsp), %xmm1
cvtsi2sdl 0xcc(%rsp), %xmm2
mulsd %xmm2, %xmm1
movsd 0xa0(%rsp), %xmm2
cvtsi2sdl 0xdc(%rsp), %xmm3
mulsd %xmm3, %xmm2
leaq 0x88(%rsp), %rdi
callq 0x6fcb0
leaq 0xe0(%rsp), %rdi
leaq 0x88(%rsp), %rsi
callq 0x70be0
movsd 0xa0(%rsp), %xmm0
addsd 0xf8(%rsp), %xmm0
movsd %xmm0, 0xf8(%rsp)
jmp 0x5f424
movl 0xbc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xbc(%rsp)
jmp 0x5f2d2
jmp 0x5f43c
movl 0xcc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xcc(%rsp)
jmp 0x5f28d
jmp 0x5f454
movl 0xdc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xdc(%rsp)
jmp 0x5f248
movsd 0xf8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x5f480
jp 0x5f480
jmp 0x5f4ea
leaq 0xe0(%rsp), %rsi
movq %rsi, %rdi
callq 0x702c0
leaq 0x119049(%rip), %rax    # 0x1784e0
movsd (%rax), %xmm1
callq 0x36b80
cmpl $0x0, %eax
je 0x5f4a7
jmp 0x5f4ea
movsd 0xf8(%rsp), %xmm0
leaq 0x70(%rsp), %rdi
leaq 0xe0(%rsp), %rsi
callq 0x711f0
leaq 0x108(%rsp), %rdi
leaq 0x70(%rsp), %rsi
callq 0x6f290
movl 0x104(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x104(%rsp)
jmp 0x5f207
leaq 0x108(%rsp), %rdi
xorl %esi, %esi
callq 0x36be0
movsd 0xd3b0f(%rip), %xmm0    # 0x133010
addsd (%rax), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x6c(%rsp)
leaq 0x108(%rsp), %rdi
movl $0x1, %esi
callq 0x36be0
movsd 0xd3ae9(%rip), %xmm0    # 0x133010
addsd (%rax), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x68(%rsp)
leaq 0x108(%rsp), %rdi
movl $0x2, %esi
callq 0x36be0
movsd 0xd3ac3(%rip), %xmm0    # 0x133010
addsd (%rax), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x64(%rsp)
cmpl $0x0, 0x6c(%rsp)
jge 0x5f568
movl $0x0, 0x6c(%rsp)
cmpl $0x0, 0x68(%rsp)
jge 0x5f577
movl $0x0, 0x68(%rsp)
cmpl $0x0, 0x64(%rsp)
jge 0x5f586
movl $0x0, 0x64(%rsp)
movq 0x38(%rsp), %rdi
movl 0x6c(%rsp), %eax
movl %eax, 0x24(%rsp)
callq 0x138c0
movl %eax, %ecx
movl 0x24(%rsp), %eax
cmpl %ecx, %eax
jl 0x5f5b3
movq 0x38(%rsp), %rdi
callq 0x138c0
subl $0x1, %eax
movl %eax, 0x6c(%rsp)
movq 0x38(%rsp), %rdi
movl 0x68(%rsp), %eax
movl %eax, 0x20(%rsp)
callq 0x138e0
movl %eax, %ecx
movl 0x20(%rsp), %eax
cmpl %ecx, %eax
jl 0x5f5e0
movq 0x38(%rsp), %rdi
callq 0x138e0
subl $0x1, %eax
movl %eax, 0x6c(%rsp)
movq 0x38(%rsp), %rdi
movl 0x64(%rsp), %eax
movl %eax, 0x1c(%rsp)
callq 0x13900
movl %eax, %ecx
movl 0x1c(%rsp), %eax
cmpl %ecx, %eax
jl 0x5f60d
movq 0x38(%rsp), %rdi
callq 0x13900
subl $0x1, %eax
movl %eax, 0x6c(%rsp)
movq 0x38(%rsp), %rdi
movl 0x6c(%rsp), %esi
movl 0x68(%rsp), %edx
movl 0x64(%rsp), %ecx
movq 0x130(%rsp), %r8
movslq 0x13c(%rsp), %rax
shlq $0x2, %rax
addq %rax, %r8
callq 0x6bd30
movl 0x13c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x13c(%rsp)
movl 0x124(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x124(%rsp)
jmp 0x5f16e
jmp 0x5f663
movl 0x128(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x128(%rsp)
jmp 0x5f140
jmp 0x5f67b
movl 0x12c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x12c(%rsp)
jmp 0x5f112
movl $0x0, 0x60(%rsp)
movl $0x0, 0x5c(%rsp)
movq 0x38(%rsp), %rdi
movl 0x5c(%rsp), %eax
movl %eax, 0x18(%rsp)
callq 0x137e0
movl %eax, %ecx
movl 0x18(%rsp), %eax
cmpl %ecx, %eax
jge 0x5f706
movq 0x130(%rsp), %rax
movslq 0x5c(%rsp), %rcx
movl (%rax,%rcx,4), %eax
cmpl 0x5c(%rsp), %eax
jne 0x5f6f7
movq 0x38(%rsp), %rdi
movl 0x5c(%rsp), %esi
movl 0x60(%rsp), %eax
addl $0x1, %eax
cvtsi2sd %eax, %xmm0
callq 0x13890
movl 0x60(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x60(%rsp)
jmp 0x5f6f9
movl 0x5c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x5c(%rsp)
jmp 0x5f6a1
movl $0x0, 0x58(%rsp)
movq 0x38(%rsp), %rdi
movl 0x58(%rsp), %eax
movl %eax, 0x14(%rsp)
callq 0x137e0
movl %eax, %ecx
movl 0x14(%rsp), %eax
cmpl %ecx, %eax
jge 0x5f7f0
movq 0x130(%rsp), %rax
movslq 0x58(%rsp), %rcx
cmpl $0x0, (%rax,%rcx,4)
jl 0x5f7de
movq 0x130(%rsp), %rax
movslq 0x58(%rsp), %rcx
movl (%rax,%rcx,4), %eax
movl %eax, 0x54(%rsp)
movl $0x0, 0x50(%rsp)
movq 0x38(%rsp), %rdi
movl 0x50(%rsp), %eax
movl %eax, 0x10(%rsp)
callq 0x137e0
movl %eax, %ecx
movl 0x10(%rsp), %eax
cmpl %ecx, %eax
jge 0x5f7b6
movl 0x54(%rsp), %eax
movq 0x130(%rsp), %rcx
movslq 0x54(%rsp), %rdx
cmpl (%rcx,%rdx,4), %eax
jne 0x5f795
jmp 0x5f7b6
movq 0x130(%rsp), %rax
movslq 0x54(%rsp), %rcx
movl (%rax,%rcx,4), %eax
movl %eax, 0x54(%rsp)
movl 0x50(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x50(%rsp)
jmp 0x5f761
movq 0x38(%rsp), %rdi
movl 0x54(%rsp), %esi
callq 0x13870
movq 0x38(%rsp), %rdi
movsd %xmm0, 0x48(%rsp)
movl 0x58(%rsp), %esi
movsd 0x48(%rsp), %xmm0
callq 0x13890
jmp 0x5f7e0
movl 0x58(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x58(%rsp)
jmp 0x5f70e
movq 0x130(%rsp), %rax
movq %rax, 0x8(%rsp)
cmpq $0x0, %rax
je 0x5f80d
movq 0x8(%rsp), %rdi
callq 0xf670
addq $0x178, %rsp            # imm = 0x178
retq
nopw %cs:(%rax,%rax)