priority-queue.lisp 5.09 KB
Newer Older
1 2 3 4 5 6
(in-package :d2clone-kit)

(defstruct (priority-queue
            (:constructor %make-priority-queue)
            (:copier nil)
            (:predicate nil))
Andrew Kravchuk's avatar
Andrew Kravchuk committed
7
  "A simple priority queue with DOUBLE-FLOAT priorities."
8
  (array nil :type simple-vector)
9
  (key nil :type (function (t) double-float) :read-only t))
10 11

(defun make-priority-queue (key-fn)
12 13 14
  "Creates priority queue using key extraction function KEY-FN.

Note: keys are expected to be DOUBLE-FLOATs."
15 16 17 18
  (%make-priority-queue :array (make-array 0) :key key-fn))

(declaim
 (inline binary-search)
19
 (ftype (function ((function (t) double-float) double-float simple-vector) array-index)
20 21 22 23 24 25 26 27 28 29 30 31 32
        binary-search))
(defun binary-search (key-fn key array)
  (if (zerop (length array))
      0
      (flet
          ((mid (first last)
             (declare (fixnum first last))
             (the array-index (+ first (truncate (- last first) 2)))))
        (declare (inline mid))
        (do* ((l 0)
              (u (1- (length array)))
              (m (mid l u) (mid l u)))
             ((> l u) l)
33
          (if (> (funcall key-fn (aref array m)) key)
34 35 36
              (setf l (1+ m))
              (setf u (1- m)))))))

37 38 39 40
(declaim
 (inline priority-queue-find)
 (ftype (function (priority-queue t)) priority-queue-find))
(defun priority-queue-find (queue element)
Andrew Kravchuk's avatar
Andrew Kravchuk committed
41 42
  "Finds ELEMENT's position in QUEUE. Returns NIL if there's no such element.
O(log N) complexity."
43 44 45 46 47 48
  (let* ((array (priority-queue-array queue))
         (key-fn (priority-queue-key queue))
         (position (binary-search key-fn (funcall key-fn element) array)))
    (when (and (< position (length array)) (equal element (aref array position)))
      position)))

49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72
(declaim
 (inline priority-queue-push)
 (ftype (function (priority-queue t)) priority-queue-push))
(defun priority-queue-push (queue element)
  "Adds element ELEMENT to priority queue QUEUE."
  (let* ((array (priority-queue-array queue))
         (key-fn (priority-queue-key queue))
         (position (binary-search key-fn (funcall key-fn element) array)))
    (setf (priority-queue-array queue)
          (adjust-array array (1+ (length array))))
    (let ((array (priority-queue-array queue)))
        (replace
         array
         array
         :start1 (1+ position)
         :end1 (length array)
         :start2 position)
      (setf (aref array position) element)))
  nil)

(declaim
 (inline priority-queue-push-many)
 (ftype (function (priority-queue vector)) priority-queue-push-many))
(defun priority-queue-push-many (queue elements)
Andrew Kravchuk's avatar
Andrew Kravchuk committed
73 74 75 76
  "Adds elements from vector ELEMENTS to priority queue QUEUE.

A bit more performance-friendly than calling PRIORITY-QUEUE-PUSH many times
(but complexity is still O(N log N))."
77 78 79 80 81 82 83 84 85 86 87 88 89
  (let* ((array (priority-queue-array queue))
         (old-length (length array)))
    (setf (priority-queue-array queue)
          (adjust-array array (+ old-length (length elements))))
    (let ((array (priority-queue-array queue)))
      (replace
       array
       elements
       :start1 old-length)
      ;; TODO : try optimizing assuming elements vector is sorted
      (sort
       array
       #'(lambda (a b)
90 91
           (declare (double-float a b))
           (> a b))
92 93 94 95 96 97 98 99 100
       :key (priority-queue-key queue))))
  nil)

(declaim
 (inline priority-queue-traverse)
 (ftype (function (priority-queue (function (t)))) priority-queue-traverse))
(defun priority-queue-traverse (queue fn)
  "Calls one argument function FN on elements of priority queue QUEUE
in appropriate order."
101 102
  (let ((array (priority-queue-array queue)))
    (loop
Andrew Kravchuk's avatar
Andrew Kravchuk committed
103 104 105
      :for i :from (1- (length array)) :downto 0
      :for element := (aref array i)
      :do (funcall fn element))))
106

107 108 109 110 111 112
(declaim
 (inline simple-vector-peek)
 (ftype (function (simple-vector) t) simple-vector-peek))
(defun simple-vector-peek (array)
  (aref array (1- (length array))))

113 114 115 116 117 118 119 120 121 122 123 124 125
(declaim
 (inline simple-vector-pop)
 (ftype (function (simple-vector) (values t simple-vector)) simple-vector-pop))
(defun simple-vector-pop (array)
  (let* ((last-index (1- (length array)))
         (element (aref array last-index))
         (new-array (adjust-array array last-index)))
    (values element new-array)))

(declaim
 (inline priority-queue-pop)
 (ftype (function (priority-queue)) priority-queue-pop))
(defun priority-queue-pop (queue)
Andrew Kravchuk's avatar
Andrew Kravchuk committed
126
  "Removes and returns the first (priority-wise) element in QUEUE."
127 128 129 130 131 132 133 134 135
  (multiple-value-bind (element new-array)
      (simple-vector-pop (priority-queue-array queue))
    (setf (priority-queue-array queue) new-array)
    element))

(declaim
 (inline priority-queue-remove)
 (ftype (function (priority-queue array-index)) priority-queue-remove))
(defun priority-queue-remove (queue index)
Andrew Kravchuk's avatar
Andrew Kravchuk committed
136
  "Removes element from QUEUE denoted by INDEX."
137 138 139 140 141
  (let ((array (priority-queue-array queue)))
    (replace (priority-queue-array queue) array :start1 index :start2 (1+ index))
    (setf (priority-queue-array queue)
          (adjust-array (priority-queue-array queue) (1- (length array)))))
  nil)
142 143 144 145 146 147 148 149 150

(declaim
 (inline priority-queue-clear)
 (ftype (function (priority-queue)) priority-queue-clear))
(defun priority-queue-clear (queue)
  "Clears priority queue QUEUE."
  (setf (priority-queue-array queue)
        (make-array 0))
  nil)