Implement grid-scan bounding box function.

Implement proper expansion of place to fit scans as they are added.

grid-scan-test.ss

@@ -23,7 +23,7 @@
    (define-values (lt1 rb1) (grid-scan-bb example-grid-scan))
    (define-values (lt2 rb2) (grid-scan-bb translated-grid-scan))
    (check-equal? lt1 (vector 2 0))
-   (check-equal? rb1 (vector 4 3))
+   (check-equal? rb1 (vector 8 1))
    (check-equal? lt2 (vector -4 -2))
    (check-equal? rb2 (vector -2 1))) 
   )

grid-scan.ss

@@ -35,7 +35,7 @@
    (assert (number->exact-integer
             (floor (+ (+ (* sina x) (* cosa y)) yt))))))
 
-(: grid-scan-bb (Grid-Scan -> (values Point Point)))
+(: grid-scan-bb (Grid-Scan -> (values Grid-Point Grid-Point)))
 ;; Calculate the bounding box for a grid scan, returning the
 ;; top left and bottom right point
 (define (grid-scan-bb scan)

place-test.ss

@@ -48,9 +48,9 @@
   (test-case
    "place expands to contain scan on update"
    (define posterior (place-add example-place translated-grid-scan))
-   (check-equal? (Place-x posterior) 16)
+   (check-equal? (Place-x posterior) 14)
    (check-equal? (Place-y posterior) 7)
    (check = (place-ref posterior 0 0) 2)
    (check = (place-ref posterior 1 0) 2)
-   (check = (place-ref posterior 0 1) 1))
+   (check = (place-ref posterior 3 0) 1))
   )

place.ss

@@ -5,6 +5,7 @@
  scheme/vector
  "types.ss"
  "point.ss"
+ "grid-scan.ss"
  "util.ss")
 
 
@@ -47,16 +48,59 @@
   (match-define (struct Place (p-x p-y pts)) p)
   (make-Place p-x p-y (vector-copy pts)))
 
+(: place-expand-to-bb (Place Grid-Point Grid-Point -> (values Place Integer Integer)))
+;;
+;; Return an expanded place and x and y offsets to convert
+;; old coordinate system to new one.
+(define (place-expand-to-bb p lt rb)
+  (match-define (struct Place (p-x p-y pts)) p)
+  (define lt-x (point-x lt))
+  (define lt-y (point-y lt))
+  (define new-lt-x (if (< lt-x 0) lt-x 0))
+  (define new-lt-y (if (< lt-y 0) lt-y 0))
+  (define rb-x (point-x rb))
+  (define rb-y (point-y rb))
+  (define new-rb-x (if (< p-x rb-x) rb-x p-x))
+  (define new-rb-y (if (< p-y rb-y) rb-y p-y))
+
+  (define new-p-x
+    (assert (number->exact-nonnegative-integer (- new-rb-x new-lt-x))))
+  (define new-p-y
+    (assert (number->exact-nonnegative-integer (- new-rb-y new-lt-y))))
+  (define new-size (* new-p-x new-p-y))
+  (define new-pts (make-vector new-size #{1.0 :: Real}))
+
+  (let loop-x ([x 0])
+    (unless (= x p-x)
+      (let loop-y ([y 0])
+        (unless (= y p-y)
+          (let* ([pt (vector-ref pts (+ (* y p-x) x))]
+               [new-x (- x new-lt-x)]
+               [new-y (- y new-lt-y)]
+               [new-idx (+ (* new-y new-p-x) new-x)])y
+          (vector-set! new-pts new-idx pt))
+          (loop-y (add1 y))))
+      (loop-x (add1 x))))
+  ;;(for* ([x (in-range p-x)]
+  ;;       [y (in-range p-y)])
+  ;;      (let* ([pt (vector-ref pts (+ (* y p-x) x))]
+  ;;             [new-x (- x new-lt-x)]
+  ;;             [new-y (- y new-lt-y)]
+  ;;             [new-idx (+ (* new-y new-p-x) new-x)])y
+  ;;        (vector-set! new-pts new-idx pt)))
+  (values (make-Place new-p-x new-p-y new-pts) new-lt-x new-lt-y))
+
 (: place-add (Place Grid-Scan -> Place))
 ;;
 ;; Update a place with a scan. The scan must be already
 ;; transformed by an appropriate pose
 (define (place-add p s)
-  (define new-p (place-copy p))
+  (define-values (lt rb) (grid-scan-bb s))
+  (define-values (new-p xt yt) (place-expand-to-bb p lt rb))
   (match-define (struct Place (p-x p-y pts)) new-p)
   (for ([pt (in-vector s)])
-       (define x (point-x pt))
-       (define y (point-y pt))
+       (define x (- (point-x pt) xt))
+       (define y (- (point-y pt) yt))
        (define idx (place-coords->index new-p x y))
        (when idx
          (vector-add1! pts idx)))

util.ss

@@ -21,6 +21,12 @@
       number
       #f))
 
+(: number->exact-nonnegative-integer (Number -> (Option Exact-Nonnegative-Integer)))
+(define (number->exact-nonnegative-integer number)
+  (if (exact-nonnegative-integer? number)
+      number
+      #f))
+
 (: assert (All (a) ((Option a) -> a)))
 (define (assert v)
   (if v
@@ -35,6 +41,7 @@
 (provide
  number->exact-integer
  number->real
+ number->exact-nonnegative-integer
 
  assert