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

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

spatial-trees: added kd-tree-size procedure

File size: 24.3 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                  (let ((v (minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe (cadr a) (cadr b)))))))
556                    v)
557                  )))
558
559             
560             (nearest-neighbor
561              (lambda (t probe)
562                (cases kd-tree t
563                       (KdLeaf (ii pp vv axis) 
564                               (let ((v
565                                      (if vv
566                                          (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))) 
567                                                      (zip (reverse (cis:elements ii)) vv ))
568                                          (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))) 
569                                                      (reverse (cis:elements ii))))))
570                                 (and v (reverse v))))
571
572                       (KdNode (l p i r axis sz)
573
574                               (if (and (tree-empty? l)
575                                        (tree-empty? r)) (list i p)
576                                        (let ((x-probe (coord axis probe))
577                                              (xp (coord axis p))
578                                              (xi i))
579
580                                          (if (<= x-probe xp) 
581                                              (find-nearest l r i p probe xp x-probe) 
582                                              (find-nearest r l i p probe xp x-probe))
583                                          ))
584                               ))
585                )))
586     
587      nearest-neighbor
588      ))
589 
590   
591  ;; nearNeighbors tree p returns all neighbors within distance r from p in tree t.
592
593  (define=> (make-kd-tree-near-neighbors <Point>)
594    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
595    (letrec ((near-neighbors
596              (lambda (t radius probe fdist)
597                (cases kd-tree t
598                       (KdLeaf (ii pp vv axis) 
599                               (let ((r2 (* radius radius)))
600                                 (filter (lambda (p) (<= (fdist probe p) r2)) pp)))
601
602                       (KdNode (l p i r axis sz)
603                               (let ((maybe-pivot
604                                      (if (<= (fdist probe p) (* radius radius)) (list p) '())))
605                                 
606                                 (if (and (tree-empty? l)
607                                          (tree-empty? r))
608
609                                     maybe-pivot
610
611                                     (let ((x-probe (coord axis probe))
612                                           (xp (coord axis p)))
613
614                                       (if (<= x-probe xp)
615
616                                           (let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist))))
617                                             (if (> (+ x-probe (abs radius)) xp)
618                                                 (append (near-neighbors r radius probe fdist) nearest)
619                                                 nearest))
620
621                                           (let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist))))
622                                             (if (< (- x-probe (abs radius)) xp)
623                                                 (append (near-neighbors l radius probe fdist) nearest)
624                                                 nearest)))
625                                       ))))
626                       ))
627              ))
628      (lambda (t radius probe #!key (factors #f))
629        (if (not factors)
630            (near-neighbors t radius probe dist2)
631            (near-neighbors t radius probe (sdist2 factors))))
632      ))
633 
634
635
636  (define=> (make-kd-tree-near-neighbors* <Point>)
637    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
638    (letrec ((near-neighbors
639              (lambda (t radius probe fdist)
640                (cases kd-tree t
641
642                       (KdLeaf (ii pp vv axis) 
643                               (let ((rr (* radius radius)))
644                                 (filter-map (lambda (p i) (and (<= (fdist probe p) rr) (cons i p)))
645                                             pp (cis:elements ii))
646                                 ))
647                                             
648
649                       (KdNode (l p i r axis sz)
650                               (let ((maybe-pivot
651                                      (if (<= (fdist probe p) (* radius radius))
652                                          (list (list i p)) '())))
653
654                                 (if (and (tree-empty? l)
655                                          (tree-empty? r))
656
657                                     maybe-pivot
658
659                                     (let ((x-probe (coord axis probe))
660                                           (xp (coord axis p)))
661
662                                       (if (<= x-probe xp)
663
664                                           (let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist))))
665                                             (if (> (+ x-probe (abs radius)) xp)
666                                                 (append (near-neighbors r radius probe fdist) nearest)
667                                                 nearest))
668
669                                           (let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist))))
670                                             (if (< (- x-probe (abs radius)) xp)
671                                                 (append (near-neighbors l radius probe fdist) nearest)
672                                                 nearest)))
673                                       ))
674                                 ))
675                       ))
676              ))
677      (lambda (t radius probe #!key (factors #f))
678        (if (not factors)
679            (near-neighbors t radius probe dist2)
680            (near-neighbors t radius probe (sdist2 factors))))
681      ))
682 
683
684 
685  ;; Returns the k nearest points to p within tree.
686  (define=> (make-kd-tree-k-nearest-neighbors <Point>)
687    (lambda (kd-tree-remove kd-tree-nearest-neighbor)
688      (letrec ((k-nearest-neighbors
689                (lambda (t k probe)
690                  (cases kd-tree t
691
692                       (KdLeaf (ii pp vv axis) 
693                               (let recur ((res '()) (pp pp) (k k))
694                                 (if (or (<= k 0) (null? pp)) 
695                                     res
696                                     (let ((nearest (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))))))
697                                       (recur (cons nearest res)
698                                              (remove (lambda (p) (equal? p nearest)) pp)
699                                              (- k 1))
700                                       ))
701                                 ))
702
703                       (else
704                        (if (<= k 0) '()
705                            (let* ((nearest (kd-tree-nearest-neighbor t probe))
706                                   (tree1 (kd-tree-remove t nearest)))
707                              (cons nearest (k-nearest-neighbors tree1 (- k 1) probe)))
708                            ))
709                       ))
710                ))
711        k-nearest-neighbors)))
712 
713 
714  ;; removes the point p from t.
715  (define=> (make-kd-tree-remove <Point>)
716    (lambda (list->kd-tree list->kd-tree*)
717      (letrec ((tree-remove
718                (lambda (t p-kill)
719                  (cases kd-tree t
720                         (KdLeaf (ii pp vv axis) 
721                                 (if vv
722                                     (let ((ipvs (filter-map
723                                                  (lambda (i p v) (and (equal? p p-kill) (list i p v)))
724                                                  (reverse (cis:elements ii)) pp vv)))
725                                       (let ((ii1 (fold (lambda (i ax) (cis:remove i ax)) 
726                                                        ii (map car ipvs)))
727                                             (pp1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
728                                                        pp (map cadr ipvs)))
729                                             (vv1 (fold (lambda (x ax)
730                                                          (remove (lambda (p) (equal? p x)) ax))
731                                                        vv (map caddr ipvs)))
732                                             )
733                                         (KdLeaf ii1 pp1 vv1 axis)
734                                         ))
735                                     (let ((ips (filter-map (lambda (i p) (and (equal? p p-kill) (list i p)))
736                                                            (reverse (cis:elements ii)) pp)))
737                                       (let ((ii1 (fold (lambda (i ax) (cis:remove i ax)) 
738                                                        ii (map car ips)))
739                                             (pp1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
740                                                        pp (map cadr ips)))
741                                             )
742                                         (KdLeaf ii1 pp1 vv axis)
743                                         ))
744                                     ))
745
746                         (KdNode (l p i r axis sz)
747                                 (if (equal? p p-kill)
748                                     (if (integer? i)
749                                         (let ((pts1 (append (kd-tree->list l) (kd-tree->list r))))
750                                           (list->kd-tree 0 (length pts1) pts1 axis))
751                                         (let ((pts1 (append (kd-tree->list* l) (kd-tree->list* r))))
752                                           (list->kd-tree* 0 (length pts1) pts1 axis)))
753                                     (if (<= (coord axis p-kill)
754                                             (coord axis p))
755                                         (let ((l1 (tree-remove l p-kill))) 
756                                           (KdNode l1 p i r axis (+ (kd-tree-size l1) (kd-tree-size r))))
757                                         (let ((r1 (tree-remove r p-kill))) 
758                                           (KdNode l p i r1 axis (+ (kd-tree-size l) (kd-tree-size r1)))))
759                                     ))
760                         ))
761                ))
762        tree-remove)))
763
764
765  ;; Checks whether the K-D tree property holds for a given tree.
766  ;;
767  ;; Specifically, it tests that all points in the left subtree lie to
768  ;; the left of the plane, p is on the plane, and all points in the
769  ;; right subtree lie to the right.
770 
771  (define=> (make-kd-tree-is-valid? <Point>)
772    (lambda (t)
773      (cases kd-tree t
774             (KdLeaf (ii pp vv axis)  #t)
775
776             (KdNode (l p i r axis sz)
777                     (let ((x (coord axis p)))
778                       (and (every (lambda (y) (<= (coord axis y) x ))
779                                   (kd-tree->list l))
780                            (every (lambda (y) (>= (coord axis y) x))
781                                   (kd-tree->list r)))))
782             )))
783 
784 
785  ;; Checks whether the K-D tree property holds for the given tree and
786  ;; all subtrees.
787 
788  (define (make-kd-tree-all-subtrees-are-valid? kd-tree-is-valid?)
789    (lambda (t) (every kd-tree-is-valid? (kd-tree-subtrees t))))
790 
791
792  (define=> (make-kd-tree-slice <Point>)
793    (lambda (x-axis x1 x2 t)
794      (let recur ((t t)  (pts '()))
795        (cases kd-tree t
796
797               (KdLeaf (ii pp vv axis) 
798                       (append (filter (lambda (p) 
799                                         (and (<= x1 (coord x-axis p))
800                                              (<= (coord x-axis p) x2)))
801                                       pp)
802                               pts))
803                           
804
805               (KdNode (l p i r axis sz)
806                       (if (= axis x-axis)
807                           
808                           (cond ((and (<= x1 (coord axis p))
809                                       (<= (coord axis p) x2))
810                                   (recur l (cons p (recur r pts))))
811                                 
812                                 ((< (coord axis p) x1)
813                                  (recur r pts))
814                               
815                                 ((< x2 (coord axis p))
816                                  (recur l pts)))
817                           
818                           (if (and (<= x1 (coord x-axis p))
819                                    (<= (coord x-axis p) x2))
820                               (recur l (cons p (recur r pts)))
821                               (recur l (recur r pts)))
822                           ))
823               ))
824      ))
825 
826 
827  (define=> (make-kd-tree-slice* <Point>)
828    (lambda (x-axis x1 x2 t)
829      (let recur ((t t)  (pts '()))
830        (cases kd-tree t
831               (KdLeaf (ii pp vv axis) 
832                       (append (filter-map (lambda (p i) 
833                                             (and (<= x1 (coord x-axis p))
834                                                  (<= (coord x-axis p) x2)
835                                                  (cons i p)))
836                                       pp (cis:elements ii))
837                               pts))
838
839               (KdNode (l p i r axis sz)
840                       (if (= axis x-axis)
841                           
842                           (cond ((and (<= x1 (coord axis p))
843                                       (<= (coord axis p) x2))
844                                   (recur l (cons (cons i p) (recur r pts))))
845                                 
846                                 ((< (coord axis p) x1)
847                                  (recur r pts))
848                               
849                                 ((< x2 (coord axis p))
850                                  (recur l pts)))
851                           
852                           (if (and (<= x1 (coord x-axis p))
853                                    (<= (coord x-axis p) x2))
854                               (recur l (cons (cons i p) (recur r pts)))
855                               (recur l (recur r pts)))
856                           ))
857               ))
858      ))
859
860  (define (default-<KdTree> point-class)
861    (let* ((list->kd-tree/depth
862            (make-list->kd-tree/depth point-class))
863           (f64vector->kd-tree/depth
864            (make-f64vector->kd-tree/depth point-class))
865           (kd-tree-remove
866            ((make-kd-tree-remove point-class) 
867             (list->kd-tree/depth identity #f)
868             (list->kd-tree/depth cadr (lambda (i v) (cadar v)))))
869           (kd-tree-nearest-neighbor
870            (make-kd-tree-nearest-neighbor point-class)))
871
872      (make-<KdTree> 
873       (lambda (points #!key (leaf-factor 10) (point-ref identity) (make-value #f))
874         ((list->kd-tree/depth point-ref make-value) 0 (length points) points 0 leaf-factor: leaf-factor))
875
876       (lambda (axial-vectors #!key (leaf-factor 10) (point-ref list->vector) (make-value #f))
877         (let ((dimensions (length axial-vectors))
878               (len (f64vector-length (car axial-vectors))))
879           (if (zero? len) (KdLeaf cis:empty '() '() 0)
880               ((f64vector->kd-tree/depth dimensions point-ref make-value) 0 (- len 1) axial-vectors 0 leaf-factor: leaf-factor))))
881
882       (make-kd-tree-nearest-neighbor point-class)
883       (make-kd-tree-nearest-neighbor* point-class)
884       (make-kd-tree-near-neighbors point-class)
885       (make-kd-tree-near-neighbors* point-class)
886       ((make-kd-tree-k-nearest-neighbors point-class)
887        kd-tree-remove kd-tree-nearest-neighbor)
888       kd-tree-remove
889       (make-kd-tree-slice point-class)
890       (make-kd-tree-slice* point-class)
891       (make-kd-tree-is-valid? point-class)
892       (make-kd-tree-all-subtrees-are-valid? 
893        (make-kd-tree-is-valid? point-class)) 
894       )))
895
896  (define KdTree3d 
897    (default-<KdTree> Point3d))
898
899  (define KdTree2d 
900    (default-<KdTree> Point2d))
901
902
903
904)
905
Note: See TracBrowser for help on using the repository browser.