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

Last change on this file since 27836 was 27836, checked in by Ivan Raikov, 7 years ago

kd-tree: specialized tree-rebuilding routine for remove procedure

File size: 29.5 KB
Line 
1;;
2;; An implementation of the K-d tree spatial indexing data structure.
3;;
4;; http://en.wikipedia.org/wiki/K-d_tree
5;;
6;; The k-d tree is a binary search tree in which every branching node
7;; contains a k-dimensional point, and every leaf node contains a set
8;; of points. Every branching node represents a splitting hyperplane
9;; that divides the space into two parts, known as half-spaces.
10;;
11;; Points to the left of the splitting hyperplane are contained in the
12;; left subtree of the node and points right of the hyperplane are
13;; contained in the right subtree. The splitting hyperplane is chosen
14;; so as to be perpendicular to one of the axes in the k-dimensional
15;; space. The axis at each branching level is chosen in a round-robin
16;; fashion. For instance, in 3-D space, at level 0, the chosen axis is
17;; X, so points are divided according to their X-coordinates; at level
18;; 1, the chosen axis is Y, so the points are divided according to
19;; their Y-coordinates; at the next branch level the chosen axis is Z,
20;; and so on.
21;;
22;;
23;; This code is based on the Haskell kd-tree library implementation of
24;; K-D trees.
25;;
26;; Copyright 2012 Ivan Raikov and the Okinawa Institute of
27;; Science and Technology.
28;;
29;; This program is free software: you can redistribute it and/or
30;; modify it under the terms of the GNU General Public License as
31;; published by the Free Software Foundation, either version 3 of the
32;; License, or (at your option) any later version.
33;;
34;; This program is distributed in the hope that it will be useful, but
35;; WITHOUT ANY WARRANTY; without even the implied warranty of
36;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
37;; General Public License for more details.
38;;
39;; A full copy of the GPL license can be found at
40;; <http://www.gnu.org/licenses/>.
41;;
42
43(module kd-tree
44       
45  (
46   <Point> default-<Point> Point3d Point2d
47   point? make-point
48
49   <KdTree> default-<KdTree> KdTree3d KdTree2d
50
51   kd-tree? 
52   kd-tree-empty?
53   kd-tree->list
54   kd-tree->list*
55   kd-tree-map
56   kd-tree-for-each
57   kd-tree-for-each*
58   kd-tree-fold-right
59   kd-tree-fold-right*
60   kd-tree-subtrees
61   kd-tree-node-points
62   kd-tree-node-indices
63   kd-tree-min-index
64   kd-tree-max-index
65   kd-tree-size
66  )
67
68  (import scheme chicken data-structures foreign)
69 
70  (require-library srfi-1 srfi-4 extras cis)
71  (require-extension typeclass datatype)
72
73  (import (only srfi-1 xcons fold list-tabulate split-at span every fold-right take filter filter-map remove zip)
74          (only srfi-4 f64vector-ref f64vector-set! f64vector-length make-f64vector f64vector->list
75                u32vector-ref u32vector-set!)
76          (only extras fprintf pp)
77          (only foreign foreign-lambda)
78          (prefix cis cis:))
79
80
81  (define-class <Point> 
82    ;; dimension returns the number of coordinates of a point.
83    dimension ;; Point -> Int
84
85    ;; gets the k'th coordinate, starting from 0.
86    coord ;; Int * Point -> Double
87
88    ;; compares the given coordinates
89    compare-coord ;; Int * Point * Point -> Bool
90
91    ;; returns the squared distance between two points.
92    dist2 ;; Point * Point -> Double
93
94    ;; returns 0, negative or positive number depending on the
95    ;; distance between two points
96    compare-distance
97
98    )
99
100  (define (minimum-by lst less? . rest)
101    (if (null? lst) #f
102        (if (null? rest)
103            (let recur ((lst (cdr lst)) (m (car lst)))
104              (if (null? lst) m
105                  (if (less? (car lst) m)
106                      (recur (cdr lst) (car lst))
107                      (recur (cdr lst) m)
108                      ))
109              )
110            (let recur ((lst (cdr lst)) 
111                        (rest (map cdr rest))
112                        (m (map car (cons lst rest))))
113              (if (null? lst) m
114                  (if (less? (car lst) (car m))
115                      (recur (cdr lst) (map cdr rest) (map car (cons lst rest)))
116                      (recur (cdr lst) (map cdr rest) m)
117                      ))
118              )
119            )))
120
121  (define (sum lst) (fold + 0. lst))
122
123  (define (default-<Point> dimension coord)
124
125    (let* ((dist2 
126            (lambda (a b)
127              (let ((diff2 (lambda (i) (let ((v (- (coord i a) (coord i b)))) (* v v)))))
128                (sum (list-tabulate (dimension a) diff2)))))
129
130           (compare-distance
131            (lambda (p a b . reltol)
132              (let ((delta (- (dist2 p a) (dist2 p b))))
133                (if (null? reltol) 
134                    delta 
135                    (if (<= delta (car reltol)) 0 delta)))))
136
137           (compare-coord 
138            (lambda (c a b)
139              (< (coord c a) (coord c b))))
140             
141           )
142           
143    (make-<Point> 
144     dimension coord compare-coord dist2 compare-distance)
145    ))
146
147  (define point? vector?)
148  (define make-point vector)
149
150
151  (define Point3d
152    (default-<Point> 
153     (lambda (p) (and (point? p) 3))
154     (lambda (i p) (vector-ref p i))
155     ))
156
157  (define Point2d
158    (default-<Point> 
159     (lambda (p) (and (point? p) 2))
160     (lambda (i p) (vector-ref p i))
161     ))
162
163
164  (define-class <KdTree> 
165
166    ;; constructs a kd-tree from a list of points
167    list->kd-tree
168    ;; nearest neighbor of a point
169    kd-tree-nearest-neighbor
170    ;; the index of the nearest neighbor of a point
171    kd-tree-nearest-neighbor*
172    ;; neighbors of a point within radius r
173    kd-tree-near-neighbors
174    ;; neighbors of a point within radius r (using point indices)
175    kd-tree-near-neighbors*
176    ;; k nearest neighbors of a point
177    kd-tree-k-nearest-neighbors
178    ;; removes a point from the tree
179    kd-tree-remove
180    ;; retrieves all points between two planes
181    kd-tree-slice
182    ;; retrieves all points between two planes (using point indices)
183    kd-tree-slice*
184    ;; checks that the kd-tree properties are preserved
185    kd-tree-is-valid?
186    kd-tree-all-subtrees-are-valid?
187
188    )
189
190
191  (define (positive-or-zero-integer? x)
192    (and (integer? x) (or (zero? x) (positive? x))))
193
194
195  (define (positive-integer? x)
196    (and (integer? x) (positive? x)))
197
198
199  (define-datatype kd-tree kd-tree?
200    (KdNode (left  kd-tree?)
201            (p     point?)
202            (i     positive-or-zero-integer?)
203            (v     (lambda (v) (or (not v) v)))
204            (right kd-tree?)
205            (axis  positive-or-zero-integer?)
206            (ci    cis:cis?)
207            )
208    (KdLeaf (ii cis:cis?) 
209            (pp (lambda (lst) (every point? lst))) 
210            (vv (lambda (v) (or (list? v) (not v))))
211            (axis positive-or-zero-integer?) )
212    )
213
214
215  (define (kd-tree-empty? t)
216    (cases kd-tree t
217           (KdLeaf (ii pp vv axis) (cis:empty? ii))
218           (else #f)))
219
220 
221  (define (kd-tree-map f t)
222    (cases kd-tree t
223           (KdLeaf (ii pp vv axis) 
224                   (KdLeaf ii (map f pp) vv axis))
225           (KdNode (l x i v r axis ci)
226                   (KdNode (kd-tree-map f l)
227                           (f x)
228                           i v
229                           (kd-tree-map f r)
230                           axis ci))
231           ))
232 
233  (define (kd-tree-for-each f t)
234    (cases kd-tree t
235           (KdLeaf (ii pp vv axis) (for-each f pp))
236           (KdNode (l x i v r axis ci)
237                   (begin
238                     (kd-tree-for-each f l)
239                     (f x)
240                     (kd-tree-for-each f r)
241                     ))
242           ))
243
244
245  (define (kd-tree-for-each* f t)
246    (cases kd-tree t
247           (KdLeaf (ii pp vv axis)
248                   (if vv (for-each f (zip (reverse (cis:elements ii)) vv) pp)
249                       (for-each f (reverse (cis:elements ii)) pp)))
250           (KdNode (l x i v r axis ci)
251                   (begin
252                     (kd-tree-for-each* f l)
253                     (if v (f (list i v) x) (f i x))
254                     (kd-tree-for-each* f r)
255                     ))
256           ))
257
258 
259  (define (kd-tree-fold-right f init t)
260    (cases kd-tree t
261           (KdLeaf (ii pp vv axis) 
262                   (fold-right f init pp))
263           (KdNode (l p i v r axis ci)
264                   (let* ((init2 (kd-tree-fold-right f init r))
265                          (init3 (f p init2)))
266                     (kd-tree-fold-right f init3 l)))
267           ))
268
269
270  (define (kd-tree-fold-right* f init t)
271    (cases kd-tree t
272           (KdLeaf (ii pp vv axis) 
273                   (if vv
274                       (fold-right f init (zip (reverse (cis:elements ii)) vv) pp)
275                       (fold-right f init (reverse (cis:elements ii)) pp)))
276           (KdNode (l x i v r axis ci)
277                   (let* ((init2 (kd-tree-fold-right* f init r))
278                          (init3 (if v (f (list i v) x init2) (f i x init2))))
279                     (kd-tree-fold-right* f init3 l)))
280           ))
281 
282 
283
284 
285  (define (kd-tree->list t)
286    (kd-tree-fold-right cons '() t))
287
288 
289  (define (kd-tree->list* t)
290    (kd-tree-fold-right* 
291     (lambda (iv x ax) (cons (list iv x) ax))
292     '() t))
293 
294 
295  ;; Returns a list containing t and all its subtrees, including the
296  ;; leaf nodes.
297 
298  (define (kd-tree-subtrees t)
299    (cases kd-tree t
300                  (KdLeaf (ii pp vv axis)  (list t))
301                  (KdNode (l x i v r axis ci)
302                          (append (kd-tree-subtrees l) 
303                                  (list t) 
304                                  (kd-tree-subtrees r)))
305                  ))
306
307 
308  (define (kd-tree-node-points t)
309    (cases kd-tree t
310                  (KdLeaf (ii pp vv axis)  pp)
311                  (KdNode (l x i v r axis ci) (list x))
312                  ))
313 
314  (define (kd-tree-node-indices t)
315    (cases kd-tree t
316                  (KdLeaf (ii pp vv axis) ii)
317                  (KdNode (l x i v r axis ci) ci)
318                  ))
319
320  (define (kd-tree-node-values t)
321    (cases kd-tree t
322                  (KdLeaf (ii pp vv axis) vv)
323                  (KdNode (l x i v r axis ci) (list v))
324                  ))
325
326  (define (kd-tree-size t)
327    (cases kd-tree t
328           (KdLeaf (ii pp vv axis) (cis:cardinal ii))
329           (KdNode (l x i v r axis ci) (cis:cardinal ci))))
330
331  (define (kd-tree-min-index t)
332    (cases kd-tree t
333           (KdLeaf (ii pp vv axis) (cis:get-min ii))
334           (KdNode (l x i v r axis ci) (cis:get-min ci))))
335
336  (define (kd-tree-max-index t)
337    (cases kd-tree t
338           (KdLeaf (ii pp vv axis) (cis:get-max ii))
339           (KdNode (l x i v r axis ci) (cis:get-max ci))))
340
341
342
343  ;; construct a kd-tree from a list of points
344  (define=> (make-list->kd-tree/depth <Point>)
345
346    (lambda (make-point make-value)
347
348      (letrec (
349               (split
350                (lambda (m n points depth)
351
352                  (let* ((axis   (modulo depth (dimension (make-point (car points)))))
353                         (cmpfn  (lambda (p0 p1) (compare-coord axis (make-point p0) (make-point p1))))
354                         (sorted (sort points cmpfn))
355                         (median-index (quotient (- n m) 2)))
356
357                    (let-values (((lte gte) (split-at sorted median-index)))
358
359                      (let ((median (make-point (car gte))))
360
361                        (let-values (((lt xeq) (span (lambda (x) (< (coord axis (make-point x)) (coord axis median))) lte)))
362                         
363                          (if (null? xeq)
364                              (values (car gte) median-index lt (cdr gte))
365                              (let ((split-index (length lt)))
366                                (values (car xeq) split-index lt (append (cdr xeq) gte))))
367                        ))
368                      ))
369                  ))
370
371                (list->kd-tree/depth
372                 (lambda (m n points depth #!key (leaf-factor 10) (offset 0))
373
374                  (cond
375                   ((null? points) (KdLeaf cis:empty '() '() depth))
376
377                   ((<= (- n m) leaf-factor)
378                    (let ((k (- n m)))
379
380                      (let* ((es (take points k))
381                             (ps (map make-point es)) 
382                             (ii (cis:shift offset (cis:interval m (- n 1))))
383                             (vs (and make-value 
384                                      (map (lambda (i e) (make-value i e))
385                                           (reverse (cis:elements ii)) 
386                                           es)))
387                             )
388                       
389                        (KdLeaf ii ps vs (modulo depth (dimension (car ps))))
390                        )))
391                   
392                   ((null? (cdr points))
393                    (let* ((e (car points))
394                           (ps (list (make-point e)) )
395                           (vs (and make-value (list (make-value m e)))))
396                     
397                      (KdLeaf (cis:shift offset (cis:singleton m) )
398                              ps vs
399                              (modulo depth (dimension (car ps))))
400                      ))
401
402                   (else
403                    (let-values (((median median-index lt gte)
404                                  (split m n points depth)))
405
406                     
407                      (let* ((depth1 (+ 1 depth))
408                             (i (+ m median-index offset))
409                             (p (make-point median))
410                             (v (and make-value (make-value i median)))
411                             (axis (modulo depth (dimension p))))
412                       
413                        (KdNode (list->kd-tree/depth m (+ m median-index) lt depth1 
414                                                     leaf-factor: leaf-factor)
415                                p i v
416                                (list->kd-tree/depth (+ m median-index 1) n gte depth1 
417                                                     leaf-factor: leaf-factor)
418                                axis 
419                                (cis:shift offset (cis:interval m (- n 1))))))
420                    )))
421                 ))
422        list->kd-tree/depth
423      )))
424 
425  ;; Returns the nearest neighbor of p in tree t.
426 
427  (define=> (make-kd-tree-nearest-neighbor <Point>)
428    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
429    (letrec ((find-nearest
430              (lambda (t1 t2 p probe xp x-probe)
431
432                (let* ((candidates1 
433                        (let ((best1 (nearest-neighbor t1 probe)))
434                          (or (and best1 (list best1 p)) (list p))))
435                       
436                       (sphere-intersects-plane? 
437                        (let ((v (- x-probe xp)))
438                          (< (* v v) (dist2 probe (car candidates1)))))
439
440                       (candidates2
441                        (if sphere-intersects-plane?
442                            (let ((nn (nearest-neighbor t2 probe)))
443                              (if nn (append candidates1 (list nn)) candidates1))
444                            candidates1)))
445
446                  (minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe a b))))
447                  )))
448
449             
450             (nearest-neighbor
451              (lambda (t probe)
452                (cases kd-tree t
453                       (KdLeaf (ii pp vv axis) 
454                               (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b)))))
455
456                       (KdNode (l p i v r axis ci)
457                               (if (and (tree-empty? l)
458                                        (tree-empty? r)) p
459                                        (let ((x-probe (coord axis probe))
460                                              (xp (coord axis p)))
461
462                                          (if (< x-probe xp) 
463                                              (find-nearest l r p probe xp x-probe) 
464                                              (find-nearest r l p probe xp x-probe))
465                                          ))
466                               ))
467                )))
468     
469      nearest-neighbor
470      ))
471 
472  ;; Returns the index of the nearest neighbor of p in tree t.
473 
474  (define=> (make-kd-tree-nearest-neighbor* <Point>)
475
476    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
477
478    (letrec ((find-nearest
479              (lambda (t1 t2 i v p probe xp x-probe)
480
481                (let* ((candidates1 
482                        (let ((best1 (nearest-neighbor t1 probe)))
483                          (or (and best1 (list best1 (if v (list (list i v) p) (list i p))))
484                              (list (if v (list (list i v) p) (list i p))))
485                          ))
486                       
487                       (sphere-intersects-plane? 
488                        (let ((v (- x-probe xp)))
489                          (< (* v v) (dist2 probe (cadar candidates1)))))
490
491                       (candidates2
492                        (if sphere-intersects-plane?
493                            (let ((nn (nearest-neighbor t2 probe)))
494                              (if nn (append candidates1 (list nn)) candidates1))
495                            candidates1)))
496
497                  (let ((v (minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe (cadr a) (cadr b)))))))
498                    v)
499                  )))
500
501             
502             (nearest-neighbor
503              (lambda (t probe)
504                (cases kd-tree t
505                       (KdLeaf (ii pp vv axis) 
506                               (let ((res
507                                      (if vv
508                                          (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))) 
509                                                      (zip (reverse (cis:elements ii)) vv ))
510                                          (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))) 
511                                                      (reverse (cis:elements ii))))))
512                                 (and res (reverse res))))
513
514                       (KdNode (l p i v r axis ci)
515
516                               (if (and (tree-empty? l)
517                                        (tree-empty? r))
518
519                                   (if v (list (list i v) p) (list i p))
520
521                                   (let ((x-probe (coord axis probe))
522                                         (xp (coord axis p))
523                                         (xi i))
524                                     (if (< x-probe xp) 
525                                         (find-nearest l r i v p probe xp x-probe) 
526                                         (find-nearest r l i v p probe xp x-probe))
527                                     ))
528                               ))
529                )))
530     
531      nearest-neighbor
532      ))
533 
534   
535  ;; near-neighbors t r p returns all neighbors within distance r from p in tree t.
536
537  (define=> (make-kd-tree-near-neighbors <Point>)
538    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
539    (letrec ((near-neighbors
540              (lambda (t radius probe fdist filter-fn)
541                (cases kd-tree t
542                       (KdLeaf (ii pp vv axis) 
543                               (let ((r2 (* radius radius)))
544                                 (filter-fn probe pp r2)))
545
546                       (KdNode (l p i v r axis ci)
547                               (let ((maybe-pivot (filter-fn probe (list p) (* radius radius))))
548                                 
549                                 (if (and (tree-empty? l)
550                                          (tree-empty? r))
551
552                                     maybe-pivot
553
554                                     (let ((x-probe (coord axis probe))
555                                           (xp (coord axis p)))
556
557                                       (if (< x-probe xp)
558
559                                           (let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist filter-fn))))
560                                             (if (> (+ x-probe (abs radius)) xp)
561                                                 (append (near-neighbors r radius probe fdist filter-fn) nearest)
562                                                 nearest))
563
564                                           (let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist filter-fn))))
565                                             (if (< (- x-probe (abs radius)) xp)
566                                                 (append (near-neighbors l radius probe fdist filter-fn) nearest)
567                                                 nearest)))
568                                       ))))
569                       ))
570              ))
571      (lambda (t radius probe #!key (with-distance? #f))
572        (let* (
573               (filter-fn (if with-distance? 
574                              (lambda (probe pp d2) (filter-map (lambda (p) (let ((pd (dist2 probe p)))
575                                                                              (and (<= pd d2) (list p (sqrt pd) )))) pp))
576                              (lambda (probe pp d2) (filter (lambda (p) (<= (dist2 probe p) d2)) pp))
577                              ))
578               )
579
580          (near-neighbors t radius probe dist2 filter-fn)
581          ))
582      ))
583
584
585  (define=> (make-kd-tree-near-neighbors* <Point>)
586    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
587    (letrec ((near-neighbors
588              (lambda (t radius probe fdist filter-fn)
589                (cases kd-tree t
590
591                       (KdLeaf (ii pp vv axis) 
592                               (let ((r2 (* radius radius)))
593                                 (filter-fn probe pp ii vv r2)))
594
595                       (KdNode (l p i v r axis ci)
596                               (let ((maybe-pivot (filter-fn probe (list p) (cis:singleton i) (list v) (* radius radius))))
597
598                                 (if (and (tree-empty? l)
599                                          (tree-empty? r))
600
601                                     maybe-pivot
602
603                                     (let ((x-probe (coord axis probe))
604                                           (xp (coord axis p)))
605
606                                       (if (< x-probe xp)
607
608                                           (let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist filter-fn))))
609                                             (if (> (+ x-probe (abs radius)) xp)
610                                                 (append (near-neighbors r radius probe fdist filter-fn) nearest)
611                                                 nearest))
612
613                                           (let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist filter-fn))))
614                                             (if (< (- x-probe (abs radius)) xp)
615                                                 (append (near-neighbors l radius probe fdist filter-fn) nearest)
616                                                 nearest)))
617                                       ))
618                                 ))
619                       ))
620              ))
621      (lambda (t radius probe #!key (with-distance? #f))
622        (let* ((dist-fn dist2)
623               (filter-fn (if with-distance? 
624                              (lambda (probe pp ii vv r2)
625                                 (if vv 
626                                     (filter-map (lambda (i v p) (let ((pd (dist-fn probe p))) (and (<= pd r2) (list (list i v) p (sqrt pd)))))
627                                                 (reverse (cis:elements ii)) vv pp)
628                                     (filter-map (lambda (i p) (let ((pd (dist-fn probe p))) (and (<= pd r2) (list i p (sqrt pd)))))
629                                                 (reverse (cis:elements ii)) pp)
630                                     ))
631                              (lambda (probe pp ii vv r2)
632                                 (if vv 
633                                     (filter-map (lambda (i v p) (and (<= (dist-fn probe p) r2) (list (list i v) p)))
634                                                 (reverse (cis:elements ii)) vv pp)
635                                     (filter-map (lambda (i p) (and (<= (dist-fn probe p) r2) (list i p)))
636                                                 (reverse (cis:elements ii)) pp)
637                                     ))
638                              ))
639               )
640
641          (near-neighbors t radius probe dist-fn filter-fn)
642
643          ))
644      ))
645 
646
647 
648  ;; Returns the k nearest points to p within tree.
649  (define=> (make-kd-tree-k-nearest-neighbors <Point>)
650    (lambda (kd-tree-remove kd-tree-nearest-neighbor)
651      (letrec ((k-nearest-neighbors
652                (lambda (t k probe)
653                  (cases kd-tree t
654
655                       (KdLeaf (ii pp vv axis) 
656                               (let recur ((res '()) (pp pp) (k k))
657                                 (if (or (<= k 0) (null? pp)) 
658                                     res
659                                     (let ((nearest (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))))))
660                                       (recur (cons nearest res)
661                                              (remove (lambda (p) (equal? p nearest)) pp)
662                                              (- k 1))
663                                       ))
664                                 ))
665
666                       (else
667                        (if (<= k 0) '()
668                            (let* ((nearest (kd-tree-nearest-neighbor t probe))
669                                   (tree1 (kd-tree-remove t nearest)))
670                              (cons nearest (k-nearest-neighbors tree1 (- k 1) probe)))
671                            ))
672                       ))
673                ))
674        k-nearest-neighbors)))
675
676
677  (define (kd-elt-index x) (let ((xp (car x))) (if (number? xp) xp (car xp))))
678  (define (kd-elt-value x) (and (pair? (car x)) (cadar x)))
679  (define (kd-elt-point x) (cadr x))
680 
681  ;; removes the point p from t.
682  (define=> (make-kd-tree-remove <Point>)
683      (letrec (
684               ;; a variant of list->kd-tree specialized for
685               ;; kd-tree-remove. Specifically, it preserves the
686               ;; original point indices.  This variant of
687               ;; list->kd-tree assumes that the input list of points
688               ;; has the form ( (INDEX POINT VALUE) ... )
689
690               (split
691                (lambda (points depth)
692
693;                 (fprintf (current-error-port) "kd-tree-remove: split: points = ~A~%" points)
694                 
695                  (let* ((axis   (modulo depth (dimension (kd-elt-point (car points)))))
696                         (cmpfn  (lambda (p0 p1) (compare-coord axis (kd-elt-point p0) (kd-elt-point p1))))
697                         (sorted (sort points cmpfn))
698                         (median-index (quotient (length sorted) 2))
699                         )
700
701                    (let-values (((lte gte) (split-at sorted median-index)))
702
703                      (let ((median (kd-elt-point (car gte))))
704
705                        (let-values (((lt xeq) (span (lambda (x) (< (coord axis (kd-elt-point x)) (coord axis median))) lte)))
706                         
707                          (if (null? xeq)
708                              (values (car gte) lt (cdr gte))
709                              (let ((split-index (length lt)))
710                                (values (car xeq) lt (append (cdr xeq) gte))))
711                        ))
712                      ))
713                  ))
714
715                (list->kd-tree/depth
716                 (lambda (points depth #!key (leaf-factor 50))
717
718;                 (fprintf (current-error-port) "kd-tree-remove:  points = ~A~%" points)
719                   
720                   (cond
721
722                    ((null? points) (KdLeaf cis:empty '() '() depth))
723                   
724                    ((<= (length points) leaf-factor)
725
726                       (let* (
727                              (ps (map kd-elt-point points)) 
728                              (ii (fold (lambda (x ax) (cis:add x ax)) cis:empty (map kd-elt-index points)))
729                              (vs (let ((vs (map kd-elt-value points))) (and (every identity vs) vs)))
730                             )
731                       
732                        (KdLeaf ii ps vs (modulo depth (dimension (car ps))))
733                        )
734                       )
735                   
736                    ((null? (cdr points))
737
738                     (let* ((ps (map kd-elt-point points))
739                            (ii (fold (lambda (x ax) (cis:add x ax)) cis:empty (map kd-elt-index points)))
740                            (vs (map kd-elt-value points)))
741                       
742                       (KdLeaf ii ps vs (modulo depth (dimension (car ps))))
743                       )
744                     )
745                   
746                   (else
747                    (let-values (((median lt gte)
748                                  (split points depth)))
749                     
750                      (let* ((depth1 (+ 1 depth))
751                             (i (kd-elt-index median))
752                             (p (kd-elt-point median))
753                             (v (kd-elt-value median))
754                             (axis (modulo depth (dimension p)))
755                             (l (list->kd-tree/depth lt depth1 leaf-factor: leaf-factor))
756                             (r (list->kd-tree/depth gte depth1 leaf-factor: leaf-factor))
757                             )
758
759                        (KdNode l p i v r axis 
760                                (cis:add i (cis:union (kd-tree-node-indices l) (kd-tree-node-indices r))))
761                        ))
762                    ))
763                 ))
764
765               (tree-remove
766                (lambda (t p-kill #!key (leaf-factor 10))
767
768                 
769;                  (fprintf (current-error-port) "kd-tree-remove: t = ~A~%" t)
770;                  (fprintf (current-error-port) "kd-tree-remove: p-kill = ~A~%" p-kill)
771
772                  (cases kd-tree t
773                         (KdLeaf (ii pp vv axis) 
774
775;                                 (fprintf (current-error-port) "kd-tree-remove (KdLeaf): ii = ~A~%" (cis:elements ii))
776;                                 (fprintf (current-error-port) "kd-tree-remove (KdLeaf): vv = ~A~%" vv)
777;                                (fprintf (current-error-port) "kd-tree-remove (KdLeaf): pp = ~A~%" pp)
778                                         
779                                 (if vv
780                                     (let ((ipvs (filter-map
781                                                  (lambda (i p v) (and (equal? p p-kill) (list i p v)))
782                                                  (reverse (cis:elements ii)) pp vv)))
783
784;                                        (fprintf (current-error-port) "kd-tree-remove (KdLeaf): ipvs = ~A~%" ipvs)
785
786                                       (and (pair? ipvs)
787                                            (let ((ii1 (fold (lambda (i ax) (cis:remove i ax)) 
788                                                             ii (map car ipvs)))
789                                                  (pp1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
790                                                             pp (map cadr ipvs)))
791                                                  (vv1 (fold (lambda (x ax)
792                                                               (remove (lambda (p) (equal? p x)) ax))
793                                                             vv (map caddr ipvs)))
794                                                  )
795;                                              (fprintf (current-error-port) "kd-tree-remove (KdLeaf): ii1 = ~A~%" (cis:elements ii1))
796;                                              (fprintf (current-error-port) "kd-tree-remove (KdLeaf): pp1 = ~A~%" pp1)
797;                                              (fprintf (current-error-port) "kd-tree-remove (KdLeaf): vv1 = ~A~%" vv1)
798
799                                              (KdLeaf ii1 pp1 vv1 axis))
800                                            ))
801
802                                     (let ((ips (filter-map (lambda (i p) (and (equal? p p-kill) (list i p)))
803                                                            (reverse (cis:elements ii)) pp)))
804                                       
805;                                      (fprintf (current-error-port) "kd-tree-remove (KdLeaf): ips = ~A~%" ips)
806
807                                       (and (pair? ips)
808                                            (let ((ii1 (fold (lambda (i ax) (cis:remove i ax)) 
809                                                             ii (map car ips)))
810                                                  (pp1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
811                                                             pp (map cadr ips)))
812                                                  )
813
814;                                              (fprintf (current-error-port) "kd-tree-remove (KdLeaf): ii1 = ~A~%" (cis:elements ii1))
815;                                              (fprintf (current-error-port) "kd-tree-remove (KdLeaf): pp1 = ~A~%" pp1)
816
817                                              (KdLeaf ii1 pp1 vv axis))
818                                            ))
819                                     ))
820
821                         (KdNode (l p i v r axis ci)
822
823                                 (cond ((equal? p p-kill)
824                                        (let ((pts1 (append (kd-tree->list* l) (kd-tree->list* r))))
825                                          (list->kd-tree/depth pts1 axis leaf-factor: leaf-factor)))
826
827                                       (else
828
829                                        (if (< (coord axis p-kill) (coord axis p))
830
831                                         (let* ((l1   (tree-remove l p-kill))
832                                                (l1-is (and l1 (kd-tree-node-indices l1)))
833                                                (r-is  (kd-tree-node-indices r))
834                                                (ci1   (cis:add i (cis:union l1-is r-is))))
835
836                                           (and l1 (KdNode l1 p i v r axis ci1))
837                                           )
838                                         
839                                         (let* ((r1   (tree-remove r p-kill))
840                                                (r1-is (and r1 (kd-tree-node-indices r1)))
841                                                (l-is  (kd-tree-node-indices l))
842                                                (ci1   (cis:add i (cis:union r1-is l-is))))
843
844                                           (and r1 (KdNode l p i v r1 axis ci1))
845
846                                           )))
847                                     
848                                     ))
849                         ))
850                ))
851        tree-remove))
852
853
854  ;; Checks whether the K-D tree property holds for a given tree.
855  ;;
856  ;; Specifically, it tests that all points in the left subtree lie to
857  ;; the left of the plane, p is on the plane, and all points in the
858  ;; right subtree lie to the right.
859 
860  (define=> (make-kd-tree-is-valid? <Point>)
861    (lambda (t)
862      (cases kd-tree t
863             (KdLeaf (ii pp vv axis)  #t)
864
865             (KdNode (l p i v r axis ci)
866                     (let ((x (coord axis p)))
867                       (and (every (lambda (y) (< (coord axis y) x ))
868                                   (kd-tree->list l))
869                            (every (lambda (y) (>= (coord axis y) x))
870                                   (kd-tree->list r)))))
871             )))
872 
873 
874  ;; Checks whether the K-D tree property holds for the given tree and
875  ;; all subtrees.
876 
877  (define (make-kd-tree-all-subtrees-are-valid? kd-tree-is-valid?)
878    (lambda (t) (every kd-tree-is-valid? (kd-tree-subtrees t))))
879 
880
881  (define=> (make-kd-tree-slice <Point>)
882    (lambda (x-axis x1 x2 t)
883      (let recur ((t t)  (pts '()))
884        (cases kd-tree t
885
886               (KdLeaf (ii pp vv axis) 
887                       (append (filter (lambda (p) 
888                                         (and (<= x1 (coord x-axis p))
889                                              (<= (coord x-axis p) x2)))
890                                       pp)
891                               pts))
892                           
893
894               (KdNode (l p i v r axis ci)
895                       (if (= axis x-axis)
896                           
897                           (cond ((and (<= x1 (coord axis p))
898                                       (<= (coord axis p) x2))
899                                   (recur l (cons p (recur r pts))))
900                                 
901                                 ((< (coord axis p) x1)
902                                  (recur r pts))
903                               
904                                 ((< x2 (coord axis p))
905                                  (recur l pts)))
906                           
907                           (if (and (<= x1 (coord x-axis p))
908                                    (<= (coord x-axis p) x2))
909                               (recur l (cons p (recur r pts)))
910                               (recur l (recur r pts)))
911                           ))
912               ))
913      ))
914 
915 
916  (define=> (make-kd-tree-slice* <Point>)
917    (lambda (x-axis x1 x2 t)
918      (let recur ((t t)  (pts '()))
919        (cases kd-tree t
920               (KdLeaf (ii pp vv axis) 
921                       (append
922                        (if vv
923                            (filter-map (lambda (i v p) 
924                                          (and (<= x1 (coord x-axis p))
925                                               (<= (coord x-axis p) x2)
926                                               (list (list i v) p)))
927                                        (reverse (cis:elements ii)) vv pp)
928                            (filter-map (lambda (i p) 
929                                          (and (<= x1 (coord x-axis p))
930                                               (<= (coord x-axis p) x2)
931                                               (list i p)))
932                                        (reverse (cis:elements ii)) pp))
933                        pts))
934
935               (KdNode (l p i v r axis ci)
936                       (if (= axis x-axis)
937                           
938                           (cond ((and (<= x1 (coord axis p))
939                                       (<= (coord axis p) x2))
940                                   (recur l (cons (if v (list (list i v) p) (list i p)) (recur r pts))))
941                                 
942                                 ((< (coord axis p) x1)
943                                  (recur r pts))
944                               
945                                 ((< x2 (coord axis p))
946                                  (recur l pts)))
947                           
948                           (if (and (<= x1 (coord x-axis p))
949                                    (<= (coord x-axis p) x2))
950                               (recur l (cons (if v (list (list i v) p) (list i p)) (recur r pts)))
951                               (recur l (recur r pts)))
952                           ))
953               ))
954      ))
955
956  (define (default-<KdTree> point-class)
957    (let* ((list->kd-tree/depth
958            (make-list->kd-tree/depth point-class))
959           (kd-tree-remove
960            (make-kd-tree-remove point-class) )
961           (kd-tree-nearest-neighbor
962            (make-kd-tree-nearest-neighbor point-class)))
963
964      (make-<KdTree> 
965       (lambda (points #!key
966                       (leaf-factor 10) 
967                       (make-point identity) 
968                       (make-value #f)
969                       (offset 0)
970                       )
971         ((list->kd-tree/depth make-point make-value) 
972          0 (length points) points 0 
973          leaf-factor: leaf-factor offset: offset))
974
975       (make-kd-tree-nearest-neighbor point-class)
976       (make-kd-tree-nearest-neighbor* point-class)
977       (make-kd-tree-near-neighbors point-class)
978       (make-kd-tree-near-neighbors* point-class)
979       ((make-kd-tree-k-nearest-neighbors point-class)
980        kd-tree-remove kd-tree-nearest-neighbor)
981       kd-tree-remove
982       (make-kd-tree-slice point-class)
983       (make-kd-tree-slice* point-class)
984       (make-kd-tree-is-valid? point-class)
985       (make-kd-tree-all-subtrees-are-valid? 
986        (make-kd-tree-is-valid? point-class)) 
987       )))
988
989  (define KdTree3d 
990    (default-<KdTree> Point3d))
991
992  (define KdTree2d 
993    (default-<KdTree> Point2d))
994
995
996
997)
998
Note: See TracBrowser for help on using the repository browser.