Changeset 25960 in project
 Timestamp:
 02/23/12 04:32:38 (8 years ago)
 File:

 1 edited
Legend:
 Unmodified
 Added
 Removed

release/4/spatialtrees/trunk/kdtree.scm
r25933 r25960 4 4 5 5 ( 6 <Point> Pointpoint3d 7 point3d? makepoint3d point3dx point3dy point3dz 8 9 kdtree? KdNode KdEmpty 6 <Point> default<Point> Point3d Point2d 7 point? makepoint 8 9 <KdTree> default<KdTree> KdTree3d KdTree2d 10 kdtree? 10 11 kdtreeempty? 11 12 kdtree>list … … 18 19 kdtreesubtrees 19 20 kdtreepoint 20 list>kdtree21 kdtreenearestneighbor22 kdtreenearneighbors23 kdtreenearneighbors*24 kdtreeknearestneighbors25 kdtreeremove26 kdtreeslice27 kdtreeslice*28 kdtreeisvalid?29 kdtreeallsubtreesarevalid?30 21 31 22 ) … … 69 60 70 61 (define (default<Point> dimension coord) 62 71 63 (let* ((dist2 72 64 (lambda (a b) … … 85 77 )) 86 78 87 88 (definerecordtype point3d 89 (makepoint3d x y z) 90 point3d? 91 (x point3dx) 92 (y point3dy) 93 (z point3dz) 79 (define point? vector?) 80 (define makepoint vector) 81 82 83 (define Point3d 84 (default<Point> 85 (lambda (p) (and (point? p) 3)) 86 (lambda (i p) (case i 87 ((0) (vectorref p 0)) 88 ((1) (vectorref p 1)) 89 ((2) (vectorref 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) (vectorref p 0)) 97 ((1) (vectorref p 1)))) 98 )) 99 100 101 (defineclass <KdTree> 102 103 ;; constructs a kdtree from a list of points 104 list>kdtree 105 ;; nearest neighbor of a point 106 kdtreenearestneighbor 107 ;; neighbors of a point within radius r 108 kdtreenearneighbors 109 ;; neighbors of a point within radius r (using point indices) 110 kdtreenearneighbors* 111 ;; k nearest neighbors of a point 112 kdtreeknearestneighbors 113 ;; removes a point from the tree 114 kdtreeremove 115 ;; retrieves all points between two planes 116 kdtreeslice 117 ;; retrieves all points between two planes (using point indices) 118 kdtreeslice* 119 ;; checks that the kdtree properties are preserved 120 kdtreeisvalid? 121 kdtreeallsubtreesarevalid? 122 94 123 ) 95 124 96 (definerecordprinter (point3d p out) 97 (fprintf out "#<~a,~a,~a>" 98 (point3dx p) 99 (point3dy p) 100 (point3dz p) 101 )) 102 103 (define Pointpoint3d 104 (default<Point> 105 (lambda (p) (and (point3d? p) 3)) 106 (lambda (i p) (case i 107 ((0) (point3dx p)) 108 ((1) (point3dy p)) 109 ((2) (point3dz p)))) 110 )) 125 126 (definedatatype kdtree kdtree? 127 (KdNode (left kdtree?) 128 (p point?) 129 (i integer?) 130 (right kdtree?) 131 (axis integer?)) 132 (KdEmpty)) 133 134 (define (kdtreeempty? t) 135 (cases kdtree t 136 (KdEmpty () #t) 137 (else #f))) 138 139 (define (kdtree>list t) 140 (kdtreefoldright cons '() t)) 141 142 (define (kdtree>list* t) 143 (kdtreefoldright* (lambda (i x ax) (cons (list i x) ax)) '() t)) 144 145 (define (kdtreemap f t) 146 (cases kdtree t 147 (KdEmpty () (KdEmpty)) 148 (KdNode (l x i r axis) 149 (KdNode (kdtreemap f l) 150 (f x) 151 i 152 (kdtreemap f r) 153 axis)) 154 )) 155 156 (define (kdtreeforeach f t) 157 (cases kdtree t 158 (KdEmpty () (begin)) 159 (KdNode (l x i r axis) 160 (begin 161 (kdtreeforeach f l) 162 (f x) 163 (kdtreeforeach f r) 164 )) 165 )) 166 167 (define (kdtreeforeach* f t) 168 (cases kdtree t 169 (KdEmpty () (begin)) 170 (KdNode (l x i r axis) 171 (begin 172 (kdtreeforeach* f l) 173 (f i x) 174 (kdtreeforeach* f r) 175 )) 176 )) 177 178 (define (kdtreefoldright f init t) 179 (cases kdtree t 180 (KdEmpty () init) 181 (KdNode (l x i r _) 182 (let* ((init2 (kdtreefoldright f init r)) 183 (init3 (f x init2))) 184 (kdtreefoldright f init3 l))) 185 )) 186 187 (define (kdtreefoldright* f init t) 188 (cases kdtree t 189 (KdEmpty () init) 190 (KdNode (l x i r _) 191 (let* ((init2 (kdtreefoldright* f init r)) 192 (init3 (f i x init2))) 193 (kdtreefoldright* 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 (kdtreesubtrees t) 203 (cases kdtree t 204 (KdEmpty () 205 (list (KdEmpty))) 206 (KdNode (l x i r axis) 207 (append (kdtreesubtrees l) 208 (list t) 209 (kdtreesubtrees r))) 210 )) 211 212 (define (kdtreepoint t) 213 (cases kdtree t 214 (KdEmpty () #f) 215 (KdNode (l x i r axis) x) 216 )) 111 217 112 218 … … 191 297 (KdEmpty () '()) 192 298 (KdNode (l p i r axis) 193 194 195 (if (and (treeempty? l) 196 (treeempty? r))197 maybepivot198 (let ((xprobe (coord axis probe))199 (xp (coord axis p)))200 (if (<= xprobe xp)201 ( let ((nearest (append maybepivot (nearneighbors l radius probe))))202 (if (> (+ xprobe (abs radius)) xp)203 ( append (nearneighbors r radius probe) nearest)204 nearest))205 (let ((nearest (append maybepivot (nearneighbors r radius probe))))206 (if (< ( xprobe (abs radius)) xp)207 ( append (nearneighbors l radius probe) nearest)208 nearest)))209 210 299 (let ((maybepivot (if (<= (dist2 probe p) (* radius radius)) 300 (list p) '()))) 301 302 (if (and (treeempty? l) 303 (treeempty? r)) 304 maybepivot 305 (let ((xprobe (coord axis probe)) 306 (xp (coord axis p))) 307 (if (<= xprobe xp) 308 (let ((nearest (append maybepivot (nearneighbors l radius probe)))) 309 (if (> (+ xprobe (abs radius)) xp) 310 (append (nearneighbors r radius probe) nearest) 311 nearest)) 312 (let ((nearest (append maybepivot (nearneighbors r radius probe)))) 313 (if (< ( xprobe (abs radius)) xp) 314 (append (nearneighbors l radius probe) nearest) 315 nearest))) 316 )))) 211 317 )) 212 318 )) … … 249 355 ;; Returns the k nearest points to p within tree. 250 356 (define=> (makekdtreeknearestneighbors <Point>) 357 (lambda (kdtreeremove kdtreenearestneighbor) 251 358 (letrec ((knearestneighbors 252 359 (lambda (t k probe) … … 261 368 )) 262 369 )) 263 knearestneighbors)) 370 knearestneighbors))) 264 371 265 372 266 373 ;; removes the point p from t. 267 374 (define=> (makekdtreeremove <Point>) 375 (lambda (list>kdtree/depth) 268 376 (letrec ((remove 269 377 (lambda (t pkill) … … 281 389 )) 282 390 )) 283 remove)) 391 remove))) 284 392 285 393 … … 306 414 ;; all subtrees. 307 415 308 (define => (makekdtreeallsubtreesarevalid? <Point>)416 (define (makekdtreeallsubtreesarevalid? kdtreeisvalid?) 309 417 (lambda (t) (every kdtreeisvalid? (kdtreesubtrees t)))) 310 418 311 312 (definedatatype kdtree kdtree?313 (KdNode (left kdtree?)314 (p point3d?)315 (i integer?)316 (right kdtree?)317 (axis integer?))318 (KdEmpty))319 320 (define (kdtreeempty? t)321 (cases kdtree t322 (KdEmpty () #t)323 (else #f)))324 325 (define (kdtree>list t)326 (kdtreefoldright cons '() t))327 328 (define (kdtree>list* t)329 (kdtreefoldright* (lambda (i x ax) (cons (list i x) ax)) '() t))330 331 (define (kdtreemap f t)332 (cases kdtree t333 (KdEmpty () (KdEmpty))334 (KdNode (l x i r axis)335 (KdNode (kdtreemap f l)336 (f x)337 (kdtreemap f r)338 axis))339 ))340 341 (define (kdtreeforeach f t)342 (cases kdtree t343 (KdEmpty () (begin))344 (KdNode (l x i r axis)345 (begin346 (kdtreeforeach f l)347 (f x)348 (kdtreeforeach f r)349 ))350 ))351 352 (define (kdtreeforeach* f t)353 (cases kdtree t354 (KdEmpty () (begin))355 (KdNode (l x i r axis)356 (begin357 (kdtreeforeach* f l)358 (f i x)359 (kdtreeforeach* f r)360 ))361 ))362 363 (define (kdtreefoldright f init t)364 (cases kdtree t365 (KdEmpty () init)366 (KdNode (l x i r _)367 (let* ((init2 (kdtreefoldright f init r))368 (init3 (f x init2)))369 (kdtreefoldright f init3 l)))370 ))371 372 (define (kdtreefoldright* f init t)373 (cases kdtree t374 (KdEmpty () init)375 (KdNode (l x i r _)376 (let* ((init2 (kdtreefoldright* f init r))377 (init3 (f i x init2)))378 (kdtreefoldright* f init3 l)))379 ))380 419 381 420 (define=> (makekdtreeslice <Point>) … … 431 470 )) 432 471 )) 433 434 435 436 437 ;; Returns a list containing t and all its subtrees, including the 438 ;; empty leaf nodes. 439 440 (define (kdtreesubtrees t) 441 (cases kdtree t 442 (KdEmpty () 443 (list (KdEmpty))) 444 (KdNode (l x i r axis) 445 (append (kdtreesubtrees l) 446 (list t) 447 (kdtreesubtrees r))) 448 )) 449 450 (define (kdtreepoint t) 451 (cases kdtree t 452 (KdEmpty () #f) 453 (KdNode (l x i r axis) x) 454 )) 455 456 (define list>kdtree/depth 457 (makelist>kdtree/depth Pointpoint3d)) 458 459 (define (list>kdtree points) 460 (list>kdtree/depth 0 (length points) points 0)) 461 462 (define kdtreenearestneighbor (makekdtreenearestneighbor Pointpoint3d)) 463 464 (define kdtreenearneighbors (makekdtreenearneighbors Pointpoint3d)) 465 466 (define kdtreenearneighbors* (makekdtreenearneighbors* Pointpoint3d)) 467 468 (define kdtreeknearestneighbors (makekdtreeknearestneighbors Pointpoint3d)) 469 470 (define kdtreeremove (makekdtreeremove Pointpoint3d)) 471 472 (define kdtreeslice (makekdtreeslice Pointpoint3d)) 473 474 (define kdtreeslice* (makekdtreeslice* Pointpoint3d)) 475 476 (define kdtreeisvalid? (makekdtreeisvalid? Pointpoint3d)) 477 478 (define kdtreeallsubtreesarevalid? (makekdtreeallsubtreesarevalid? Pointpoint3d)) 472 473 (define (default<KdTree> pointclass) 474 (let* ((list>kdtree/depth (makelist>kdtree/depth pointclass)) 475 (kdtreeremove ((makekdtreeremove pointclass) list>kdtree/depth)) 476 (kdtreenearestneighbor(makekdtreenearestneighbor pointclass))) 477 478 479 (make<KdTree> 480 (lambda (points) (list>kdtree/depth 0 (length points) points 0)) 481 (makekdtreenearestneighbor pointclass) 482 (makekdtreenearneighbors pointclass) 483 (makekdtreenearneighbors* pointclass) 484 ((makekdtreeknearestneighbors pointclass) 485 kdtreeremove kdtreenearestneighbor) 486 kdtreeremove 487 (makekdtreeslice pointclass) 488 (makekdtreeslice* pointclass) 489 (makekdtreeisvalid? pointclass) 490 (makekdtreeallsubtreesarevalid? 491 (makekdtreeisvalid? pointclass)) 492 ))) 493 494 (define KdTree3d 495 (default<KdTree> Point3d)) 496 497 (define KdTree2d 498 (default<KdTree> Point2d)) 479 499 480 500
Note: See TracChangeset
for help on using the changeset viewer.