This is a uLisp program to find the shortest route between two places. This program illustrates an important problem that needs to be solved by every car navigation system, and could be used as the basis for a robot navigation program, to find the shortest route through a maze.
It is too large to run on an Arduino Uno, but runs on an Arduino Mega 2560.
Defining the map
The map program stores the map as a list of roads, where each road interconnects two locations. Here's the map we'll use to test the program:
For simplicity each location has a one-letter name, such as A. The numbers show the time, in minutes, to get along the road between each pair of locations, and our goal is to get from A to Z by the quickest route possible.
Before looking at the answer below see if you can find the shortest route by hand.
Entering the data
The map data will be stored in a global variable called md (for map data), which we define as:
(defvar md nil)
(from to time)
where from and to are the locations at each end of the path, and time is the time to get between the locations. For example, the first path on the above map is:
(a b 2)
Here's the procedure adr (add road) to add a path to the database:
(defun adr (frm to tim) (push (list frm to tim) md) (push (list to frm tim) md))
> (adr 'a 'b 2)
((b a 2) (a b 2))
Maintaining a queue
Now that we can enter the map, how is the program going to find the shortest route? The technique is called an "ink-blot" or "breadth-first" search. It is like pouring ink into the starting location, and watching it spread out uniformly along the network, colouring each location as it reaches it. The shortest route to the destination location will be the route taken by the river of ink to arrive first.
To do this we will keep a list of the paths the ink is flowing along, arranged so the location reached first gets processed next. An ordered list of this sort is called a priority queue. We will store the entries on the queue as:
(time location from)
where time is the total time taken from the starting point, location is the location reached by the ink blot, and from is the location we've come from.
Here's the procedure adi to add an item to the correct position in the queue, and return the new queue:
(defun adi (i q) (if (null q) (cons i q) (if (< (first i) (first (first q))) (cons i q) (cons (first q) (adi i (rest q))))))
For convenience we also have the following procedure adq (add-to-queue) that takes the priority-queue as the fourth parameter, and returns the updated queue:
(defun adq (tim loc via pq) (setq pq (adi (list tim loc via) pq)) pq)
For example, if we want to add the road
(3 e f)
to the queue:
((2 a b) (4 c d))
> (adq 3 'e 'f '((2 a b) (4 c d))) ((2 a b) (3 e f) (4 c d))
Processing a new location
When the ink reaches a new location we want to add all the paths extending from that location to the queue. This is done by the routine ars (add-roads), which adds all the paths leading from our current location to the queue, and returns the new queue:
(defun ars (loc go pq) (dolist (i md pq) (let* ((frm (first i)) (to (second i)) (tim (third i))) (when (eq frm loc) (setq pq (adq (+ go tim) to loc pq))))))
It works as follows:
For every entry in the map data:
- If the from item in the map data matches our current location, add the road to the queue, with the time added to our starting time.
Growing the search
We are now ready to write the main procedure gro (grow) for growing the ink from the starting location until we reach the destination. We will return a list of all the locations encountered as the ink spreads, and the location we came from:
(defun gro (frm to) (let* ((vis (list (cons frm nil))) (pq (ars frm 0 nil)) w) (loop (when (eq frm to) (return (reverse vis))) (unless pq (return)) (setq w (first pq)) (setq frm (second w)) (setq pq (cdr pq)) (unless (assoc frm vis) (setq vis (cons (cons frm (third w)) vis)) (setq pq (ars frm (car w) pq))))))
To see how gro works let's set up a very simple map:
Clear the map data:
> (setq md nil) nil
> (adr 'a 'b 2) ((b a 2) (a b 2))
Add the road between B and C:
> (adr 'b 'c 1) ((c b 1) (b c 1) (b a 2) (a b 2))
Then grow the ink blot until we reach the destination "C":
> (gro 'a 'c) ((a) (b . a) (c . b))
The procedure grow-search returns a list of the locations visited, and in each case the location we came from.
To complete the project we need to create the actual route from this list of locations visited.
Listing the route
Here is the procedure lis to call gro and list the route:
(defun lis (frm to) (let* ((vis (gro frm to)) rte) (when vis (loop (push to rte) (when (eq frm to) (return rte)) (setq to (cdr (assoc to vis)))))))
Let's try out the route map program to find the shortest route through the map at the beginning of this section. Here's a procedure mm (make map) to define the map:
(defun mm () (adr 'a 'b 2) (adr 'b 'c 3) (adr 'a 'd 9) (adr 'b 'e 3) (adr 'c 'f 7) (adr 'd 'e 3) (adr 'e 'f 6) (adr 'd 'g 2) (adr 'e 'h 8) (adr 'f 'z 6) (adr 'g 'h 2) (adr 'h 'z 4))
So we execute:
(setq md nil) (mm) (lis 'a 'z)
The answer comes back:
(a b e d g h z)
If you investigate you'll find that this is indeed the shortest route from A to Z.
Here's the whole route finder program: Route finder program.