;;
;; An implementation of the K-D tree spatial indexing data structures.
;; http://en.wikipedia.org/wiki/K-d_tree
;;
;; This code is based on the Haskell kd-tree library implementation of
;; K-D trees.
;;
;; Copyright 2012 Ivan Raikov and the Okinawa Institute of
;; Science and Technology.
;;
;; This program is free software: you can redistribute it and/or
;; modify it under the terms of the GNU General Public License as
;; published by the Free Software Foundation, either version 3 of the
;; License, or (at your option) any later version.
;;
;; This program is distributed in the hope that it will be useful, but
;; WITHOUT ANY WARRANTY; without even the implied warranty of
;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
;; General Public License for more details.
;;
;; A full copy of the GPL license can be found at
;; .
;;
(module kd-tree
(
default- Point3d Point2d
point? make-point
default- KdTree3d KdTree2d
kd-tree?
kd-tree-empty?
kd-tree->list
kd-tree->list*
kd-tree-map
kd-tree-for-each
kd-tree-for-each*
kd-tree-fold-right
kd-tree-fold-right*
kd-tree-subtrees
kd-tree-node-points
kd-tree-node-indices
kd-tree-min-index
kd-tree-max-index
kd-tree-size
)
(import scheme chicken data-structures foreign)
(require-library srfi-1 srfi-4 extras cis)
(require-extension typeclass datatype)
(import (only srfi-1 xcons fold list-tabulate split-at every fold-right take filter filter-map remove zip)
(only srfi-4 f64vector-ref f64vector-set! f64vector-length make-f64vector f64vector->list
u32vector-ref u32vector-set!)
(only extras fprintf pp)
(only foreign foreign-lambda)
(prefix cis cis:))
(define-class
;; dimension returns the number of coordinates of a point.
dimension ;; Point -> Int
;; gets the k'th coordinate, starting from 0.
coord ;; Int * Point -> Double
;; compares the given coordinates
compare-coord ;; Int * Point * Point -> Bool
;; returns the squared distance between two points.
dist2 ;; Point * Point -> Double
;; returns the scaled squared distance between two points.
sdist2 ;; Point * Point * [Int] -> Double
;; returns 0, negative or positive number depending on the
;; distance between two points
compare-distance
)
(define (minimum-by lst less? . rest)
(if (null? lst) #f
(if (null? rest)
(let recur ((lst (cdr lst)) (m (car lst)))
(if (null? lst) m
(if (less? (car lst) m)
(recur (cdr lst) (car lst))
(recur (cdr lst) m)
))
)
(let recur ((lst (cdr lst))
(rest (map cdr rest))
(m (map car (cons lst rest))))
(if (null? lst) m
(if (less? (car lst) (car m))
(recur (cdr lst) (map cdr rest) (map car (cons lst rest)))
(recur (cdr lst) (map cdr rest) m)
))
)
)))
(define (sum lst) (fold + 0. lst))
(define (default- dimension coord)
(let* ((dist2
(lambda (a b)
(let ((diff2 (lambda (i) (let ((v (- (coord i a) (coord i b)))) (* v v)))))
(sum (list-tabulate (dimension a) diff2)))))
(sdist2
(lambda (factors)
(let ((factors2 (map (lambda (n) (* n n)) factors)))
(lambda (a b)
(let ((diff2 (lambda (i) (let ((v (- (coord i a) (coord i b)))) (* v v)))))
(let ((v (sum (map * (list-tabulate (dimension a) diff2) factors2))))
v))))))
(compare-distance
(lambda (p a b . reltol)
(let ((delta (- (dist2 p a) (dist2 p b))))
(if (null? reltol)
delta
(if (<= delta (car reltol)) 0 delta)))))
(compare-coord
(lambda (c a b)
(< (coord c a) (coord c b))))
)
(make-
dimension coord compare-coord dist2 sdist2 compare-distance)
))
(define point? vector?)
(define make-point vector)
(define Point3d
(default-
(lambda (p) (and (point? p) 3))
(lambda (i p) (vector-ref p i))
))
(define Point2d
(default-
(lambda (p) (and (point? p) 2))
(lambda (i p) (vector-ref p i))
))
(define-class
;; constructs a kd-tree from a list of points
list->kd-tree
;; nearest neighbor of a point
kd-tree-nearest-neighbor
;; the index of the nearest neighbor of a point
kd-tree-nearest-neighbor*
;; neighbors of a point within radius r
kd-tree-near-neighbors
;; neighbors of a point within radius r (using point indices)
kd-tree-near-neighbors*
;; k nearest neighbors of a point
kd-tree-k-nearest-neighbors
;; removes a point from the tree
kd-tree-remove
;; retrieves all points between two planes
kd-tree-slice
;; retrieves all points between two planes (using point indices)
kd-tree-slice*
;; checks that the kd-tree properties are preserved
kd-tree-is-valid?
kd-tree-all-subtrees-are-valid?
)
(define (positive-or-zero-integer? x)
(and (integer? x) (or (zero? x) (positive? x))))
(define (positive-integer? x)
(and (integer? x) (positive? x)))
(define-datatype kd-tree kd-tree?
(KdNode (left kd-tree?)
(p point?)
(i positive-or-zero-integer?)
(v (lambda (v) (or (not v) v)))
(right kd-tree?)
(axis positive-or-zero-integer?)
(ci cis:cis?)
)
(KdLeaf (ii cis:cis?)
(pp (lambda (lst) (every point? lst)))
(vv (lambda (v) (or (list? v) (not v))))
(axis positive-or-zero-integer?) )
)
(define (kd-tree-empty? t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) (cis:empty? ii))
(else #f)))
(define (kd-tree-map f t)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(KdLeaf ii (map f pp) vv axis))
(KdNode (l x i v r axis ci)
(KdNode (kd-tree-map f l)
(f x)
i v
(kd-tree-map f r)
axis ci))
))
(define (kd-tree-for-each f t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) (for-each f pp))
(KdNode (l x i v r axis ci)
(begin
(kd-tree-for-each f l)
(f x)
(kd-tree-for-each f r)
))
))
(define (kd-tree-for-each* f t)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(if vv (for-each f (zip (reverse (cis:elements ii)) vv) pp)
(for-each f (reverse (cis:elements ii)) pp)))
(KdNode (l x i v r axis ci)
(begin
(kd-tree-for-each* f l)
(if v (f (list i v) x) (f i x))
(kd-tree-for-each* f r)
))
))
(define (kd-tree-fold-right f init t)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(fold-right f init pp))
(KdNode (l p i v r axis ci)
(let* ((init2 (kd-tree-fold-right f init r))
(init3 (f p init2)))
(kd-tree-fold-right f init3 l)))
))
(define (kd-tree-fold-right* f init t)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(if vv
(fold-right f init (zip (reverse (cis:elements ii)) vv) pp)
(fold-right f init (reverse (cis:elements ii)) pp)))
(KdNode (l x i v r axis ci)
(let* ((init2 (kd-tree-fold-right* f init r))
(init3 (if v (f (list i v) x init2) (f i x init2))))
(kd-tree-fold-right* f init3 l)))
))
(define (kd-tree->list t)
(kd-tree-fold-right cons '() t))
(define (kd-tree->list* t)
(kd-tree-fold-right*
(lambda (iv x ax) (cons (list iv x) ax))
'() t))
;; Returns a list containing t and all its subtrees, including the
;; leaf nodes.
(define (kd-tree-subtrees t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) (list t))
(KdNode (l x i v r axis ci)
(append (kd-tree-subtrees l)
(list t)
(kd-tree-subtrees r)))
))
(define (kd-tree-node-points t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) pp)
(KdNode (l x i v r axis ci) (list x))
))
(define (kd-tree-node-indices t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) (cis:elements ii))
(KdNode (l x i v r axis ci) (list i))
))
(define (kd-tree-node-values t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) vv)
(KdNode (l x i v r axis ci) (list v))
))
(define (kd-tree-size t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) (cis:cardinal ii))
(KdNode (l x i v r axis ci) (cis:cardinal ci))))
(define (kd-tree-min-index t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) (cis:get-min ii))
(KdNode (l x i v r axis ci) (cis:get-min ci))))
(define (kd-tree-max-index t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) (cis:get-max ii))
(KdNode (l x i v r axis ci) (cis:get-max ci))))
;; construct a kd-tree from a list of points
(define=> (make-list->kd-tree/depth )
(lambda (make-point make-value)
(letrec (
(split
(lambda (m n points depth)
(let* ((axis (modulo depth (dimension (make-point (car points)))))
(cmpfn (lambda (p0 p1)
(compare-coord axis (make-point p0) (make-point p1))))
(sorted (sort points cmpfn))
(median-index (quotient (- n m) 2)))
(let-values (((lt gte) (split-at sorted median-index)))
(values (car gte) median-index lt (cdr gte))))
))
(list->kd-tree/depth
(lambda (m n points depth #!key (leaf-factor 10) (offset 0))
(cond
((null? points) (KdLeaf cis:empty '() '() depth))
((<= (- n m) leaf-factor)
(let ((k (- n m)))
(let* ((es (take points k))
(ps (map make-point es))
(ii (cis:shift offset (cis:interval m (- n 1))))
(vs (and make-value
(map (lambda (i e) (make-value i e))
(reverse (cis:elements ii))
es)))
)
(KdLeaf ii ps vs (modulo depth (dimension (car ps))))
)))
((null? (cdr points))
(let* ((e (car points))
(ps (list (make-point e)) )
(vs (and make-value (list (make-value m e)))))
(KdLeaf (cis:shift offset (cis:singleton m) )
ps vs
(modulo depth (dimension (car ps))))
))
(else
(let-values (((median median-index lt gte)
(split m n points depth)))
(let* ((depth1 (+ 1 depth))
(i (+ m median-index offset))
(p (make-point median))
(v (and make-value (make-value i median)))
(axis (modulo depth (dimension p))))
(KdNode (list->kd-tree/depth m (+ m median-index) lt depth1
leaf-factor: leaf-factor)
p i v
(list->kd-tree/depth (+ m median-index 1) n gte depth1
leaf-factor: leaf-factor)
axis
(cis:shift offset (cis:interval m (- n 1))))))
)))
))
list->kd-tree/depth
)))
;; Returns the nearest neighbor of p in tree t.
(define=> (make-kd-tree-nearest-neighbor )
(define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
(letrec ((find-nearest
(lambda (t1 t2 p probe xp x-probe)
(let* ((candidates1
(let ((best1 (nearest-neighbor t1 probe)))
(or (and best1 (list best1 p)) (list p))))
(sphere-intersects-plane?
(let ((v (- x-probe xp)))
(< (* v v) (dist2 probe (car candidates1)))))
(candidates2
(if sphere-intersects-plane?
(let ((nn (nearest-neighbor t2 probe)))
(if nn (append candidates1 (list nn)) candidates1))
candidates1)))
(minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe a b))))
)))
(nearest-neighbor
(lambda (t probe)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(minimum-by pp (lambda (a b) (negative? (compare-distance probe a b)))))
(KdNode (l p i v r axis ci)
(if (and (tree-empty? l)
(tree-empty? r)) p
(let ((x-probe (coord axis probe))
(xp (coord axis p)))
(if (<= x-probe xp)
(find-nearest l r p probe xp x-probe)
(find-nearest r l p probe xp x-probe))
))
))
)))
nearest-neighbor
))
;; Returns the index of the nearest neighbor of p in tree t.
(define=> (make-kd-tree-nearest-neighbor* )
(define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
(letrec ((find-nearest
(lambda (t1 t2 i v p probe xp x-probe)
(let* ((candidates1
(let ((best1 (nearest-neighbor t1 probe)))
(or (and best1 (list best1 (if v (list (list i v) p) (list i p))))
(list (if v (list (list i v) p) (list i p))))
))
(sphere-intersects-plane?
(let ((v (- x-probe xp)))
(< (* v v) (dist2 probe (cadar candidates1)))))
(candidates2
(if sphere-intersects-plane?
(let ((nn (nearest-neighbor t2 probe)))
(if nn (append candidates1 (list nn)) candidates1))
candidates1)))
(let ((v (minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe (cadr a) (cadr b)))))))
v)
)))
(nearest-neighbor
(lambda (t probe)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(let ((res
(if vv
(minimum-by pp (lambda (a b) (negative? (compare-distance probe a b)))
(zip (reverse (cis:elements ii)) vv ))
(minimum-by pp (lambda (a b) (negative? (compare-distance probe a b)))
(reverse (cis:elements ii))))))
(and res (reverse res))))
(KdNode (l p i v r axis ci)
(if (and (tree-empty? l)
(tree-empty? r))
(if v (list (list i v) p) (list i p))
(let ((x-probe (coord axis probe))
(xp (coord axis p))
(xi i))
(if (<= x-probe xp)
(find-nearest l r i v p probe xp x-probe)
(find-nearest r l i v p probe xp x-probe))
))
))
)))
nearest-neighbor
))
;; near-neighbors t r p returns all neighbors within distance r from p in tree t.
(define=> (make-kd-tree-near-neighbors )
(define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
(letrec ((near-neighbors
(lambda (t radius probe fdist filter-fn)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(let ((r2 (* radius radius)))
(filter-fn probe pp r2)))
(KdNode (l p i v r axis ci)
(let ((maybe-pivot (filter-fn probe (list p) (* radius radius))))
(if (and (tree-empty? l)
(tree-empty? r))
maybe-pivot
(let ((x-probe (coord axis probe))
(xp (coord axis p)))
(if (<= x-probe xp)
(let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist filter-fn))))
(if (> (+ x-probe (abs radius)) xp)
(append (near-neighbors r radius probe fdist filter-fn) nearest)
nearest))
(let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist filter-fn))))
(if (< (- x-probe (abs radius)) xp)
(append (near-neighbors l radius probe fdist filter-fn) nearest)
nearest)))
))))
))
))
(lambda (t radius probe #!key (factors #f) (with-distance? #f))
(let* ((dist-fn (if factors (sdist2 factors) dist2))
(filter-fn (if with-distance?
(lambda (probe pp d2) (filter-map (lambda (p) (let ((pd (dist-fn probe p)))
(and (<= pd d2) (list p (sqrt pd) )))) pp))
(lambda (probe pp d2) (filter (lambda (p) (<= (dist-fn probe p) d2)) pp))
))
)
(near-neighbors t radius probe dist-fn filter-fn)
))
))
(define=> (make-kd-tree-near-neighbors* )
(define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
(letrec ((near-neighbors
(lambda (t radius probe fdist filter-fn)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(let ((r2 (* radius radius)))
(filter-fn probe pp ii vv r2)))
(KdNode (l p i v r axis ci)
(let ((maybe-pivot (filter-fn probe (list p) (cis:singleton i) (list v) (* radius radius))))
(if (and (tree-empty? l)
(tree-empty? r))
maybe-pivot
(let ((x-probe (coord axis probe))
(xp (coord axis p)))
(if (<= x-probe xp)
(let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist filter-fn))))
(if (> (+ x-probe (abs radius)) xp)
(append (near-neighbors r radius probe fdist filter-fn) nearest)
nearest))
(let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist filter-fn))))
(if (< (- x-probe (abs radius)) xp)
(append (near-neighbors l radius probe fdist filter-fn) nearest)
nearest)))
))
))
))
))
(lambda (t radius probe #!key (factors #f) (with-distance? #f))
(let* ((dist-fn (if factors (sdist2 factors) dist2))
(filter-fn (if with-distance?
(lambda (probe pp ii vv r2)
(if vv
(filter-map (lambda (i v p) (let ((pd (dist-fn probe p))) (and (<= pd r2) (list (list i v) p (sqrt pd)))))
(reverse (cis:elements ii)) vv pp)
(filter-map (lambda (i p) (let ((pd (dist-fn probe p))) (and (<= pd r2) (list i p (sqrt pd)))))
(reverse (cis:elements ii)) pp)
))
(lambda (probe pp ii vv r2)
(if vv
(filter-map (lambda (i v p) (and (<= (dist-fn probe p) r2) (list (list i v) p)))
(reverse (cis:elements ii)) vv pp)
(filter-map (lambda (i p) (and (<= (dist-fn probe p) r2) (list i p)))
(reverse (cis:elements ii)) pp)
))
))
)
(near-neighbors t radius probe dist-fn filter-fn)
))
))
;; Returns the k nearest points to p within tree.
(define=> (make-kd-tree-k-nearest-neighbors )
(lambda (kd-tree-remove kd-tree-nearest-neighbor)
(letrec ((k-nearest-neighbors
(lambda (t k probe)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(let recur ((res '()) (pp pp) (k k))
(if (or (<= k 0) (null? pp))
res
(let ((nearest (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))))))
(recur (cons nearest res)
(remove (lambda (p) (equal? p nearest)) pp)
(- k 1))
))
))
(else
(if (<= k 0) '()
(let* ((nearest (kd-tree-nearest-neighbor t probe))
(tree1 (kd-tree-remove t nearest)))
(cons nearest (k-nearest-neighbors tree1 (- k 1) probe)))
))
))
))
k-nearest-neighbors)))
;; removes the point p from t.
(define=> (make-kd-tree-remove )
(lambda (list->kd-tree list->kd-tree*)
(letrec ((tree-remove
(lambda (t p-kill)
; (fprintf (current-error-port) "kd-tree-remove: t = ~A~%" t)
; (fprintf (current-error-port) "kd-tree-remove: p-kill = ~A~%" p-kill)
(cases kd-tree t
(KdLeaf (ii pp vv axis)
; (fprintf (current-error-port) "kd-tree-remove (KdLeaf): vv = ~A~%" vv)
(if vv
(let ((ipvs (filter-map
(lambda (i p v) (and (equal? p p-kill) (list i p v)))
(reverse (cis:elements ii)) pp vv)))
; (fprintf (current-error-port) "kd-tree-remove (KdLeaf): ipvs = ~A~%" ipvs)
; (fprintf (current-error-port) "kd-tree-remove (KdLeaf): pp = ~A~%" pp)
; (fprintf (current-error-port) "kd-tree-remove (KdLeaf): vv = ~A~%" vv)
(and (pair? ipvs)
(let ((ii1 (fold (lambda (i ax) (cis:remove i ax))
ii (map car ipvs)))
(pp1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
pp (map cadr ipvs)))
(vv1 (fold (lambda (x ax)
(remove (lambda (p) (equal? p x)) ax))
vv (map caddr ipvs)))
)
; (fprintf (current-error-port) "kd-tree-remove (KdLeaf): ii1 = ~A~%" ii1)
(KdLeaf ii1 pp1 vv1 axis))
))
(let ((ips (filter-map (lambda (i p) (and (equal? p p-kill) (list i p)))
(reverse (cis:elements ii)) pp)))
(and (pair? ips)
(let ((ii1 (fold (lambda (i ax) (cis:remove i ax))
ii (map car ips)))
(pp1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
pp (map cadr ips)))
)
(KdLeaf ii1 pp1 vv axis))
))
))
(KdNode (l p i v r axis ci)
; (fprintf (current-error-port) "kd-tree-remove (KdNode): p = ~A~%" p)
; (fprintf (current-error-port) "kd-tree-remove (KdNode): p-kill = ~A~%" p-kill)
(if (equal? p p-kill)
(let ((offset (if (kd-tree-empty? l) (+ i 1) (kd-tree-min-index l))))
(if v
(let ((pts1 (append (kd-tree->list* l) (kd-tree->list* r))))
(list->kd-tree* 0 (length pts1) pts1 axis offset: offset))
(let ((pts1 (append (kd-tree->list l) (kd-tree->list r))))
(list->kd-tree 0 (length pts1) pts1 axis offset: offset))
))
(if (< (coord axis p-kill)
(coord axis p))
(let* ((l1 (tree-remove l p-kill))
(r1 (or (and l1 r) (tree-remove r p-kill)))
(ll (or l1 l))
(rr (or r1 r)))
(and (or l1 r1)
(let ((min1 (cond ((and (kd-tree-empty? ll)
(kd-tree-empty? rr))
i)
((kd-tree-empty? ll)
(kd-tree-min-index rr))
(else
(kd-tree-min-index ll))))
(max1 (cond ((and (kd-tree-empty? ll)
(kd-tree-empty? rr))
i)
((kd-tree-empty? rr)
(kd-tree-max-index ll))
(else
(kd-tree-max-index rr))))
)
(KdNode ll p i v rr axis
(cis:add i (cis:interval min1 max1)))
)))
(let* ((r1 (tree-remove r p-kill))
(l1 (or (and r1 l) (tree-remove l p-kill)))
(ll (or l1 l))
(rr (or r1 r))
)
(and (or r1 l1)
(let ((min1 (cond ((and (kd-tree-empty? ll)
(kd-tree-empty? rr))
i)
((kd-tree-empty? ll)
(kd-tree-min-index rr))
(else
(kd-tree-min-index ll))))
(max1 (cond ((and (kd-tree-empty? ll)
(kd-tree-empty? rr))
i)
((kd-tree-empty? rr)
(kd-tree-max-index ll))
(else
(kd-tree-max-index rr))))
)
(KdNode ll p i v rr axis
(cis:add i (cis:interval min1 max1)))
)))
)))
))
))
tree-remove)))
;; Checks whether the K-D tree property holds for a given tree.
;;
;; Specifically, it tests that all points in the left subtree lie to
;; the left of the plane, p is on the plane, and all points in the
;; right subtree lie to the right.
(define=> (make-kd-tree-is-valid? )
(lambda (t)
(cases kd-tree t
(KdLeaf (ii pp vv axis) #t)
(KdNode (l p i v r axis ci)
(let ((x (coord axis p)))
(and (every (lambda (y) (<= (coord axis y) x ))
(kd-tree->list l))
(every (lambda (y) (>= (coord axis y) x))
(kd-tree->list r)))))
)))
;; Checks whether the K-D tree property holds for the given tree and
;; all subtrees.
(define (make-kd-tree-all-subtrees-are-valid? kd-tree-is-valid?)
(lambda (t) (every kd-tree-is-valid? (kd-tree-subtrees t))))
(define=> (make-kd-tree-slice )
(lambda (x-axis x1 x2 t)
(let recur ((t t) (pts '()))
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(append (filter (lambda (p)
(and (<= x1 (coord x-axis p))
(<= (coord x-axis p) x2)))
pp)
pts))
(KdNode (l p i v r axis ci)
(if (= axis x-axis)
(cond ((and (<= x1 (coord axis p))
(<= (coord axis p) x2))
(recur l (cons p (recur r pts))))
((< (coord axis p) x1)
(recur r pts))
((< x2 (coord axis p))
(recur l pts)))
(if (and (<= x1 (coord x-axis p))
(<= (coord x-axis p) x2))
(recur l (cons p (recur r pts)))
(recur l (recur r pts)))
))
))
))
(define=> (make-kd-tree-slice* )
(lambda (x-axis x1 x2 t)
(let recur ((t t) (pts '()))
(cases kd-tree t
(KdLeaf (ii pp vv axis)
(append
(if vv
(filter-map (lambda (i v p)
(and (<= x1 (coord x-axis p))
(<= (coord x-axis p) x2)
(list (list i v) p)))
(reverse (cis:elements ii)) vv pp)
(filter-map (lambda (i p)
(and (<= x1 (coord x-axis p))
(<= (coord x-axis p) x2)
(list i p)))
(reverse (cis:elements ii)) pp))
pts))
(KdNode (l p i v r axis ci)
(if (= axis x-axis)
(cond ((and (<= x1 (coord axis p))
(<= (coord axis p) x2))
(recur l (cons (if v (list (list i v) p) (list i p)) (recur r pts))))
((< (coord axis p) x1)
(recur r pts))
((< x2 (coord axis p))
(recur l pts)))
(if (and (<= x1 (coord x-axis p))
(<= (coord x-axis p) x2))
(recur l (cons (if v (list (list i v) p) (list i p)) (recur r pts)))
(recur l (recur r pts)))
))
))
))
(define (default- point-class)
(let* ((list->kd-tree/depth
(make-list->kd-tree/depth point-class))
(kd-tree-remove
((make-kd-tree-remove point-class)
(list->kd-tree/depth identity #f)
(list->kd-tree/depth cadr (lambda (i v) (cadar v)))))
(kd-tree-nearest-neighbor
(make-kd-tree-nearest-neighbor point-class)))
(make-
(lambda (points #!key
(leaf-factor 10)
(make-point identity)
(make-value #f)
(offset 0)
)
((list->kd-tree/depth make-point make-value)
0 (length points) points 0
leaf-factor: leaf-factor offset: offset))
(make-kd-tree-nearest-neighbor point-class)
(make-kd-tree-nearest-neighbor* point-class)
(make-kd-tree-near-neighbors point-class)
(make-kd-tree-near-neighbors* point-class)
((make-kd-tree-k-nearest-neighbors point-class)
kd-tree-remove kd-tree-nearest-neighbor)
kd-tree-remove
(make-kd-tree-slice point-class)
(make-kd-tree-slice* point-class)
(make-kd-tree-is-valid? point-class)
(make-kd-tree-all-subtrees-are-valid?
(make-kd-tree-is-valid? point-class))
)))
(define KdTree3d
(default- Point3d))
(define KdTree2d
(default- Point2d))
)