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

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

spatial-trees: bug fix in kd-tree-remove where the size of the new tree was not computed properly

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