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

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

kd-tree: added some introductory explanation to source file

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