1926 lines
52 KiB
Fortran
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
|