Changeset 25912 in project

Timestamp:

02/15/12 08:00:43 (8 years ago)

Author:

Ivan Raikov

Message:

spatial-trees: bug fixes and testsuite for kd-tree

Location:

release/4/spatial-trees/trunk

Files:

• kd-tree.scm (modified) (8 diffs)
• spatial-tree.meta (deleted)
• spatial-tree.setup (deleted)
• spatial-trees.meta (added)
• spatial-trees.setup (added)

release/4/spatial-trees/trunk/kd-tree.scm

@@ -25 +25 @@
   (import scheme chicken data-structures)
   
-  (require-library srfi-1)
+  (require-library srfi-1 extras)
   (require-extension typeclass datatype)
 
-  (import (only srfi-1 fold list-tabulate drop take every))
+  (import (only srfi-1 xcons fold list-tabulate drop take every)
+          (only extras fprintf))
 
 
@@ -46 +47 @@
 
     )
+
+  (define (minimum-by lst less?)
+    (if (null? lst) #f
+        (let recur ((lst (cdr lst)) (m (car lst)))
+          (if (null? lst) m
+              (if (less? (car lst) m)
+                  (recur (cdr lst) (car lst))
+                  (recur (cdr lst) m)
+                  ))
+          )))
 
   (define (sum lst) (fold + 0. lst))
@@ -53 +64 @@
             (lambda (a b)
               (let ((diff2 (lambda (i) (let ((v (- (coord i a) (coord i b)))) (* v v)))))
-                (sum (list-tabulate (- (dimension a) 1) diff2)))))
+                (sum (list-tabulate (dimension a) diff2)))))
 
            (compare-distance
@@ -75 +86 @@
     )
 
+  (define-record-printer (point3d p out)
+    (fprintf out "#<~a,~a,~a>"
+             (point3d-x p)
+             (point3d-y p)
+             (point3d-z p)
+             ))
 
   (define Point-point3d
@@ -99 +116 @@
                             (sort points (lambda (a b) (< (coord axis a) (coord axis b)))))
                            (median-index
-                            (quotient (length sorted-points) 2)))
-                      
+                            (quotient (- (length sorted-points) 1) 2)))
+
                       (KdNode (list->kd-tree/depth (take sorted-points median-index) (+ 1 depth))
                               (list-ref sorted-points median-index)
-                              (list->kd-tree/depth (drop sorted-points (+ 1 median-index)) (+ 1 depth))
+                              (list->kd-tree/depth (drop sorted-points (+ median-index 1)) (+ 1 depth))
                               axis)
                       ))
@@ -116 +133 @@
     (letrec ((find-nearest
               (lambda (t1 t2 p probe xp x-probe)
+
                 (let* ((candidates1 
                         (let ((best1 (nearest-neighbor t1 probe)))
                           (or (and best1 (list best1 p)) (list p))))
-                       (sphere-intersects-plane?
-                        (<= (expt (- x-probe xp) 2.) (dist2 probe p)))
+                       
+                       (sphere-intersects-plane? 
+                        (let ((v (- x-probe xp)))
+                          (< (* v v) (dist2 probe (car candidates1)))))
+
                        (candidates2
                         (if sphere-intersects-plane?
-                            (append candidates1 (or (nearest-neighbor t2 probe) '()))
+                            (let ((nn (nearest-neighbor t2 probe)))
+                              (if nn (append candidates1 (list nn)) candidates1))
                             candidates1)))
-                  (car (sort candidates2 (lambda (a b) (positive? (compare-distance probe a b)))))
+
+                  (minimum-by candidates2 (lambda (a b) (negative? (compare-distance probe a b))))
                   )))
+
              
              (nearest-neighbor
@@ -137 +161 @@
                                         (let ((x-probe (coord axis probe))
                                               (xp (coord axis p)))
+
                                           (if (<= x-probe xp) 
                                               (find-nearest l r p probe xp x-probe) 
@@ -234 +259 @@
              (KdNode (l p r axis)
                      (let ((x (coord axis p)))
-                       (and (every (lambda (y) (<= x (coord axis y))) (kd-tree->list l))
-                            (every (lambda (y) (<= x (coord axis y))) (kd-tree->list r)))))
+                       (and (every (lambda (y) (<= (coord axis y) x ))
+                                   (kd-tree->list l))
+                            (every (lambda (y) (>= (coord axis y) x))
+                                   (kd-tree->list r)))))
              )))