// File: tomfunkhouser[P]gaps/apps/msh2df/../../pkgs/R3Shapes/R3Kdtree.cpp
// Function: gaps::R3Kdtree<Point*>::FindClosest(gaps::R3KdtreeNode<Point*>*, gaps::R3Box const&, Point*, gaps::R3Point const&, double, double, int, int (*)(Point*, Point*, void*), void*, gaps::RNArray<Point*>&, double*) const
void R3Kdtree<PtrType>::
FindClosest(R3KdtreeNode<PtrType> *node, const R3Box& node_box, 
  PtrType query_point, const R3Point& query_position, 
  RNScalar min_distance_squared, RNScalar max_distance_squared, int max_points,
  int (*IsCompatible)(PtrType, PtrType, void *), void *compatible_data, 
  RNArray<PtrType>& points, RNLength *distances_squared) const
{
  // Update max distance squared
  if (points.NEntries() == max_points) {
    max_distance_squared = distances_squared[max_points-1];
  }

  // Check if node is interior
  if (node->children[0]) {
    assert(node->children[1]);

    // Find and check axial distances from point to node box
    RNLength dx, dy, dz;
    if (RNIsGreater(query_position.X(), node_box.XMax())) dx = query_position.X() - node_box.XMax();
    else if (RNIsLess(query_position.X(), node_box.XMin())) dx = node_box.XMin()- query_position.X();
    else dx = 0.0;
    RNLength dx_squared = dx * dx;
    if (dx_squared > max_distance_squared) return;
    if (RNIsGreater(query_position.Y(), node_box.YMax())) dy = query_position.Y() - node_box.YMax();
    else if (RNIsLess(query_position.Y(), node_box.YMin())) dy = node_box.YMin()- query_position.Y();
    else dy = 0.0;
    RNLength dy_squared = dy * dy;
    if (dy_squared > max_distance_squared) return;
    if (RNIsGreater(query_position.Z(), node_box.ZMax())) dz = query_position.Z() - node_box.ZMax();
    else if (RNIsLess(query_position.Z(), node_box.ZMin())) dz = node_box.ZMin()- query_position.Z();
    else dz = 0.0;
    RNLength dz_squared = dz * dz;
    if (dz_squared > max_distance_squared) return;
    
    // Find and check actual distance from point to node box
    RNLength distance_squared = 0;
    if ((dy == 0.0) && (dz == 0.0)) distance_squared = dx_squared;
    else if ((dx == 0.0) && (dz == 0.0)) distance_squared = dy_squared;
    else if ((dx == 0.0) && (dy == 0.0)) distance_squared = dz_squared;
    else distance_squared = dx_squared + dy_squared + dz_squared;
    if (distance_squared > max_distance_squared) return;

    // Compute distance from point to split plane
    RNLength side = query_position[node->split_dimension] - node->split_coordinate;

    // Search children nodes
    if ((side <= 0) || (side*side <= max_distance_squared)) {
      // Search negative side 
      R3Box child_box(node_box);
      child_box[RN_HI][node->split_dimension] = node->split_coordinate;
      FindClosest(node->children[0], child_box, query_point, query_position, 
        min_distance_squared, max_distance_squared, max_points, IsCompatible, compatible_data,
        points, distances_squared);
    }
    if ((side >= 0) || (side*side <= max_distance_squared)) {
      R3Box child_box(node_box);
      child_box[RN_LO][node->split_dimension] = node->split_coordinate;
      FindClosest(node->children[1], child_box, query_point, query_position, 
        min_distance_squared, max_distance_squared, max_points, IsCompatible, compatible_data,
        points, distances_squared);
    }
  }
  else {
    // Search points
    for (int i = 0; i < node->npoints; i++) {
      PtrType point = node->points[i];
      RNLength distance_squared = R3SquaredDistance(query_position, Position(point));
      if ((distance_squared >= min_distance_squared) && 
          (distance_squared <= max_distance_squared)) {

        // Check if point is compatible
        if (!IsCompatible || !query_point || IsCompatible(query_point, point, compatible_data)) {

          // Find slot for point (points are sorted by distance)
          int slot = 0;
          while (slot < points.NEntries()) {
            if (distance_squared < distances_squared[slot]) break;
            slot++;
          }
          
          // Insert point and distance into sorted arrays
          if (slot < max_points) {
            int first = points.NEntries();
            if (first >= max_points) first = max_points-1;
            for (int j = first; j > slot; j--) distances_squared[j] = distances_squared[j-1];
            distances_squared[slot] = distance_squared;
            points.InsertKth(point, slot);
            points.Truncate(max_points);
          }
        }
      }
    }
  }
}

