(in-package #:3dr.octcone2)
(defun make-adj-arr (&optional (size 25))
(make-array size :adjustable t :fill-pointer 0))
(defclass quadtree ()
((nw-bucket :initarg :nw-bucket :accessor nw-bucket :initform nil)
(ne-bucket :initarg :ne-bucket :accessor ne-bucket :initform nil)
(sw-bucket :initarg :sw-bucket :accessor sw-bucket :initform nil)
(se-bucket :initarg :se-bucket :accessor se-bucket :initform nil)
(nw-bucket-bg :initarg :nw-bucket-bg :accessor nw-bucket-bg :initform nil)
(ne-bucket-bg :initarg :ne-bucket-bg :accessor ne-bucket-bg :initform nil)
(sw-bucket-bg :initarg :sw-bucket-bg :accessor sw-bucket-bg :initform nil)
(se-bucket-bg :initarg :se-bucket-bg :accessor se-bucket-bg :initform nil)
(duplicates :initarg :duplicates :accessor duplicates :initform (make-adj-arr))
(center :initarg :center :accessor center)))
(defgeneric insert-in-quadtree (quadtree point))
(defstruct (point (:type vector)) x y z)
(defun maybe-replace-bucket (quadtree bucket point)
(declare (optimize (debug 3)))
(let ((bucket-val (slot-value quadtree bucket)))
(if (not bucket-val)
(setf (slot-value quadtree bucket) point)
(etypecase bucket-val
(vector (setf (slot-value quadtree bucket)
(insert-in-quadtree (make-instance 'quadtree
:center bucket-val)
point)))
(quadtree (if (< (c-distance (center quadtree) point)
(c-distance (center quadtree) (center bucket-val)))
(let ((new-quadtree (make-instance 'quadtree :center point)))
(setf (slot-value quadtree bucket) new-quadtree)
(insert-in-quadtree new-quadtree bucket-val))
(insert-in-quadtree bucket-val point)))))))
(defun c-distance (p1 p2)
(declare (optimize (speed 3) (debug 0))
(inline c-distance))
(sqrt (loop for c1 across p1
for c2 across p2
sum (expt (- c2 c1) 2))))
(defmethod insert-in-quadtree ((quadtree quadtree) (child quadtree))
(with-slots ((parent-center center) duplicates) quadtree
(with-slots ((child-center center)) child
(with-accessors ((point-x point-x) (point-y point-y) (point-z point-z)) child-center
(with-accessors ((center-x point-x) (center-y point-y) (center-z point-z)) parent-center
(if (equalp parent-center child-center)
(vector-push-extend child-center duplicates)
(maybe-replace-bucket quadtree
(if (< point-x center-x)
(if (< point-y center-y)
(if (< point-z center-z) 'sw-bucket-bg 'sw-bucket)
(if (< point-z center-z) 'nw-bucket-bg 'nw-bucket))
(if (< point-y center-y)
(if (< point-z center-z) 'se-bucket-bg 'se-bucket)
(if (< point-z center-z) 'ne-bucket-bg 'ne-bucket)))
child)))))))
(defmethod insert-in-quadtree ((quadtree quadtree) (point vector))
(declare (optimize (debug 3)))
(prog1 quadtree
(with-slots (duplicates center) quadtree
(with-accessors ((point-x point-x) (point-y point-y) (point-z point-z)) point
(with-accessors ((center-x point-x) (center-y point-y) (center-z point-z)) center
(if (equalp point center)
(vector-push-extend point duplicates)
(maybe-replace-bucket quadtree
(if (< point-x center-x)
(if (< point-y center-y)
(if (< point-z center-z) 'sw-bucket-bg 'sw-bucket)
(if (< point-z center-z) 'nw-bucket-bg 'nw-bucket))
(if (< point-y center-y)
(if (< point-z center-z) 'se-bucket-bg 'se-bucket)
(if (< point-z center-z) 'ne-bucket-bg 'ne-bucket)))
point)))))))
(defmethod print-object ((object quadtree) s)
(declare (optimize (debug 3)))
(print-unreadable-object (object s :type t :identity t)
(with-slots (center nw-bucket nw-bucket-bg ne-bucket ne-bucket-bg
se-bucket se-bucket-bg sw-bucket sw-bucket-bg
duplicates) object
(format s "~s x~d: ~@{~a~^ ~}"
center
(length duplicates)
(not (null nw-bucket))
(not (null nw-bucket-bg))
(not (null ne-bucket))
(not (null ne-bucket-bg))
(not (null sw-bucket))
(not (null sw-bucket-bg))
(not (null se-bucket))
(not (null se-bucket-bg))))))
(defun make-graph-conns (conns)
(format nil "~:{~4t~(\"~a\" -> \"~a\"~%~)~}" conns))
(defun make-graph (conns)
(format nil "digraph {splines=ortho;~%node [shape=box];~%~a~%}~%" (make-graph-conns conns)))
(defun quadtree-to-graph (quadtree &optional parent)
(declare (optimize (debug 3)))
(flet ((graph-bucket (bucket)
(typecase quadtree
(quadtree (quadtree-to-graph (slot-value quadtree bucket)
quadtree)))))
(when quadtree
(with-slots (center) quadtree
(with-slots ((parent-center center)) parent
(append (when parent
(list (list parent-center
(typecase quadtree
(quadtree center)
(vector quadtree)))))
(graph-bucket 'nw-bucket-bg)
(graph-bucket 'ne-bucket-bg)
(graph-bucket 'sw-bucket-bg)
(graph-bucket 'se-bucket-bg)
(graph-bucket 'nw-bucket)
(graph-bucket 'ne-bucket)
(graph-bucket 'sw-bucket)
(graph-bucket 'se-bucket)))))))
(defun draw-line (buffer p1 p2)
(with-accessors ((x1 point-x) (y1 point-y)) p1
(let ((x1 (+ 64 x1))
(y1 (+ 64 y1)))
(with-accessors ((x2 point-x) (y2 point-y)) p2
(let ((x2 (+ 64 x2))
(y2 (+ 64 y2)))
(let* ((dist-x (abs (- x1 x2)))
(dist-y (abs (- y1 y2)))
(steep (> dist-y dist-x)))
(when steep
(psetf x1 y1 y1 x1
x2 y2 y2 x2))
(when (> x1 x2)
(psetf x1 x2 x2 x1
y1 y2 y2 y1))
(let* ((delta-x (- x2 x1))
(delta-y (abs (- y1 y2)))
(error (floor delta-x 2))
(y-step (if (< y1 y2) 1 -1))
(y y1))
(loop
:for x :upfrom x1 :to x2
:do (if steep
(setf (aref buffer x y) 1)
(setf (aref buffer y x) 1))
(setf error (- error delta-y))
(when (< error 0)
(incf y y-step)
(incf error delta-x))))
buffer))))))
(defun one-layer-of-octree (quadtree)
(flet ((bucket-as-point (b)
(etypecase b
(null nil)
(vector b)
(quadtree (center b)))))
(with-slots (nw-bucket nw-bucket-bg ne-bucket ne-bucket-bg se-bucket se-bucket-bg sw-bucket sw-bucket-bg) quadtree
(list
(mapcar #'bucket-as-point (list quadtree
nw-bucket nw-bucket-bg
ne-bucket ne-bucket-bg
se-bucket se-bucket-bg
sw-bucket sw-bucket-bg))))))
(defun octree-to-list (octree)
(typecase octree
(vector (list (list octree nil nil nil nil nil nil nil nil)))
(quadtree (with-slots (nw-bucket nw-bucket-bg ne-bucket ne-bucket-bg se-bucket se-bucket-bg sw-bucket sw-bucket-bg) octree
(append (one-layer-of-octree octree)
(when nw-bucket (octree-to-list nw-bucket))
(when nw-bucket-bg (octree-to-list nw-bucket-bg))
(when ne-bucket (octree-to-list ne-bucket))
(when ne-bucket-bg (octree-to-list ne-bucket-bg))
(when se-bucket (octree-to-list se-bucket))
(when se-bucket-bg (octree-to-list se-bucket-bg))
(when sw-bucket (octree-to-list sw-bucket))
(when sw-bucket-bg (octree-to-list sw-bucket-bg)))))))
(defun main ()
(declare (optimize (debug 3)))
(labels ((50- (x) (- x 50))
(random-point (&optional (limit 100))
(make-point :x (50- (random limit))
:y (50- (random limit))
:z 0 #|(50- (random limit))|#)))
(let ((quadtree (make-instance 'quadtree :center #(0 0 0)))
(points (loop repeat 100 collect (random-point))))
(map nil (lambda (point) (insert-in-quadtree quadtree point))
points)
(values quadtree points))))