(defpackage #:3dr.octcone3
(:use #:cl #:alexandria #:fw.lu))
(in-package #:3dr.octcone3)
(defun make-adj-arr (&optional (size 25))
(make-array size :adjustable t :fill-pointer 0))
(defstruct (point (:type vector)) x y z)
(defclass bintree ()
((left :initarg :left :accessor left :initform nil)
(right :initarg :right :accessor right :initform nil)
(duplicates :initarg :duplicates :accessor duplicates :initform (make-adj-arr))
(center :initarg :center :accessor center)))
(defclass quadtree ()
((nw :initarg :nw :accessor nw :initform nil)
(ne :initarg :ne :accessor ne :initform nil)
(se :initarg :se :accessor se :initform nil)
(sw :initarg :sw :accessor sw :initform nil)
(duplicates :initarg :duplicates :accessor duplicates :initform (make-adj-arr))
(center :initarg :center :accessor center)))
(defgeneric insert-in-tree (bintree child))
(defmethod insert-in-tree ((bintree bintree) (child real))
(with-slots (left right duplicates center) bintree
(if (= child center)
(vector-push-extend center duplicates)
(if (< child center)
(etypecase left
(bintree (insert-in-tree left child))
(real (setf left (make-instance 'bintree :center left))
(insert-in-tree left child))
(null (setf left child)))
(etypecase right
(bintree (insert-in-tree right child))
(real (setf right (make-instance 'bintree :center right))
(insert-in-tree right child))
(null (setf right child)))))))
(defmethod insert-in-tree ((quadtree quadtree) (child vector))
(labels ((vector-value (child)
(etypecase child
(quadtree (center child))
(vector child)))
(dist-to-child (child)
(c-distance (center quadtree) (vector-value child)))
(replace-bucket (name)
(etypecase (slot-value quadtree name)
(quadtree (insert-in-tree (slot-value quadtree name) child))
(vector (let ((old-bucket (slot-value quadtree name)))
(if (<= (dist-to-child child) (dist-to-child old-bucket))
(let ((new-bucket (make-instance 'quadtree :center child)))
(setf (slot-value quadtree name) new-bucket)
(with-accessors ((old-bucket-x point-x) (old-bucket-y point-y)) (vector-value old-bucket)
(with-accessors ((new-bucket-x point-x) (new-bucket-y point-y)) (vector-value new-bucket)
(if (< old-bucket-x new-bucket-x)
(if (>= old-bucket-y new-bucket-y)
(setf (nw new-bucket) old-bucket)
(setf (sw new-bucket) old-bucket))
(if (>= old-bucket-y new-bucket-y)
(setf (ne new-bucket) old-bucket)
(setf (se new-bucket) old-bucket))))))
(setf (slot-value quadtree name)
(make-instance 'quadtree :center old-bucket))))
(insert-in-tree (slot-value quadtree name) child))
(null (setf (slot-value quadtree name) child)))))
(with-slots (nw ne se sw duplicates center) quadtree
(if (equalp child center)
(vector-push-extend center duplicates)
(with-accessors ((center-x point-x) (center-y point-y)) center
(with-accessors ((child-x point-x) (child-y point-y)) child
(if (< child-x center-x)
(if (>= child-y center-y)
(replace-bucket 'nw)
(replace-bucket 'sw))
(if (>= child-y center-y)
(replace-bucket 'ne)
(replace-bucket 'se)))))))))
(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 one-layer-of-bintree (bintree)
(flet ((bintree-as-point (b)
(etypecase b
(null nil)
(real b)
(bintree (center b)))))
(with-slots (left right) bintree
(list (mapcar #'bintree-as-point (list bintree left right))))))
(defun one-layer-of-quadtree (quadtree)
(flet ((quadtree-as-point (b)
(etypecase b
(null nil)
(vector b)
(quadtree (center b)))))
(with-slots (nw ne se sw) quadtree
(list (mapcar #'quadtree-as-point (list quadtree nw ne se sw))))))
(defun bintree-to-list (bintree)
(typecase bintree
(vector (list (list bintree nil nil)))
(bintree (with-slots (left right) bintree
(append (one-layer-of-bintree bintree)
(when left (bintree-to-list left))
(when right (bintree-to-list right)))))))
(defun quadtree-to-list (quadtree)
(typecase quadtree
(real (list (list quadtree nil nil)))
(quadtree (with-slots (nw ne se sw) quadtree
(append (one-layer-of-quadtree quadtree)
(when nw (quadtree-to-list nw))
(when ne (quadtree-to-list ne))
(when se (quadtree-to-list se))
(when sw (quadtree-to-list sw)))))))
(defun bintreelist-to-graph (bintreelist)
(loop for list in bintreelist
append (loop with head = (car list)
for tail in (cdr list)
when tail collect (list head tail))))
(defmethod print-object ((object quadtree) s)
(declare (optimize (debug 3)))
(print-unreadable-object (object s :type t :identity t)
(with-slots (center nw ne se sw duplicates) object
(format s "~s x~d: ~@{~a~^ ~}"
center
(length duplicates)
(not (null nw))
(not (null ne))
(not (null sw))
(not (null se))))))
(defun draw-line (buffer p1 p2)
(with-accessors ((x1 point-x) (y1 point-y)) p1
(with-accessors ((x2 point-x) (y2 point-y)) p2
(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
(incf (aref buffer x y) 1)
(incf (aref buffer y x) 1))
(setf error (- error delta-y))
(when (< error 0)
(incf y y-step)
(incf error delta-x))))
buffer))))
(defun c-distance (p1 p2)
(declare (optimize (debug 3))
(inline c-distance))
(sqrt (loop for c1 across p1
for c2 across p2
sum (expt (- c2 c1) 2))))
(defun main ()
(let ((res (make-instance 'quadtree :center #(50 50))))
(loop repeat 400
do (insert-in-tree res (vector (random 100) (random 100)))
finally (return-from main res))))
|