Unfortunately it's not really self-contained enough to post; the most complicated thing in it is basically a copy/paste of the "Dijkstra's Algorithm" sample-code on Wikipedia, though.