diff options
| -rw-r--r-- | chickadee/math/grid.scm | 336 |
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." |