~jl2/quadtree

85434a0bf6d35f6e4cb27162b8723e1d2fd28f55 — Jeremiah LaRocco 3 years ago f65248e
Store children quadtrees in an array instead of as slots.
7 files changed, 122 insertions(+), 113 deletions(-)

M bounds.lisp
M package.lisp
M point-quadtree.lisp
M pr-quadtree.lisp
M quadtree.lisp
M t/package.lisp
M visualization.lisp
M bounds.lisp => bounds.lisp +15 -12
@@ 65,18 65,21 @@
  (with-slots (x-min x-max y-min y-max) bounds
    (let ((x-mid (/ (+ x-max x-min) 2))
          (y-mid (/ (+ y-max y-min) 2)))
      (list (cons 'top-left (make-instance 'quadtree-bounds
                                              :x-min x-min :x-max x-mid
                                              :y-min y-mid :y-max y-max))
            (cons 'top-right (make-instance 'quadtree-bounds
                                               :x-min x-mid :x-max x-max
                                               :y-min y-mid :y-max y-max))
            (cons 'bottom-left (make-instance 'quadtree-bounds
                                           :x-min x-min :x-max x-mid
                                           :y-min y-min :y-max y-mid))
            (cons 'bottom-right (make-instance 'quadtree-bounds
                                            :x-min x-mid :x-max x-max
                                            :y-min y-min :y-max y-mid))))))
      (make-array 4
                  :initial-contents
                  (list 
                   (make-instance 'quadtree-bounds
                                  :x-min x-min :x-max x-mid
                                  :y-min y-mid :y-max y-max)
                   (make-instance 'quadtree-bounds
                                  :x-min x-mid :x-max x-max
                                  :y-min y-mid :y-max y-max)
                   (make-instance 'quadtree-bounds
                                  :x-min x-min :x-max x-mid
                                  :y-min y-min :y-max y-mid)
                   (make-instance 'quadtree-bounds
                                  :x-min x-mid :x-max x-max
                                  :y-min y-min :y-max y-mid))))))

