root/trunk/projects/bos/web/quad-tree.lisp

Revision 3792, 15.1 kB (checked in by ksprotte, 3 months ago)

safer print-object method for node-extension

Line 
1 (in-package :bos.web)
2
3 ;;; geo-box
4 (deftype geo-box ()
5   '(simple-array double-float (4)))
6
7 (macrolet ((frob (name index &optional (type 'double-float))
8              `(defmacro ,name (geo-box)
9                 `(the ,',type (aref (the geo-box ,geo-box) ,',index)))))
10   (frob geo-box-west   0 (double-float -180d0 180d0))
11   (frob geo-box-north  1 (double-float -90d0 90d0))
12   (frob geo-box-east   2 (double-float -180d0 180d0))
13   (frob geo-box-south  3 (double-float -90d0 90d0)))
14
15 (defun make-geo-box (west north east south)
16   (declare (optimize (speed 3) (safety 1) (debug 1))
17            (double-float west north east south))
18   (let ((box (make-array 4 :element-type 'double-float)))
19     (setf (geo-box-west box) west
20           (geo-box-north box) north
21           (geo-box-east box) east
22           (geo-box-south box) south)
23     box))
24
25 (defun geo-box-intersect-p (a b)
26   (declare (optimize speed))
27   (not (or (>= (geo-box-west a) (geo-box-east b))
28            (<= (geo-box-east a) (geo-box-west b))
29            (<= (geo-box-north a) (geo-box-south b)) ; north -> south: + -> -
30            (>= (geo-box-south a) (geo-box-north b))))) ; north -> south: + -> -
31
32 (defun geo-box-encloses-p (super-box sub-box)
33   "Does SUPER-BOX completely contain SUB-BOX?"
34   (declare (optimize speed))
35   (and
36    ;; west - east
37    (<= (geo-box-west SUPER-BOX) (geo-box-west sub-box))
38    (< (geo-box-west sub-box) (geo-box-east SUPER-BOX))
39    (< (geo-box-west SUPER-BOX) (geo-box-east sub-box))
40    (<= (geo-box-east sub-box) (geo-box-east SUPER-BOX))
41    ;; north - south
42    (>= (geo-box-north SUPER-BOX) (geo-box-north sub-box)) ; north -> south: + -> -
43    (> (geo-box-north sub-box) (geo-box-south SUPER-BOX))
44    (> (geo-box-north SUPER-BOX) (geo-box-south sub-box)) ; north -> south: + -> -
45    (>= (geo-box-south sub-box) (geo-box-south SUPER-BOX))))
46
47 (defun geo-box-intersection (a b)
48   (assert (geo-box-intersect-p a b))
49   (make-geo-box (max (geo-box-west a) (geo-box-west b))
50                 (min (geo-box-north a) (geo-box-north b))
51                 (min (geo-box-east a) (geo-box-east b))
52                 (max (geo-box-south a) (geo-box-south b))))
53
54 (defun geo-point-in-box-p (box point)
55   (destructuring-bind (west north)
56       point
57     (and (<= (geo-box-west box) west)
58          (<  west (geo-box-east box))
59          (>= (geo-box-north box) north)    ; north -> south: + -> -
60          (>  north (geo-box-south box))))) ; north -> south: + -> -
61
62 (defun geo-box-rectangle (box)
63   (make-instance 'rectangle
64                  :top-left (make-point :lon (geo-box-west box) :lat (geo-box-north box))
65                  :bottom-right (make-point :lon (geo-box-east box) :lat (geo-box-south box))))
66
67 (defun rectangle-geo-box (rectangle)
68   (multiple-value-bind (west north)
69       (point-lon-lat (top-left rectangle))
70     (multiple-value-bind (east south)
71         (point-lon-lat (bottom-right rectangle))
72       (make-geo-box west north east south))))
73
74 (defun geo-subbox (box x y divisor subbox)
75   (declare (optimize speed)
76            (fixnum x y divisor) (geo-box subbox))
77   (with-accessors ((north geo-box-north)
78                    (south geo-box-south)
79                    (west geo-box-west)
80                    (east geo-box-east))
81       box
82     (let* ((divisor (float divisor 0d0))
83            (width (- east west))
84            (height (- north south))
85            (width-unit (/ width divisor))
86            (height-unit (/ height divisor)))
87       (setf (geo-box-north subbox) (- north (* y height-unit))
88             (geo-box-south subbox) (- north (* (1+ y) height-unit))
89             (geo-box-west subbox) (+ west (* x width-unit))
90             (geo-box-east subbox) (+ west (* (1+ x) width-unit)))
91       subbox)))
92
93 (let ((float-pair (geo-utm:make-float-pair)))
94   (defun geo-box-middle-m2coord (box)
95     (declare (optimize speed))
96     (labels ((geo-box-middle (box)
97                (with-accessors ((north geo-box-north)
98                                 (south geo-box-south)
99                                 (west geo-box-west)
100                                 (east geo-box-east))
101                    box
102                  (let ((width (- east west))
103                        (height (- north south)))
104                    (values (+ west (/ width 2))
105                            (- north (/ height 2))))))
106              (geo-box-middle-utm (box)
107                (multiple-value-bind (lon lat)
108                    (geo-box-middle box)
109                  (geo-utm:lon-lat-to-utm-x-y* lon lat float-pair))))
110       (let* ((x-y (geo-box-middle-utm box))
111              (x (aref x-y 0))
112              (y (aref x-y 1)))
113         (values (truncate (the (double-float 0d0 #.(float most-positive-fixnum 0d0))
114                             (- x +nw-utm-x+)))
115                 (truncate (the (double-float 0d0 #.(float most-positive-fixnum 0d0))
116                             (- +nw-utm-y+ y))))))))
117
118 (defvar *m2-geo-box* (make-geo-box 116.92538417241805d0 -0.9942953097298868d0
119                                    117.02245623511905d0 -1.0920067364569994d0))
120
121 ;;; quad-node
122 (defclass quad-node ()
123   ((geo-box :reader geo-box :initarg :geo-box :type geo-box)
124    (children :reader children :initarg :children :initform (make-array 4 :initial-element nil))
125    (depth :reader depth :initarg :depth :initform 0)
126    (extensions :reader extensions :accessor %extensions :initarg :extensions :initform nil)))
127
128 (defmethod shared-initialize ((obj quad-node) slot-names &key parent-node index &allow-other-keys)
129   (declare (ignore parent-node index))
130   (call-next-method))
131
132 (defmethod shared-initialize :after ((obj quad-node) slot-names &key)
133   (assert (and (slot-boundp obj 'geo-box)
134                (geo-box obj)
135                (typep (geo-box obj) 'geo-box))
136           ((slot-value obj 'geo-box))
137           "~s needs a geo-box" obj))
138
139 (defmethod print-object ((node quad-node) stream)
140   (print-unreadable-object (node stream :type t :identity t)
141     (format stream "path: ~a" (node-path node))))
142
143 (defmethod extensions ((node null)) nil)
144
145 ;;; node-extension
146 (eval-when (:compile-toplevel :load-toplevel :execute)
147   (defclass node-extension ()
148     ((base-node :reader base-node :accessor %base-node :initform nil)
149      (name :reader name :initarg :name :initform nil))))
150
151 (defmethod (setf %base-node) :before (base-node (node node-extension))
152   (assert (not (member node (%extensions base-node) :test #'equal-extension-type)) nil
153           "Cannot add ~s to extensions of ~s.~
154          ~%An extension of same class and name already exists." node base-node))
155
156 (defmethod (setf %base-node) :after (base-node (node node-extension))
157   (push node (%extensions base-node)))
158
159 (defmethod shared-initialize :after ((obj node-extension) slot-names
160                                      &key base-node parent-node index
161                                      &allow-other-keys)
162   (flet ((xor (a b)
163            (or (and (not a) b)
164                (and a (not b)))))
165     (assert (xor base-node (and parent-node index)))
166     (if base-node
167         (setf (%base-node obj) base-node)
168         (setf (%base-node obj) (ensure-child (base-node parent-node) index)
169               (slot-value obj 'name) (name parent-node)))
170     ;; (assert (base-node obj) nil "~s needs a base-node" obj)
171     (assert (name obj) ((slot-value obj 'name)) "~s needs a name" obj)))
172
173 (macrolet ((def-extension-reader (reader)
174              `(defmethod ,reader ((node node-extension))
175                 (,reader (base-node node)))))
176   (def-extension-reader geo-box)
177   (def-extension-reader depth))
178
179 (defmethod print-object ((node node-extension) stream)
180   (handler-case
181       (let ((name (name node))
182             (node-path (node-path node)))
183         (print-unreadable-object (node stream :type t :identity t)
184           (format stream "name: ~s path: ~s" name node-path)))
185     (error ()
186       (print-unreadable-object (node stream :type t :identity t)))))
187
188 (defmethod delete-node-extension ((node node-extension))
189   (setf (%extensions (base-node node))
190         (delete node (%extensions (base-node node)))))
191
192 (defun equal-extension-type (a b)
193   (and (eql (type-of a)
194             (type-of b))
195        (eql (name a)
196             (name b))))
197
198 (defgeneric leaf-node-p (node))
199
200 (defun compute-child-geo-box (node index)
201   (declare #+nil(optimize speed)
202            (fixnum index))
203   (with-accessors ((north geo-box-north)
204                    (south geo-box-south)
205                    (west geo-box-west)
206                    (east geo-box-east))
207       (geo-box node)
208     (let ((middle-north (- north (/ (- north south) 2d0)))
209           (middle-west (+ west (/ (- east west) 2d0))))
210       (ecase index
211         (0 (make-geo-box   west         north         middle-west  middle-north))
212         (1 (make-geo-box   middle-west  north         east         middle-north))
213         (2 (make-geo-box   west         middle-north  middle-west  south))
214         (3 (make-geo-box   middle-west  middle-north  east         south))))))
215
216 (defun intersecting-children-indices (node geo-box)
217   "Independently of whether a certain child of NODE actually exists,
218 returns indices of those children that would intersect with GEO-BOX."
219   (loop for index from 0 to 3
220      for child-box = (compute-child-geo-box node index)
221      when (geo-box-intersect-p child-box geo-box)
222      collect index))
223
224 (defgeneric child (node index)
225   (:method ((node quad-node) index)
226     (aref (children node) index))
227   (:method ((node node-extension) index)
228     (let ((base-child (child (base-node node) index)))
229       (find node (extensions base-child) :test #'equal-extension-type))))
230
231 (defgeneric (setf child) (child node index)
232   (:method (new-value (node quad-node) index)
233     (setf (aref (children node) index) new-value))
234   (:method (child (node node-extension) index)
235     (let ((base-child (child (base-node node) index)))
236       (pushnew child (%extensions base-child ))
237       child)))
238
239 (defun ensure-child (node index)
240   (let ((child (child node index)))
241     (or child
242         (setf (child node index)
243               (make-instance (class-of node)
244                              :parent-node node
245                              :index index
246                              :geo-box (compute-child-geo-box node index)
247                              :depth (1+ (depth node)))))))
248
249 (defun node-has-children-p (node)
250   (any-child node))
251
252 (defgeneric any-child (node)
253   (:method ((node quad-node))
254     (find-if #'identity (children node)))
255   (:method ((node node-extension))
256     (dotimes (i 4)
257       (let ((child (child node i)))
258         (when child (return child))))))
259
260 (defun child-index (node child)
261   (dotimes (i 4)
262     (when (eq (child node i) child)
263       (return i))))
264
265 (defun find-node-with-path (node path)
266   (if (null path)
267       node
268       (let ((child (child node (first path))))
269         (if child
270             (find-node-with-path child (rest path))
271             (error "~s has no child to descend on (sub)path ~s" node path)))))
272
273 (defun ensure-node-with-path (node path)
274   (if (null path)
275       node
276       (ensure-node-with-path (ensure-child node (first path)) (rest path))))
277
278 (defun ensure-intersecting-children (node geo-box &optional function (leaf-test #'leaf-node-p))
279   "Maps FUNCTION over NODE and all children of NODE that intersect
280 with GEO-BOX.  Children that dont exist yet are created on the fly. If
281 LEAF-TEST returns true, the children of the current node are not
282 further recursed into."
283   (when function
284     (funcall function node))
285   (unless (funcall leaf-test node)
286     (dolist (index (intersecting-children-indices node geo-box))
287       (ensure-intersecting-children (ensure-child node index) geo-box function leaf-test))))
288
289 (defun map-nodes-internal (nodes function prune-test remove-node add-node)
290   "Used by MAP-NODES for depth-first and breadth-first
291 traversal.
292
293 NODES is an opaque collection, that is accessed via the given
294 functions REMOVE-NODE and ADD-NODE.
295
296 REMOVE-NODE will be called with NODES and has to return two
297 values: The removed node and the updated NODES.
298
299 ADD-NODE will be called with a node to be added and NODES. It has
300 to return the updated NODES.
301
302 FUNCTION will be called on each visited node.
303
304 If PRUNE-TEST returns true, the given node will not be visited."
305   (labels ((pop* ()
306              (multiple-value-bind (node new-nodes)
307                  (funcall remove-node nodes)
308                (setq nodes new-nodes)
309                node))
310            (push* (node)
311              (setq nodes (funcall add-node node nodes))))
312     (let ((node (pop*)))
313       (when node
314         (funcall function node)
315         (dotimes (i 4)
316           (let ((child (child node i)))
317             (when (and child (not (funcall prune-test child)))
318               (push* child))))
319         (map-nodes-internal nodes function prune-test remove-node add-node)))))
320
321 (defun map-nodes-depth-first (function node prune-test)
322   ;; nodes is here a stack
323   (map-nodes-internal (list node) function prune-test
324                       (lambda (nodes)
325                         (values (car nodes) (cdr nodes)))
326                       (lambda (node nodes)
327                         (cons node nodes))))
328
329 (defun map-nodes-breadth-first (function node prune-test)
330   ;; nodes is here a queue
331   (let ((nodes (make-queue)))
332     (enqueue node nodes)
333     (map-nodes-internal nodes function prune-test
334                         (lambda (nodes)
335                           (values (dequeue nodes) nodes))
336                         (lambda (node nodes)
337                           (enqueue node nodes)
338                           nodes))))
339
340 (defun map-nodes (function node &key (prune-test (constantly nil)) (order :depth-first))
341   (check-type order (member :depth-first :breadth-first))
342   (let ((mapper (case order
343                   (:depth-first #'map-nodes-depth-first)
344                   (:breadth-first #'map-nodes-breadth-first))))
345     (funcall mapper function node prune-test)))
346
347 (defun find-node-if (test node &key (prune-test (constantly nil)) (order :depth-first))
348   (block nil
349     (map-nodes (lambda (node)
350                  (when (funcall test node)
351                    (return node)))
352                node
353                :prune-test prune-test
354                :order order)
355     nil))
356
357 (defun collect-nodes (test node &key (prune-test (constantly nil)) (order :depth-first))
358   (let (nodes)
359     (map-nodes (lambda (node)
360                  (when (funcall test node)
361                    (push node nodes)))
362                node
363                :prune-test prune-test
364                :order order)
365     (nreverse nodes)))
366
367 (defmethod node-path ((node node-extension))
368   (node-path (base-node node)))
369
370 ;;; *quad-tree*
371 (defvar *quad-tree*)
372
373 (defun make-quad-tree ()
374   (setq *quad-tree* (make-instance 'quad-node :geo-box *m2-geo-box*)))
375
376 (defmethod node-path ((node quad-node))
377   (let (prev-n path)
378     (map-nodes (lambda (n)
379                  (when prev-n
380                    (push (child-index prev-n n) path))
381                  (when (eq n node)
382                    (return-from node-path (nreverse path)))
383                  (setq prev-n n))
384                *quad-tree*
385                :prune-test (lambda (n) (not (geo-box-intersect-p (geo-box n)
386                                                                  (geo-box node))))
387                :order :depth-first)))
388
389 (defun node-lod (node)
390   (if (zerop (depth node))
391       '(:min 16 :max -1)
392       '(:min 512 :max -1)))
393
394 (defconstant +max-num-of-local-draw-order-levels+ 10)
395
396 (defun compute-draw-order (node local-draw-order)
397   (+ local-draw-order
398      (* (depth node) +max-num-of-local-draw-order-levels+)))
399
400 (register-transient-init-function 'make-quad-tree)
Note: See TracBrowser for help on using the browser.