share/graphs/dijkstra.lisp

   1 ;;;
   2 ;;;  GRAPHS - graph theory package for Maxima
   3 ;;;
   4 ;;;  Copyright (C) 2008 Andrej Vodopivec <andrej.vodopivec@gmail.com>
   5 ;;;
   6 ;;;  This program is free software; you can redistribute it and/or modify
   7 ;;;  it under the terms of the GNU General Public License as published by
   8 ;;;  the Free Software Foundation; either version 2 of the License, or
   9 ;;;  (at your option) any later version.
  10 ;;;
  11 ;;;  This program is distributed in the hope that it will be useful,
  12 ;;;  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 ;;;  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 ;;;  GNU General Public License for more details.
  15 ;;;
  16 ;;;  You should have received a copy of the GNU General Public License
  17 ;;;  along with this program; if not, write to the Free Software
  18 ;;;  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
  19 ;;;
  20
  21 ;;;;;;;;;;;;;;;;;;;;;;;;
  22 ;;;
  23 ;;; Implementation of a binary heap priority queue
  24 ;;;
  25
  26 (in-package :maxima)
  27
  28 (defstruct graphs-pqueue
  29   (data (make-array 129 :adjustable t))
  30   (last 0)  ;; last index used
  31   (max 128) ;; max possible index
  32   (index (make-hash-table))
  33   (weights (make-hash-table)))
  34
  35 (defun graphs-pqueue-insert (v w queue)
  36   (if (= (graphs-pqueue-last queue) 0)
  37       (progn
  38         ;; insert into an empty queue
  39         (setf (graphs-pqueue-last queue) 1)
  40         (setf (aref (graphs-pqueue-data queue) 1) v)
  41         (setf (gethash v (graphs-pqueue-index queue)) 1)
  42         (setf (gethash v (graphs-pqueue-weights queue)) w))
  43       (progn
  44         ;; resize the queue if needed
  45         (when (= (graphs-pqueue-max queue) (graphs-pqueue-last queue))
  46           (let ((new-array (adjust-array (graphs-pqueue-data queue)
  47                                                                          (1+ (* (graphs-pqueue-max queue) 2)))))
  48                 (setf (graphs-pqueue-data queue) new-array))
  49           (setf (graphs-pqueue-max queue)
  50                 (* (graphs-pqueue-max queue) 2)))
  51         ;; insert the element
  52         (incf (graphs-pqueue-last queue))
  53         (setf (aref (graphs-pqueue-data queue) (graphs-pqueue-last queue)) v)
  54         (setf (gethash v (graphs-pqueue-index queue)) (graphs-pqueue-last queue))
  55         (setf (gethash v (graphs-pqueue-weights queue)) w)
  56         ;; balance the queue
  57         (let* ((ind (graphs-pqueue-last queue))
  58                (ind-new (truncate ind 2)))
  59           (loop while (and (> ind 1)
  60                            (mlsp w (gethash (aref (graphs-pqueue-data queue) ind-new)
  61                                             (graphs-pqueue-weights queue))))
  62              do
  63                (rotatef (aref (graphs-pqueue-data queue) ind)
  64                         (aref (graphs-pqueue-data queue) ind-new))
  65                (rotatef (gethash (aref (graphs-pqueue-data queue) ind) (graphs-pqueue-index queue))
  66                         (gethash (aref (graphs-pqueue-data queue) ind-new) (graphs-pqueue-index queue)))
  67                (setq ind ind-new)
  68                (setq ind-new (truncate ind 2)))))))
  69
  70 (defun graphs-pqueue-emptyp (queue)
  71   (= (graphs-pqueue-last queue) 0))
  72
  73 (defun graphs-pqueue-first (queue)
  74   (aref (graphs-pqueue-data queue) 1))
  75
  76 (defun graphs-pqueue-get-weight (v queue)
  77   (gethash v (graphs-pqueue-weights queue)))
  78
  79 (defun graphs-pqueue-pop (queue)
  80   (when (graphs-pqueue-emptyp queue)
  81     (return-from graphs-pqueue-pop nil))
  82   (let ((top (graphs-pqueue-first queue))
  83         (last (aref (graphs-pqueue-data queue)
  84                     (graphs-pqueue-last queue)))
  85         (pos 1)
  86         min-child)
  87     ;; remove the top element
  88     (remhash top (graphs-pqueue-weights queue))
  89     (remhash top (graphs-pqueue-index queue))
  90     (decf (graphs-pqueue-last queue))
  91     ;; put the last element into the first position
  92     (setf (aref (graphs-pqueue-data queue) 1) last)
  93     (setf (gethash last (graphs-pqueue-index queue)) 1)
  94     ;; rebalance the queue
  95     (loop while t do
  96          ;; find the min child
  97          (setf min-child (* 2 pos))
  98          (when (> min-child (graphs-pqueue-last queue))
  99            (return-from graphs-pqueue-pop top))
 100          (when (and (<= (1+ min-child) (graphs-pqueue-last queue))
 101                     (mlsp (gethash (aref (graphs-pqueue-data queue) (1+ min-child))
 102                                    (graphs-pqueue-weights queue))
 103                           (gethash (aref (graphs-pqueue-data queue) min-child)
 104                                    (graphs-pqueue-weights queue))))
 105            (setq min-child (1+ min-child)))
 106          ;; rotate if needed
 107          (when (mlsp (gethash (aref (graphs-pqueue-data queue) pos)
 108                               (graphs-pqueue-weights queue))
 109                      (gethash (aref (graphs-pqueue-data queue) min-child)
 110                               (graphs-pqueue-weights queue)))
 111            (return-from graphs-pqueue-pop top))
 112          (rotatef (aref (graphs-pqueue-data queue) pos)
 113                   (aref (graphs-pqueue-data queue) min-child))
 114          (rotatef (gethash (aref (graphs-pqueue-data queue) pos) (graphs-pqueue-index queue))
 115                   (gethash (aref (graphs-pqueue-data queue) min-child) (graphs-pqueue-index queue)))
 116          (setq pos min-child))))
 117
 118 (defun graphs-pqueue-contains (v queue)
 119   (not (eq (gethash v (graphs-pqueue-weights queue) 'not-contained)
 120            'not-contained)))
 121
 122 (defun graphs-pqueue-set-weight (v w queue)
 123   (setf (gethash v (graphs-pqueue-weights queue)) w)
 124   (let ((pos (gethash v (graphs-pqueue-index queue))))
 125     ;; rebalance the queue
 126     (let* ((ind pos)
 127            (ind-new (truncate ind 2)))
 128       (loop while (and (> ind 1)
 129                        (mlsp w (gethash (aref (graphs-pqueue-data queue) ind-new)
 130                                         (graphs-pqueue-weights queue))))
 131          do
 132            (rotatef (aref (graphs-pqueue-data queue) ind)
 133                     (aref (graphs-pqueue-data queue) ind-new))
 134            (rotatef (gethash (aref (graphs-pqueue-data queue) ind) (graphs-pqueue-index queue))
 135                     (gethash (aref (graphs-pqueue-data queue) ind-new) (graphs-pqueue-index queue)))
 136            (setq ind ind-new)
 137            (setq ind-new (truncate ind 2))))))
 138
 139 ;;;;;;;;;;;;;;;;;;;;;;;;
 140 ;;;
 141 ;;; Dijkstra's algorithm for weighted shortest paths
 142 ;;;
 143
 144 (defun dijkstra (v v1 g)
 145   (let ((previous (make-hash-table))
 146         (distance (make-hash-table))
 147         (cont t)
 148         (my-inf 1)
 149         (pq (make-graphs-pqueue)))
 150
 151     (dolist (e (cdr ($edges g)))
 152       (setq my-inf (m+ my-inf ($abs ($get_edge_weight e g 1)))))
 153
 154     ;; initialize the pqueue
 155     (dolist (u (vertices g))
 156       (if (= u v)
 157           (progn
 158             (graphs-pqueue-insert u 0 pq)
 159             (setf (gethash u distance) 0))
 160           (progn
 161             (graphs-pqueue-insert u my-inf pq)
 162             (setf (gethash u distance) my-inf))))
 163     ;; find the shortest path
 164     (loop while (and cont (not (graphs-pqueue-emptyp pq))) do
 165          ;; find the closest remaining vertex
 166          (let* ((u (graphs-pqueue-pop pq))
 167                 (u-distance (gethash u distance)))
 168            (if (or (eq (gethash u distance) '$inf)
 169                    (= u v1))
 170                (setq cont nil)
 171                (progn
 172                  (dolist (w (if (graph-p g) (neighbors u g) (out-neighbors u g)))
 173                    (when (graphs-pqueue-contains w pq)
 174                      (let ((alt (m+ u-distance ($get_edge_weight `((mlist simp) ,u ,w) g))))
 175                        (when (mlsp alt (graphs-pqueue-get-weight w pq))
 176                          (setf (gethash w previous) u)
 177                          (setf (gethash w distance) alt)
 178                          (graphs-pqueue-set-weight w alt pq)))))))))
 179
 180     (dolist (v (vertices g))
 181       (when (equal (gethash v distance) my-inf)
 182         (setf (gethash v distance) '$inf)))
 183
 184     (values distance previous)))
 185
 186 (defmfun $shortest_weighted_path (v u g)
 187   (require-graph-or-digraph 'shortest_weighted_path 3 g)
 188   (require-vertex 'shortest_weighted_path 1 v)
 189   (require-vertex-in-graph 'shortest_weighted_path v g)
 190   (require-vertex 'shortest_weighted_path 2 u)
 191   (require-vertex-in-graph 'shortest_weighted_path u g)
 192   (multiple-value-bind (l prev)
 193       (dijkstra v u g)
 194     (setq l (gethash u l))
 195     (if (eq l '$inf)
 196         '((mlist simp) $inf ((mlist simp)))
 197         (let ((tu u) (p (list u)))
 198           (loop while (not (= tu v)) do
 199                (setq tu (gethash tu prev))
 200                (setq p (cons tu p)))
 201           `((mlist simp) ,l ((mlist simp) ,@p))))))