90 lines
4.5 KiB
Common Lisp
90 lines
4.5 KiB
Common Lisp
(defun main ()
|
|
(let ((dims 0) (target nil) (hits 0))
|
|
|
|
;;; distance node to target:
|
|
;;; returns squared euclidean distance, or squared semi distance if option set
|
|
(defun distance (n &optional (semi nil))
|
|
(if semi (expt (- (nth (first n) (second n)) (nth (first n) target)) 2)
|
|
(reduce #'+ (mapcar (lambda (x y) (* (- x y) (- x y))) (second n) target))))
|
|
|
|
;;; returns true if target is to its left in axis dim
|
|
(defun target< (n)
|
|
(< (nth (first n) target) (nth (first n) (second n))))
|
|
|
|
;;; return the next child when nn searching, return opposing child if option oppose set
|
|
(defun next-node (n &optional (oppose nil))
|
|
(if (or (and (target< n) (not oppose)) (and (not (target< n)) oppose)) (third n) (fourth n)))
|
|
|
|
;;; a kdtree is a binary tree where nodes are:
|
|
;;; terminal: (axis data-point), or
|
|
;;; branch: (axis split-point (left-kdtree) (right-kdtree))
|
|
(defun make-kdtree(axis data)
|
|
(if (null data) nil
|
|
(if (eql (length data) 1) ; singleton?
|
|
(list axis (first data)) ;; terminal node
|
|
;; else branch node:
|
|
;; #pts=odd splits list into 2 even parts with sp in middle
|
|
;; #pts=even splits list into 2 uneven parts with shorter length first (but never nil)
|
|
(let ((sd (sort (copy-list data) #'< :key (lambda (x) (nth axis x)))) ;; sort the axis ordinates
|
|
(sp (truncate (/ (length data) 2))) ;; get mid pt
|
|
(nxta (mod (1+ axis) dims)))
|
|
(list axis (nth sp sd) (make-kdtree nxta (subseq sd 0 sp)) (make-kdtree nxta (subseq sd (1+ sp))))))))
|
|
|
|
;;; depth first visit all nodes in kdtree and optionally apply a function to each node visited
|
|
(defun visit-kdtree (kdt &key (node-function null))
|
|
(when kdt
|
|
(when node-function (funcall node-function kdt))
|
|
(visit-kdtree (third kdt) :node-function node-function)
|
|
(visit-kdtree (fourth kdt) :node-function node-function)))
|
|
|
|
;;; count of the terminal nodes
|
|
(defun count-nodes (kdt)
|
|
(if kdt
|
|
(if (eql (length kdt) 2) 1
|
|
(+ 1 (count-nodes (third kdt)) (count-nodes (fourth kdt))))
|
|
0))
|
|
|
|
;;; nearest neighbour search
|
|
(defun nn-kdtree (kdt node-stack)
|
|
(when kdt
|
|
;; stage 1 - find the 'closest' terminal node using insertion logic
|
|
(let ((best (do ((node kdt (next-node node))) ((not (next-node node)) (incf hits) node) ;; return first best est.
|
|
(push node node-stack) (incf hits)))) ; iteration
|
|
|
|
;; stage 2 - unwind the path, at each node if node is closer then make it best
|
|
(do ((node (pop node-stack) (pop node-stack))) ((null node) best) ;; return nearest pt
|
|
;; iteration: update best if node is closer
|
|
(when (< (distance node) (distance best))
|
|
(setf best node))
|
|
|
|
;; venture down opposing side if split point is inside HS
|
|
(let ((opposing-best
|
|
(if (< (distance node 'semi) (distance best)) ; use semi dist here
|
|
(nn-kdtree (next-node node 'opposite) (list))
|
|
nil))) ;; otherwise ignore this subtree
|
|
|
|
(when (and opposing-best (< (distance opposing-best) (distance best)))
|
|
(setf best opposing-best)))))))
|
|
|
|
;;; process one set of data & optionally display tree
|
|
(defun process (data tgt &optional (render nil))
|
|
(setf target tgt)
|
|
(setf dims (length target))
|
|
(setf hits 0)
|
|
(let* ((kdt (make-kdtree 0 data)) (nn (nn-kdtree kdt (list))))
|
|
(when render
|
|
(visit-kdtree kdt
|
|
:node-function (lambda (n)
|
|
(format t "~A node: axis:~A point: ~A target:~A semi-distance-sqd:~A euclidean-distance-sqd:~A~%"
|
|
(if (not (next-node n)) "TERMINAL" "BRANCH") (first n) (second n)
|
|
target (distance n 'semi) (distance n)))))
|
|
(format t "~%NN to ~A is ~A, distance ~A [tree has ~A nodes, ~A were visited.]~%" target (second nn) (sqrt (distance nn)) (count-nodes kdt) hits)))
|
|
|
|
;; MAIN: TASK 1 - nn search small set of 2D points
|
|
(process '((2 3) (5 4) (9 6) (4 7) (8 1) (7 2)) '(9 2) 'render)
|
|
|
|
;; TASK 2 - nn search 1000 coordinate points in 3D space
|
|
(process
|
|
(progn (let ((ll (list))) (dotimes (i 10) (dotimes (j 10) (dotimes (k 10) (push (list i j k) ll)))) ll))
|
|
(list (float (/ (random 1000) 100)) (float (/ (random 1000) 100)) (float (/ (random 1000) 100))))))
|