math: grid: Refactor to improve performance a little bit.

There's still a lot of work needed to make the collision grid scale to
any reasonable number of moving objects, but this is a first step.

* chickadee/math/grid.scm: Big ol' refactor that I can't be bothered
  to list all the changes for.

1 files changed, 163 insertions, 173 deletions

diff --git a/chickadee/math/grid.scm b/chickadee/math/grid.scm
index d131921..58232fc 100644
--- a/chickadee/math/grid.scm
+++ b/chickadee/math/grid.scm
@@ -28,6 +28,7 @@
   #:use-module (chickadee math rect)
   #:use-module (chickadee math vector)
   #:use-module (ice-9 match)
+  #:use-module (rnrs bytevectors)
   #:use-module (srfi srfi-9)
   #:use-module (srfi srfi-11)
   #:export (slide
@@ -92,12 +93,6 @@ needed to make the rectangles no longer overlap."
   (hashq-remove! (cell-items cell) item)
   (set-cell-count! cell (- (cell-count cell) 1)))
 
-(define (for-each-cell-item proc cell)
-  "Call PROC with each item in CELL."
-  (hash-for-each (lambda (item unused)
-                   (proc item))
-                 (cell-items cell)))
-
 
 ;;;
 ;;; Grid
@@ -106,7 +101,7 @@ needed to make the rectangles no longer overlap."
 (define-record-type <grid>
   (%make-grid cell-size rects rows scratch-rect buffer visited)
   grid?
-  (cell-size grid-cell-size)
+  (cell-size %grid-cell-size)
   (rects grid-rects)
   (rows grid-rows)
   ;; The below fields are scratch space data structures that are
@@ -116,15 +111,18 @@ needed to make the rectangles no longer overlap."
   (buffer grid-buffer)
   (visited grid-visited))
 
-(define* (make-grid #:optional (cell-size 64))
+(define* (make-grid #:optional (cell-size 64.0))
   "Create new grid partitioned by CELL-SIZE."
-  (%make-grid cell-size
+  (%make-grid (f32vector cell-size)
               (make-hash-table)
               (make-hash-table)
               (make-rect 0.0 0.0 0.0 0.0)
               (make-array-list)
               (make-hash-table)))
 
+(define-inlinable (grid-cell-size grid)
+  (f32vector-ref (%grid-cell-size grid) 0))
+
 (define (item-in-grid? grid item)
   "Return #t if ITEM is in GRID."
   (hashq-ref (grid-rects grid) item))
@@ -156,16 +154,21 @@ needed to make the rectangles no longer overlap."
   "Return the range of cells that RECT occupies in GRID.  The first
 two return values are the min/max x coordinate, the last two are the
 min/max y coordinate."
-  (define (to-cell n)
-    (inexact->exact (floor (/ n (grid-cell-size grid)))))
-  (values (to-cell (rect-left rect))
-          (to-cell (rect-right rect))
-          (to-cell (rect-bottom rect))
-          (to-cell (rect-top rect))))
-
-(define (for-each-coord proc minx maxx miny maxy)
-  "Call PROC with each (X, Y) coordinate pair formed by the inclusive
-ranges [MINX, MAXX] and [MINY, MAXY]."
+  (let ((cell-size (grid-cell-size grid))
+        (x (rect-x rect))
+        (y (rect-y rect))
+        (w (rect-width rect))
+        (h (rect-height rect)))
+    (define (to-cell n)
+      (inexact->exact (floor (/ n cell-size))))
+    (values (to-cell x)
+            (to-cell (+ x w))
+            (to-cell y)
+            (to-cell (+ y h)))))
+
+(define-inlinable (for-each-coord proc minx maxx miny maxy)
+;;   "Call PROC with each (X, Y) coordinate pair formed by the inclusive
+;; ranges [MINX, MAXX] and [MINY, MAXY]."
   (let yloop ((y miny))
     (when (<= y maxy)
       (let xloop ((x minx))
@@ -188,15 +191,6 @@ cell if RECT is #f."
                                       row))
                      (grid-rows grid))))
 
-(define (for-each-item-in-area proc grid rect)
-  "Call PROC with each item in GRID that overlaps the area defined by
-RECT."
-  (for-each-cell (lambda (cell x y)
-                   (for-each-cell-item (lambda (item)
-                                         (proc item (grid-rect-ref grid item)))
-                                       cell))
-                 grid rect))
-
 (define (for-each-item proc grid)
   "Call PROC for each item in GRID."
   (hash-for-each proc (grid-rects grid)))
@@ -222,144 +216,126 @@ by X, Y, WIDTH, HEIGHT."
                      (cell-remove cell item))
                    grid rect)))
 
