DAMASK_EICMD/code/kdtree2.f90

1926 lines
52 KiB
Fortran

!#############################################################################################################################
! BEGIN KDTREE2
!#############################################################################################################################
!(c) Matthew Kennel, Institute for Nonlinear Science (2004)
!
! Licensed under the Academic Free License version 1.1 found in file LICENSE
! with additional provisions found in that same file.
!
!#######################################################
! modifications: changed precision according to prec.f90
! k.komerla, m.diehl
!#######################################################
module kdtree2_priority_queue_module
use prec, only: pInt, &
pReal
!
! maintain a priority queue (PQ) of data, pairs of 'priority/payload',
! implemented with a binary heap. This is the type, and the 'dis' field
! is the priority.
!
implicit none
private
type kdtree2_result
! a pair of distances, indexes
real(pReal) :: dis = 0.0_pReal
integer(pInt) :: idx = -1_pInt !Initializers cause some bugs in compilers.
end type kdtree2_result
!
! A heap-based priority queue lets one efficiently implement the following
! operations, each in log(N) time, as opposed to linear time.
!
! 1) add a datum (push a datum onto the queue, increasing its length)
! 2) return the priority value of the maximum priority element
! 3) pop-off (and delete) the element with the maximum priority, decreasing
! the size of the queue.
! 4) replace the datum with the maximum priority with a supplied datum
! (of either higher or lower priority), maintaining the size of the
! queue.
!
!
! In the k-d tree case, the 'priority' is the square distance of a point in
! the data set to a reference point. The goal is to keep the smallest M
! distances to a reference point. The tree algorithm searches terminal
! nodes to decide whether to add points under consideration.
!
! A priority queue is useful here because it lets one quickly return the
! largest distance currently existing in the list. If a new candidate
! distance is smaller than this, then the new candidate ought to replace
! the old candidate. In priority queue terms, this means removing the
! highest priority element, and inserting the new one.
!
! Algorithms based on Cormen, Leiserson, Rivest, _Introduction
! to Algorithms_, 1990, with further optimization by the author.
!
! Originally informed by a C implementation by Sriranga Veeraraghavan.
!
! This module is not written in the most clear way, but is implemented such
! for speed, as it its operations will be called many times during searches
! of large numbers of neighbors.
!
type pq
!
! The priority queue consists of elements
! priority(1:heap_size), with associated payload(:).
!
! There are heap_size active elements.
! Assumes the allocation is always sufficient. Will NOT increase it
! to match.
integer(pInt) :: heap_size = 0_pInt
type(kdtree2_result), pointer :: elems(:)
end type pq
public :: kdtree2_result
public :: pq
public :: pq_create
public :: pq_delete, pq_insert
public :: pq_extract_max, pq_max, pq_replace_max, pq_maxpri
contains
function pq_create(results_in) result(res)
!
! Create a priority queue from ALREADY allocated
! array pointers for storage. NOTE! It will NOT
! add any alements to the heap, i.e. any existing
! data in the input arrays will NOT be used and may
! be overwritten.
!
! usage:
! real(pReal), pointer :: x(:)
! integer(pInt), pointer :: k(:)
! allocate(x(1000),k(1000))
! pq => pq_create(x,k)
!
implicit none
type(kdtree2_result), target:: results_in(:)
type(pq) :: res
!
!
integer :: nalloc
nalloc = size(results_in,1)
if (nalloc .lt. 1) then
write (*,*) 'PQ_CREATE: error, input arrays must be allocated.'
end if
res%elems => results_in
res%heap_size = 0_pInt
return
end function pq_create
!
! operations for getting parents and left + right children
! of elements in a binary heap.
!
!
! These are written inline for speed.
!
! integer(pInt) function parent(i)
! integer(pInt), intent(in) :: i
! parent = (i/2)
! return
! end function parent
! integer(pInt) function left(i)
! integer(pInt), intent(in) ::i
! left = (2*i)
! return
! end function left
! integer(pInt) function right(i)
! integer(pInt), intent(in) :: i
! right = (2*i)+1
! return
! end function right
! logical function compare_priority(p1,p2)
! real(pReal), intent(in) :: p1, p2
!
! compare_priority = (p1 .gt. p2)
! return
! end function compare_priority
subroutine heapify(a,i_in)
!
! take a heap rooted at 'i' and force it to be in the
! heap canonical form. This is performance critical
! and has been tweaked a little to reflect this.
!
implicit none
type(pq),pointer :: a
integer(pInt), intent(in) :: i_in
!
integer(pInt) :: i, l, r, largest
real(pReal) :: pri_i, pri_l, pri_r, pri_largest
type(kdtree2_result) :: temp
i = i_in
bigloop: do
l = 2_pInt*i ! left(i)
r = l+1_pInt ! right(i)
!
! set 'largest' to the index of either i, l, r
! depending on whose priority is largest.
!
! note that l or r can be larger than the heap size
! in which case they do not count.
! does left child have higher priority?
if (l .gt. a%heap_size) then
! we know that i is the largest as both l and r are invalid.
exit
else
pri_i = a%elems(i)%dis
pri_l = a%elems(l)%dis
if (pri_l .gt. pri_i) then
largest = l
pri_largest = pri_l
else
largest = i
pri_largest = pri_i
endif
!
! between i and l we have a winner
! now choose between that and r.
!
if (r .le. a%heap_size) then
pri_r = a%elems(r)%dis
if (pri_r .gt. pri_largest) then
largest = r
endif
endif
endif
if (largest .ne. i) then
! swap data in nodes largest and i, then heapify
temp = a%elems(i)
a%elems(i) = a%elems(largest)
a%elems(largest) = temp
!
! Canonical heapify() algorithm has tail-ecursive call:
!
! call heapify(a,largest)
! we will simulate with cycle
!
i = largest
cycle bigloop ! continue the loop
else
return ! break from the loop
end if
enddo bigloop
return
end subroutine heapify
subroutine pq_max(a,e)
!
! return the priority and its payload of the maximum priority element
! on the queue, which should be the first one, if it is
! in heapified form.
!
use IO, only: IO_error
implicit none
type(pq),pointer :: a
type(kdtree2_result),intent(out) :: e
if (a%heap_size .gt. 0) then
e = a%elems(1)
else
call IO_error (460_pInt, ext_msg='PQ_MAX: heap_size < 1')
endif
return
end subroutine pq_max
real(pReal) function pq_maxpri(a)
use IO, only: IO_error
implicit none
type(pq), pointer :: a
if (a%heap_size .gt. 0) then
pq_maxpri = a%elems(1)%dis
else
call IO_error (460_pInt,ext_msg='PPQ_MAX_PRI: heap_size < 1')
endif
return
end function pq_maxpri
subroutine pq_extract_max(a,e)
!
! return the priority and payload of maximum priority
! element, and remove it from the queue.
! (equivalent to 'pop()' on a stack)
!
use IO, only: IO_error
implicit none
type(pq),pointer :: a
type(kdtree2_result), intent(out) :: e
if (a%heap_size .ge. 1) then
!
! return max as first element
!
e = a%elems(1)
!
! move last element to first
!
a%elems(1) = a%elems(a%heap_size)
a%heap_size = a%heap_size-1_pInt
call heapify(a,1_pInt)
return
else
call IO_error (460_pInt,ext_msg='PQ_EXTRACT_MAX: attempted to pop non-positive PQ')
end if
end subroutine pq_extract_max
real(pReal) function pq_insert(a,dis,idx)
!
! Insert a new element and return the new maximum priority,
! which may or may not be the same as the old maximum priority.
!
implicit none
type(pq),pointer :: a
real(pReal), intent(in) :: dis
integer(pInt), intent(in) :: idx
! type(kdtree2_result), intent(in) :: e
!
integer(pInt) :: i, isparent
real(pReal) :: parentdis
!
! if (a%heap_size .ge. a%max_elems) then
! write (*,*) 'PQ_INSERT: error, attempt made to insert element on full PQ'
! stop
! else
a%heap_size = a%heap_size + 1_pInt
i = a%heap_size
do while (i .gt. 1)
isparent = int(i/2_pInt, pInt) !needed casting?
parentdis = a%elems(isparent)%dis
if (dis .gt. parentdis) then
! move what was in i's parent into i.
a%elems(i)%dis = parentdis
a%elems(i)%idx = a%elems(isparent)%idx
i = isparent
else
exit
endif
end do
! insert the element at the determined position
a%elems(i)%dis = dis
a%elems(i)%idx = idx
pq_insert = a%elems(1)%dis
return
! end if
end function pq_insert
subroutine pq_adjust_heap(a,i)
implicit none
type(pq), pointer :: a
integer(pInt), intent(in) :: i
!
! nominally arguments (a,i), but specialize for a=1
!
! This routine assumes that the trees with roots 2 and 3 are already heaps, i.e.
! the children of '1' are heaps. When the procedure is completed, the
! tree rooted at 1 is a heap.
real(pReal) :: prichild
integer(pInt) :: parent, child, N
type(kdtree2_result) :: e
e = a%elems(i)
parent = i
child = 2_pInt*i
N = a%heap_size
do while (child .le. N)
if (child .lt. N) then
if (a%elems(child)%dis .lt. a%elems(child+1)%dis) then
child = child+1_pInt
endif
endif
prichild = a%elems(child)%dis
if (e%dis .ge. prichild) then
exit
else
! move child into parent.
a%elems(parent) = a%elems(child)
parent = child
child = 2_pInt*parent
end if
end do
a%elems(parent) = e
return
end subroutine pq_adjust_heap
real(pReal) function pq_replace_max(a,dis,idx)
!
! Replace the extant maximum priority element
! in the PQ with (dis,idx). Return
! the new maximum priority, which may be larger
! or smaller than the old one.
!
implicit none
type(pq),pointer :: a
real(pReal), intent(in) :: dis
integer(pInt), intent(in) :: idx
!type(kdtree2_result), intent(in) :: e
! not tested as well!
integer(pInt) :: parent, child, N
real(pReal) :: prichild, prichildp1
type(kdtree2_result) :: etmp
if (.true.) then
N=a%heap_size
if (N .ge. 1) then
parent =1_pInt
child=2_pInt
loop: do while (child .le. N)
prichild = a%elems(child)%dis
!
! posibly child+1 has higher priority, and if
! so, get it, and increment child.
!
if (child .lt. N) then
prichildp1 = a%elems(child+1_pInt)%dis
if (prichild .lt. prichildp1) then
child = child+1_pInt
prichild = prichildp1
endif
endif
if (dis .ge. prichild) then
exit loop
! we have a proper place for our new element,
! bigger than either children's priority.
else
! move child into parent.
a%elems(parent) = a%elems(child)
parent = child
child = 2_pInt*parent
end if
end do loop
a%elems(parent)%dis = dis
a%elems(parent)%idx = idx
pq_replace_max = a%elems(1)%dis
else
a%elems(1)%dis = dis
a%elems(1)%idx = idx
pq_replace_max = dis
endif
else
!
! slower version using elementary pop and push operations.
!
call pq_extract_max(a,etmp)
etmp%dis = dis
etmp%idx = idx
pq_replace_max = pq_insert(a,dis,idx)
endif
return
end function pq_replace_max
subroutine pq_delete(a,i)
!
! delete item with index 'i'
!
use IO, only: IO_error
implicit none
type(pq),pointer :: a
integer(pInt) :: i
if ((i .lt. 1) .or. (i .gt. a%heap_size)) then
call IO_error (460_pInt,ext_msg='PQ_DELETE: attempt to remove out of bounds element')
endif
! swap the item to be deleted with the last element
! and shorten heap by one.
a%elems(i) = a%elems(a%heap_size)
a%heap_size = a%heap_size - 1_pInt
call heapify(a,i)
end subroutine pq_delete
end module kdtree2_priority_queue_module
module kdtree2_module
use prec
use kdtree2_priority_queue_module
implicit none
private
! K-D tree routines in Fortran 90 by Matt Kennel.
! Original program was written in Sather by Steve Omohundro and
! Matt Kennel. Only the Euclidean metric is supported.
!
!
! This module is identical to 'kd_tree', except that the order
! of subscripts is reversed in the data file.
! In otherwords for an embedding of N D-dimensional vectors, the
! data file is here, in natural Fortran order myData(1:D, 1:N)
! because Fortran lays out columns first,
!
! whereas conventionally (C-style) it is myData(1:N,1:D)
! as in the original kd_tree module.
!
!-------------DATA TYPE, CREATION, DELETION---------------------
public :: pReal
public :: kdtree2, kdtree2_result, tree_node, kdtree2_create, kdtree2_destroy
!---------------------------------------------------------------
!-------------------SEARCH ROUTINES-----------------------------
public :: kdtree2_n_nearest,kdtree2_n_nearest_around_point
! Return fixed number of nearest neighbors around arbitrary vector,
! or extant point in dataset, with decorrelation window.
!
public :: kdtree2_r_nearest, kdtree2_r_nearest_around_point
! Return points within a fixed ball of arb vector/extant point
!
public :: kdtree2_sort_results
! Sort, in order of increasing distance, rseults from above.
!
public :: kdtree2_r_count, kdtree2_r_count_around_point
! Count points within a fixed ball of arb vector/extant point
!
public :: kdtree2_n_nearest_brute_force, kdtree2_r_nearest_brute_force
! brute force of kdtree2_[n|r]_nearest
!----------------------------------------------------------------
integer(pInt), parameter :: bucket_size = 12_pInt
! The maximum number of points to keep in a terminal node.
type interval
real(pReal) :: lower,upper
end type interval
type :: tree_node
! an internal tree node
private
integer(pInt) :: cut_dim
! the dimension to cut
real(pReal) :: cut_val
! where to cut the dimension
real(pReal) :: cut_val_left, cut_val_right
! improved cutoffs knowing the spread in child boxes.
integer(pInt) :: l, u
type (tree_node), pointer :: left, right
type(interval), pointer :: box(:) => null()
! child pointers
! Points included in this node are indexes[k] with k \in [l,u]
end type tree_node
type :: kdtree2
! Global information about the tree, one per tree
integer(pInt) :: dimen=0_pInt, n=0_pInt
! dimensionality and total # of points
real(pReal), pointer :: the_data(:,:) => null()
! pointer to the actual data array
!
! IMPORTANT NOTE: IT IS DIMENSIONED the_data(1:d,1:N)
! which may be opposite of what may be conventional.
! This is, because in Fortran, the memory layout is such that
! the first dimension is in sequential order. Hence, with
! (1:d,1:N), all components of the vector will be in consecutive
! memory locations. The search time is dominated by the
! evaluation of distances in the terminal nodes. Putting all
! vector components in consecutive memory location improves
! memory cache locality, and hence search speed, and may enable
! vectorization on some processors and compilers.
integer(pInt), pointer :: ind(:) => null()
! permuted index into the data, so that indexes[l..u] of some
! bucket represent the indexes of the actual points in that
! bucket.
logical :: sort = .false.
! do we always sort output results?
logical :: rearrange = .false.
real(pReal), pointer :: rearranged_data(:,:) => null()
! if (rearrange .eqv. .true.) then rearranged_data has been
! created so that rearranged_data(:,i) = the_data(:,ind(i)),
! permitting search to use more cache-friendly rearranged_data, at
! some initial computation and storage cost.
type (tree_node), pointer :: root => null()
! root pointer of the tree
end type kdtree2
type :: tree_search_record
!
! One of these is created for each search.
!
private
!
! Many fields are copied from the tree structure, in order to
! speed up the search.
!
integer(pInt) :: dimen
integer(pInt) :: nn, nfound
real(pReal) :: ballsize
integer(pInt) :: centeridx=999_pInt, correltime=9999_pInt
! exclude points within 'correltime' of 'centeridx', iff centeridx >= 0
integer(pInt) :: nalloc ! how much allocated for results(:)?
logical :: rearrange ! are the data rearranged or original?
! did the # of points found overflow the storage provided?
logical :: overflow
real(pReal), pointer :: qv(:) ! query vector
type(kdtree2_result), pointer :: results(:) ! results
type(pq) :: pq
real(pReal), pointer :: myData(:,:) ! temp pointer to data
integer(pInt), pointer :: ind(:) ! temp pointer to indexes
end type tree_search_record
type(tree_search_record), save, target :: sr ! A GLOBAL VARIABLE for search
contains
function kdtree2_create(input_data,myDim,sort,rearrange) result (mr)
!
! create the actual tree structure, given an input array of data.
!
! Note, input data is input_data(1:d,1:N), NOT the other way around.
! THIS IS THE REVERSE OF THE PREVIOUS VERSION OF THIS MODULE.
! The reason for it is cache friendliness, improving performance.
!
! Optional arguments: If 'myDim' is specified, then the tree
! will only search the first 'myDim' components
! of input_data, otherwise, myDim is inferred
! from SIZE(input_data,1).
!
! if sort .eqv. .true. then output results
! will be sorted by increasing distance.
! default=.false., as it is faster to not sort.
!
! if rearrange .eqv. .true. then an internal
! copy of the data, rearranged by terminal node,
! will be made for cache friendliness.
! default=.true., as it speeds searches, but
! building takes longer, and extra memory is used.
!
! .. Function Return Cut_value ..
use IO, only: IO_error
implicit none
type (kdtree2), pointer :: mr
integer(pInt), intent(in), optional :: myDim
logical, intent(in), optional :: sort
logical, intent(in), optional :: rearrange
! ..
! .. Array Arguments ..
real(pReal), target :: input_data(:,:)
!
integer(pInt) :: i
! ..
allocate (mr)
mr%the_data => input_data
! pointer assignment
if (present(myDim)) then
mr%dimen = myDim
else
mr%dimen = int(size(input_data,1), pInt) ! size returns default integer
end if
mr%n = int(size(input_data,2), pInt) ! size returns default integer
if (mr%dimen > mr%n) then
! unlikely to be correct
write (*,*) 'KD_TREE_TRANS: likely user error.'
write (*,*) 'KD_TREE_TRANS: You passed in matrix with D=',mr%dimen
write (*,*) 'KD_TREE_TRANS: and N=',mr%n
write (*,*) 'KD_TREE_TRANS: note, that new format is myData(1:D,1:N)'
write (*,*) 'KD_TREE_TRANS: with usually N >> D. If N =approx= D, then a k-d tree'
write (*,*) 'KD_TREE_TRANS: is not an appropriate data structure.'
call IO_error (460_pInt)
end if
call build_tree(mr)
if (present(sort)) then
mr%sort = sort
else
mr%sort = .false.
endif
if (present(rearrange)) then
mr%rearrange = rearrange
else
mr%rearrange = .true.
endif
if (mr%rearrange) then
allocate(mr%rearranged_data(mr%dimen,mr%n))
do i=1_pInt,mr%n
mr%rearranged_data(:,i) = mr%the_data(:, &
mr%ind(i))
enddo
else
nullify(mr%rearranged_data)
endif
end function kdtree2_create
subroutine build_tree(tp)
implicit none
type (kdtree2), pointer :: tp
! ..
integer(pInt) :: j
type(tree_node), pointer :: dummy => null()
! ..
allocate (tp%ind(tp%n))
forall (j=1_pInt:tp%n)
tp%ind(j) = j
end forall
tp%root => build_tree_for_range(tp,1_pInt,tp%n, dummy)
end subroutine build_tree
recursive function build_tree_for_range(tp,l,u,parent) result (res)
! .. Function Return Cut_value ..
implicit none
type (tree_node), pointer :: res
! ..
! .. Structure Arguments ..
type (kdtree2), pointer :: tp
type (tree_node),pointer :: parent
! ..
! .. Scalar Arguments ..
integer(pInt), intent (In) :: l, u
! ..
! .. Local Scalars ..
integer(pInt) :: i, c, m, dimen
logical :: recompute
real(pReal) :: average
!!$ If (.False.) Then
!!$ If ((l .Lt. 1) .Or. (l .Gt. tp%n)) Then
!!$ Stop 'illegal L value in build_tree_for_range'
!!$ End If
!!$ If ((u .Lt. 1) .Or. (u .Gt. tp%n)) Then
!!$ Stop 'illegal u value in build_tree_for_range'
!!$ End If
!!$ If (u .Lt. l) Then
!!$ Stop 'U is less than L, thats illegal.'
!!$ End If
!!$ Endif
!!$
! first compute min and max
dimen = tp%dimen
allocate (res)
allocate(res%box(dimen))
! First, compute an APPROXIMATE bounding box of all points associated with this node.
if ( u < l ) then
! no points in this box
nullify(res)
return
end if
if ((u-l)<=bucket_size) then
!
! always compute true bounding box for terminal nodes.
!
do i=1_pInt,dimen
call spread_in_coordinate(tp,i,l,u,res%box(i))
end do
res%cut_dim = 0_pInt
res%cut_val = 0.0_pReal
res%l = l
res%u = u
res%left =>null()
res%right => null()
else
!
! modify approximate bounding box. This will be an
! overestimate of the true bounding box, as we are only recomputing
! the bounding box for the dimension that the parent split on.
!
! Going to a true bounding box computation would significantly
! increase the time necessary to build the tree, and usually
! has only a very small difference. This box is not used
! for searching but only for deciding which coordinate to split on.
!
do i=1_pInt,dimen
recompute=.true.
if (associated(parent)) then
if (i .ne. parent%cut_dim) then
recompute=.false.
end if
endif
if (recompute) then
call spread_in_coordinate(tp,i,l,u,res%box(i))
else
res%box(i) = parent%box(i)
endif
end do
c = int(maxloc(res%box(1:dimen)%upper-res%box(1:dimen)%lower,1), pInt)
!
! c is the identity of which coordinate has the greatest spread.
!
if (.false.) then
! select exact median to have fully balanced tree.
m = (l+u)/2_pInt
call select_on_coordinate(tp%the_data,tp%ind,c,m,l,u)
else
!
! select point halfway between min and max, as per A. Moore,
! who says this helps in some degenerate cases, or
! actual arithmetic average.
!
if (.true.) then
! actually compute average
average = sum(tp%the_data(c,tp%ind(l:u))) / real(u-l+1_pInt,pReal)
else
average = (res%box(c)%upper + res%box(c)%lower)/2.0_pReal
endif
res%cut_val = average
m = select_on_coordinate_value(tp%the_data,tp%ind,c,average,l,u)
endif
! moves indexes around
res%cut_dim = c
res%l = l
res%u = u
! res%cut_val = tp%the_data(c,tp%ind(m))
res%left => build_tree_for_range(tp,l,m,res)
res%right => build_tree_for_range(tp,m+1_pInt,u,res)
if (associated(res%right) .eqv. .false.) then
res%box = res%left%box
res%cut_val_left = res%left%box(c)%upper
res%cut_val = res%cut_val_left
elseif (associated(res%left) .eqv. .false.) then
res%box = res%right%box
res%cut_val_right = res%right%box(c)%lower
res%cut_val = res%cut_val_right
else
res%cut_val_right = res%right%box(c)%lower
res%cut_val_left = res%left%box(c)%upper
res%cut_val = (res%cut_val_left + res%cut_val_right)/2
! now remake the true bounding box for self.
! Since we are taking unions (in effect) of a tree structure,
! this is much faster than doing an exhaustive
! search over all points
res%box%upper = max(res%left%box%upper,res%right%box%upper)
res%box%lower = min(res%left%box%lower,res%right%box%lower)
endif
end if
end function build_tree_for_range
integer(pInt) function select_on_coordinate_value(v,ind,c,alpha,li,ui) result(res)
! Move elts of ind around between l and u, so that all points
! <= than alpha (in c cooordinate) are first, and then
! all points > alpha are second.
!
! Algorithm (matt kennel).
!
! Consider the list as having three parts: on the left,
! the points known to be <= alpha. On the right, the points
! known to be > alpha, and in the middle, the currently unknown
! points. The algorithm is to scan the unknown points, starting
! from the left, and swapping them so that they are added to
! the left stack or the right stack, as appropriate.
!
! The algorithm finishes when the unknown stack is empty.
!
! .. Scalar Arguments ..
implicit none
integer(pInt), intent (In) :: c, li, ui
real(pReal), intent(in) :: alpha
! ..
real(pReal) :: v(1:,1:)
integer(pInt) :: ind(1:)
integer(pInt) :: tmp
! ..
integer(pInt) :: lb, rb
!
! The points known to be <= alpha are in
! [l,lb-1]
!
! The points known to be > alpha are in
! [rb+1,u].
!
! Therefore we add new points into lb or
! rb as appropriate. When lb=rb
! we are done. We return the location of the last point <= alpha.
!
!
lb = li; rb = ui
do while (lb < rb)
if ( v(c,ind(lb)) <= alpha ) then
! it is good where it is.
lb = lb+1_pInt
else
! swap it with rb.
tmp = ind(lb); ind(lb) = ind(rb); ind(rb) = tmp
rb = rb-1_pInt
endif
end do
! now lb .eq. ub
if (v(c,ind(lb)) <= alpha) then
res = lb
else
res = lb-1_pInt
endif
end function select_on_coordinate_value
subroutine select_on_coordinate(v,ind,c,k,li,ui)
! Move elts of ind around between l and u, so that the kth
! element
! is >= those below, <= those above, in the coordinate c.
! .. Scalar Arguments ..
implicit none
integer(pInt), intent (In) :: c, k, li, ui
! ..
integer(pInt) :: i, l, m, s, t, u
! ..
real(pReal) :: v(:,:)
integer(pInt) :: ind(:)
! ..
l = li
u = ui
do while (l<u)
t = ind(l)
m = l
do i = l + 1_pInt, u
if (v(c,ind(i))<v(c,t)) then
m = m + 1_pInt
s = ind(m)
ind(m) = ind(i)
ind(i) = s
end if
end do
s = ind(l)
ind(l) = ind(m)
ind(m) = s
if (m<=k) l = m + 1_pInt
if (m>=k) u = m - 1_pInt
end do
end subroutine select_on_coordinate
subroutine spread_in_coordinate(tp,c,l,u,interv)
! the spread in coordinate 'c', between l and u.
!
! Return lower bound in 'smin', and upper in 'smax',
! ..
! .. Structure Arguments ..
implicit none
type (kdtree2), pointer :: tp
type(interval), intent(out) :: interv
! ..
! .. Scalar Arguments ..
integer(pInt), intent (In) :: c, l, u
! ..
! .. Local Scalars ..
real(pReal) :: last, lmax, lmin, t, smin,smax
integer(pInt) :: i, ulocal
! ..
! .. Local Arrays ..
real(pReal), pointer :: v(:,:)
integer(pInt), pointer :: ind(:)
! ..
v => tp%the_data(1:,1:)
ind => tp%ind(1:)
smin = v(c,ind(l))
smax = smin
ulocal = u
do i = l + 2_pInt, ulocal, 2_pInt
lmin = v(c,ind(i-1_pInt))
lmax = v(c,ind(i))
if (lmin>lmax) then
t = lmin
lmin = lmax
lmax = t
end if
if (smin>lmin) smin = lmin
if (smax<lmax) smax = lmax
end do
if (i==ulocal+1) then
last = v(c,ind(ulocal))
if (smin>last) smin = last
if (smax<last) smax = last
end if
interv%lower = smin
interv%upper = smax
end subroutine spread_in_coordinate
subroutine kdtree2_destroy(tp)
! Deallocates all memory for the tree, except input data matrix
! .. Structure Arguments ..
implicit none
type (kdtree2), pointer :: tp
! ..
call destroy_node(tp%root)
deallocate (tp%ind)
nullify (tp%ind)
if (tp%rearrange) then
deallocate(tp%rearranged_data)
nullify(tp%rearranged_data)
endif
deallocate(tp)
return
contains
recursive subroutine destroy_node(np)
! .. Structure Arguments ..
implicit none
type (tree_node), pointer :: np
! ..
! .. Intrinsic Functions ..
intrinsic ASSOCIATED
! ..
if (associated(np%left)) then
call destroy_node(np%left)
nullify (np%left)
end if
if (associated(np%right)) then
call destroy_node(np%right)
nullify (np%right)
end if
if (associated(np%box)) deallocate(np%box)
deallocate(np)
return
end subroutine destroy_node
end subroutine kdtree2_destroy
subroutine kdtree2_n_nearest(tp,qv,nn,results)
! Find the 'nn' vectors in the tree nearest to 'qv' in euclidean norm
! returning their indexes and distances in 'indexes' and 'distances'
! arrays already allocated passed to this subroutine.
implicit none
type (kdtree2), pointer :: tp
real(pReal), target, intent (In) :: qv(:)
integer(pInt), intent (In) :: nn
type(kdtree2_result), target :: results(:)
sr%ballsize = huge(1.0_pReal)
sr%qv => qv
sr%nn = nn
sr%nfound = 0_pInt
sr%centeridx = -1_pInt
sr%correltime = 0_pInt
sr%overflow = .false.
sr%results => results
sr%nalloc = nn ! will be checked
sr%ind => tp%ind
sr%rearrange = tp%rearrange
if (tp%rearrange) then
sr%myData => tp%rearranged_data
else
sr%myData => tp%the_data
endif
sr%dimen = tp%dimen
call validate_query_storage(nn)
sr%pq = pq_create(results)
call search(tp%root)
if (tp%sort) then
call kdtree2_sort_results(nn, results)
endif
! deallocate(sr%pqp)
return
end subroutine kdtree2_n_nearest
subroutine kdtree2_n_nearest_around_point(tp,idxin,correltime,nn,results)
! Find the 'nn' vectors in the tree nearest to point 'idxin',
! with correlation window 'correltime', returing results in
! results(:), which must be pre-allocated upon entry.
implicit none
type (kdtree2), pointer :: tp
integer(pInt), intent (In) :: idxin, correltime, nn
type(kdtree2_result), target :: results(:)
allocate (sr%qv(tp%dimen))
sr%qv = tp%the_data(:,idxin) ! copy the vector
sr%ballsize = huge(1.0_pReal) ! the largest real(pReal) number
sr%centeridx = idxin
sr%correltime = correltime
sr%nn = nn
sr%nfound = 0_pInt
sr%dimen = tp%dimen
sr%nalloc = nn
sr%results => results
sr%ind => tp%ind
sr%rearrange = tp%rearrange
if (sr%rearrange) then
sr%myData => tp%rearranged_data
else
sr%myData => tp%the_data
endif
call validate_query_storage(nn)
sr%pq = pq_create(results)
call search(tp%root)
if (tp%sort) then
call kdtree2_sort_results(nn, results)
endif
deallocate (sr%qv)
return
end subroutine kdtree2_n_nearest_around_point
subroutine kdtree2_r_nearest(tp,qv,r2,nfound,nalloc,results)
! find the nearest neighbors to point 'idxin', within SQUARED
! Euclidean distance 'r2'. Upon ENTRY, nalloc must be the
! size of memory allocated for results(1:nalloc). Upon
! EXIT, nfound is the number actually found within the ball.
!
! Note that if nfound .gt. nalloc then more neighbors were found
! than there were storage to store. The resulting list is NOT
! the smallest ball inside norm r^2
!
! Results are NOT sorted unless tree was created with sort option.
implicit none
type (kdtree2), pointer :: tp
real(pReal), target, intent (In) :: qv(:)
real(pReal), intent(in) :: r2
integer(pInt), intent(out) :: nfound
integer(pInt), intent (In) :: nalloc
type(kdtree2_result), target :: results(:)
!
sr%qv => qv
sr%ballsize = r2
sr%nn = 0_pInt ! flag for fixed ball search
sr%nfound = 0_pInt
sr%centeridx = -1_pInt
sr%correltime = 0_pInt
sr%results => results
call validate_query_storage(nalloc)
sr%nalloc = nalloc
sr%overflow = .false.
sr%ind => tp%ind
sr%rearrange= tp%rearrange
if (tp%rearrange) then
sr%myData => tp%rearranged_data
else
sr%myData => tp%the_data
endif
sr%dimen = tp%dimen
!
!sr%dsl = Huge(sr%dsl) ! set to huge positive values
!sr%il = -1 ! set to invalid indexes
!
call search(tp%root)
nfound = sr%nfound
if (tp%sort) then
call kdtree2_sort_results(nfound, results)
endif
if (sr%overflow) then
write (*,*) 'KD_TREE_TRANS: warning! return from kdtree2_r_nearest found more neighbors'
write (*,*) 'KD_TREE_TRANS: than storage was provided for. Answer is NOT smallest ball'
write (*,*) 'KD_TREE_TRANS: with that number of neighbors! I.e. it is wrong.'
endif
return
end subroutine kdtree2_r_nearest
subroutine kdtree2_r_nearest_around_point(tp,idxin,correltime,r2,nfound,nalloc,results)
!
! Like kdtree2_r_nearest, but around a point 'idxin' already existing
! in the data set.
!
! Results are NOT sorted unless tree was created with sort option.
!
implicit none
type (kdtree2), pointer :: tp
integer(pInt), intent (In) :: idxin, correltime, nalloc
real(pReal), intent(in) :: r2
integer(pInt), intent(out) :: nfound
type(kdtree2_result), target :: results(:)
! ..
! .. Intrinsic Functions ..
intrinsic HUGE
! ..
allocate (sr%qv(tp%dimen))
sr%qv = tp%the_data(:,idxin) ! copy the vector
sr%ballsize = r2
sr%nn = 0_pInt ! flag for fixed r search
sr%nfound = 0_pInt
sr%centeridx = idxin
sr%correltime = correltime
sr%results => results
sr%nalloc = nalloc
sr%overflow = .false.
call validate_query_storage(nalloc)
! sr%dsl = HUGE(sr%dsl) ! set to huge positive values
! sr%il = -1 ! set to invalid indexes
sr%ind => tp%ind
sr%rearrange = tp%rearrange
if (tp%rearrange) then
sr%myData => tp%rearranged_data
else
sr%myData => tp%the_data
endif
sr%rearrange = tp%rearrange
sr%dimen = tp%dimen
!
!sr%dsl = Huge(sr%dsl) ! set to huge positive values
!sr%il = -1 ! set to invalid indexes
!
call search(tp%root)
nfound = sr%nfound
if (tp%sort) then
call kdtree2_sort_results(nfound,results)
endif
if (sr%overflow) then
write (*,*) 'KD_TREE_TRANS: warning! return from kdtree2_r_nearest found more neighbors'
write (*,*) 'KD_TREE_TRANS: than storage was provided for. Answer is NOT smallest ball'
write (*,*) 'KD_TREE_TRANS: with that number of neighbors! I.e. it is wrong.'
endif
deallocate (sr%qv)
return
end subroutine kdtree2_r_nearest_around_point
function kdtree2_r_count(tp,qv,r2) result(nfound)
! Count the number of neighbors within square distance 'r2'.
implicit none
type (kdtree2), pointer :: tp
real(pReal), target, intent (In) :: qv(:)
real(pReal), intent(in) :: r2
integer(pInt) :: nfound
! ..
! .. Intrinsic Functions ..
intrinsic HUGE
! ..
sr%qv => qv
sr%ballsize = r2
sr%nn = 0_pInt ! flag for fixed r search
sr%nfound = 0_pInt
sr%centeridx = -1_pInt
sr%correltime = 0_pInt
nullify(sr%results) ! for some reason, FTN 95 chokes on '=> null()'
sr%nalloc = 0_pInt ! we do not allocate any storage but that's OK
! for counting.
sr%ind => tp%ind
sr%rearrange = tp%rearrange
if (tp%rearrange) then
sr%myData => tp%rearranged_data
else
sr%myData => tp%the_data
endif
sr%dimen = tp%dimen
!
!sr%dsl = Huge(sr%dsl) ! set to huge positive values
!sr%il = -1 ! set to invalid indexes
!
sr%overflow = .false.
call search(tp%root)
nfound = sr%nfound
return
end function kdtree2_r_count
function kdtree2_r_count_around_point(tp,idxin,correltime,r2) result(nfound)
! Count the number of neighbors within square distance 'r2' around
! point 'idxin' with decorrelation time 'correltime'.
!
implicit none
type (kdtree2), pointer :: tp
integer(pInt), intent (In) :: correltime, idxin
real(pReal), intent(in) :: r2
integer(pInt) :: nfound
! ..
! ..
! .. Intrinsic Functions ..
intrinsic HUGE
! ..
allocate (sr%qv(tp%dimen))
sr%qv = tp%the_data(:,idxin)
sr%ballsize = r2
sr%nn = 0_pInt ! flag for fixed r search
sr%nfound = 0_pInt
sr%centeridx = idxin
sr%correltime = correltime
nullify(sr%results)
sr%nalloc = 0_pInt ! we do not allocate any storage but that's OK
! for counting.
sr%ind => tp%ind
sr%rearrange = tp%rearrange
if (sr%rearrange) then
sr%myData => tp%rearranged_data
else
sr%myData => tp%the_data
endif
sr%dimen = tp%dimen
!
!sr%dsl = Huge(sr%dsl) ! set to huge positive values
!sr%il = -1 ! set to invalid indexes
!
sr%overflow = .false.
call search(tp%root)
nfound = sr%nfound
return
end function kdtree2_r_count_around_point
subroutine validate_query_storage(n)
!
! make sure we have enough storage for n
!
use IO, only: IO_error
implicit none
integer(pInt), intent(in) :: n
if (int(size(sr%results,1),pInt) .lt. n) then
call IO_error (460_pInt,ext_msg='KD_TREE_TRANS: not enough storage for results(1:n)')
endif
return
end subroutine validate_query_storage
function square_distance(d, iv,qv) result (res)
! distance between iv[1:n] and qv[1:n]
! .. Function Return Value ..
! re-implemented to improve vectorization.
implicit none
real(pReal) :: res
! .. Scalar Arguments ..
integer(pInt) :: d
! .. Array Arguments ..
real(pReal) :: iv(:),qv(:)
! ..
! ..
res = sum( (iv(1:d)-qv(1:d))**2 )
end function square_distance
recursive subroutine search(node)
!
! This is the innermost core routine of the kd-tree search. Along
! with "process_terminal_node", it is the performance bottleneck.
!
! This version uses a logically complete secondary search of
! "box in bounds", whether the sear
!
implicit none
type (Tree_node), pointer :: node
! ..
type(tree_node),pointer :: ncloser, nfarther
!
integer(pInt) :: cut_dim, i
! ..
real(pReal) :: qval, dis
real(pReal) :: ballsize
real(pReal), pointer :: qv(:)
type(interval), pointer :: box(:)
if ((associated(node%left) .and. associated(node%right)) .eqv. .false.) then
! we are on a terminal node
if (sr%nn .eq. 0) then
call process_terminal_node_fixedball(node)
else
call process_terminal_node(node)
endif
else
! we are not on a terminal node
qv => sr%qv(1:)
cut_dim = node%cut_dim
qval = qv(cut_dim)
if (qval < node%cut_val) then
ncloser => node%left
nfarther => node%right
dis = (node%cut_val_right - qval)**2
! extra = node%cut_val - qval
else
ncloser => node%right
nfarther => node%left
dis = (node%cut_val_left - qval)**2
! extra = qval- node%cut_val_left
endif
if (associated(ncloser)) call search(ncloser)
! we may need to search the second node.
if (associated(nfarther)) then
ballsize = sr%ballsize
! dis=extra**2
if (dis <= ballsize) then
!
! we do this separately as going on the first cut dimen is often
! a good idea.
! note that if extra**2 < sr%ballsize, then the next
! check will also be false.
!
box => node%box(1:)
do i=1_pInt,sr%dimen
if (i .ne. cut_dim) then
dis = dis + dis2_from_bnd(qv(i),box(i)%lower,box(i)%upper)
if (dis > ballsize) then
return
endif
endif
end do
!
! if we are still here then we need to search mroe.
!
call search(nfarther)
endif
endif
end if
end subroutine search
real(pReal) function dis2_from_bnd(x,amin,amax) result (res)
implicit none
real(pReal), intent(in) :: x, amin,amax
if (x > amax) then
res = (x-amax)**2;
return
else
if (x < amin) then
res = (amin-x)**2;
return
else
res = 0.0_pReal
return
endif
endif
return
end function dis2_from_bnd
logical function box_in_search_range(node, sr) result(res)
!
! Return the distance from 'qv' to the CLOSEST corner of node's
! bounding box
! for all coordinates outside the box. Coordinates inside the box
! contribute nothing to the distance.
!
implicit none
type (tree_node), pointer :: node
type (tree_search_record), pointer :: sr
integer(pInt) :: dimen, i
real(pReal) :: dis, ballsize
real(pReal) :: l, u
dimen = sr%dimen
ballsize = sr%ballsize
dis = 0.0_pReal
res = .true.
do i=1_pInt,dimen
l = node%box(i)%lower
u = node%box(i)%upper
dis = dis + (dis2_from_bnd(sr%qv(i),l,u))
if (dis > ballsize) then
res = .false.
return
endif
end do
res = .true.
return
end function box_in_search_range
subroutine process_terminal_node(node)
!
! Look for actual near neighbors in 'node', and update
! the search results on the sr data structure.
!
implicit none
type (tree_node), pointer :: node
!
real(pReal), pointer :: qv(:)
integer(pInt), pointer :: ind(:)
real(pReal), pointer :: myData(:,:)
!
integer(pInt) :: dimen, i, indexofi, k, centeridx, correltime
real(pReal) :: ballsize, sd, newpri
logical :: rearrange
type(pq), pointer :: pqp
!
! copy values from sr to local variables
!
!
! Notice, making local pointers with an EXPLICIT lower bound
! seems to generate faster code.
! why? I don't know.
qv => sr%qv(1:)
pqp => sr%pq
dimen = sr%dimen
ballsize = sr%ballsize
rearrange = sr%rearrange
ind => sr%ind(1:)
myData => sr%myData(1:,1:)
centeridx = sr%centeridx
correltime = sr%correltime
! doing_correl = (centeridx >= 0) ! Do we have a decorrelation window?
! include_point = .true. ! by default include all points
! search through terminal bucket.
mainloop: do i = node%l, node%u
if (rearrange) then
sd = 0.0_pReal
do k = 1_pInt,dimen
sd = sd + (myData(k,i) - qv(k))**2.0_pReal
if (sd>ballsize) cycle mainloop
end do
indexofi = ind(i) ! only read it if we have not broken out
else
indexofi = ind(i)
sd = 0.0_pReal
do k = 1_pInt,dimen
sd = sd + (myData(k,indexofi) - qv(k))**2.0_pReal
if (sd>ballsize) cycle mainloop
end do
endif
if (centeridx > 0_pInt) then ! doing correlation interval?
if (abs(indexofi-centeridx) < correltime) cycle mainloop
endif
!
! two choices for any point. The list so far is either undersized,
! or it is not.
!
! If it is undersized, then add the point and its distance
! unconditionally. If the point added fills up the working
! list then set the sr%ballsize, maximum distance bound (largest distance on
! list) to be that distance, instead of the initialized +infinity.
!
! If the running list is full size, then compute the
! distance but break out immediately if it is larger
! than sr%ballsize, "best squared distance" (of the largest element),
! as it cannot be a good neighbor.
!
! Once computed, compare to best_square distance.
! if it is smaller, then delete the previous largest
! element and add the new one.
if (sr%nfound .lt. sr%nn) then
!
! add this point unconditionally to fill list.
!
sr%nfound = sr%nfound +1_pInt
newpri = pq_insert(pqp,sd,indexofi)
if (sr%nfound .eq. sr%nn) ballsize = newpri
! we have just filled the working list.
! put the best square distance to the maximum value
! on the list, which is extractable from the PQ.
else
!
! now, if we get here,
! we know that the current node has a squared
! distance smaller than the largest one on the list, and
! belongs on the list.
! Hence we replace that with the current one.
!
ballsize = pq_replace_max(pqp,sd,indexofi)
endif
end do mainloop
!
! Reset sr variables which may have changed during loop
!
sr%ballsize = ballsize
end subroutine process_terminal_node
subroutine process_terminal_node_fixedball(node)
!
! Look for actual near neighbors in 'node', and update
! the search results on the sr data structure, i.e.
! save all within a fixed ball.
!
implicit none
type (tree_node), pointer :: node
!
real(pReal), pointer :: qv(:)
integer(pInt), pointer :: ind(:)
real(pReal), pointer :: myData(:,:)
!
integer(pInt) :: nfound
integer(pInt) :: dimen, i, indexofi, k
integer(pInt) :: centeridx, correltime, nn
real(pReal) :: ballsize, sd
logical :: rearrange
!
! copy values from sr to local variables
!
qv => sr%qv(1:)
dimen = sr%dimen
ballsize = sr%ballsize
rearrange = sr%rearrange
ind => sr%ind(1:)
myData => sr%myData(1:,1:)
centeridx = sr%centeridx
correltime = sr%correltime
nn = sr%nn ! number to search for
nfound = sr%nfound
! search through terminal bucket.
mainloop: do i = node%l, node%u
!
! two choices for any point. The list so far is either undersized,
! or it is not.
!
! If it is undersized, then add the point and its distance
! unconditionally. If the point added fills up the working
! list then set the sr%ballsize, maximum distance bound (largest distance on
! list) to be that distance, instead of the initialized +infinity.
!
! If the running list is full size, then compute the
! distance but break out immediately if it is larger
! than sr%ballsize, "best squared distance" (of the largest element),
! as it cannot be a good neighbor.
!
! Once computed, compare to best_square distance.
! if it is smaller, then delete the previous largest
! element and add the new one.
! which index to the point do we use?
if (rearrange) then
sd = 0.0_pReal
do k = 1_pInt,dimen
sd = sd + (myData(k,i) - qv(k))**2.0_pReal
if (sd>ballsize) cycle mainloop
end do
indexofi = ind(i) ! only read it if we have not broken out
else
indexofi = ind(i)
sd = 0.0_pReal
do k = 1_pInt,dimen
sd = sd + (myData(k,indexofi) - qv(k))**2.0_pReal
if (sd>ballsize) cycle mainloop
end do
endif
if (centeridx > 0_pInt) then ! doing correlation interval?
if (abs(indexofi-centeridx)<correltime) cycle mainloop
endif
nfound = nfound+1_pInt
if (nfound .gt. sr%nalloc) then
! oh nuts, we have to add another one to the tree but
! there isn't enough room.
sr%overflow = .true.
else
sr%results(nfound)%dis = sd
sr%results(nfound)%idx = indexofi
endif
end do mainloop
!
! Reset sr variables which may have changed during loop
!
sr%nfound = nfound
end subroutine process_terminal_node_fixedball
subroutine kdtree2_n_nearest_brute_force(tp,qv,nn,results)
! find the 'n' nearest neighbors to 'qv' by exhaustive search.
! only use this subroutine for testing, as it is SLOW! The
! whole point of a k-d tree is to avoid doing what this subroutine
! does.
implicit none
type (kdtree2), pointer :: tp
real(pReal), intent(in) :: qv(:)
integer(pInt), intent(in) :: nn
type(kdtree2_result) :: results(:)
integer(pInt) :: i, j, k
real(pReal), allocatable :: all_distances(:)
! ..
allocate (all_distances(tp%n))
do i = 1_pInt, tp%n
all_distances(i) = square_distance(tp%dimen,qv,tp%the_data(:,i))
end do
! now find 'n' smallest distances
do i = 1_pInt, nn
results(i)%dis = huge(1.0_pReal)
results(i)%idx = -1_pInt
end do
do i = 1_pInt, tp%n
if (all_distances(i)<results(nn)%dis) then
! insert it somewhere on the list
do j = 1_pInt, nn
if (all_distances(i)<results(j)%dis) exit
end do
! now we know 'j'
do k = nn - 1_pInt, j, -1_pInt
results(k+1_pInt) = results(k)
end do
results(j)%dis = all_distances(i)
results(j)%idx = i
end if
end do
deallocate (all_distances)
end subroutine kdtree2_n_nearest_brute_force
subroutine kdtree2_r_nearest_brute_force(tp,qv,r2,nfound,results)
! find the nearest neighbors to 'qv' with distance**2 <= r2 by exhaustive search.
! only use this subroutine for testing, as it is SLOW! The
! whole point of a k-d tree is to avoid doing what this subroutine
! does.
implicit none
type (kdtree2), pointer :: tp
real(pReal), intent(in) :: qv(:)
real(pReal), intent(in) :: r2
integer(pInt), intent(out) :: nfound
type(kdtree2_result) :: results(:)
integer(pInt) :: i
integer :: nalloc
real(pReal), allocatable :: all_distances(:)
! ..
allocate (all_distances(tp%n))
do i = 1_pInt, tp%n
all_distances(i) = square_distance(tp%dimen,qv,tp%the_data(:,i))
end do
nfound = 0_pInt
nalloc = size(results,1)
do i = 1_pInt, tp%n
if (all_distances(i)< r2) then
! insert it somewhere on the list
if (nfound .lt. nalloc) then
nfound = nfound+1_pInt
results(nfound)%dis = all_distances(i)
results(nfound)%idx = i
endif
end if
enddo
deallocate (all_distances)
call kdtree2_sort_results(nfound,results)
end subroutine kdtree2_r_nearest_brute_force
subroutine kdtree2_sort_results(nfound,results)
! Use after search to sort results(1:nfound) in order of increasing
! distance.
implicit none
integer(pInt), intent(in) :: nfound
type(kdtree2_result), target :: results(:)
!
!
!THIS IS BUGGY WITH INTEL FORTRAN
! If (nfound .Gt. 1) Call heapsort(results(1:nfound)%dis,results(1:nfound)%ind,nfound)
!
if (nfound .gt. 1_pInt) call heapsort_struct(results,nfound)
return
end subroutine kdtree2_sort_results
subroutine heapsort(a,ind,n)
!
! Sort a(1:n) in ascending order, permuting ind(1:n) similarly.
!
! If ind(k) = k upon input, then it will give a sort index upon output.
!
implicit none
integer(pInt),intent(in) :: n
real(pReal), intent(inout) :: a(:)
integer(pInt), intent(inout) :: ind(:)
!
!
real(pReal) :: value ! temporary for a value from a()
integer(pInt) :: ivalue ! temporary for a value from ind()
integer(pInt) :: i,j
integer(pInt) :: ileft,iright
ileft=n/2_pInt+1_pInt
iright=n
! do i=1,n
! ind(i)=i
! Generate initial idum array
! end do
if(n.eq.1_pInt) return
do
if(ileft > 1_pInt)then
ileft=ileft-1_pInt
value=a(ileft); ivalue=ind(ileft)
else
value=a(iright); ivalue=ind(iright)
a(iright)=a(1); ind(iright)=ind(1)
iright=iright-1_pInt
if (iright == 1_pInt) then
a(1)=value;ind(1)=ivalue
return
endif
endif
i=ileft
j=2_pInt*ileft
do while (j <= iright)
if(j < iright) then
if(a(j) < a(j+1_pInt)) j=j+1_pInt
endif
if(value < a(j)) then
a(i)=a(j); ind(i)=ind(j)
i=j
j=j+j
else
j=iright+1_pInt
endif
end do
a(i)=value; ind(i)=ivalue
end do
end subroutine heapsort
subroutine heapsort_struct(a,n)
!
! Sort a(1:n) in ascending order
!
!
implicit none
integer(pInt),intent(in) :: n
type(kdtree2_result),intent(inout) :: a(:)
!
!
type(kdtree2_result) :: value ! temporary value
integer(pInt) :: i,j
integer(pInt) :: ileft,iright
ileft=n/2_pInt+1_pInt
iright=n
! do i=1,n
! ind(i)=i
! Generate initial idum array
! end do
if(n.eq.1_pInt) return
do
if(ileft > 1_pInt)then
ileft=ileft-1_pInt
value=a(ileft)
else
value=a(iright)
a(iright)=a(1)
iright=iright-1_pInt
if (iright == 1_pInt) then
a(1) = value
return
endif
endif
i=ileft
j=2_pInt*ileft
do while (j <= iright)
if(j < iright) then
if(a(j)%dis < a(j+1_pInt)%dis) j=j+1_pInt
endif
if(value%dis < a(j)%dis) then
a(i)=a(j);
i=j
j=j+j
else
j=iright+1_pInt
endif
end do
a(i)=value
end do
end subroutine heapsort_struct
end module kdtree2_module