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

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

kd-tree: additional bugfixes due to new internal format

File size: 24.9 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 (zip (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
324                  (let* ((axis   (modulo depth (dimension (make-point (car points)))))
325                         (cmpfn  (lambda (p0 p1) 
326                                   (compare-coord axis (make-point p0) (make-point p1))))
327                         (sorted (sort points cmpfn))
328                         (median-index (quotient (- n m) 2)))
329
330                    (let-values (((lt gte) (split-at sorted median-index)))
331                      (values (car gte) median-index lt (cdr gte))))
332                  ))
333
334                (list->kd-tree/depth
335                 (lambda (m n points depth #!key (leaf-factor 10) (offset 0))
336
337                  (cond
338                   ((null? points) (KdLeaf cis:empty '() '() depth))
339
340                   ((<= (- n m) leaf-factor)
341                    (let ((k (- n m)))
342
343                      (let* ((es (take points k))
344                             (ps (map make-point es)) 
345                             (ii (cis:shift offset (cis:interval m (- n 1))))
346                             (vs (and make-value 
347                                      (map (lambda (i e) (make-value i e))
348                                           (reverse (cis:elements ii)) 
349                                           es)))
350                             )
351                       
352                        (KdLeaf ii ps vs (modulo depth (dimension (car ps))))
353                        )))
354                   
355                   ((null? (cdr points))
356                    (let* ((e (car points))
357                           (ps (list (make-point e)) )
358                           (vs (and make-value (list (make-value m e)))))
359                     
360                      (KdLeaf (cis:shift offset (cis:singleton m) )
361                              ps vs
362                              (modulo depth (dimension (car ps))))
363                      ))
364
365                   (else
366                    (let-values (((median median-index lt gte)
367                                  (split m n points depth)))
368
369                     
370                      (let* ((depth1 (+ 1 depth))
371                             (i (+ m median-index offset))
372                             (p (make-point median))
373                             (v (and make-value (make-value i median)))
374
375                             (axis (modulo depth (dimension p))))
376                       
377                        (KdNode (list->kd-tree/depth m (+ m median-index) lt depth1 
378                                                     leaf-factor: leaf-factor)
379                                p i v
380                                (list->kd-tree/depth (+ m median-index 1) n gte depth1 
381                                                     leaf-factor: leaf-factor)
382                                axis 
383                                (cis:shift offset (cis:interval m (- n 1))))))
384                    )))
385                 ))
386        list->kd-tree/depth
387      )))
388 
389  ;; Returns the nearest neighbor of p in tree t.
390 
391  (define=> (make-kd-tree-nearest-neighbor <Point>)
392    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
393    (letrec ((find-nearest
394              (lambda (t1 t2 p probe xp x-probe)
395
396                (let* ((candidates1 
397                        (let ((best1 (nearest-neighbor t1 probe)))
398                          (or (and best1 (list best1 p)) (list p))))
399                       
400                       (sphere-intersects-plane? 
401                        (let ((v (- x-probe xp)))
402                          (< (* v v) (dist2 probe (car candidates1)))))
403
404                       (candidates2
405                        (if sphere-intersects-plane?
406                            (let ((nn (nearest-neighbor t2 probe)))
407                              (if nn (append candidates1 (list nn)) candidates1))
408                            candidates1)))
409
410                  (minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe a b))))
411                  )))
412
413             
414             (nearest-neighbor
415              (lambda (t probe)
416                (cases kd-tree t
417                       (KdLeaf (ii pp vv axis) 
418                               (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b)))))
419
420                       (KdNode (l p i v r axis ci)
421                               (if (and (tree-empty? l)
422                                        (tree-empty? r)) p
423                                        (let ((x-probe (coord axis probe))
424                                              (xp (coord axis p)))
425
426                                          (if (<= x-probe xp) 
427                                              (find-nearest l r p probe xp x-probe) 
428                                              (find-nearest r l p probe xp x-probe))
429                                          ))
430                               ))
431                )))
432     
433      nearest-neighbor
434      ))
435 
436  ;; Returns the index of the nearest neighbor of p in tree t.
437 
438  (define=> (make-kd-tree-nearest-neighbor* <Point>)
439
440    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
441
442    (letrec ((find-nearest
443              (lambda (t1 t2 i v p probe xp x-probe)
444
445                (let* ((candidates1 
446                        (let ((best1 (nearest-neighbor t1 probe)))
447                          (or (and best1 (list best1 (if v (list (list i v) p) (list i p))))
448                              (list (if v (list (list i v) p) (list i p))))
449                          ))
450                       
451                       (sphere-intersects-plane? 
452                        (let ((v (- x-probe xp)))
453                          (< (* v v) (dist2 probe (cadar candidates1)))))
454
455                       (candidates2
456                        (if sphere-intersects-plane?
457                            (let ((nn (nearest-neighbor t2 probe)))
458                              (if nn (append candidates1 (list nn)) candidates1))
459                            candidates1)))
460
461                  (let ((v (minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe (cadr a) (cadr b)))))))
462                    v)
463                  )))
464
465             
466             (nearest-neighbor
467              (lambda (t probe)
468                (cases kd-tree t
469                       (KdLeaf (ii pp vv axis) 
470                               (let ((res
471                                      (if vv
472                                          (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))) 
473                                                      (zip (reverse (cis:elements ii)) vv ))
474                                          (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))) 
475                                                      (reverse (cis:elements ii))))))
476                                 (and res (reverse res))))
477
478                       (KdNode (l p i v r axis ci)
479
480                               (if (and (tree-empty? l)
481                                        (tree-empty? r))
482
483                                   (if v (list (list i v) p) (list i p))
484
485                                   (let ((x-probe (coord axis probe))
486                                         (xp (coord axis p))
487                                         (xi i))
488                                     (if (<= x-probe xp) 
489                                         (find-nearest l r i v p probe xp x-probe) 
490                                         (find-nearest r l i v p probe xp x-probe))
491                                     ))
492                               ))
493                )))
494     
495      nearest-neighbor
496      ))
497 
498   
499  ;; nearNeighbors tree p returns all neighbors within distance r from p in tree t.
500
501  (define=> (make-kd-tree-near-neighbors <Point>)
502    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
503    (letrec ((near-neighbors
504              (lambda (t radius probe fdist)
505                (cases kd-tree t
506                       (KdLeaf (ii pp vv axis) 
507                               (let ((r2 (* radius radius)))
508                                 (filter (lambda (p) (<= (fdist probe p) r2)) pp)))
509
510                       (KdNode (l p i v r axis ci)
511                               (let ((maybe-pivot
512                                      (if (<= (fdist probe p) (* radius radius)) (list p) '())))
513                                 
514                                 (if (and (tree-empty? l)
515                                          (tree-empty? r))
516
517                                     maybe-pivot
518
519                                     (let ((x-probe (coord axis probe))
520                                           (xp (coord axis p)))
521
522                                       (if (<= x-probe xp)
523
524                                           (let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist))))
525                                             (if (> (+ x-probe (abs radius)) xp)
526                                                 (append (near-neighbors r radius probe fdist) nearest)
527                                                 nearest))
528
529                                           (let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist))))
530                                             (if (< (- x-probe (abs radius)) xp)
531                                                 (append (near-neighbors l radius probe fdist) nearest)
532                                                 nearest)))
533                                       ))))
534                       ))
535              ))
536      (lambda (t radius probe #!key (factors #f))
537        (if (not factors)
538            (near-neighbors t radius probe dist2)
539            (near-neighbors t radius probe (sdist2 factors))))
540      ))
541 
542
543
544  (define=> (make-kd-tree-near-neighbors* <Point>)
545    (define (tree-empty? t) (cases kd-tree t (KdLeaf (ii pp vv axis) (cis:empty? ii)) (else #f)))
546    (letrec ((near-neighbors
547              (lambda (t radius probe fdist)
548                (cases kd-tree t
549
550                       (KdLeaf (ii pp vv axis) 
551                               (let ((rr (* radius radius)))
552                                 (if vv 
553                                     (filter-map (lambda (i v p) (and (<= (fdist probe p) rr) (list (list i v) p)))
554                                                 (reverse (cis:elements ii)) vv pp)
555                                     (filter-map (lambda (i p) (and (<= (fdist probe p) rr) (list i p)))
556                                                 (reverse (cis:elements ii)) pp)
557                                     )))
558                                             
559
560                       (KdNode (l p i v r axis ci)
561                               (let ((maybe-pivot
562                                      (if (<= (fdist probe p) (* radius radius))
563                                          (list (list i p)) '())))
564
565                                 (if (and (tree-empty? l)
566                                          (tree-empty? r))
567
568                                     maybe-pivot
569
570                                     (let ((x-probe (coord axis probe))
571                                           (xp (coord axis p)))
572
573                                       (if (<= x-probe xp)
574
575                                           (let ((nearest (append maybe-pivot (near-neighbors l radius probe fdist))))
576                                             (if (> (+ x-probe (abs radius)) xp)
577                                                 (append (near-neighbors r radius probe fdist) nearest)
578                                                 nearest))
579
580                                           (let ((nearest (append maybe-pivot (near-neighbors r radius probe fdist))))
581                                             (if (< (- x-probe (abs radius)) xp)
582                                                 (append (near-neighbors l radius probe fdist) nearest)
583                                                 nearest)))
584                                       ))
585                                 ))
586                       ))
587              ))
588      (lambda (t radius probe #!key (factors #f))
589        (if (not factors)
590            (near-neighbors t radius probe dist2)
591            (near-neighbors t radius probe (sdist2 factors))))
592      ))
593 
594
595 
596  ;; Returns the k nearest points to p within tree.
597  (define=> (make-kd-tree-k-nearest-neighbors <Point>)
598    (lambda (kd-tree-remove kd-tree-nearest-neighbor)
599      (letrec ((k-nearest-neighbors
600                (lambda (t k probe)
601                  (cases kd-tree t
602
603                       (KdLeaf (ii pp vv axis) 
604                               (let recur ((res '()) (pp pp) (k k))
605                                 (if (or (<= k 0) (null? pp)) 
606                                     res
607                                     (let ((nearest (minimum-by pp (lambda (a b) (negative? (compare-distance probe a b))))))
608                                       (recur (cons nearest res)
609                                              (remove (lambda (p) (equal? p nearest)) pp)
610                                              (- k 1))
611                                       ))
612                                 ))
613
614                       (else
615                        (if (<= k 0) '()
616                            (let* ((nearest (kd-tree-nearest-neighbor t probe))
617                                   (tree1 (kd-tree-remove t nearest)))
618                              (cons nearest (k-nearest-neighbors tree1 (- k 1) probe)))
619                            ))
620                       ))
621                ))
622        k-nearest-neighbors)))
623 
624 
625  ;; removes the point p from t.
626  (define=> (make-kd-tree-remove <Point>)
627    (lambda (list->kd-tree list->kd-tree*)
628      (letrec ((tree-remove
629                (lambda (t p-kill)
630                  (cases kd-tree t
631                         (KdLeaf (ii pp vv axis) 
632                                 (if vv
633                                     (let ((ipvs (filter-map
634                                                  (lambda (i p v) (and (equal? p p-kill) (list i p v)))
635                                                  (reverse (cis:elements ii)) pp vv)))
636                                         ;(fprintf (current-error-port) "kd-tree-remove: ipvs = ~A~%" ipvs)
637                                         ;(fprintf (current-error-port) "kd-tree-remove: pp = ~A~%" pp)
638                                         ;(fprintf (current-error-port) "kd-tree-remove: vv = ~A~%" vv)
639                                       (and (pair? ipvs)
640                                            (let ((ii1 (fold (lambda (i ax) (cis:remove i ax)) 
641                                                             ii (map car ipvs)))
642                                                  (pp1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
643                                                             pp (map cadr ipvs)))
644                                                  (vv1 (fold (lambda (x ax)
645                                                               (remove (lambda (p) (equal? p x)) ax))
646                                                             vv (map caddr ipvs)))
647                                                  )
648                                              (KdLeaf ii1 pp1 vv1 axis))
649                                            ))
650
651                                     (let ((ips (filter-map (lambda (i p) (and (equal? p p-kill) (list i p)))
652                                                            (reverse (cis:elements ii)) pp)))
653                                       (and (pair? ips)
654                                            (let ((ii1 (fold (lambda (i ax) (cis:remove i ax)) 
655                                                             ii (map car ips)))
656                                                  (pp1 (fold (lambda (x ax) (remove (lambda (p) (equal? p x)) ax))
657                                                             pp (map cadr ips)))
658                                                  )
659                                              (KdLeaf ii1 pp1 vv axis))
660                                            ))
661                                     ))
662
663                         (KdNode (l p i v r axis ci)
664
665                                 ;(fprintf (current-error-port) "kd-tree-remove: p = ~A~%" p)
666                                 ;(fprintf (current-error-port) "kd-tree-remove: p-kill = ~A~%" p-kill)
667                                 
668                                 (if (equal? p p-kill)
669
670                                     (let ((offset (if (kd-tree-empty? l) (+ i 1) (kd-tree-min-index l))))
671                                       (if v
672                                           (let ((pts1 (append (kd-tree->list* l) (kd-tree->list* r))))
673                                             (list->kd-tree* 0 (length pts1) pts1 axis offset: offset))
674                                           (let ((pts1 (append (kd-tree->list l) (kd-tree->list r))))
675                                             (list->kd-tree 0 (length pts1) pts1 axis offset: offset))
676                                         ))
677
678                                     (if (< (coord axis p-kill)
679                                            (coord axis p))
680
681                                         (let* ((l1   (tree-remove l p-kill))
682                                                (r1   (or (and l1 r) (tree-remove r p-kill)))
683                                                (ll   (or l1 l))
684                                                (rr   (or r1 r)))
685
686                                           (and (or l1 r1)
687                                                (let ((min1 (cond ((and (kd-tree-empty? ll) 
688                                                                        (kd-tree-empty? rr))
689                                                                   (cis:singleton i))
690                                                                  ((kd-tree-empty? ll)
691                                                                   (kd-tree-min-index rr))
692                                                                  (else
693                                                                   (kd-tree-min-index ll))))
694                                                      (max1 (cond ((and (kd-tree-empty? ll) 
695                                                                        (kd-tree-empty? rr))
696                                                                   (cis:empty))
697                                                                  ((kd-tree-empty? rr)
698                                                                   (kd-tree-max-index ll))
699                                                                  (else
700                                                                   (kd-tree-max-index rr))))
701                                                      )
702
703                                                (KdNode ll p i v rr axis 
704                                                        (cis:add (if (integer? i) i (car i))
705                                                                 (cis:interval min1 max1)))
706                                                )))
707
708                                         (let* ((r1 (tree-remove r p-kill))
709                                                (l1 (or (and r1 l) (tree-remove l p-kill)))
710                                                (ll   (or l1 l))
711                                                (rr   (or r1 r))
712                                                )
713
714                                           (and (or r1 l1)
715                                                (let ((min1 (cond ((and (kd-tree-empty? ll) 
716                                                                        (kd-tree-empty? rr))
717                                                                   (cis:singleton i))
718                                                                  ((kd-tree-empty? ll)
719                                                                   (kd-tree-min-index rr))
720                                                                  (else
721                                                                   (kd-tree-min-index ll))))
722                                                      (max1 (cond ((and (kd-tree-empty? ll) 
723                                                                        (kd-tree-empty? rr))
724                                                                   (cis:empty))
725                                                                  ((kd-tree-empty? rr)
726                                                                   (kd-tree-max-index ll))
727                                                                  (else
728                                                                   (kd-tree-max-index rr))))
729                                                      )
730                                                (KdNode ll p i v rr axis 
731                                                        (cis:add (if (integer? i) i (car i))
732                                                                 (cis:interval min1 max1)))
733                                                )))
734                                     )))
735                         ))
736                ))
737        tree-remove)))
738
739
740  ;; Checks whether the K-D tree property holds for a given tree.
741  ;;
742  ;; Specifically, it tests that all points in the left subtree lie to
743  ;; the left of the plane, p is on the plane, and all points in the
744  ;; right subtree lie to the right.
745 
746  (define=> (make-kd-tree-is-valid? <Point>)
747    (lambda (t)
748      (cases kd-tree t
749             (KdLeaf (ii pp vv axis)  #t)
750
751             (KdNode (l p i v r axis ci)
752                     (let ((x (coord axis p)))
753                       (and (every (lambda (y) (<= (coord axis y) x ))
754                                   (kd-tree->list l))
755                            (every (lambda (y) (>= (coord axis y) x))
756                                   (kd-tree->list r)))))
757             )))
758 
759 
760  ;; Checks whether the K-D tree property holds for the given tree and
761  ;; all subtrees.
762 
763  (define (make-kd-tree-all-subtrees-are-valid? kd-tree-is-valid?)
764    (lambda (t) (every kd-tree-is-valid? (kd-tree-subtrees t))))
765 
766
767  (define=> (make-kd-tree-slice <Point>)
768    (lambda (x-axis x1 x2 t)
769      (let recur ((t t)  (pts '()))
770        (cases kd-tree t
771
772               (KdLeaf (ii pp vv axis) 
773                       (append (filter (lambda (p) 
774                                         (and (<= x1 (coord x-axis p))
775                                              (<= (coord x-axis p) x2)))
776                                       pp)
777                               pts))
778                           
779
780               (KdNode (l p i v r axis ci)
781                       (if (= axis x-axis)
782                           
783                           (cond ((and (<= x1 (coord axis p))
784                                       (<= (coord axis p) x2))
785                                   (recur l (cons p (recur r pts))))
786                                 
787                                 ((< (coord axis p) x1)
788                                  (recur r pts))
789                               
790                                 ((< x2 (coord axis p))
791                                  (recur l pts)))
792                           
793                           (if (and (<= x1 (coord x-axis p))
794                                    (<= (coord x-axis p) x2))
795                               (recur l (cons p (recur r pts)))
796                               (recur l (recur r pts)))
797                           ))
798               ))
799      ))
800 
801 
802  (define=> (make-kd-tree-slice* <Point>)
803    (lambda (x-axis x1 x2 t)
804      (let recur ((t t)  (pts '()))
805        (cases kd-tree t
806               (KdLeaf (ii pp vv axis) 
807                       (append
808                        (if vv
809                            (filter-map (lambda (i v p) 
810                                          (and (<= x1 (coord x-axis p))
811                                               (<= (coord x-axis p) x2)
812                                               (list (list i v) p)))
813                                        (reverse (cis:elements ii)) vv pp)
814                            (filter-map (lambda (i p) 
815                                          (and (<= x1 (coord x-axis p))
816                                               (<= (coord x-axis p) x2)
817                                               (list i p)))
818                                        (reverse (cis:elements ii)) pp))
819                        pts))
820
821               (KdNode (l p i v r axis ci)
822                       (if (= axis x-axis)
823                           
824                           (cond ((and (<= x1 (coord axis p))
825                                       (<= (coord axis p) x2))
826                                   (recur l (cons (if v (list (list i v) p) (list i p)) (recur r pts))))
827                                 
828                                 ((< (coord axis p) x1)
829                                  (recur r pts))
830                               
831                                 ((< x2 (coord axis p))
832                                  (recur l pts)))
833                           
834                           (if (and (<= x1 (coord x-axis p))
835                                    (<= (coord x-axis p) x2))
836                               (recur l (cons (if v (list (list i v) p) (list i p)) (recur r pts)))
837                               (recur l (recur r pts)))
838                           ))
839               ))
840      ))
841
842  (define (default-<KdTree> point-class)
843    (let* ((list->kd-tree/depth
844            (make-list->kd-tree/depth point-class))
845           (kd-tree-remove
846            ((make-kd-tree-remove point-class) 
847             (list->kd-tree/depth identity #f)
848             (list->kd-tree/depth cadr (lambda (i v) (cadar v)))))
849           (kd-tree-nearest-neighbor
850            (make-kd-tree-nearest-neighbor point-class)))
851
852      (make-<KdTree> 
853       (lambda (points #!key
854                       (leaf-factor 10) 
855                       (make-point identity) 
856                       (make-value #f)
857                       (offset 0)
858                       )
859         ((list->kd-tree/depth make-point make-value) 
860          0 (length points) points 0 
861          leaf-factor: leaf-factor offset: offset))
862
863       (make-kd-tree-nearest-neighbor point-class)
864       (make-kd-tree-nearest-neighbor* point-class)
865       (make-kd-tree-near-neighbors point-class)
866       (make-kd-tree-near-neighbors* point-class)
867       ((make-kd-tree-k-nearest-neighbors point-class)
868        kd-tree-remove kd-tree-nearest-neighbor)
869       kd-tree-remove
870       (make-kd-tree-slice point-class)
871       (make-kd-tree-slice* point-class)
872       (make-kd-tree-is-valid? point-class)
873       (make-kd-tree-all-subtrees-are-valid? 
874        (make-kd-tree-is-valid? point-class)) 
875       )))
876
877  (define KdTree3d 
878    (default-<KdTree> Point3d))
879
880  (define KdTree2d 
881    (default-<KdTree> Point2d))
882
883
884
885)
886
Note: See TracBrowser for help on using the repository browser.