source: project/release/4/spatial-trees/trunk/kd-tree.scm @ 27008

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

spatial-trees: disabled f64vector->kd-tree constructor and tests

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