Fixed the "not-so-shortest-path" bug (thanks to Cédric Jules)

Got an email back from Cédric Jules about the issue. It was a simple typo in the AStar code.