(defmethod print-object ((bound quadtree-bounds) stream)
  (with-slots (x-min y-min x-max y-max) bound

M package.lisp => package.lisp +5 -7
@@ 17,7 17,10 @@
(defpackage :quadtree
  (:use #:cl #:j-utils #:alexandria #:3d-vectors)
  (:export

   #:*top-left*
   #:*top-right*
   #:*bottom-left*
   #:*bottom-right*
           #:quadtree-bounds
           #:from-point-range
           #:inside-p


@@ 44,11 47,6 @@
           #:quadrant-of
           #:opposite-quadrant

           #:top-left
           #:top-right
           #:bottom-left
           #:bottom-right

           #:make-entry
           #:is-point
           #:remove-value


@@ 56,7 54,7 @@
           #:contains
           #:view-quadtree
           #:random-quadtree

           #:build-grid-quadtree
           #:parametric-quadtree
           #:parametric-animation
           #:benchmark-random-pr-quadtree-search

M point-quadtree.lisp => point-quadtree.lisp +17 -17
@@ 22,7 22,7 @@

(defmethod insert ((qt point-quadtree) new-point new-item)
  (declare (optimize (speed 3) (safety 1) (debug 1)))
  (with-slots (entry size) qt
  (with-slots (entry children size) qt
    (incf size)
    (cond ((null entry)
           (setf entry (make-entry new-point new-item)))


@@ 32,21 32,21 @@

          (t
           (let ((quad (quadrant-of (slot-value entry 'point) new-point)))
             (when (null (slot-value qt quad))
               (setf (slot-value qt quad) (make-instance 'point-quadtree)))
             (insert (slot-value qt quad) new-point new-item))))))
             (when (null (aref children quad))
               (setf (aref children quad) (make-instance 'point-quadtree)))
             (insert (aref children quad) new-point new-item))))))

(defmethod depth-first ((qt point-quadtree) function)
  (declare (optimize (speed 3) (safety 1) (debug 1)))
  (with-slots (entry size top-left top-right bottom-left bottom-right) qt
    (when top-left
      (depth-first top-left function))
    (when top-right
      (depth-first top-right function))
    (when bottom-right
      (depth-first bottom-right function))
    (when bottom-left
      (depth-first bottom-left function))
  (with-slots (entry children size) qt
    (when (aref children *top-left*)
      (depth-first (aref children *top-left*) function))
    (when (aref children *top-right*)
      (depth-first (aref children *top-right*) function))
    (when (aref children *bottom-right*)
      (depth-first (aref children *bottom-right*) function))
    (when (aref children *bottom-left*)
      (depth-first (aref children *bottom-left*) function))
    (when entry
      (funcall function entry))))



@@ 68,14 68,14 @@
  (let ((bounds (from-point-range search-point range)))
    (labels
        ((rfind (qt)
           (with-slots (entry size) qt
           (with-slots (entry children size) qt
             (let* ((quadrants (mapcar (curry #'quadrant-of (slot-value entry 'point))
                                       (bounds-to-points bounds)))
                    (unique-quads (remove-duplicates quadrants :test #'equal))
                    (unique-quads (remove-duplicates quadrants :test #'=))
                    (rvals (loop
                              for quad in unique-quads
                              when (slot-value qt quad)
                              append (rfind (slot-value qt quad)))))
                              when (aref children quad)
                              append (rfind (aref children quad)))))
               (if (inside-p (slot-value entry 'point) bounds)
                 (cons entry rvals)
                 rvals)))))

M pr-quadtree.lisp => pr-quadtree.lisp +17 -17
@@ 33,27 33,27 @@
(defun split-quadtree (qt)
  "Split a quadtree into 4 new nodes."
  (declare (optimize (speed 3) (safety 1) (debug 1)))
  (with-slots (bounds entries size) qt
    (dolist (new-bound (split-bounds bounds))
      (let ((quad-name (car new-bound))
            (this-bound (cdr new-bound)))
        (setf (slot-value qt quad-name) (make-instance 'pr-quadtree :bounds this-bound))
        (dolist (entry entries)
          (when (inside-p (slot-value entry 'point) this-bound)
            (incf (slot-value (slot-value qt quad-name) 'size))
            (push entry (slot-value (slot-value qt quad-name) 'entries))))
        (when (needs-split (slot-value qt quad-name))
          (split-quadtree (slot-value qt quad-name)))))
  (with-slots (bounds children entries size) qt
    (loop for this-bound across (split-bounds bounds)
       for quad-idx from 0
       do
         (setf (aref children quad-idx) (make-instance 'pr-quadtree :bounds this-bound))
         (dolist (entry entries)
           (when (inside-p (slot-value entry 'point) this-bound)
             (incf (slot-value (aref children quad-idx) 'size))
             (push entry (slot-value (aref children quad-idx) 'entries))))
         (when (needs-split (aref children quad-idx))
           (split-quadtree (aref children quad-idx))))
    (setf entries nil)))

(defmethod insert ((qt pr-quadtree) new-point new-item)
  (declare (optimize (speed 3) (safety 1) (debug 1)))
  (with-slots (bounds entries size top-left top-right bottom-left bottom-right) qt
  (with-slots (bounds children entries size ) qt
    (when (not (inside-p new-point bounds))
      (error "~a is not inside quadtree bounds." new-point))
    (cond
      ;; Empty tree
      ((every #'null (list entries top-left top-right bottom-left bottom-right))
      ((every #'null (list entries (aref children *top-left*) (aref children *top-right*) (aref children *bottom-left*) (aref children *bottom-right*)))
       (setf entries (list (make-entry new-point new-item))))

      ;; Not empty but smaller than *split-size*


@@ 67,7 67,7 @@
                  (split-quadtree qt))))))
      (t
       (let ((quad (quadrant-of (midpoint bounds) new-point)))
         (insert (slot-value qt quad) new-point new-item))))
         (insert (aref children quad) new-point new-item))))
    (incf size)))




@@ 76,13 76,13 @@
  (let ((find-bound (from-point-range search-point range)))
    (labels
        ((rfind (qt)
           (with-slots (entries bounds size) qt
           (with-slots (entries children bounds size) qt
             (if (null entries)
                 ;; Recursion case
                 ;; Entries is null, so all data is in subtrees
                 ;; For each quadtree, if it's qsize > 0 then 
                 (loop for quad in '(top-left bottom-left top-right bottom-right)
                    for sub-tree = (slot-value qt quad) then (slot-value qt quad)
                 (loop for quad in (list *top-left* *bottom-left* *top-right* *bottom-right*)
                    for sub-tree = (aref children quad) then (aref children quad)
                    when (and sub-tree (not (zerop (qsize sub-tree))))
                    append (rfind sub-tree))
                 ;; End recursion case

M quadtree.lisp => quadtree.lisp +24 -18
@@ 16,13 16,19 @@

(in-package :quadtree)

(defparameter *top-left* 0)
(defparameter *top-right* 1)
(defparameter *bottom-left* 2)
(defparameter *bottom-right* 3)

(defclass quadtree ()
  (
   (size :initform 0 :type fixnum)
   (top-left :initform nil :type (or null quadtree))
   (top-right :initform nil :type (or null quadtree))
   (bottom-left :initform nil :type (or null quadtree))
   (bottom-right :initform nil :type (or null quadtree)))
  ((size :initform 0 :type fixnum)
   (children :initform (make-array 4 :element-type '(or null quadtree::quadtree) :initial-element nil) :type (simple-vector 4))
   ;; (top-left :initform nil :type (or null quadtree))
   ;; (top-right :initform nil :type (or null quadtree))
   ;; (bottom-left :initform nil :type (or null quadtree))
   ;; (bottom-right :initform nil :type (or null quadtree))
   )
  (:documentation "A QuadTree class."))

(defgeneric qsize (qt)


@@ 67,27 73,27 @@
  "Returns the quadrant of pt relative to root."
  (cond ((and (< (vx pt) (vx root))
              (>= (vy pt) (vy root)))
         'top-left)
         *top-left*)
        ((and (< (vx pt) (vx root))
              (< (vy pt) (vy root)))
         'bottom-left)
         *bottom-left*)
        ((and (>= (vx pt) (vx root))
              (>= (vy pt) (vy root)))
         'top-right)
         *top-right*)
        ((and (>= (vx pt) (vx root))
              (< (vy pt) (vy root)))
         'bottom-right)))
         *bottom-right*)))

(defun opposite-quadrant (quad)
  "Returns the quadrant 180 degrees from quad."
  (cond ((eq quad 'top-left)
         'bottom-right)
        ((eq quad 'top-right)
         'bottom-left)
        ((eq quad 'bottom-left)
         'top-right)
        ((eq quad 'bottom-right)
         'top-left)
  (cond ((eq quad *top-left*)
         *bottom-right*)
        ((eq quad *top-right*)
         *bottom-left*)
        ((eq quad *bottom-left*)
         *top-right*)
        ((eq quad *bottom-right*)
         *top-left*)
        (t (error "Unknown quadrant! ~a" quad))))



M t/package.lisp => t/package.lisp +10 -11
@@ 71,8 71,7 @@
    (insert qt (vec2 4.0 0.0) 5)
    (insert qt (vec2 5.0 0.0) 6)
    (is-true (= 2 (length (range-find qt (vec2 2.5 0.0) 1.0)))))

  (let* ((qt (build-grid-quadtree 'point-quadtree 5 5))
  (let* ((qt (build-grid-quadtree 'point-quadtree (from-point-range (vec2 2.5 2.5) 2.5) 5 5))
         (first-results (range-find qt (vec2 2 2) 1.0))
         (second-results (range-find qt (vec2 2.5 2.5) 1.0)))
    (is-true (= 4 (length first-results)))


@@ 137,10 136,10 @@


(test quadrant-of
  (is-true (eq 'top-left     (quadrant-of (vec2 0.0 0.0) (vec2 -1.0 1.0))))
  (is-true (eq 'top-right    (quadrant-of (vec2 0.0 0.0) (vec2 1.0 1.0))))
  (is-true (eq 'bottom-left  (quadrant-of (vec2 0.0 0.0) (vec2 -1.0 -1.0))))
  (is-true (eq 'bottom-right (quadrant-of (vec2 0.0 0.0) (vec2 1.0 -1.0)))))
  (is-true (= *top-left*     (quadrant-of (vec2 0.0 0.0) (vec2 -1.0 1.0))))
  (is-true (= *top-right*    (quadrant-of (vec2 0.0 0.0) (vec2 1.0 1.0))))
  (is-true (= *bottom-left*  (quadrant-of (vec2 0.0 0.0) (vec2 -1.0 -1.0))))
  (is-true (= *bottom-right* (quadrant-of (vec2 0.0 0.0) (vec2 1.0 -1.0)))))

(test quadtree-bounds
  (let* ((bounds (from-point-range (vec2 0.0 0.0) 5.0))


@@ 155,10 154,10 @@
    (is-true  (inside-p (vec2 -2.5 2.5) bounds))
    (is-true  (inside-p (vec2 -2.5 -2.5) bounds))
    (let ((sb (split-bounds bounds)))
      (is-true (inside-p (vec2 2.5 2.5) (cdr (assoc 'quadtree:top-right sb))))
      (is-true (inside-p (vec2 -2.5 2.5) (cdr (assoc 'quadtree:top-left sb))))
      (is-true (inside-p (vec2 2.5 -2.5) (cdr (assoc 'quadtree:bottom-right sb))))
      (is-true (inside-p (vec2 -2.5 -2.5) (cdr (assoc 'quadtree:bottom-left sb)))))))
      (is-true (inside-p (vec2 2.5 2.5) (aref sb quadtree:*top-right*)))
      (is-true (inside-p (vec2 -2.5 2.5) (aref sb quadtree:*top-left*)))
      (is-true (inside-p (vec2 2.5 -2.5) (aref sb quadtree:*bottom-right*)))
      (is-true (inside-p (vec2 -2.5 -2.5) (aref sb quadtree:*bottom-left*))))))


(test entries


@@ 208,7 207,7 @@
    (insert qt (vec2 5.0 0.0) 6)
    (is-true (= 2 (length (range-find qt (vec2 2.5 0.0) 1.0)))))

  (let* ((qt (build-grid-quadtree 'pr-quadtree 5 5))
  (let* ((qt (build-grid-quadtree 'pr-quadtree (from-point-range (vec2 2.5 2.5) 2.5) 5 5))
         (first-results (range-find qt (vec2 2 2) 1.0))
         (second-results (range-find qt (vec2 2.5 2.5) 1.0)))
    (is-true (= 4 (length first-results)))

M visualization.lisp => visualization.lisp +34 -31
@@ 82,38 82,42 @@
      (cl-cairo2:scale x-scale y-scale)
      (labels
          ((draw-quadtree (qt bound)
             (with-slots (entry top-left bottom-left top-right bottom-right) qt
             (with-slots (entry children) qt
               (with-slots (point) entry
                 (draw-point point (/ 4 x-scale))

                 (when (or top-left bottom-left top-right bottom-right)
                   (draw-cross point bound)
                 (let ((top-left (aref children *top-left*))
                       (bottom-left (aref children *bottom-left*))
                       (top-right (aref children *top-right*))
                       (bottom-right (aref children *bottom-right*)))
                   (when (or top-left bottom-left top-right bottom-right)
                     (draw-cross point bound)

                   (with-slots (x-min y-min x-max y-max) bound
                     (when top-left
                       (draw-quadtree top-left (make-instance 'quadtree-bounds
                                                              :x-min x-min
                                                              :x-max (vx point)
                                                              :y-min (vy point)
                                                              :y-max y-max)))
                     (when top-right
                       (draw-quadtree top-right (make-instance 'quadtree-bounds
                                                               :x-min (vx point)
                                                               :x-max x-max
                                                               :y-min (vy point)
                                                               :y-max y-max)))
                     (when bottom-left
                       (draw-quadtree bottom-left (make-instance 'quadtree-bounds
                                                                 :x-min x-min
                                                                 :x-max (vx point)
                                                                 :y-min y-min
                                                                 :y-max (vy point))))
                     (when bottom-right
                       (draw-quadtree bottom-right (make-instance 'quadtree-bounds
                                                                  :x-min (vx point)
                                                                  :x-max x-max
                                                                  :y-min y-min
                                                                  :y-max (vy point))))))))))
                     (with-slots (x-min y-min x-max y-max) bound
                       (when top-left
                         (draw-quadtree top-left (make-instance 'quadtree-bounds
                                                                :x-min x-min
                                                                :x-max (vx point)
                                                                :y-min (vy point)
                                                                :y-max y-max)))
                       (when top-right
                         (draw-quadtree top-right (make-instance 'quadtree-bounds
                                                                 :x-min (vx point)
                                                                 :x-max x-max
                                                                 :y-min (vy point)
                                                                 :y-max y-max)))
                       (when bottom-left
                         (draw-quadtree bottom-left (make-instance 'quadtree-bounds
                                                                   :x-min x-min
                                                                   :x-max (vx point)
                                                                   :y-min y-min
                                                                   :y-max (vy point))))
                       (when bottom-right
                         (draw-quadtree bottom-right (make-instance 'quadtree-bounds
                                                                    :x-min (vx point)
                                                                    :x-max x-max
                                                                    :y-min y-min
                                                                    :y-max (vy point)))))))))))
        (draw-quadtree qt (make-instance 'quadtree-bounds
                                         :x-min -1000.0
                                         :x-max 1000.0


@@ 143,15 147,14 @@
      (cl-cairo2:scale x-scale y-scale)
      (labels
          ((draw-quadtree (qt)
             (with-slots (entries bounds) qt
             (with-slots (entries children bounds) qt
               (draw-bound bounds)

               (dolist (entry entries)
                 (draw-point (slot-value entry 'point) (/ 4 x-scale)))

               (loop
                  for quad in '(top-left bottom-left top-right bottom-right)
                  for sub-tree = (slot-value qt quad) then (slot-value qt quad)
                  for sub-tree across children
                  when (and sub-tree
                            (not (zerop (qsize sub-tree))))
                  do