Changeset 25960 in project


Ignore:
Timestamp:
02/23/12 04:32:38 (8 years ago)
Author:
Ivan Raikov
Message:

spatial-tree: some reorganization to kd-tree to allow both 2D and 3D operations on the same set of points

File:
1 edited

Legend:

Unmodified
Added
Removed
  • release/4/spatial-trees/trunk/kd-tree.scm

    r25933 r25960  
    44       
    55  (
    6    <Point> Point-point3d
    7    point3d? make-point3d point3d-x point3d-y point3d-z
    8 
    9    kd-tree? KdNode KdEmpty
     6   <Point> default-<Point> Point3d Point2d
     7   point? make-point
     8
     9   <KdTree> default-<KdTree> KdTree3d KdTree2d
     10   kd-tree?
    1011   kd-tree-empty?
    1112   kd-tree->list
     
    1819   kd-tree-subtrees
    1920   kd-tree-point
    20    list->kd-tree
    21    kd-tree-nearest-neighbor
    22    kd-tree-near-neighbors
    23    kd-tree-near-neighbors*
    24    kd-tree-k-nearest-neighbors
    25    kd-tree-remove
    26    kd-tree-slice
    27    kd-tree-slice*
    28    kd-tree-is-valid?
    29    kd-tree-all-subtrees-are-valid?
    3021 
    3122  )
     
    6960
    7061  (define (default-<Point> dimension coord)
     62
    7163    (let* ((dist2
    7264            (lambda (a b)
     
    8577    ))
    8678
    87  
    88   (define-record-type point3d
    89     (make-point3d x y z)
    90     point3d?
    91     (x point3d-x)
    92     (y point3d-y)
    93     (z point3d-z)
     79  (define point? vector?)
     80  (define make-point vector)
     81
     82
     83  (define Point3d
     84    (default-<Point>
     85     (lambda (p) (and (point? p) 3))
     86     (lambda (i p) (case i
     87                     ((0) (vector-ref p 0))
     88                     ((1) (vector-ref p 1))
     89                     ((2) (vector-ref p 2))))
     90     ))
     91
     92  (define Point2d
     93    (default-<Point>
     94     (lambda (p) (and (point? p) 2))
     95     (lambda (i p) (case i
     96                     ((0) (vector-ref p 0))
     97                     ((1) (vector-ref p 1))))
     98     ))
     99
     100
     101  (define-class <KdTree>
     102
     103    ;; constructs a kd-tree from a list of points
     104    list->kd-tree
     105    ;; nearest neighbor of a point
     106    kd-tree-nearest-neighbor
     107    ;; neighbors of a point within radius r
     108    kd-tree-near-neighbors
     109    ;; neighbors of a point within radius r (using point indices)
     110    kd-tree-near-neighbors*
     111    ;; k nearest neighbors of a point
     112    kd-tree-k-nearest-neighbors
     113    ;; removes a point from the tree
     114    kd-tree-remove
     115    ;; retrieves all points between two planes
     116    kd-tree-slice
     117    ;; retrieves all points between two planes (using point indices)
     118    kd-tree-slice*
     119    ;; checks that the kd-tree properties are preserved
     120    kd-tree-is-valid?
     121    kd-tree-all-subtrees-are-valid?
     122
    94123    )
    95124
    96   (define-record-printer (point3d p out)
    97     (fprintf out "#<~a,~a,~a>"
    98              (point3d-x p)
    99              (point3d-y p)
    100              (point3d-z p)
    101              ))
    102 
    103   (define Point-point3d
    104     (default-<Point>
    105      (lambda (p) (and (point3d? p) 3))
    106      (lambda (i p) (case i
    107                      ((0) (point3d-x p))
    108                      ((1) (point3d-y p))
    109                      ((2) (point3d-z p))))
    110      ))
     125
     126  (define-datatype kd-tree kd-tree?
     127    (KdNode (left  kd-tree?)
     128            (p     point?)
     129            (i     integer?)
     130            (right kd-tree?)
     131            (axis  integer?))
     132    (KdEmpty))
     133
     134  (define (kd-tree-empty? t)
     135    (cases kd-tree t
     136           (KdEmpty () #t)
     137           (else #f)))
     138 
     139  (define (kd-tree->list t)
     140    (kd-tree-fold-right cons '() t))
     141 
     142  (define (kd-tree->list* t)
     143    (kd-tree-fold-right* (lambda (i x ax) (cons (list i x) ax)) '() t))
     144 
     145  (define (kd-tree-map f t)
     146    (cases kd-tree t
     147           (KdEmpty () (KdEmpty))
     148           (KdNode (l x i r axis)
     149                   (KdNode (kd-tree-map f l)
     150                           (f x)
     151                           i
     152                           (kd-tree-map f r)
     153                           axis))
     154           ))
     155 
     156  (define (kd-tree-for-each f t)
     157    (cases kd-tree t
     158           (KdEmpty () (begin))
     159           (KdNode (l x i r axis)
     160                   (begin
     161                     (kd-tree-for-each f l)
     162                     (f x)
     163                     (kd-tree-for-each f r)
     164                     ))
     165           ))
     166
     167  (define (kd-tree-for-each* f t)
     168    (cases kd-tree t
     169           (KdEmpty () (begin))
     170           (KdNode (l x i r axis)
     171                   (begin
     172                     (kd-tree-for-each* f l)
     173                     (f i x)
     174                     (kd-tree-for-each* f r)
     175                     ))
     176           ))
     177 
     178  (define (kd-tree-fold-right f init t)
     179    (cases kd-tree t
     180           (KdEmpty () init)
     181           (KdNode (l x i r _)
     182                   (let* ((init2 (kd-tree-fold-right f init r))
     183                          (init3 (f x init2)))
     184                     (kd-tree-fold-right f init3 l)))
     185           ))
     186
     187  (define (kd-tree-fold-right* f init t)
     188    (cases kd-tree t
     189           (KdEmpty () init)
     190           (KdNode (l x i r _)
     191                   (let* ((init2 (kd-tree-fold-right* f init r))
     192                          (init3 (f i x init2)))
     193                     (kd-tree-fold-right* f init3 l)))
     194           ))
     195 
     196 
     197 
     198 
     199  ;; Returns a list containing t and all its subtrees, including the
     200  ;; empty leaf nodes.
     201 
     202  (define (kd-tree-subtrees t)
     203    (cases kd-tree t
     204                  (KdEmpty ()
     205                           (list (KdEmpty)))
     206                  (KdNode (l x i r axis)
     207                          (append (kd-tree-subtrees l)
     208                                  (list t)
     209                                  (kd-tree-subtrees r)))
     210                  ))
     211 
     212  (define (kd-tree-point t)
     213    (cases kd-tree t
     214                  (KdEmpty ()  #f)
     215                  (KdNode (l x i r axis) x)
     216                  ))
    111217
    112218
     
    191297                       (KdEmpty ()  '())
    192298                       (KdNode (l p i r axis)
    193                                (let ((maybe-pivot (if (<= (dist2 probe p) (* radius radius))
    194                                                       (list p) '())))
    195                                  (if (and (tree-empty? l)
    196                                           (tree-empty? r))
    197                                      maybe-pivot
    198                                      (let ((x-probe (coord axis probe))
    199                                            (xp (coord axis p)))
    200                                        (if (<= x-probe xp)
    201                                            (let ((nearest (append maybe-pivot (near-neighbors l radius probe))))
    202                                              (if (> (+ x-probe (abs radius)) xp)
    203                                                  (append (near-neighbors r radius probe) nearest)
    204                                                  nearest))
    205                                            (let ((nearest (append maybe-pivot (near-neighbors r radius probe))))
    206                                              (if (< (- x-probe (abs radius)) xp)
    207                                                  (append (near-neighbors l radius probe) nearest)
    208                                                  nearest)))
    209                                        ))
    210                                  ))
     299                                   (let ((maybe-pivot (if (<= (dist2 probe p) (* radius radius))
     300                                                          (list p) '())))
     301
     302                                     (if (and (tree-empty? l)
     303                                              (tree-empty? r))
     304                                         maybe-pivot
     305                                         (let ((x-probe (coord axis probe))
     306                                               (xp (coord axis p)))
     307                                           (if (<= x-probe xp)
     308                                               (let ((nearest (append maybe-pivot (near-neighbors l radius probe))))
     309                                                 (if (> (+ x-probe (abs radius)) xp)
     310                                                     (append (near-neighbors r radius probe) nearest)
     311                                                     nearest))
     312                                               (let ((nearest (append maybe-pivot (near-neighbors r radius probe))))
     313                                                 (if (< (- x-probe (abs radius)) xp)
     314                                                     (append (near-neighbors l radius probe) nearest)
     315                                                     nearest)))
     316                                           ))))
    211317                       ))
    212318              ))
     
    249355  ;; Returns the k nearest points to p within tree.
    250356  (define=> (make-kd-tree-k-nearest-neighbors <Point>)
     357    (lambda (kd-tree-remove kd-tree-nearest-neighbor)
    251358    (letrec ((k-nearest-neighbors
    252359              (lambda (t k probe)
     
    261368                       ))
    262369              ))
    263       k-nearest-neighbors))
     370      k-nearest-neighbors)))
    264371 
    265372 
    266373  ;; removes the point p from t.
    267374  (define=> (make-kd-tree-remove <Point>)
     375    (lambda (list->kd-tree/depth)
    268376    (letrec ((remove
    269377              (lambda (t p-kill)
     
    281389                       ))
    282390              ))
    283       remove))
     391      remove)))
    284392
    285393
     
    306414  ;; all subtrees.
    307415 
    308   (define=> (make-kd-tree-all-subtrees-are-valid? <Point>)
     416  (define (make-kd-tree-all-subtrees-are-valid? kd-tree-is-valid?)
    309417    (lambda (t) (every kd-tree-is-valid? (kd-tree-subtrees t))))
    310418 
    311 
    312   (define-datatype kd-tree kd-tree?
    313     (KdNode (left  kd-tree?)
    314             (p     point3d?)
    315             (i     integer?)
    316             (right kd-tree?)
    317             (axis  integer?))
    318     (KdEmpty))
    319 
    320   (define (kd-tree-empty? t)
    321     (cases kd-tree t
    322            (KdEmpty () #t)
    323            (else #f)))
    324  
    325   (define (kd-tree->list t)
    326     (kd-tree-fold-right cons '() t))
    327  
    328   (define (kd-tree->list* t)
    329     (kd-tree-fold-right* (lambda (i x ax) (cons (list i x) ax)) '() t))
    330  
    331   (define (kd-tree-map f t)
    332     (cases kd-tree t
    333            (KdEmpty () (KdEmpty))
    334            (KdNode (l x i r axis)
    335                    (KdNode (kd-tree-map f l)
    336                            (f x)
    337                            (kd-tree-map f r)
    338                            axis))
    339            ))
    340  
    341   (define (kd-tree-for-each f t)
    342     (cases kd-tree t
    343            (KdEmpty () (begin))
    344            (KdNode (l x i r axis)
    345                    (begin
    346                      (kd-tree-for-each f l)
    347                      (f x)
    348                      (kd-tree-for-each f r)
    349                      ))
    350            ))
    351 
    352   (define (kd-tree-for-each* f t)
    353     (cases kd-tree t
    354            (KdEmpty () (begin))
    355            (KdNode (l x i r axis)
    356                    (begin
    357                      (kd-tree-for-each* f l)
    358                      (f i x)
    359                      (kd-tree-for-each* f r)
    360                      ))
    361            ))
    362  
    363   (define (kd-tree-fold-right f init t)
    364     (cases kd-tree t
    365            (KdEmpty () init)
    366            (KdNode (l x i r _)
    367                    (let* ((init2 (kd-tree-fold-right f init r))
    368                           (init3 (f x init2)))
    369                      (kd-tree-fold-right f init3 l)))
    370            ))
    371 
    372   (define (kd-tree-fold-right* f init t)
    373     (cases kd-tree t
    374            (KdEmpty () init)
    375            (KdNode (l x i r _)
    376                    (let* ((init2 (kd-tree-fold-right* f init r))
    377                           (init3 (f i x init2)))
    378                      (kd-tree-fold-right* f init3 l)))
    379            ))
    380419
    381420  (define=> (make-kd-tree-slice <Point>)
     
    431470               ))
    432471      ))
    433  
    434  
    435  
    436  
    437   ;; Returns a list containing t and all its subtrees, including the
    438   ;; empty leaf nodes.
    439  
    440   (define (kd-tree-subtrees t)
    441     (cases kd-tree t
    442                   (KdEmpty ()
    443                            (list (KdEmpty)))
    444                   (KdNode (l x i r axis)
    445                           (append (kd-tree-subtrees l)
    446                                   (list t)
    447                                   (kd-tree-subtrees r)))
    448                   ))
    449  
    450   (define (kd-tree-point t)
    451     (cases kd-tree t
    452                   (KdEmpty ()  #f)
    453                   (KdNode (l x i r axis) x)
    454                   ))
    455  
    456   (define list->kd-tree/depth
    457     (make-list->kd-tree/depth Point-point3d))
    458 
    459   (define (list->kd-tree points)
    460     (list->kd-tree/depth 0 (length points) points 0))
    461 
    462   (define kd-tree-nearest-neighbor (make-kd-tree-nearest-neighbor Point-point3d))
    463 
    464   (define kd-tree-near-neighbors (make-kd-tree-near-neighbors Point-point3d))
    465 
    466   (define kd-tree-near-neighbors* (make-kd-tree-near-neighbors* Point-point3d))
    467  
    468   (define kd-tree-k-nearest-neighbors (make-kd-tree-k-nearest-neighbors Point-point3d))
    469  
    470   (define kd-tree-remove (make-kd-tree-remove Point-point3d))
    471  
    472   (define kd-tree-slice (make-kd-tree-slice Point-point3d))
    473 
    474   (define kd-tree-slice* (make-kd-tree-slice* Point-point3d))
    475  
    476   (define kd-tree-is-valid? (make-kd-tree-is-valid? Point-point3d))
    477  
    478   (define kd-tree-all-subtrees-are-valid? (make-kd-tree-all-subtrees-are-valid? Point-point3d))
     472
     473  (define (default-<KdTree> point-class)
     474    (let* ((list->kd-tree/depth (make-list->kd-tree/depth point-class))
     475           (kd-tree-remove ((make-kd-tree-remove point-class) list->kd-tree/depth))
     476           (kd-tree-nearest-neighbor(make-kd-tree-nearest-neighbor point-class)))
     477
     478
     479      (make-<KdTree>
     480       (lambda (points) (list->kd-tree/depth 0 (length points) points 0))
     481       (make-kd-tree-nearest-neighbor point-class)
     482       (make-kd-tree-near-neighbors point-class)
     483       (make-kd-tree-near-neighbors* point-class)
     484       ((make-kd-tree-k-nearest-neighbors point-class)
     485        kd-tree-remove kd-tree-nearest-neighbor)
     486       kd-tree-remove
     487       (make-kd-tree-slice point-class)
     488       (make-kd-tree-slice* point-class)
     489       (make-kd-tree-is-valid? point-class)
     490       (make-kd-tree-all-subtrees-are-valid?
     491        (make-kd-tree-is-valid? point-class))
     492       )))
     493
     494  (define KdTree3d
     495    (default-<KdTree> Point3d))
     496
     497  (define KdTree2d
     498    (default-<KdTree> Point2d))
    479499
    480500
Note: See TracChangeset for help on using the changeset viewer.