source: project/release/4/spatial-trees/trunk/kd-tree.scm @ 27033

Last change on this file since 27033 was 27033, checked in by Ivan Raikov, 9 years ago

spatial-trees: added get-min and get-max operations to kd-tree

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