d131921bbe73ca25b7847353f6513032ebe01e7e
[chickadee.git] / chickadee / math / grid.scm
1 ;;; Chickadee Game Toolkit
2 ;;; Copyright © 2018 David Thompson <davet@gnu.org>
3 ;;;
4 ;;; Chickadee is free software: you can redistribute it and/or modify
5 ;;; it under the terms of the GNU General Public License as published
6 ;;; by the Free Software Foundation, either version 3 of the License,
7 ;;; or (at your option) any later version.
8 ;;;
9 ;;; Chickadee is distributed in the hope that it will be useful, but
10 ;;; WITHOUT ANY WARRANTY; without even the implied warranty of
11 ;;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 ;;; General Public License for more details.
13 ;;;
14 ;;; You should have received a copy of the GNU General Public License
15 ;;; along with this program. If not, see
16 ;;; <http://www.gnu.org/licenses/>.
17
18 ;;; Commentary:
19 ;;
20 ;; Axis-aligned bounding box collision grid.
21 ;;
22 ;; Lots of inspiration drawn from https://github.com/kikito/bump.lua
23 ;;
24 ;;; Code:
25
26 (define-module (chickadee math grid)
27 #:use-module (chickadee array-list)
28 #:use-module (chickadee math rect)
29 #:use-module (chickadee math vector)
30 #:use-module (ice-9 match)
31 #:use-module (srfi srfi-9)
32 #:use-module (srfi srfi-11)
33 #:export (slide
34 cell?
35 cell-count
36 make-grid
37 grid?
38 grid-cell-size
39 grid-add
40 grid-remove
41 grid-move
42 grid-clear
43 grid-cell-count
44 grid-item-count
45 for-each-cell
46 for-each-item))
47
48 \f
49 ;;;
50 ;;; Collision resolvers
51 ;;;
52
53 (define (slide item item-rect other other-rect goal)
54 "Resolve the collision that occurs between ITEM and OTHER when
55 moving ITEM-RECT to GOAL by sliding ITEM-RECT the minimum amount
56 needed to make the rectangles no longer overlap."
57 (let* ((goal-x (vec2-x goal))
58 (goal-y (vec2-y goal))
59 (x1 (max goal-x (rect-left other-rect)))
60 (x2 (min (+ goal-x (rect-width item-rect)) (rect-right other-rect)))
61 (y1 (max goal-y (rect-bottom other-rect)))
62 (y2 (min (+ goal-y (rect-height item-rect)) (rect-top other-rect)))
63 (x-fix (- x2 x1))
64 (y-fix (- y2 y1)))
65 (if (< x-fix y-fix)
66 (if (= goal-x x1)
67 (set-vec2-x! goal (+ (vec2-x goal) x-fix))
68 (set-vec2-x! goal (+ (vec2-x goal) (- x-fix))))
69 (if (= goal-y y1)
70 (set-vec2-y! goal (+ (vec2-y goal) y-fix))
71 (set-vec2-y! goal (+ (vec2-y goal) (- y-fix)))))))
72
73 \f
74 ;;;
75 ;;; Cells
76 ;;;
77
78 (define-record-type <cell>
79 (make-cell items count)
80 cell?
81 (items cell-items)
82 (count cell-count set-cell-count!))
83
84 (define (make-empty-cell)
85 (make-cell (make-hash-table) 0))
86
87 (define (cell-add cell item)
88 (hashq-set! (cell-items cell) item #t)
89 (set-cell-count! cell (+ (cell-count cell) 1)))
90
91 (define (cell-remove cell item)
92 (hashq-remove! (cell-items cell) item)
93 (set-cell-count! cell (- (cell-count cell) 1)))
94
95 (define (for-each-cell-item proc cell)
96 "Call PROC with each item in CELL."
97 (hash-for-each (lambda (item unused)
98 (proc item))
99 (cell-items cell)))
100
101 \f
102 ;;;
103 ;;; Grid
104 ;;;
105
106 (define-record-type <grid>
107 (%make-grid cell-size rects rows scratch-rect buffer visited)
108 grid?
109 (cell-size grid-cell-size)
110 (rects grid-rects)
111 (rows grid-rows)
112 ;; The below fields are scratch space data structures that are
113 ;; allocated once when the grid is created to cut down on
114 ;; allocations while modifying the grid and checking for collisions.
115 (scratch-rect grid-scratch-rect)
116 (buffer grid-buffer)
117 (visited grid-visited))
118
119 (define* (make-grid #:optional (cell-size 64))
120 "Create new grid partitioned by CELL-SIZE."
121 (%make-grid cell-size
122 (make-hash-table)
123 (make-hash-table)
124 (make-rect 0.0 0.0 0.0 0.0)
125 (make-array-list)
126 (make-hash-table)))
127
128 (define (item-in-grid? grid item)
129 "Return #t if ITEM is in GRID."
130 (hashq-ref (grid-rects grid) item))
131
132 (define (grid-row-ref grid y)
133 "Return the row at index Y in GRID."
134 (let ((rows (grid-rows grid)))
135 (or (hashq-ref rows y)
136 (let ((new-row (make-hash-table)))
137 (hashq-set! rows y new-row)
138 new-row))))
139
140 (define (row-column-ref row x)
141 "Return the cell at index X in ROW."
142 (or (hashq-ref row x)
143 (let ((new-cell (make-empty-cell)))
144 (hashq-set! row x new-cell)
145 new-cell)))
146
147 (define (grid-cell-ref grid x y)
148 "Return the cell in GRID at (X, Y)."
149 (row-column-ref (grid-row-ref grid y) x))
150
151 (define (grid-rect-ref grid item)
152 "Return the rect for ITEM in GRID."
153 (hashq-ref (grid-rects grid) item))
154
155 (define (grid-cell-bounds grid rect)
156 "Return the range of cells that RECT occupies in GRID. The first
157 two return values are the min/max x coordinate, the last two are the
158 min/max y coordinate."
159 (define (to-cell n)
160 (inexact->exact (floor (/ n (grid-cell-size grid)))))
161 (values (to-cell (rect-left rect))
162 (to-cell (rect-right rect))
163 (to-cell (rect-bottom rect))
164 (to-cell (rect-top rect))))
165
166 (define (for-each-coord proc minx maxx miny maxy)
167 "Call PROC with each (X, Y) coordinate pair formed by the inclusive
168 ranges [MINX, MAXX] and [MINY, MAXY]."
169 (let yloop ((y miny))
170 (when (<= y maxy)
171 (let xloop ((x minx))
172 (when (<= x maxx)
173 (proc x y)
174 (xloop (+ x 1))))
175 (yloop (+ y 1)))))
176
177 (define* (for-each-cell proc grid #:optional rect)
178 "Call PROC with each cell in GRID that intersects RECT, or every
179 cell if RECT is #f."
180 (if rect
181 (let-values (((minx maxx miny maxy) (grid-cell-bounds grid rect)))
182 (for-each-coord (lambda (x y)
183 (proc (grid-cell-ref grid x y) x y))
184 minx maxx miny maxy))
185 (hash-for-each (lambda (y row)
186 (hash-for-each (lambda (x cell)
187 (proc cell x y))
188 row))
189 (grid-rows grid))))
190
191 (define (for-each-item-in-area proc grid rect)
192 "Call PROC with each item in GRID that overlaps the area defined by
193 RECT."
194 (for-each-cell (lambda (cell x y)
195 (for-each-cell-item (lambda (item)
196 (proc item (grid-rect-ref grid item)))
197 cell))
198 grid rect))
199
200 (define (for-each-item proc grid)
201 "Call PROC for each item in GRID."
202 (hash-for-each proc (grid-rects grid)))
203
204 (define (grid-add grid item x y width height)
205 "Add ITEM to GRID represented by axis-aligned bounding box defined
206 by X, Y, WIDTH, HEIGHT."
207 (when (item-in-grid? grid item)
208 (error "item already in grid" item))
209 (let ((rect (make-rect x y width height)))
210 (hashq-set! (grid-rects grid) item rect)
211 (for-each-cell (lambda (cell x y)
212 (cell-add cell item))
213 grid rect)))
214
215 (define (grid-remove grid item)
216 "Remove ITEM from GRID."
217 (let ((rect (grid-rect-ref grid item)))
218 (unless rect
219 (error "item not in grid" item))
220 (hashq-remove! (grid-rects grid) item)
221 (for-each-cell (lambda (cell x y)
222 (cell-remove cell item))
223 grid rect)))
224
225 (define (grid-update grid item position)
226 "Move ITEM in GRID to POSITION."
227 (let* ((rect (grid-rect-ref grid item))
228 (moved-rect (grid-scratch-rect grid)))
229 (set-rect-x! moved-rect (vec2-x position))
230 (set-rect-y! moved-rect (vec2-y position))
231 (set-rect-width! moved-rect (rect-width rect))
232 (set-rect-height! moved-rect (rect-height rect))
233 (let-values (((minx1 maxx1 miny1 maxy1)
234 (grid-cell-bounds grid rect))
235 ((minx2 maxx2 miny2 maxy2)
236 (grid-cell-bounds grid moved-rect)))
237 (set-rect-x! rect (vec2-x position))
238 (set-rect-y! rect (vec2-y position))
239 (for-each-coord (lambda (x y)
240 (when (or (< x minx2) (> x maxx2)
241 (< y miny2) (> y maxy2))
242 (cell-remove (grid-cell-ref grid x y) item)))
243 minx1 maxx1 miny1 maxy1)
244 (for-each-coord (lambda (x y)
245 (let ((cell (grid-cell-ref grid x y)))
246 (when (or (< x minx1) (> x maxx1)
247 (< y miny1) (> y maxy1))
248 (cell-add cell item))))
249 minx2 maxx2 miny2 maxy2))))
250
251 (define (grid-find-collision grid item goal filter)
252 "Return the object that ITEM in GRID collides with while trying to
253 move to GOAL.
254
255 FILTER is a procedure that determines how such a
256 collision should be resolved between two objects. It is called
257 like (FILTER ITEM OTHER), where OTHER is an object in close promimity
258 to other. If ITEM and OTHER should not collide with each other,
259 FILTER must return #f. Otherwise, FILTER returns a collision
260 resolution procedure, such as 'slide'.
261
262 Collision resolution procedures are called like (RESOLVE RECT1 RECT2
263 GOAL). RECT1 is the rectangle that is attempting to move to GOAL, but
264 has collided with RECT2. It is the responsibility of the resolution
265 procedure to modify GOAL such that RECT1 and RECT2 are no longer
266 colliding."
267 (define (sort-by-area collisions)
268 ;; This is just an insertion sort, which will be slow if there are a
269 ;; large number of simultaneous collisions. I think that the number
270 ;; of simultaneous collisions is almost always a single digit
271 ;; number, so a more efficient sorting algorithm doesn't gain us
272 ;; anything.
273 (define (compare a b)
274 (match a
275 ((_ area-a _)
276 (match b
277 ((_ area-b _)
278 (< area-a area-b))))))
279 (define (swap i j)
280 (let ((tmp (array-list-ref collisions i)))
281 (array-list-set! collisions i (array-list-ref collisions j))
282 (array-list-set! collisions j tmp)))
283 (let ((size (array-list-size collisions)))
284 (let outer
285 ((i 0))
286 (when (< i size)
287 (let inner ((j (+ i 1)))
288 (when (< j size)
289 (when (compare (array-list-ref collisions i)
290 (array-list-ref collisions j))
291 (swap i j))
292 (inner (+ j 1))))
293 (outer (+ i 1))))))
294 (define (collision? rect1 rect2 goal)
295 (let ((goal-x (vec2-x goal))
296 (goal-y (vec2-y goal)))
297 (and (< goal-x (rect-right rect2))
298 (> (+ goal-x (rect-width rect1)) (rect-left rect2))
299 (< goal-y (rect-top rect2))
300 (> (+ goal-y (rect-height rect1)) (rect-bottom rect2)))))
301 (define (overlap-area rect1 rect2 goal)
302 (let ((goal-x (vec2-x goal))
303 (goal-y (vec2-y goal)))
304 (* (- (min (+ goal-x (rect-width rect1)) (rect-right rect2))
305 (max goal-x (rect-left rect2)))
306 (- (min (+ goal-y (rect-height rect1)) (rect-top rect2))
307 (max goal-y (rect-bottom rect2))))))
308 (let ((rect (grid-rect-ref grid item))
309 (search-rect (grid-scratch-rect grid))
310 (collisions (grid-buffer grid))
311 (visited (grid-visited grid)))
312 ;; The search area is the bounding box formed by union of the
313 ;; current rect and the rect formed by moving it to the desired
314 ;; position.
315 (set-rect-x! search-rect (min (vec2-x goal) (rect-x rect)))
316 (set-rect-y! search-rect (min (vec2-y goal) (rect-y rect)))
317 (set-rect-width! search-rect (+ (rect-width rect)
318 (min (- (vec2-x goal)
319 (rect-x rect))
320 0)))
321 (set-rect-height! search-rect (+ (rect-height rect)
322 (min (- (vec2-y goal)
323 (rect-y rect))
324 0)))
325 ;; Reset our scratch space.
326 (array-list-clear! collisions)
327 ;; Visit every cell in the search area.
328 (for-each-item-in-area
329 (lambda (other other-rect)
330 ;; Since items can occupy multiple cells, we must track which
331 ;; items have been processed already so that we don't have
332 ;; duplicate collision results which will almost certainly
333 ;; yield strange behavior.
334 (unless (hashq-ref visited other)
335 ;; The user-provided filter is expected to return a procedure
336 ;; that can resolve a collision between itself and other,
337 ;; should one occur. If the items should clip through each
338 ;; other without any collision, the filter returns #f and we
339 ;; do not waste any time testing for collision.
340 (let ((resolve (filter item other)))
341 (when resolve
342 (when (collision? rect other-rect goal)
343 (array-list-push! collisions
344 (list other
345 (overlap-area rect other-rect goal)
346 resolve)))))))
347 grid search-rect)
348 ;; Sort collisions by overlap area and return the biggest
349 ;; collision. There's definitely improvements that can be made in
350 ;; the heuristic department here, but it's enough for now.
351 (sort-by-area collisions)
352 ;; Return the biggest collision.
353 (if (array-list-empty? collisions)
354 #f
355 (array-list-ref collisions 0))))
356
357 (define (grid-resolve-collisions grid item goal filter)
358 "Check if moving ITEM in GRID to GOAL causes collisions, and if
359 so, resolve them using HANDLER."
360 (let ((visited (grid-visited grid)))
361 (define (collide)
362 (match (grid-find-collision grid item goal filter)
363 ((other _ resolve)
364 (hashq-set! visited other #t)
365 (resolve item
366 (grid-rect-ref grid item)
367 other
368 (grid-rect-ref grid other)
369 goal)
370 ;; Resolving the collision may have caused an another
371 ;; collision, so we must perform the collision test again.
372 ;; This loop continues until the item is no longer colliding
373 ;; with any other item.
374 (collide))
375 (#f #f)))
376 ;; Reset shared scratch space.
377 (hash-clear! visited)
378 ;; Never check collision against ourselves.
379 (hashq-set! visited item #t)
380 (collide)))
381
382 (define (grid-move grid item position filter)
383 "Attempt to move ITEM in GRID to POSITION (a 2D vector) and check
384 for collisions. For each collision, FILTER will be called with two
385 arguments: ITEM and the item it collided with. If a collision occurs,
386 POSITION may be modified to resolve the colliding objects."
387 (grid-resolve-collisions grid item position filter)
388 (grid-update grid item position))
389
390 (define (grid-clear grid)
391 "Remove all items from GRID."
392 (hash-clear! (grid-rects grid))
393 (hash-clear! (grid-rows grid)))
394
395 (define (grid-cell-count grid)
396 "Return the number of cells in GRID."
397 (hash-fold (lambda (y row result)
398 (+ result (hash-count (const #t) row)))
399 0
400 (grid-rows grid)))
401
402 (define (grid-item-count grid)
403 "Return the number of items in GRID."
404 (hash-count (const #t) (grid-rects grid)))