new file mode 100644

@@ -0,0 +1,2 @@

+*.fasl

+*.swp

new file mode 100644

@@ -0,0 +1,141 @@

+(defpackage quadtree

+  (:use :cl)

+  (:export make-rect quick-point new-quadtree contains-point intersects-boundary

+           apply-to-children query-range insert))

+

+(in-package :quadtree)

+

+(defstruct point x y)

+(defstruct size  w h)

+(defstruct bounds center size)

+(defun make-rect (x y w h)

+  (make-bounds

+    :center (make-point :x x :y y)

+    :size   (make-size :w w :h h)))

+

+(defclass quadtree ()

+

+  ((bounds :initarg :bounds

+           :initform (error "must provide boundaries"))

+   (point-count :initform 1)

+   (points :initform (make-array 5 :fill-pointer 0 :adjustable t))

+   (nw :initform nil :type quadtree)

+   (ne :initform nil :type quadtree)

+   (sw :initform nil :type quadtree)

+   (se :initform nil :type quadtree)))

+

+(defun twiddle (lis prefix &rest end)

+  (cond ((null lis) `(progn ,@end))

+        (t (let ((a (car lis))

+                 (b (cdr lis)))

+             `(,prefix ,(car a) ,(cadr a) ,(apply #'twiddle b prefix end))))))

+

+(defmacro mwith-slots (arg-forms &body rst)

+  (apply #'twiddle arg-forms 'with-slots rst))

+

+(defmacro with-foursides (names center size &body body)

+  "Binds the coordinates of the foor sides of a region: the names are bound to w,e,s,n in that order"

+  `(mwith-slots (((x y) ,center)

+                 ((w h) ,size))

+     (let ((,(first names) (- x w))

+           (,(second names) (+ x w))

+           (,(third names) (- y h))

+           (,(fourth names) (+ y h)))

+       ,@body)))

+

+(defmacro with-bounds-foursides (names bounds &body bod)

+  `(with-slots ((a center) (b size)) ,bounds

+     (with-foursides ,names a b ,@bod)))

+

+(defun contains-point (bounds point)

+  (with-slots ((x2 x) (y2 y)) point

+    (with-bounds-foursides (w_x e_x s_y n_y) bounds

+      (and (and (<= w_x x2) (> e_x x2))

+           (and (<= s_y y2) (> n_y y2))))))

+

+(defun between (x a b)

+  "Return T if x is between a and b (order insensitive)"

+  (if (> a b) (rotatef a b)) (and (< a x) (< x b)))

+

+(defun quick-point (x y) (make-point :x x :y y))

+

+(defun intersects~boundary (bounds1 bounds2)

+  (flet

+    ((inner (bounds1 bounds2)

+       (with-bounds-foursides (w_x1 e_x1 s_y1 n_y1) bounds1

+         (with-bounds-foursides (w_x2 e_x2 s_y2 n_y2) bounds2

+           (or (contains-point bounds1 (quick-point w_x2 n_y2))

+               (contains-point bounds1 (quick-point e_x2 n_y2))

+               (contains-point bounds1 (quick-point w_x2 s_y2))

+               (contains-point bounds1 (quick-point e_x2 s_y2)))

+           ))))

+    (or (inner bounds1 bounds2) (inner bounds2 bounds1))))

+

+(defmacro combine (names func first_forms second_forms &body body)

+  (flet ((name_combs (n1s n2s) (loop for n1 in n1s append (loop for n2 in n2s collect (list n1 n2)))))

+    `(let

+       ,(loop for n in names and cs in (name_combs first_forms second_forms) collect (list n (cons func cs)))

+       (progn ,@body))))

+

+(defun subdivide (quadtree)

+  (flet ((divquad (bounds)

+           (mwith-slots (((center size) bounds)

+                         ((x y) center)

+                         ((w h) size))

+             (let* ((half_w (/ w 2))

+                    (w_x (- x half_w))

+                    (e_x (+ x half_w))

+                    (half_h (/ h 2))

+                    (n_y (- y half_h))

+                    (s_y (+ y half_h))

+                    (newsize (make-size :w half_w :h half_h)))

+               (combine (p1 p2 p3 p4) quick-point (n_y s_y) (w_x e_x)

+                 (vector (make-bounds :center p1 :size newsize)

+                       (make-bounds :center p2 :size newsize)

+                       (make-bounds :center p3 :size newsize)

+                       (make-bounds :center p4 :size newsize)))))))

+    (let ((bounds (divquad (slot-value quadtree 'bounds))))

+      (with-slots (nw sw ne se) quadtree

+        (if (null nw) (setf nw (make-instance 'quadtree :bounds (elt bounds 0))))

+        (if (null ne) (setf ne (make-instance 'quadtree :bounds (elt bounds 1)))) 

+        (if (null sw) (setf sw (make-instance 'quadtree :bounds (elt bounds 2)))) 

+        (if (null se) (setf se (make-instance 'quadtree :bounds (elt bounds 3))))))))

+

+(defmacro apply-to-children (quadtree func &rest args)

+  (let*

+    ((nw (gensym))

+     (ne (gensym))

+     (sw (gensym))

+     (se (gensym))

+     (qt (gensym))

+     )

+    `(let ((,qt quadtree))

+       (with-slots (,nw ,ne ,sw ,se) ,qt

+         ,@(loop for x in `(,nw ,ne ,sw ,se) collect

+                 `(,func ,x ,@args))))))

+

+(defun insert (quadtree point)

+  (with-slots (bounds points point-count nw ne sw se) quadtree

+    (if (contains-point bounds point)

+      (if (< (length points) point-count)

+        (progn

+          (vector-push-extend point points)

+          t)

+        (progn

+          (subdivide quadtree)

+          (or (apply-to-children quadtree insert point))))

+      nil))) 

+

+(defun query-range (quadtree bounds)

+  (let ((pir (make-array 5 :fill-pointer 0 :adjustable t)))

+    (with-slots (qbounds qpoints nw) quadtree

+      (if (intersects-boundary qbounds bounds)

+        (progn

+          (loop for p across qpoints

+                if (contains-point bounds p)

+                do (vector-push-extend p pir))

+          (if (not (null nw))

+            (setf pir

+                  (concatenate 'vector

+                               (apply-to-children quadtree query-range bounds)))))))

+    pir))