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

Last change on this file since 27840 was 27840, checked in by Ivan Raikov, 7 years ago

kd-tree: removed some unnecessary imports

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