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

Last change on this file since 27040 was 27040, checked in by Ivan Raikov, 8 years ago

spatial-trees: separate kd-tree into its own egg for now

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