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

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

kd-tree: bug fixes related to changed internal node representation

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