source: project/release/4/spatial-trees/tags/2.3/kd-tree.scm @ 26910

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

spatial-trees release 2.3

File size: 22.4 KB
Line 
1;; http://en.wikipedia.org/wiki/K-d_tree
2
3(module kd-tree
4       
5  (
6   <Point> default-<Point> Point3d Point2d
7   point? make-point
8
9   <KdTree> default-<KdTree> KdTree3d KdTree2d
10
11   kd-tree? 
12   kd-tree-empty?
13   kd-tree->list
14   kd-tree->list*
15   kd-tree-map
16   kd-tree-for-each
17   kd-tree-for-each*
18   kd-tree-fold-right
19   kd-tree-fold-right*
20   kd-tree-subtrees
21   kd-tree-points
22   kd-tree-indices
23
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)
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-datatype kd-tree kd-tree?
190    (KdNode (left  kd-tree?)
191            (p     point?)
192            (i     (lambda (v) (or (integer? v) (and (pair? v) (integer? (car v))))))
193            (right kd-tree?)
194            (axis  integer?))
195    (KdLeaf (ii cis:cis?) (pp (lambda (lst) (every point? lst))) (vv list?) (axis integer?) )
196    )
197
198  (define (kd-tree-empty? t)
199    (cases kd-tree t
200           (KdLeaf (ii pp vv axis) (cis:empty? ii))
201           (else #f)))
202 
203  (define (kd-tree->list t)
204    (kd-tree-fold-right cons '() t))
205 
206  (define (kd-tree->list* t)
207    (kd-tree-fold-right* (lambda (i x ax) (cons (list i x) ax)) '() t))
208 
209  (define (kd-tree-map f t)
210    (cases kd-tree t
211           (KdLeaf (ii pp vv axis) 
212                   (KdLeaf ii (map f pp) vv axis))
213           (KdNode (l x i r axis)
214                   (KdNode (kd-tree-map f l)
215                           (f x)
216                           i
217                           (kd-tree-map f r)
218                           axis))
219           ))
220 
221  (define (kd-tree-for-each f t)
222    (cases kd-tree t
223           (KdLeaf (ii pp vv axis) (for-each f pp))
224           (KdNode (l x i r axis)
225                   (begin
226                     (kd-tree-for-each f l)
227                     (f x)
228                     (kd-tree-for-each f r)
229                     ))
230           ))
231
232  (define (kd-tree-for-each* f t)
233    (cases kd-tree t
234           (KdLeaf (ii pp vv axis) (for-each f (cis:elements ii) pp))
235           (KdNode (l x i r axis)
236                   (begin
237                     (kd-tree-for-each* f l)
238                     (f i x)
239                     (kd-tree-for-each* f r)
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 x i r _)
248                   (let* ((init2 (kd-tree-fold-right f init r))
249                          (init3 (f x init2)))
250                     (kd-tree-fold-right f init3 l)))
251           ))
252
253  (define (kd-tree-fold-right* f init t)
254    (cases kd-tree t
255           (KdLeaf (ii pp vv axis) 
256                   (fold-right f init (reverse (cis:elements ii)) pp))
257           (KdNode (l x i r _)
258                   (let* ((init2 (kd-tree-fold-right* f init r))
259                          (init3 (f i x init2)))
260                     (kd-tree-fold-right* f init3 l)))
261           ))
262 
263 
264 
265 
266  ;; Returns a list containing t and all its subtrees, including the
267  ;; leaf nodes.
268 
269  (define (kd-tree-subtrees t)
270    (cases kd-tree t
271                  (KdLeaf (ii pp vv axis)  (list t))
272                  (KdNode (l x i r axis)
273                          (append (kd-tree-subtrees l) 
274                                  (list t) 
275                                  (kd-tree-subtrees r)))
276                  ))
277 
278  (define (kd-tree-points t)
279    (cases kd-tree t
280                  (KdLeaf (ii pp vv axis)  pp)
281                  (KdNode (l x i r axis) (list x))
282                  ))
283 
284  (define (kd-tree-indices t)
285    (cases kd-tree t
286                  (KdLeaf (ii pp vv axis) (cis:elements ii))
287                  (KdNode (l x i r axis) (list i))
288                  ))
289
290  ;; construct a kd-tree from a list of points
291  (define=> (make-list->kd-tree/depth <Point>)
292
293    (lambda (make-point make-value)
294
295      (letrec (
296               (split
297                (lambda (m n points depth)
298                  (let* ((axis   (modulo depth (dimension (make-point (car points)))))
299                         (cmpfn  (lambda (p0 p1) 
300                                   (compare-coord axis (make-point p0) (make-point p1))))
301                         (sorted (sort points cmpfn))
302                         (median-index (quotient (- n m) 2)))
303
304                    (let-values (((lt gte) (split-at sorted median-index)))
305                      (values (car gte) median-index lt (cdr gte))))
306                  ))
307
308                (list->kd-tree/depth
309                 (lambda (m n points depth #!key (leaf-factor 10))
310                  (cond
311                   ((null? points) (KdLeaf cis:empty '() '() depth))
312
313                   ((<= (- n m) leaf-factor)
314                    (let ((k (- n m)))
315
316                      (let* ((es (take points k))
317                             (ps (map make-point es)) 
318                             (ii (cis:interval m (- n 1)))
319                             (vs (map make-value (reverse (cis:elements ii)) es)))
320                        (KdLeaf ii ps vs (modulo depth (dimension (car ps))))
321                        )))
322                   
323                   ((null? (cdr points))
324                    (let* ((e (car points))
325                           (p (make-point e)) (v (make-value m e)))
326                      (KdLeaf (cis:singleton (car v)) (list p) (cdr v)
327                              (modulo depth (dimension p)))
328                      ))
329
330                   (else
331                    (let-values (((median median-index lt gte)
332                                  (split m n points depth)))
333                     
334                      (let* ((depth1 (+ 1 depth))
335                             (p (make-point median))
336                             (v (make-value (+ m median-index) median))
337                             (axis (modulo depth (dimension p))))
338                       
339                        (KdNode (list->kd-tree/depth m (+ m median-index) lt depth1 
340                                                     leaf-factor: leaf-factor)
341                                p v
342                                (list->kd-tree/depth (+ m median-index 1) n gte depth1 
343                                                     leaf-factor: leaf-factor)
344                                axis)))
345                    )))
346                 ))
347        list->kd-tree/depth
348      )))
349
350
351  (include "axial-vectors.scm")
352
353  ;; construct a kd-tree from f64vectors with point coordinates
354  ;; the points argument must be a list of f64vector for each axis:
355  ;;
356  ;;  ( F64VECTOR-X F64VECTOR-Y F64VECTOR-Z ... )
357  ;;
358
359  (define=> (make-f64vector->kd-tree/depth <Point>)
360
361    (lambda (dimensions make-point make-value)
362
363      (letrec (
364
365               (f64vector->kd-tree/depth
366
367                (lambda (m n axial-vectors depth #!key (leaf-factor 10))
368
369                  (cond
370                   ((> m n) (KdLeaf cis:empty  '() '() depth))
371
372                   ((<= (- n m) leaf-factor)
373                    (let ((k (- n m)))
374
375                      (if (zero? k)
376
377                          (let ((elt (axial-vectors-ref axial-vectors m))
378                                (axis (modulo depth dimensions)))
379                            (let* ((ii (cis:singleton m))
380                                   (es (cis:elements ii))
381                                   (ps (list (make-point elt)))
382                                   (vs (map make-value es ps)))
383
384                              (KdLeaf ii ps vs axis)))
385
386                          (let* ((sl   (axial-vectors-slice axial-vectors m n))
387                                 (axis (modulo depth dimensions))
388                                 (elt< (lambda (p0 p1) 
389                                         (compare-coord axis (make-point p0) (make-point p1)))))
390
391                            (axial-vectors-quick-sort! sl elt<)
392                           
393                            (let*((ii (cis:interval m n))
394                                  (es (reverse (cis:elements ii)))
395                                  (ps (map (compose make-point (lambda (i) (axial-vectors-ref sl (- i m)))) es))
396                                  (vs (map make-value es ps)))
397
398                              (KdLeaf ii ps vs axis)
399                              )))
400                      ))
401
402                   ((= m n)
403                    (let* ((e (axial-vectors-ref axial-vectors m))
404                           (p (make-point e)) (v (make-value m e)))
405                      (KdLeaf (cis:singleton (car v)) (list p) (cdr v)
406                              (modulo depth (dimension p)))
407                      ))
408
409                   (else
410                    (let* ((axis (modulo depth dimensions))
411                           (elt< (lambda (p0 p1) 
412                                   (compare-coord axis (make-point p0) (make-point p1)))))
413
414                      (axial-vectors-quick-sort! axial-vectors elt< m (+ 1 n))
415
416                      (let* ((depth1 (+ 1 depth))
417                             (median-index (+ m (quotient (- n m) 2)))
418                             (median (axial-vectors-ref axial-vectors median-index))
419                             (p (make-point median))
420                             (v (make-value median-index median)))
421
422                        (KdNode (f64vector->kd-tree/depth
423                                 m (- median-index 1) axial-vectors depth1 
424                                 leaf-factor: leaf-factor)
425                                p v
426                                (f64vector->kd-tree/depth 
427                                 (+ median-index 1) n axial-vectors depth1
428                                 leaf-factor: leaf-factor)
429                                axis))
430                      ))
431                   )))
432               )
433      f64vector->kd-tree/depth
434      )))
435
436 
437  ;; Returns the nearest neighbor of p in tree t.
438 
439  (define=> (make-kd-tree-nearest-neighbor <Point>)
440    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
441    (letrec ((find-nearest
442              (lambda (t1 t2 p probe xp x-probe)
443
444                (let* ((candidates1 
445                        (let ((best1 (nearest-neighbor t1 probe)))
446                          (or (and best1 (list best1 p)) (list p))))
447                       
448                       (sphere-intersects-plane? 
449                        (let ((v (- x-probe xp)))
450                          (< (* v v) (dist2 probe (car candidates1)))))
451
452                       (candidates2
453                        (if sphere-intersects-plane?
454                            (let ((nn (nearest-neighbor t2 probe)))
455                              (if nn (append candidates1 (list nn)) candidates1))
456                            candidates1)))
457
458                  (minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe a b))))
459                  )))
460
461             
462             (nearest-neighbor
463              (lambda (t probe)
464                (cases kd-tree t
465                       (KdLeaf (ii pp vv axis) 
466                               (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b)))))
467
468                       (KdNode (l p i r axis)
469                               (if (and (tree-empty? l)
470                                        (tree-empty? r)) p
471                                        (let ((x-probe (coord axis probe))
472                                              (xp (coord axis p)))
473
474                                          (if (<= x-probe xp) 
475                                              (find-nearest l r p probe xp x-probe) 
476                                              (find-nearest r l p probe xp x-probe))
477                                          ))
478                               ))
479                )))
480     
481      nearest-neighbor
482      ))
483 
484  ;; Returns the index of the nearest neighbor of p in tree t.
485 
486  (define=> (make-kd-tree-nearest-neighbor* <Point>)
487
488    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
489
490    (letrec ((find-nearest
491              (lambda (t1 t2 i p probe xp x-probe)
492
493                (let* ((candidates1 
494                        (let ((best1 (nearest-neighbor t1 probe)))
495                          (or (and best1 (list best1 (list i p)))
496                              (list (list i p)))))
497                       
498                       (sphere-intersects-plane? 
499                        (let ((v (- x-probe xp)))
500                          (< (* v v) (dist2 probe (cadar candidates1)))))
501
502                       (candidates2
503                        (if sphere-intersects-plane?
504                            (let ((nn (nearest-neighbor t2 probe)))
505                              (if nn (append candidates1 (list nn)) candidates1))
506                            candidates1)))
507                  (let ((v (minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe (cadr a) (cadr b)))))))
508                    v)
509                  )))
510
511             
512             (nearest-neighbor
513              (lambda (t probe)
514                (cases kd-tree t
515                       (KdLeaf (ii pp vv axis) 
516                               (let ((v
517                                      (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))) vv )))
518                                 (and v (reverse v))))
519
520                       (KdNode (l p i r axis)
521
522                               (if (and (tree-empty? l)
523                                        (tree-empty? r)) (list i p)
524                                        (let ((x-probe (coord axis probe))
525                                              (xp (coord axis p))
526                                              (xi i))
527
528                                          (if (<= x-probe xp) 
529                                              (find-nearest l r i p probe xp x-probe) 
530                                              (find-nearest r l i p probe xp x-probe))
531                                          ))
532                               ))
533                )))
534     
535      nearest-neighbor
536      ))
537 
538   
539  ;; nearNeighbors tree p returns all neighbors within distance r from p in tree t.
540
541  (define=> (make-kd-tree-near-neighbors <Point>)
542    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
543    (letrec ((near-neighbors
544              (lambda (t radius probe fdist)
545                (cases kd-tree t
546                       (KdLeaf (ii pp vv axis) 
547                               (let ((r2 (* radius radius)))
548                                 (filter (lambda (p) (<= (fdist probe p) r2)) pp)))
549
550                       (KdNode (l p i r axis)
551                               (let ((maybe-pivot
552                                      (if (<= (fdist probe p) (* radius radius)) (list p) '())))
553                                 
554                                 (if (and (tree-empty? l)
555                                          (tree-empty? r))
556
557                                     maybe-pivot
558
559                                     (let ((x-probe (coord axis probe))
560                                           (xp (coord axis p)))
561
562                                       (if (<= x-probe xp)
563
564                                           (let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist))))
565                                             (if (> (+ x-probe (abs radius)) xp)
566                                                 (append (near-neighbors r radius probe fdist) nearest)
567                                                 nearest))
568
569                                           (let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist))))
570                                             (if (< (- x-probe (abs radius)) xp)
571                                                 (append (near-neighbors l radius probe fdist) nearest)
572                                                 nearest)))
573                                       ))))
574                       ))
575              ))
576      (lambda (t radius probe #!key (factors #f))
577        (if (not factors)
578            (near-neighbors t radius probe dist2)
579            (near-neighbors t radius probe (sdist2 factors))))
580      ))
581 
582
583
584  (define=> (make-kd-tree-near-neighbors* <Point>)
585    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
586    (letrec ((near-neighbors
587              (lambda (t radius probe fdist)
588                (cases kd-tree t
589
590                       (KdLeaf (ii pp vv axis) 
591                               (let ((rr (* radius radius)))
592                                 (filter-map (lambda (p i) (and (<= (fdist probe p) rr) (cons i p)))
593                                             pp (cis:elements ii))
594                                 ))
595                                             
596
597                       (KdNode (l p i r axis)
598                               (let ((maybe-pivot
599                                      (if (<= (fdist probe p) (* radius radius))
600                                          (list (list i p)) '())))
601
602                                 (if (and (tree-empty? l)
603                                          (tree-empty? r))
604
605                                     maybe-pivot
606
607                                     (let ((x-probe (coord axis probe))
608                                           (xp (coord axis p)))
609
610                                       (if (<= x-probe xp)
611
612                                           (let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist))))
613                                             (if (> (+ x-probe (abs radius)) xp)
614                                                 (append (near-neighbors r radius probe fdist) nearest)
615                                                 nearest))
616
617                                           (let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist))))
618                                             (if (< (- x-probe (abs radius)) xp)
619                                                 (append (near-neighbors l radius probe fdist) nearest)
620                                                 nearest)))
621                                       ))
622                                 ))
623                       ))
624              ))
625      (lambda (t radius probe #!key (factors #f))
626        (if (not factors)
627            (near-neighbors t radius probe dist2)
628            (near-neighbors t radius probe (sdist2 factors))))
629      ))
630 
631
632 
633  ;; Returns the k nearest points to p within tree.
634  (define=> (make-kd-tree-k-nearest-neighbors <Point>)
635    (lambda (kd-tree-remove kd-tree-nearest-neighbor)
636      (letrec ((k-nearest-neighbors
637                (lambda (t k probe)
638                  (cases kd-tree t
639
640                       (KdLeaf (ii pp vv axis) 
641                               (let recur ((res '()) (pp pp) (k k))
642                                 (if (or (<= k 0) (null? pp)) 
643                                     res
644                                     (let ((nearest (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))))))
645                                       (recur (cons nearest res)
646                                              (remove (lambda (p) (equal? p nearest)) pp)
647                                              (- k 1))
648                                       ))
649                                 ))
650
651                       (else
652                        (if (<= k 0) '()
653                            (let* ((nearest (kd-tree-nearest-neighbor t probe))
654                                   (tree1 (kd-tree-remove t nearest)))
655                              (cons nearest (k-nearest-neighbors tree1 (- k 1) probe)))
656                            ))
657                       ))
658                ))
659        k-nearest-neighbors)))
660 
661 
662  ;; removes the point p from t.
663  (define=> (make-kd-tree-remove <Point>)
664    (lambda (list->kd-tree/depth)
665      (letrec ((tree-remove
666                (lambda (t p-kill)
667                  (cases kd-tree t
668                         (KdLeaf (ii pp vv axis) 
669                                 (let ((ipv (filter-map (lambda (p i v) (and (equal? p p-kill) (list i p v))) 
670                                                        pp (reverse (cis:elements ii)) vv)))
671                                   (let* ((ii1 (fold (lambda (i ax) (cis:remove i ax)) ii (map car ipv)))
672                                          (pp1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
673                                                     pp (map cadr ipv)))
674                                          (vv1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
675                                                     vv (map caddr ipv))))
676                                     (KdLeaf ii1 pp1 vv1 axis))
677                                   ))
678
679                         (KdNode (l p i r axis)
680                                 (if (equal? p p-kill)
681                                     (let ((pts1 (append (kd-tree->list l) (kd-tree->list r))))
682                                       (list->kd-tree/depth 0 (length pts1) pts1 axis))
683                                     (if (<= (coord axis p-kill)
684                                             (coord axis p))
685                                         (KdNode (tree-remove l p-kill) p i r axis)
686                                         (KdNode l p i (tree-remove r p-kill) axis))
687                                     ))
688                         ))
689                ))
690        tree-remove)))
691
692
693  ;; Checks whether the K-D tree property holds for a given tree.
694  ;;
695  ;; Specifically, it tests that all points in the left subtree lie to
696  ;; the left of the plane, p is on the plane, and all points in the
697  ;; right subtree lie to the right.
698 
699  (define=> (make-kd-tree-is-valid? <Point>)
700    (lambda (t)
701      (cases kd-tree t
702             (KdLeaf (ii pp vv axis)  #t)
703
704             (KdNode (l p i r axis)
705                     (let ((x (coord axis p)))
706                       (and (every (lambda (y) (<= (coord axis y) x ))
707                                   (kd-tree->list l))
708                            (every (lambda (y) (>= (coord axis y) x))
709                                   (kd-tree->list r)))))
710             )))
711 
712 
713  ;; Checks whether the K-D tree property holds for the given tree and
714  ;; all subtrees.
715 
716  (define (make-kd-tree-all-subtrees-are-valid? kd-tree-is-valid?)
717    (lambda (t) (every kd-tree-is-valid? (kd-tree-subtrees t))))
718 
719
720  (define=> (make-kd-tree-slice <Point>)
721    (lambda (x-axis x1 x2 t)
722      (let recur ((t t)  (pts '()))
723        (cases kd-tree t
724
725               (KdLeaf (ii pp vv axis) 
726                       (append (filter (lambda (p) 
727                                         (and (<= x1 (coord x-axis p))
728                                              (<= (coord x-axis p) x2)))
729                                       pp)
730                               pts))
731                           
732
733               (KdNode (l p i r axis)
734                       (if (= axis x-axis)
735                           
736                           (cond ((and (<= x1 (coord axis p))
737                                       (<= (coord axis p) x2))
738                                   (recur l (cons p (recur r pts))))
739                                 
740                                 ((< (coord axis p) x1)
741                                  (recur r pts))
742                               
743                                 ((< x2 (coord axis p))
744                                  (recur l pts)))
745                           
746                           (if (and (<= x1 (coord x-axis p))
747                                    (<= (coord x-axis p) x2))
748                               (recur l (cons p (recur r pts)))
749                               (recur l (recur r pts)))
750                           ))
751               ))
752      ))
753 
754 
755  (define=> (make-kd-tree-slice* <Point>)
756    (lambda (x-axis x1 x2 t)
757      (let recur ((t t)  (pts '()))
758        (cases kd-tree t
759               (KdLeaf (ii pp vv axis) 
760                       (append (filter-map (lambda (p i) 
761                                             (and (<= x1 (coord x-axis p))
762                                                  (<= (coord x-axis p) x2)
763                                                  (cons i p)))
764                                       pp (cis:elements ii))
765                               pts))
766
767               (KdNode (l p i r axis)
768                       (if (= axis x-axis)
769                           
770                           (cond ((and (<= x1 (coord axis p))
771                                       (<= (coord axis p) x2))
772                                   (recur l (cons (cons i p) (recur r pts))))
773                                 
774                                 ((< (coord axis p) x1)
775                                  (recur r pts))
776                               
777                                 ((< x2 (coord axis p))
778                                  (recur l pts)))
779                           
780                           (if (and (<= x1 (coord x-axis p))
781                                    (<= (coord x-axis p) x2))
782                               (recur l (cons (cons i p) (recur r pts)))
783                               (recur l (recur r pts)))
784                           ))
785               ))
786      ))
787
788  (define (default-<KdTree> point-class)
789    (let* ((list->kd-tree/depth
790            (make-list->kd-tree/depth point-class))
791           (f64vector->kd-tree/depth
792            (make-f64vector->kd-tree/depth point-class))
793           (kd-tree-remove
794            ((make-kd-tree-remove point-class) 
795             (list->kd-tree/depth identity (lambda (i v) i))))
796           (kd-tree-nearest-neighbor
797            (make-kd-tree-nearest-neighbor point-class)))
798
799      (make-<KdTree> 
800       (lambda (points #!key (leaf-factor 10) (point-ref identity) (make-value (lambda (i v) i)))
801         ((list->kd-tree/depth point-ref make-value) 0 (length points) points 0 leaf-factor: leaf-factor))
802
803       (lambda (axial-vectors #!key (leaf-factor 10) (point-ref list->vector) (make-value (lambda (i v) i)))
804         (let ((dimensions (length axial-vectors))
805               (len (f64vector-length (car axial-vectors))))
806           (if (zero? len) (KdLeaf cis:empty '() '() 0)
807               ((f64vector->kd-tree/depth dimensions point-ref make-value) 0 (- len 1) axial-vectors 0 leaf-factor: leaf-factor))))
808
809       (make-kd-tree-nearest-neighbor point-class)
810       (make-kd-tree-nearest-neighbor* point-class)
811       (make-kd-tree-near-neighbors point-class)
812       (make-kd-tree-near-neighbors* point-class)
813       ((make-kd-tree-k-nearest-neighbors point-class)
814        kd-tree-remove kd-tree-nearest-neighbor)
815       kd-tree-remove
816       (make-kd-tree-slice point-class)
817       (make-kd-tree-slice* point-class)
818       (make-kd-tree-is-valid? point-class)
819       (make-kd-tree-all-subtrees-are-valid? 
820        (make-kd-tree-is-valid? point-class)) 
821       )))
822
823  (define KdTree3d 
824    (default-<KdTree> Point3d))
825
826  (define KdTree2d 
827    (default-<KdTree> Point2d))
828
829
830
831)
Note: See TracBrowser for help on using the repository browser.