replicated all code from http://code.google.com/p/jianwikis/wiki/AStarAlgorithmForPathPlanning