// File: /workspace/github_cpp_makefile_star2_O3/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 %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x88, %rsp
movsd %xmm1, 0x8(%rsp)
movsd %xmm0, 0x30(%rsp)
movq %r8, 0x18(%rsp)
movq %rcx, %rbx
movq %rdx, %r12
movq %rdi, %rbp
movq 0xd8(%rsp), %r13
movq 0xd0(%rsp), %rax
movq %r9, 0x10(%rsp)
cmpl %r9d, 0xc(%rax)
jne 0x153ba
movslq 0x10(%rsp), %rax
movsd -0x8(%r13,%rax,8), %xmm0
movsd %xmm0, 0x8(%rsp)
cmpq $0x0, 0x8(%rsi)
movq %rsi, 0x40(%rsp)
je 0x15528
movq 0x18(%rsp), %rax
movsd (%rax), %xmm0
leaq 0xda7ce(%rip), %rax     # 0xefba8
movsd (%rax), %xmm2
movapd %xmm0, %xmm1
subsd 0x20(%r12), %xmm1
ucomisd %xmm2, %xmm1
ja 0x1541c
movsd 0x8(%r12), %xmm3
movapd %xmm0, %xmm4
subsd %xmm3, %xmm4
movapd 0x94c2a(%rip), %xmm5    # 0xaa030
xorpd %xmm2, %xmm5
xorpd %xmm1, %xmm1
ucomisd %xmm4, %xmm5
jbe 0x1541c
subsd %xmm0, %xmm3
movapd %xmm3, %xmm1
movapd %xmm1, %xmm0
mulsd %xmm1, %xmm0
ucomisd 0x8(%rsp), %xmm0
ja 0x15677
movq 0x18(%rsp), %rax
movsd 0x8(%rax), %xmm4
movapd %xmm4, %xmm3
subsd 0x28(%r12), %xmm3
ucomisd %xmm2, %xmm3
ja 0x15478
movsd 0x10(%r12), %xmm5
movapd %xmm4, %xmm6
subsd %xmm5, %xmm6
movapd 0x94bce(%rip), %xmm7    # 0xaa030
xorpd %xmm2, %xmm7
xorpd %xmm3, %xmm3
ucomisd %xmm6, %xmm7
jbe 0x15478
subsd %xmm4, %xmm5
movapd %xmm5, %xmm3
movapd %xmm3, %xmm4
mulsd %xmm3, %xmm4
ucomisd 0x8(%rsp), %xmm4
ja 0x15677
movq 0x18(%rsp), %rax
movsd 0x10(%rax), %xmm6
movapd %xmm6, %xmm5
subsd 0x30(%r12), %xmm5
ucomisd %xmm2, %xmm5
ja 0x154d3
movsd 0x18(%r12), %xmm7
movapd %xmm6, %xmm8
xorpd 0x94b75(%rip), %xmm2    # 0xaa030
subsd %xmm7, %xmm8
xorpd %xmm5, %xmm5
ucomisd %xmm8, %xmm2
jbe 0x154d3
subsd %xmm6, %xmm7
movapd %xmm7, %xmm5
movapd %xmm5, %xmm2
mulsd %xmm5, %xmm2
ucomisd 0x8(%rsp), %xmm2
ja 0x15677
xorpd %xmm6, %xmm6
ucomisd %xmm6, %xmm3
jne 0x154ff
jp 0x154ff
ucomisd %xmm6, %xmm5
jne 0x154ff
jnp 0x156ab
ucomisd %xmm6, %xmm1
jne 0x15689
jp 0x15689
ucomisd %xmm6, %xmm5
jne 0x15689
jp 0x15689
movapd %xmm4, %xmm0
jmp 0x156ab
cmpl $0x0, 0x128(%rsi)
jle 0x15677
cmpq $0x0, 0xc0(%rsp)
setne %al
testq %rbx, %rbx
setne %cl
andb %al, %cl
movb %cl, 0x28(%rsp)
movq 0x10(%rsp), %rax
decl %eax
movl %eax, 0x3c(%rsp)
xorl %r14d, %r14d
leaq 0x50(%rsp), %r12
movq %rbp, 0x48(%rsp)
movq 0x28(%rsi,%r14,8), %r15
movq %r12, %rdi
movq %rbp, %rsi
movq %r15, %rdx
callq 0x14f6e
movq 0x18(%rsp), %rdi
movq %r12, %rsi
callq 0x2070f
ucomisd 0x30(%rsp), %xmm0
jb 0x1565f
movapd %xmm0, %xmm1
movsd 0x8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jb 0x1565f
cmpb $0x0, 0x28(%rsp)
je 0x155d5
movq %rbx, %rdi
movq %r15, %rsi
movq 0xc8(%rsp), %rdx
movsd %xmm1, 0x20(%rsp)
callq *0xc0(%rsp)
movsd 0x20(%rsp), %xmm1
testl %eax, %eax
je 0x1565f
movq 0xd0(%rsp), %rax
movl 0xc(%rax), %eax
xorl %edx, %edx
testl %eax, %eax
jle 0x155fd
movsd (%r13,%rdx,8), %xmm0
ucomisd %xmm1, %xmm0
ja 0x155fd
incq %rdx
cmpq %rdx, %rax
jne 0x155e6
movl %eax, %edx
cmpl 0x10(%rsp), %edx
jge 0x1565f
movq %rbx, %rbp
cmpl 0x10(%rsp), %eax
cmovgel 0x3c(%rsp), %eax
cmpl %edx, %eax
jle 0x1562e
cltq
movslq %edx, %rcx
movsd -0x8(%r13,%rax,8), %xmm0
movsd %xmm0, (%r13,%rax,8)
decq %rax
cmpq %rcx, %rax
jg 0x15618
movl %edx, %eax
movsd %xmm1, (%r13,%rax,8)
movq 0xd0(%rsp), %rbx
movq %rbx, %rdi
movq %r15, %rsi
callq 0x7769e
movq %rbx, %rdi
movq 0x10(%rsp), %rsi
callq 0x778a0
movq %rbp, %rbx
movq 0x48(%rsp), %rbp
incq %r14
movq 0x40(%rsp), %rsi
movslq 0x128(%rsi), %rax
cmpq %rax, %r14
jl 0x15565
addq $0x88, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
xorpd %xmm5, %xmm5
ucomisd %xmm5, %xmm1
jne 0x156a3
jp 0x156a3
ucomisd %xmm5, %xmm3
jne 0x156a3
jp 0x156a3
movapd %xmm2, %xmm0
jmp 0x156ab
addsd %xmm4, %xmm0
addsd %xmm2, %xmm0
ucomisd 0x8(%rsp), %xmm0
ja 0x15677
movslq 0x20(%rsi), %rax
movq 0x18(%rsp), %rcx
movsd (%rcx,%rax,8), %xmm1
subsd 0x18(%rsi), %xmm1
xorpd %xmm0, %xmm0
ucomisd %xmm1, %xmm0
movapd %xmm1, %xmm2
mulsd %xmm1, %xmm2
jae 0x156e8
movsd 0x8(%rsp), %xmm0
ucomisd %xmm2, %xmm0
jb 0x15775
movsd %xmm2, 0x20(%rsp)
movsd %xmm1, 0x28(%rsp)
leaq 0x50(%rsp), %r15
movq %r15, %rdi
movq %rsi, %r14
movq %r12, %rsi
callq 0x3aa88
movsd 0x18(%r14), %xmm0
movslq 0x20(%r14), %rax
movsd %xmm0, 0x70(%rsp,%rax,8)
movq 0x8(%r14), %rsi
movq %rbp, %rdi
movq %r15, %rdx
movq %rbx, %rcx
movq 0x18(%rsp), %r8
movsd 0x30(%rsp), %xmm0
movsd 0x8(%rsp), %xmm1
movq 0x10(%rsp), %r9
pushq %r13
pushq 0xd8(%rsp)
pushq 0xd8(%rsp)
pushq 0xd8(%rsp)
callq 0x15362
addq $0x20, %rsp
leaq 0x50(%rsp), %rdi
callq 0x3b8a8
movq 0x40(%rsp), %rsi
movsd 0x28(%rsp), %xmm1
movsd 0x20(%rsp), %xmm2
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm1
jae 0x1578f
movsd 0x8(%rsp), %xmm0
ucomisd %xmm2, %xmm0
jb 0x15677
leaq 0x50(%rsp), %r15
movq %r15, %rdi
movq %rsi, %r14
movq %r12, %rsi
callq 0x3aa88
movsd 0x18(%r14), %xmm0
movslq 0x20(%r14), %rax
movsd %xmm0, 0x58(%rsp,%rax,8)
movq 0x10(%r14), %rsi
movq %rbp, %rdi
movq %r15, %rdx
movq %rbx, %rcx
movq 0x18(%rsp), %r8
movsd 0x30(%rsp), %xmm0
movsd 0x8(%rsp), %xmm1
movq 0x10(%rsp), %r9
pushq %r13
pushq 0xd8(%rsp)
pushq 0xd8(%rsp)
pushq 0xd8(%rsp)
callq 0x15362
addq $0x20, %rsp
leaq 0x50(%rsp), %rdi
callq 0x3b8a8
jmp 0x15677
jmp 0x15806
movq %rax, %rbx
leaq 0x50(%rsp), %rdi
callq 0x3b8a8
movq %rbx, %rdi
callq 0xe7f0