replicated all code from http://code.google.com/p/jianwikis/wiki/AStarAlgorithmForPathPlanning
Added my own code from scratch