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