-(define (grid-update grid item position)
-  "Move ITEM in GRID to POSITION."
-  (let* ((rect (grid-rect-ref grid item))
-         (moved-rect (grid-scratch-rect grid)))
-    (set-rect-x! moved-rect (vec2-x position))
-    (set-rect-y! moved-rect (vec2-y position))
-    (set-rect-width! moved-rect (rect-width rect))
-    (set-rect-height! moved-rect (rect-height rect))
-    (let-values (((minx1 maxx1 miny1 maxy1)
-                  (grid-cell-bounds grid rect))
-                 ((minx2 maxx2 miny2 maxy2)
-                  (grid-cell-bounds grid moved-rect)))
-      (set-rect-x! rect (vec2-x position))
-      (set-rect-y! rect (vec2-y position))
-      (for-each-coord (lambda (x y)
-                        (when (or (< x minx2) (> x maxx2)
-                                  (< y miny2) (> y maxy2))
-                          (cell-remove (grid-cell-ref grid x y) item)))
-                      minx1 maxx1 miny1 maxy1)
-      (for-each-coord (lambda (x y)
-                        (let ((cell (grid-cell-ref grid x y)))
-                          (when (or (< x minx1) (> x maxx1)
-                                    (< y miny1) (> y maxy1))
-                            (cell-add cell item))))
-                      minx2 maxx2 miny2 maxy2))))
-
-(define (grid-find-collision grid item goal filter)
-  "Return the object that ITEM in GRID collides with while trying to
-move to GOAL.
-
-FILTER is a procedure that determines how such a
-collision should be resolved between two objects.  It is called
-like (FILTER ITEM OTHER), where OTHER is an object in close promimity
-to other.  If ITEM and OTHER should not collide with each other,
-FILTER must return #f.  Otherwise, FILTER returns a collision
-resolution procedure, such as 'slide'.
+(define inexact->exact*
+  (let ((cache '()))
+    (lambda (n)
+      (or (assv-ref cache n)
+          (let ((result (inexact->exact n)))
+            (set! cache (cons (cons n result) cache))
+            result)))))
 
-Collision resolution procedures are called like (RESOLVE RECT1 RECT2
-GOAL).  RECT1 is the rectangle that is attempting to move to GOAL, but
-has collided with RECT2.  It is the responsibility of the resolution
-procedure to modify GOAL such that RECT1 and RECT2 are no longer
-colliding."
-  (define (sort-by-area collisions)
-    ;; This is just an insertion sort, which will be slow if there are a
-    ;; large number of simultaneous collisions.  I think that the number
-    ;; of simultaneous collisions is almost always a single digit
-    ;; number, so a more efficient sorting algorithm doesn't gain us
-    ;; anything.
-    (define (compare a b)
-      (match a
-        ((_ area-a _)
-         (match b
-           ((_ area-b _)
-            (< area-a area-b))))))
-    (define (swap i j)
-      (let ((tmp (array-list-ref collisions i)))
-        (array-list-set! collisions i (array-list-ref collisions j))
-        (array-list-set! collisions j tmp)))
-    (let ((size (array-list-size collisions)))
-      (let outer
-          ((i 0))
-        (when (< i size)
-          (let inner ((j (+ i 1)))
-            (when (< j size)
-              (when (compare (array-list-ref collisions i)
-                             (array-list-ref collisions j))
-                (swap i j))
-              (inner (+ j 1))))
-          (outer (+ i 1))))))
-  (define (collision? rect1 rect2 goal)
-    (let ((goal-x (vec2-x goal))
-          (goal-y (vec2-y goal)))
-      (and (< goal-x (rect-right rect2))
-           (> (+ goal-x (rect-width rect1)) (rect-left rect2))
-           (< goal-y (rect-top rect2))
-           (> (+ goal-y (rect-height rect1)) (rect-bottom rect2)))))
-  (define (overlap-area rect1 rect2 goal)
-    (let ((goal-x (vec2-x goal))
-          (goal-y (vec2-y goal)))
-      (* (- (min (+ goal-x (rect-width rect1)) (rect-right rect2))
-            (max goal-x (rect-left rect2)))
-         (- (min (+ goal-y (rect-height rect1)) (rect-top rect2))
-            (max goal-y (rect-bottom rect2))))))
-  (let ((rect (grid-rect-ref grid item))
-        (search-rect (grid-scratch-rect grid))
-        (collisions (grid-buffer grid))
-        (visited (grid-visited grid)))
-    ;; The search area is the bounding box formed by union of the
-    ;; current rect and the rect formed by moving it to the desired
-    ;; position.
-    (set-rect-x! search-rect (min (vec2-x goal) (rect-x rect)))
-    (set-rect-y! search-rect (min (vec2-y goal) (rect-y rect)))
-    (set-rect-width! search-rect (+ (rect-width rect)
-                                    (min (- (vec2-x goal)
-                                            (rect-x rect))
-                                         0)))
-    (set-rect-height! search-rect (+ (rect-height rect)
-                                     (min (- (vec2-y goal)
-                                             (rect-y rect))
-                                          0)))
-    ;; Reset our scratch space.
-    (array-list-clear! collisions)
-    ;; Visit every cell in the search area.
-    (for-each-item-in-area
-     (lambda (other other-rect)
-       ;; Since items can occupy multiple cells, we must track which
-       ;; items have been processed already so that we don't have
-       ;; duplicate collision results which will almost certainly
-       ;; yield strange behavior.
-       (unless (hashq-ref visited other)
-         ;; The user-provided filter is expected to return a procedure
-         ;; that can resolve a collision between itself and other,
-         ;; should one occur.  If the items should clip through each
-         ;; other without any collision, the filter returns #f and we
-         ;; do not waste any time testing for collision.
-         (let ((resolve (filter item other)))
-           (when resolve
-             (when (collision? rect other-rect goal)
-               (array-list-push! collisions
-                                 (list other
-                                       (overlap-area rect other-rect goal)
-                                       resolve)))))))
-     grid search-rect)
-    ;; Sort collisions by overlap area and return the biggest
-    ;; collision.  There's definitely improvements that can be made in
-    ;; the heuristic department here, but it's enough for now.
-    (sort-by-area collisions)
-    ;; Return the biggest collision.
-    (if (array-list-empty? collisions)
-        #f
-        (array-list-ref collisions 0))))
-
-(define (grid-resolve-collisions grid item goal filter)
-  "Check if moving ITEM in GRID to GOAL causes collisions, and if
-so, resolve them using HANDLER."
-  (let ((visited (grid-visited grid)))
+(define (grid-move grid item goal filter)
+  "Attempt to move ITEM in GRID to POSITION (a 2D vector) and check
+for collisions.  For each collision, FILTER will be called with two
+arguments: ITEM and the item it collided with.  If a collision occurs,
+POSITION may be modified to resolve the colliding objects."
+  (let* ((rect (grid-rect-ref grid item))
+         (x (rect-x rect))
+         (y (rect-y rect))
+         (w (rect-width rect))
+         (h (rect-height rect))
+         (cell-size (grid-cell-size grid))
+         (collisions (grid-buffer grid))
+         (visited (grid-visited grid)))
+    (define (to-cell n)
+      (inexact->exact (floor (/ n cell-size))))
+    (define (collision? rect1 rect2 goal)
+      (let ((goal-x (vec2-x goal))
+            (goal-y (vec2-y goal)))
+        (and (< goal-x (rect-right rect2))
+             (> (+ goal-x (rect-width rect1)) (rect-left rect2))
+             (< goal-y (rect-top rect2))
+             (> (+ goal-y (rect-height rect1)) (rect-bottom rect2)))))
+    (define (overlap-area rect1 rect2 goal)
+      (let ((goal-x (vec2-x goal))
+            (goal-y (vec2-y goal)))
+        (* (- (min (+ goal-x (rect-width rect1)) (rect-right rect2))
+              (max goal-x (rect-left rect2)))
+           (- (min (+ goal-y (rect-height rect1)) (rect-top rect2))
+              (max goal-y (rect-bottom rect2))))))
+    (define (check other other-rect)
+      ;; Since items can occupy multiple cells, we must track which
+      ;; items have been processed already so that we don't have
+      ;; duplicate collision results which will almost certainly
+      ;; yield strange behavior.
+      (unless (hashq-ref visited other)
+        ;; The user-provided filter is expected to return a procedure
+        ;; that can resolve a collision between itself and other,
+        ;; should one occur.  If the items should clip through each
+        ;; other without any collision, the filter returns #f and we
+        ;; do not waste any time testing for collision.
+        (let ((resolve (filter item other)))
+          (when (and resolve (collision? rect other-rect goal))
+            (array-list-push! collisions
+                              (list other
+                                    (overlap-area rect other-rect goal)
+                                    resolve))))))
+    (define (sort-by-area)
+      ;; This is just an insertion sort, which will be slow if there are a
+      ;; large number of simultaneous collisions.  I think that the number
+      ;; of simultaneous collisions is almost always a single digit
+      ;; number, so a more efficient sorting algorithm doesn't gain us
+      ;; anything.
+      (define (compare a b)
+        (match a
+          ((_ area-a _)
+           (match b
+             ((_ area-b _)
+              (< area-a area-b))))))
+      (define (swap i j)
+        (let ((tmp (array-list-ref collisions i)))
+          (array-list-set! collisions i (array-list-ref collisions j))
+          (array-list-set! collisions j tmp)))
+      (let ((size (array-list-size collisions)))
+        (let outer
+            ((i 0))
+          (when (< i size)
+            (let inner ((j (+ i 1)))
+              (when (< j size)
+                (when (compare (array-list-ref collisions i)
+                               (array-list-ref collisions j))
+                  (swap i j))
+                (inner (+ j 1))))
+            (outer (+ i 1))))))
+    (define (find-collisions)
+      ;; The search area is the bounding box formed by union of the
+      ;; current rect and the rect formed by moving it to the desired
+      ;; position.
+      (let* ((goal-x (vec2-x goal))
+             (goal-y (vec2-y goal))
+             (search-x (min goal-x x))
+             (search-y (min goal-y y))
+             (search-w (+ w (min (- goal-x x) 0.0)))
+             (search-h (+ h (min (- goal-y y) 0.0)))
+             (minx (to-cell x))
+             (maxx (to-cell (+ x w)))
+             (miny (to-cell y))
+             (maxy (to-cell (+ y h))))
+        ;; Reset our scratch space.
+        (array-list-clear! collisions)
+        ;; Visit every cell in the search area.
+        (let yloop ((cy miny))
+          (when (<= cy maxy)
+            (let ((row (grid-row-ref grid cy)))
+              (let xloop ((cx minx))
+                (when (<= cx maxx)
+                  (let ((cell (row-column-ref row cx)))
+                    (hash-for-each (lambda (other unused)
+                                     (check other (grid-rect-ref grid other))
+                                     #f)
+                                   (cell-items cell)))
+                  (xloop (+ cx 1)))))
+            (yloop (+ cy 1))))
+        ;; Sort collisions by overlap area and return the biggest
+        ;; collision.  There's definitely improvements that can be made in
+        ;; the heuristic department here, but it's enough for now.
+        (sort-by-area)
+        ;; Return the biggest collision.
+        (if (array-list-empty? collisions)
+            #f
+            (array-list-ref collisions 0))))
     (define (collide)
-      (match (grid-find-collision grid item goal filter)
+      (match (find-collisions)
         ((other _ resolve)
          (hashq-set! visited other #t)
          (resolve item
@@ -377,15 +353,29 @@ so, resolve them using HANDLER."
     (hash-clear! visited)
     ;; Never check collision against ourselves.
     (hashq-set! visited item #t)
-    (collide)))
-
-(define (grid-move grid item position filter)
-  "Attempt to move ITEM in GRID to POSITION (a 2D vector) and check
-for collisions.  For each collision, FILTER will be called with two
-arguments: ITEM and the item it collided with.  If a collision occurs,
-POSITION may be modified to resolve the colliding objects."
-  (grid-resolve-collisions grid item position filter)
-  (grid-update grid item position))
+    (collide)
+    (let* ((new-x (vec2-x goal))
+           (new-y (vec2-y goal))
+           (minx1 (to-cell x))
+           (miny1 (to-cell y))
+           (maxx1 (to-cell (+ x w)))
+           (maxy1 (to-cell (+ y h)))
+           (minx2 (to-cell new-x))
+           (miny2 (to-cell new-y))
+           (maxx2 (to-cell (+ new-x w)))
+           (maxy2 (to-cell (+ new-y h))))
+      (set-rect-x! rect new-x)
+      (set-rect-y! rect new-y)
+      (for-each-coord (lambda (x y)
+                        (when (or (< x minx2) (> x maxx2)
+                                  (< y miny2) (> y maxy2))
+                          (cell-remove (grid-cell-ref grid x y) item)))
+                      minx1 maxx1 miny1 maxy1)
+      (for-each-coord (lambda (x y)
+                        (when (or (< x minx1) (> x maxx1)
+                                  (< y miny1) (> y maxy1))
+                          (cell-add (grid-cell-ref grid x y) item)))
+                      minx2 maxx2 miny2 maxy2))))
 
 (define (grid-clear grid)
   "Remove all items from GRID."