Алгоритм AStar(A*) для поиска пути
Загрузить пример для Visual Stduio 2019
ЗДЕСЬ
.