// Assembly:
pushq %r14
pushq %rbx
subq $0x1f8, %rsp            # imm = 0x1F8
movq 0x228(%rsp), %rax
movq 0x220(%rsp), %rax
movq 0x218(%rsp), %rax
movq 0x210(%rsp), %rax
movq %rdi, 0x1f0(%rsp)
movq %rsi, 0x1e8(%rsp)
movq %rdx, 0x1e0(%rsp)
movq %rcx, 0x1d8(%rsp)
movq %r8, 0x1d0(%rsp)
movsd %xmm0, 0x1c8(%rsp)
movsd %xmm1, 0x1c0(%rsp)
movl %r9d, 0x1bc(%rsp)
movq 0x1f0(%rsp), %rax
movq %rax, 0xb0(%rsp)
movq 0x220(%rsp), %rdi
callq 0x178e0
cmpl 0x1bc(%rsp), %eax
jne 0x195c5
movq 0x228(%rsp), %rax
movl 0x1bc(%rsp), %ecx
subl $0x1, %ecx
movslq %ecx, %rcx
movsd (%rax,%rcx,8), %xmm0
movsd %xmm0, 0x1c0(%rsp)
movq 0x1e8(%rsp), %rax
cmpq $0x0, 0x8(%rax)
je 0x19d41
movq 0x1d0(%rsp), %rdi
callq 0x178f0
movsd %xmm0, 0xa8(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x1a010
movaps %xmm0, %xmm1
movsd 0xa8(%rsp), %xmm0
leaq 0x16cee2(%rip), %rax    # 0x1864f0
movsd (%rax), %xmm2
callq 0x19fe0
cmpl $0x0, %eax
je 0x1965d
movq 0x1d0(%rsp), %rdi
callq 0x178f0
movsd %xmm0, 0xa0(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x1a010
movaps %xmm0, %xmm1
movsd 0xa0(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x1b0(%rsp)
jmp 0x196ed
movq 0x1d0(%rsp), %rdi
callq 0x178f0
movsd %xmm0, 0x98(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x1a060
movaps %xmm0, %xmm1
movsd 0x98(%rsp), %xmm0
leaq 0x16ce5d(%rip), %rax    # 0x1864f0
movsd (%rax), %xmm2
callq 0x1a030
cmpl $0x0, %eax
je 0x196df
movq 0x1e0(%rsp), %rdi
callq 0x1a060
movsd %xmm0, 0x90(%rsp)
movq 0x1d0(%rsp), %rdi
callq 0x178f0
movaps %xmm0, %xmm1
movsd 0x90(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x1b0(%rsp)
jmp 0x196eb
xorps %xmm0, %xmm0
movsd %xmm0, 0x1b0(%rsp)
jmp 0x196ed
movsd 0x1b0(%rsp), %xmm0
mulsd 0x1b0(%rsp), %xmm0
movsd %xmm0, 0x198(%rsp)
movsd 0x198(%rsp), %xmm0
ucomisd 0x1c0(%rsp), %xmm0
jbe 0x19721
jmp 0x19fb9
movq 0x1d0(%rsp), %rdi
callq 0x17900
movsd %xmm0, 0x88(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x1a080
movaps %xmm0, %xmm1
movsd 0x88(%rsp), %xmm0
leaq 0x16cd99(%rip), %rax    # 0x1864f0
movsd (%rax), %xmm2
callq 0x19fe0
cmpl $0x0, %eax
je 0x197a6
movq 0x1d0(%rsp), %rdi
callq 0x17900
movsd %xmm0, 0x80(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x1a080
movaps %xmm0, %xmm1
movsd 0x80(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x1a8(%rsp)
jmp 0x1982a
movq 0x1d0(%rsp), %rdi
callq 0x17900
movsd %xmm0, 0x78(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x1a0a0
movaps %xmm0, %xmm1
movsd 0x78(%rsp), %xmm0
leaq 0x16cd1a(%rip), %rax    # 0x1864f0
movsd (%rax), %xmm2
callq 0x1a030
cmpl $0x0, %eax
je 0x1981c
movq 0x1e0(%rsp), %rdi
callq 0x1a0a0
movsd %xmm0, 0x70(%rsp)
movq 0x1d0(%rsp), %rdi
callq 0x17900
movaps %xmm0, %xmm1
movsd 0x70(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x1a8(%rsp)
jmp 0x19828
xorps %xmm0, %xmm0
movsd %xmm0, 0x1a8(%rsp)
jmp 0x1982a
movsd 0x1a8(%rsp), %xmm0
mulsd 0x1a8(%rsp), %xmm0
movsd %xmm0, 0x190(%rsp)
movsd 0x190(%rsp), %xmm0
ucomisd 0x1c0(%rsp), %xmm0
jbe 0x1985e
jmp 0x19fb9
movq 0x1d0(%rsp), %rdi
callq 0x17910
movsd %xmm0, 0x68(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x1a0c0
movaps %xmm0, %xmm1
movsd 0x68(%rsp), %xmm0
leaq 0x16cc62(%rip), %rax    # 0x1864f0
movsd (%rax), %xmm2
callq 0x19fe0
cmpl $0x0, %eax
je 0x198d7
movq 0x1d0(%rsp), %rdi
callq 0x17910
movsd %xmm0, 0x60(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x1a0c0
movaps %xmm0, %xmm1
movsd 0x60(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x1a0(%rsp)
jmp 0x1995b
movq 0x1d0(%rsp), %rdi
callq 0x17910
movsd %xmm0, 0x58(%rsp)
movq 0x1e0(%rsp), %rdi
callq 0x1a0e0
movaps %xmm0, %xmm1
movsd 0x58(%rsp), %xmm0
leaq 0x16cbe9(%rip), %rax    # 0x1864f0
movsd (%rax), %xmm2
callq 0x1a030
cmpl $0x0, %eax
je 0x1994d
movq 0x1e0(%rsp), %rdi
callq 0x1a0e0
movsd %xmm0, 0x50(%rsp)
movq 0x1d0(%rsp), %rdi
callq 0x17910
movaps %xmm0, %xmm1
movsd 0x50(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x1a0(%rsp)
jmp 0x19959
xorps %xmm0, %xmm0
movsd %xmm0, 0x1a0(%rsp)
jmp 0x1995b
movsd 0x1a0(%rsp), %xmm0
mulsd 0x1a0(%rsp), %xmm0
movsd %xmm0, 0x188(%rsp)
movsd 0x188(%rsp), %xmm0
ucomisd 0x1c0(%rsp), %xmm0
jbe 0x1998f
jmp 0x19fb9
xorps %xmm0, %xmm0
movsd %xmm0, 0x180(%rsp)
movsd 0x1a8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x199da
jp 0x199da
movsd 0x1a0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x199da
jp 0x199da
movsd 0x198(%rsp), %xmm0
movsd %xmm0, 0x180(%rsp)
jmp 0x19a7a
movsd 0x1b0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x19a16
jp 0x19a16
movsd 0x1a0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x19a16
jp 0x19a16
movsd 0x190(%rsp), %xmm0
movsd %xmm0, 0x180(%rsp)
jmp 0x19a78
movsd 0x1b0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x19a52
jp 0x19a52
movsd 0x1a8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x19a52
jp 0x19a52
movsd 0x188(%rsp), %xmm0
movsd %xmm0, 0x180(%rsp)
jmp 0x19a76
movsd 0x198(%rsp), %xmm0
addsd 0x190(%rsp), %xmm0
addsd 0x188(%rsp), %xmm0
movsd %xmm0, 0x180(%rsp)
jmp 0x19a78
jmp 0x19a7a
movsd 0x180(%rsp), %xmm0
ucomisd 0x1c0(%rsp), %xmm0
jbe 0x19a93
jmp 0x19fb9
movq 0x1d0(%rsp), %rdi
movq 0x1e8(%rsp), %rax
movl 0x20(%rax), %esi
callq 0x17e60
movq 0x1e8(%rsp), %rax
subsd 0x18(%rax), %xmm0
movsd %xmm0, 0x178(%rsp)
xorps %xmm0, %xmm0
ucomisd 0x178(%rsp), %xmm0
jae 0x19af4
movsd 0x178(%rsp), %xmm1
mulsd 0x178(%rsp), %xmm1
movsd 0x1c0(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0x19bfe
movq 0x1e0(%rsp), %rsi
leaq 0x140(%rsp), %rdi
movq %rdi, 0x48(%rsp)
callq 0x67100
movq 0x48(%rsp), %rdi
movq 0x1e8(%rsp), %rax
movsd 0x18(%rax), %xmm0
movsd %xmm0, 0x40(%rsp)
movl $0x1, %esi
callq 0x17820
movq %rax, %rdi
movq 0x1e8(%rsp), %rax
movl 0x20(%rax), %esi
callq 0x17800
movsd 0x40(%rsp), %xmm0
movq 0xb0(%rsp), %rdi
movq 0x48(%rsp), %rdx
movsd %xmm0, (%rax)
movq 0x1e8(%rsp), %rax
movq 0x8(%rax), %rsi
movq 0x1d8(%rsp), %rcx
movq 0x1d0(%rsp), %r8
movsd 0x1c8(%rsp), %xmm0
movsd 0x1c0(%rsp), %xmm1
movl 0x1bc(%rsp), %r9d
movq 0x210(%rsp), %r10
movq 0x218(%rsp), %r11
movq 0x220(%rsp), %rbx
movq 0x228(%rsp), %r14
movq %rsp, %rax
movq %r14, 0x18(%rax)
movq %rbx, 0x10(%rax)
movq %r11, 0x8(%rax)
movq %r10, (%rax)
callq 0x19510
jmp 0x19bc9
leaq 0x140(%rsp), %rdi
callq 0x17670
jmp 0x19bfe
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x138(%rsp)
movl %eax, 0x134(%rsp)
leaq 0x140(%rsp), %rdi
callq 0x17670
jmp 0x19fc4
movsd 0x178(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jae 0x19c35
movsd 0x178(%rsp), %xmm1
mulsd 0x178(%rsp), %xmm1
movsd 0x1c0(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0x19d3c
movq 0x1e0(%rsp), %rsi
leaq 0xf8(%rsp), %rdi
movq %rdi, 0x38(%rsp)
callq 0x67100
movq 0x38(%rsp), %rdi
movq 0x1e8(%rsp), %rax
movsd 0x18(%rax), %xmm0
movsd %xmm0, 0x30(%rsp)
xorl %esi, %esi
callq 0x17820
movq %rax, %rdi
movq 0x1e8(%rsp), %rax
movl 0x20(%rax), %esi
callq 0x17800
movsd 0x30(%rsp), %xmm0
movq 0xb0(%rsp), %rdi
movq 0x38(%rsp), %rdx
movsd %xmm0, (%rax)
movq 0x1e8(%rsp), %rax
movq 0x10(%rax), %rsi
movq 0x1d8(%rsp), %rcx
movq 0x1d0(%rsp), %r8
movsd 0x1c8(%rsp), %xmm0
movsd 0x1c0(%rsp), %xmm1
movl 0x1bc(%rsp), %r9d
movq 0x210(%rsp), %r10
movq 0x218(%rsp), %r11
movq 0x220(%rsp), %rbx
movq 0x228(%rsp), %r14
movq %rsp, %rax
movq %r14, 0x18(%rax)
movq %rbx, 0x10(%rax)
movq %r11, 0x8(%rax)
movq %r10, (%rax)
callq 0x19510
jmp 0x19d07
leaq 0xf8(%rsp), %rdi
callq 0x17670
jmp 0x19d3c
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x138(%rsp)
movl %eax, 0x134(%rsp)
leaq 0xf8(%rsp), %rdi
callq 0x17670
jmp 0x19fc4
jmp 0x19fb9
movl $0x0, 0xf4(%rsp)
movl 0xf4(%rsp), %eax
movq 0x1e8(%rsp), %rcx
cmpl 0x128(%rcx), %eax
jge 0x19fb7
movq 0xb0(%rsp), %rsi
movq 0x1e8(%rsp), %rax
movslq 0xf4(%rsp), %rcx
movq 0x28(%rax,%rcx,8), %rax
movq %rax, 0xe8(%rsp)
movq 0x1d0(%rsp), %rax
movq %rax, 0x28(%rsp)
movq 0xe8(%rsp), %rdx
leaq 0xc8(%rsp), %rdi
callq 0x18bc0
movq 0x28(%rsp), %rdi
leaq 0xc8(%rsp), %rsi
callq 0x304d0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm0
ucomisd 0x1c8(%rsp), %xmm0
jb 0x19f9f
movsd 0xe0(%rsp), %xmm1
movsd 0x1c0(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0x19f9f
cmpq $0x0, 0x210(%rsp)
je 0x19e3e
cmpq $0x0, 0x1d8(%rsp)
je 0x19e3e
movq 0x210(%rsp), %rax
movq 0x1d8(%rsp), %rdi
movq 0xe8(%rsp), %rsi
movq 0x218(%rsp), %rdx
callq *%rax
cmpl $0x0, %eax
je 0x19f9d
movl $0x0, 0xc4(%rsp)
movl 0xc4(%rsp), %eax
movl %eax, 0x24(%rsp)
movq 0x220(%rsp), %rdi
callq 0x178e0
movl %eax, %ecx
movl 0x24(%rsp), %eax
cmpl %ecx, %eax
jge 0x19ea4
movsd 0xe0(%rsp), %xmm1
movq 0x228(%rsp), %rax
movslq 0xc4(%rsp), %rcx
movsd (%rax,%rcx,8), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x19e91
jmp 0x19ea4
movl 0xc4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xc4(%rsp)
jmp 0x19e49
movl 0xc4(%rsp), %eax
cmpl 0x1bc(%rsp), %eax
jge 0x19f9b
movq 0x220(%rsp), %rdi
callq 0x178e0
movl %eax, 0xc0(%rsp)
movl 0xc0(%rsp), %eax
cmpl 0x1bc(%rsp), %eax
jl 0x19eed
movl 0x1bc(%rsp), %eax
subl $0x1, %eax
movl %eax, 0xc0(%rsp)
movl 0xc0(%rsp), %eax
movl %eax, 0xbc(%rsp)
movl 0xbc(%rsp), %eax
cmpl 0xc4(%rsp), %eax
jle 0x19f4d
movq 0x228(%rsp), %rax
movl 0xbc(%rsp), %ecx
subl $0x1, %ecx
movslq %ecx, %rcx
movsd (%rax,%rcx,8), %xmm0
movq 0x228(%rsp), %rax
movslq 0xbc(%rsp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl 0xbc(%rsp), %eax
addl $-0x1, %eax
movl %eax, 0xbc(%rsp)
jmp 0x19efb
movsd 0xe0(%rsp), %xmm0
movq 0x228(%rsp), %rax
movslq 0xc4(%rsp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movq 0x220(%rsp), %rdi
movq 0xe8(%rsp), %rsi
movl 0xc4(%rsp), %edx
callq 0x1a100
movq 0x220(%rsp), %rdi
movl 0x1bc(%rsp), %esi
callq 0xdefc0
jmp 0x19f9d
jmp 0x19f9f
jmp 0x19fa1
movl 0xf4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xf4(%rsp)
jmp 0x19d4c
jmp 0x19fb9
addq $0x1f8, %rsp            # imm = 0x1F8
popq %rbx
popq %r14
retq
movq 0x138(%rsp), %rdi
callq 0xf890
nopw %cs:(%rax,%rax)
nopl (%rax,%rax)