Route solver

nodes.dat

 27 273  1 3 6
 21 128  0 2
125  23  1 9
117 221  0 4 5 6
141 178  3 7
167 178  3 7
195 178  0 3 7
192 110  4 5 6 8
193  91  7 9
151  41  2 8

route_solver.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys, os, math, heapq, Image, ImageDraw

class Node:
    def __init__(self):
        self.pos = (0, 0)
        self.neighbors = []
        self.dist = float("inf")
        self.path = []

# A* Search Algorithm
class AStar:
    def __init__(self, start, goal):
        self.start, self.goal = start, goal
        self.start.dist = 0.0
        self.start.path = [self.start]

    def distance(self, node1, node2):
        return math.sqrt((node1.pos[0] - node2.pos[0])**2 + (node1.pos[1] - node2.pos[1])**2)

    def heuristic(self, node):
        return self.distance(node, self.goal)

    def search(self):
        queue = []
        heapq.heappush(queue, (self.heuristic(self.start), self.start))
        while queue:
            score, last = heapq.heappop(queue)
            if last == self.goal: return last.path, last.dist
            for nb in last.neighbors:
                newdist = last.dist + self.distance(last, nb)
                if nb.dist < newdist: continue
                nb.dist = newdist
                nb.path = last.path + [nb]
                heapq.heappush(queue, (nb.dist + self.heuristic(nb), nb))
        return [], 0.0

def draw_path(in_image, out_image, path):
    im = Image.open(in_image)
    im = im.convert("RGB")
    draw = ImageDraw.Draw(im)
    draw.line([(int(p.pos[0]), int(p.pos[1])) for p in path], fill=255, width=2)
    im.save(out_image)

def main(args):
    if len(args) < 4:
        print >>sys.stderr, "Usage: %s datafile in_image out_image" % os.path.basename(args[0])
        sys.exit(1)

    datafile, in_image, out_image = args[1:4];

    print "Reading nodes data..."
    data = [map(int, l.strip().split()) for l in file(datafile).readlines()]
    nodes = [Node() for i in range(len(data))]
    for i, node in enumerate(nodes):
        node.pos = tuple(data[i][:2])
        for j in data[i][2:]:
            node.neighbors.append(nodes[j])
    print " Number of nodes: %d" % len(nodes)

    print "A* searching phase..."
    start, goal = nodes[0], nodes[-1]
    print " Start: (%d, %d), Goal: (%d, %d)" % (start.pos + goal.pos)
    astar = AStar(start, goal)
    path, dist = astar.search()
    if path:
        print " Found route:"
        for p in path: print "  (%d, %d)" % p.pos
        print " Distance: %f" % dist
    else:
        print " No route."

    print "Drawing path on the imagefile..."
    draw_path(in_image, out_image, path)

if __name__ == "__main__": main(sys.argv)

http://handasse.blogspot.com/2011/10/blog-post.html