source: project/release/4/kd-tree/trunk/kd-tree.scm @ 27415

;;
;; An implementation of the K-d tree spatial indexing data structure.
;; 
;; http://en.wikipedia.org/wiki/K-d_tree
;;
;; The k-d tree is a binary search tree in which every branching node
;; contains a k-dimensional point, and every leaf node contains a set
;; of points. Every branching node represents a splitting hyperplane
;; that divides the space into two parts, known as half-spaces.
;;
;; Points to the left of the splitting hyperplane are contained in the
;; left subtree of the node and points right of the hyperplane are
;; contained in the right subtree. The splitting hyperplane is chosen
;; so as to be perpendicular to one of the axes in the k-dimensional
;; space. The axis at each branching level is chosen in a round-robin
;; fashion. For instance, in 3-D space, at level 0, the chosen axis is
;; X, so points are divided according to their X-coordinates; at level
;; 1, the chosen axis is Y, so the points are divided according to
;; their Y-coordinates; at the next branch level the chosen axis is Z,
;; and so on.
;;
;;
;; 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
;; <http://www.gnu.org/licenses/>.
;;

(module kd-tree
        
  (
   <Point> default-<Point> Point3d Point2d
   point? make-point

   <KdTree> default-<KdTree> 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 <Point> 
    ;; 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-<Point> 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-<Point> 
     dimension coord compare-coord dist2 sdist2 compare-distance)
    ))

  (define point? vector?)
  (define make-point vector)


  (define Point3d
    (default-<Point> 
     (lambda (p) (and (point? p) 3))
     (lambda (i p) (vector-ref p i))
     ))

  (define Point2d
    (default-<Point> 
     (lambda (p) (and (point? p) 2))
     (lambda (i p) (vector-ref p i))
     ))


  (define-class <KdTree> 

    ;; 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 <Point>)

    (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 <Point>)
    (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* <Point>)

    (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 <Point>)
    (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* <Point>)
    (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 <Point>)
    (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 <Point>)
    (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? <Point>)
    (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 <Point>)
    (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* <Point>)
    (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-<KdTree> 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-<KdTree> 
       (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-<KdTree> Point3d))

  (define KdTree2d 
    (default-<KdTree> Point2d))



)