-
Notifications
You must be signed in to change notification settings - Fork 9
/
quadtree.lisp
263 lines (228 loc) · 9.5 KB
/
quadtree.lisp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
;;; quadtree.lisp --- for spatial indexing and stuff
;; Copyright (C) 2011, 2012 David O'Toole
;; Author: David O'Toole <[email protected]>
;; Keywords:
;; This program is free software; you can redistribute it and/or modify
;; it under the terms of the GNU General Public License as published by
;; the Free Software Foundation, either version 3 of the License, or
;; (at your option) any later version.
;; This program is distributed in the hope that it will be useful,
;; but WITHOUT ANY WARRANTY; without even the implied warranty of
;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
;; GNU General Public License for more details.
;; You should have received a copy of the GNU General Public License
;; along with this program. If not, see <http://www.gnu.org/licenses/>.
;;; Code:
(in-package :blocky)
(defvar *quadtree* nil)
(defvar *buffer* nil
"The current buffer object. Only one may be active at a time. See also
buffers.lisp. Sprites and cells are free to send messages to `*buffer*'
at any time, because `*buffer*' is always bound to the buffer containing
the object when the method is run.")
(defmacro with-quadtree (quadtree &rest body)
`(let* ((*quadtree* ,quadtree))
,@body))
(defvar *quadtree-depth* 0)
(defparameter *default-quadtree-depth* 9)
(defstruct quadtree
objects bounding-box level
southwest northeast northwest southeast)
(defmethod print-object ((tree blocky::quadtree) stream)
(format stream "#<BLOCKY:QUADTREE count: ~S>"
(length (quadtree-objects tree))))
(defun leafp (node)
;; testing any quadrant will suffice
(null (quadtree-southwest node)))
(defun bounding-box-contains (box0 box1)
(destructuring-bind (top0 left0 right0 bottom0) box0
(destructuring-bind (top1 left1 right1 bottom1) box1
(and (<= top0 top1)
(<= left0 left1)
(>= right0 right1)
(>= bottom0 bottom1)))))
(defun scale-bounding-box (box factor)
(destructuring-bind (top left right bottom) box
(let ((margin-x (* (- right left)
(- factor 1.0)))
(margin-y (* (- bottom top)
(- factor 1.0))))
(values (- top margin-y)
(- left margin-x)
(+ right margin-x)
(+ bottom margin-y)))))
(defun valid-bounding-box (box)
(and (listp box)
(= 4 (length box))
(destructuring-bind (top left right bottom) box
(and (<= left right) (<= top bottom)))))
(defun northeast-quadrant (bounding-box)
(assert (valid-bounding-box bounding-box))
(destructuring-bind (top left right bottom) bounding-box
(list top (float (/ (+ left right) 2))
right (float (/ (+ top bottom) 2)))))
(defun southeast-quadrant (bounding-box)
(assert (valid-bounding-box bounding-box))
(destructuring-bind (top left right bottom) bounding-box
(list (float (/ (+ top bottom) 2)) (float (/ (+ left right) 2))
right bottom)))
(defun northwest-quadrant (bounding-box)
(assert (valid-bounding-box bounding-box))
(destructuring-bind (top left right bottom) bounding-box
(list top left
(float (/ (+ left right) 2)) (float (/ (+ top bottom) 2)))))
(defun southwest-quadrant (bounding-box)
(assert (valid-bounding-box bounding-box))
(destructuring-bind (top left right bottom) bounding-box
(list (float (/ (+ top bottom) 2)) left
(float (/ (+ left right) 2)) bottom)))
(defun quadtree-process (bounding-box processor &optional (node *quadtree*))
(assert (quadtree-p node))
(assert (valid-bounding-box bounding-box))
(assert (functionp processor))
(when (bounding-box-contains (quadtree-bounding-box node) bounding-box)
(when (not (leafp node))
(let ((*quadtree-depth* (1+ *quadtree-depth*)))
(quadtree-process bounding-box processor (quadtree-northwest node))
(quadtree-process bounding-box processor (quadtree-northeast node))
(quadtree-process bounding-box processor (quadtree-southwest node))
(quadtree-process bounding-box processor (quadtree-southeast node))))
(funcall processor node)))
(defun build-quadtree (bounding-box0 &optional (depth *default-quadtree-depth*))
(assert (plusp depth))
(assert (valid-bounding-box bounding-box0))
(let ((bounding-box (mapcar #'float bounding-box0)))
(decf depth)
(if (zerop depth)
(make-quadtree :bounding-box bounding-box)
(make-quadtree :bounding-box bounding-box
:northwest (build-quadtree (northwest-quadrant bounding-box) depth)
:northeast (build-quadtree (northeast-quadrant bounding-box) depth)
:southwest (build-quadtree (southwest-quadrant bounding-box) depth)
:southeast (build-quadtree (southeast-quadrant bounding-box) depth)))))
(defun quadtree-search (bounding-box &optional (node *quadtree*))
"Return the smallest quadrant enclosing BOUNDING-BOX at or below
NODE, if any."
(assert (quadtree-p node))
(assert (valid-bounding-box bounding-box))
;; (message "~A ~A Searching quadrant ~S for bounding box ~S"
;; *quadtree-depth* (make-string (1+ *quadtree-depth*) :initial-element (character "."))
;; (quadtree-bounding-box node) bounding-box)
(when (bounding-box-contains (quadtree-bounding-box node) bounding-box)
;; ok, it's in the overall bounding-box.
(if (leafp node)
;; there aren't any quadrants to search.
node
(or
;; search the quadrants.
(let ((*quadtree-depth* (1+ *quadtree-depth*)))
(or (quadtree-search bounding-box (quadtree-northwest node))
(quadtree-search bounding-box (quadtree-northeast node))
(quadtree-search bounding-box (quadtree-southwest node))
(quadtree-search bounding-box (quadtree-southeast node))))
;; none of them are suitable. stay here
node))))
(defun quadtree-insert (object &optional (tree *quadtree*))
(let ((node0
(quadtree-search
(multiple-value-list
(bounding-box object))
tree)))
(let ((node (or node0 tree)))
;; (message "Inserting ~S ~S"
;; (get-some-object-name object)
;; (object-address-string object))
;; (assert (not (find (find-object object)
;; (quadtree-objects node)
;; :test 'eq)))
(pushnew (find-object object)
(quadtree-objects node)
:test 'eq)
;; save pointer to node so we can avoid searching when it's time
;; to delete (i.e. move) the object later.
(blocky:set-field-value :quadtree-node object node)
(assert (find (find-object object)
(quadtree-objects node)
:test 'eq)))))
(defun quadtree-delete (object0 &optional (tree *quadtree*))
(let ((object (find-object object0)))
;; grab the cached quadtree node
(let ((node (or (field-value :quadtree-node object) tree)))
(assert node)
(assert (find object
(quadtree-objects node)
:test 'eq))
(setf (quadtree-objects node)
(delete object (quadtree-objects node) :test 'eq))
(set-field-value :quadtree-node object nil)
(assert (not (find object
(quadtree-objects node)
:test 'eq))))))
(defun quadtree-insert-maybe (object &optional (tree *quadtree*))
(when tree
(quadtree-insert object tree)))
(defun quadtree-delete-maybe (object &optional (tree *quadtree*))
(when (and tree (field-value :quadtree-node object))
(quadtree-delete object tree)))
(defun quadtree-map-collisions (bounding-box processor &optional (tree *quadtree*))
(assert (functionp processor))
(assert (valid-bounding-box bounding-box))
(quadtree-process
bounding-box
#'(lambda (node)
(dolist (object (quadtree-objects node))
(when (colliding-with-bounding-box object bounding-box)
(funcall processor object))))
tree))
(defun quadtree-collide (object &optional (tree *quadtree*))
(quadtree-map-collisions
(multiple-value-list (bounding-box object))
#'(lambda (thing)
(when (and (blockyp thing) (blockyp object)
(field-value :collision-type thing)
(colliding-with object thing)
(not (object-eq object thing)))
(with-quadtree tree
(collide object thing))))
tree))
(defun find-bounding-box (objects)
;; calculate the bounding box of a list of objects
(assert (not (null objects)))
(labels ((left (thing) (field-value :x thing))
(right (thing) (+ (field-value :x thing)
(field-value :width thing)))
(top (thing) (field-value :y thing))
(bottom (thing) (+ (field-value :y thing)
(field-value :height thing))))
;; let's find the bounding box.
(values (reduce #'min (mapcar #'top objects))
(reduce #'min (mapcar #'left objects))
(reduce #'max (mapcar #'right objects))
(reduce #'max (mapcar #'bottom objects)))))
(defun quadtree-fill (set &optional (quadtree *quadtree*))
(let ((objects (etypecase set
(list set)
(hash-table (loop for object being the hash-keys in set collect object)))))
(dolist (object objects)
; (message "Filling ~S into quadtree" object)
(set-field-value :quatree-node object nil)
(quadtree-insert object quadtree))))
(defun quadtree-show (tree &optional object)
(when tree
(let ((bounding-box (quadtree-bounding-box tree)))
(destructuring-bind (top left right bottom) bounding-box
(if (null object)
(draw-box (+ left 10) (+ top 10) (- right left 10) (- bottom top 10)
:color "magenta"
:alpha 0.1)
(when (colliding-with-rectangle
object top left (- right left) (- bottom top))
(draw-box left top (- right left) (- bottom top)
:color "cyan"
:alpha 0.2)))))
(let ((*quadtree-depth* (1+ *quadtree-depth*)))
(quadtree-show (quadtree-northeast tree) object)
(quadtree-show (quadtree-northwest tree) object)
(quadtree-show (quadtree-southeast tree) object)
(quadtree-show (quadtree-southwest tree) object))))
;;; quadtree.lisp ends here