В общем сделал поиск пути по Дейкстре - мне он показался проще для понимания.
Если кому нужен код - он
ЗДЕСЬ
Написан на C++11, если кто будет компилировать\запускать, то результат смотреть через отладчик - ибо мне было лень делать вывод в консольку. В скором времени запилю его в игру.