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

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

kd-tree: restructuring handling of node indices

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