csys/space: core functionality relevant for environ: distance, neighbors, free locations

csys/space.lisp

@@ -32,19 +32,26 @@
 
 (defun neighbors (spc loc &key (dist 2))
   (let* ((range (floor (sqrt dist)))
-         nbrs)
-    (util:with-iterator (loc-iterator range)
+         nbrs empty)
+    (util:with-iterator (loc-iterator range :dist dist)
       (lambda (rloc)
         (let* ((cloc (loc-add rloc loc))
                (cell (fetch spc cloc)))
-          (when cell (push cell nbrs)))))
-    nbrs))
+          (if cell 
+            (push cell nbrs)
+            (push cloc empty)))))
+    (values nbrs empty)))
 
-(defun free-loc-at (spc loc &key (dist 2)))
+(defun free-loc-at (spc loc &key (dist 2))
+  (multiple-value-bind (nbrs free) (neighbors spc loc :dist dist)
+    free))
 
-(defun distance-sq (spc loc1 loc2))
+(defun distance-sq (loc1 loc2)
+  (flet ((square (x) (* x x))) 
+    (loop for v1 across loc1 and v2 across loc2 
+          sum (square (- v2 v1)))))
 
-(defun loc-iterator (range &key (dim 2))
+(defun loc-iterator (range &key (dim 2) dist)
   (let* ((from (- range))
          (zero (make-array dim :initial-element 0))
          (cur (make-array dim :initial-element from)))
@@ -60,6 +67,8 @@
       (lambda ()
         (when (equalp cur zero)
           (setf cur (inc cur)))
+        (when (and cur dist (> (distance-sq zero cur) dist))
+          (setf cur (inc cur)))
         (when cur
           (prog1 (make-array dim :initial-contents cur)
             (setf cur (inc cur))))))))

test/test-csys.lisp

@@ -80,6 +80,7 @@
  (deftest test-space ()
   (let ((spc (space:create))
         locs)
+    (== (space:distance-sq #(-1 1) #(1 2)) 5)
     (space:put spc #(1 1) 42)
     (space:put spc #(1 2) 46)
     (ignore-errors 
@@ -95,6 +96,7 @@
     (== (car locs) #(1 1))
     (== (car (last locs)) #(-1 -1))
     (== (car (space:neighbors spc #(1 1))) 46)
+    (== (car (space:free-loc-at spc #(1 1))) #(2 2))
     ))
  
 (deftest test-basic-0 ()