[C++] Jak ulepszyć moją implementację algorytmu A*?

Jeżeli mój algorytm działałby jak nadmuchiwany balon, praktycznie cała mapa byłaby sprawdzona, a tak są małe kieszenie nie-sprawdzonego terenu w labiryncie (które na pewno sprawdziłby Breadth First Search), co oznacza że mój algorytm nie jest w tym zakresie upośledzony.

Jak widać w moim kodzie mam dwie metody obliczania A i B (dystansu od celu i od startu) - odległość euklidesowa i manhattańska (Taxicab Distance). Pierwsza oblicza dystans w linii prostej (lot ptaka nad miastem) natomiast druga po krawędziach pikseli (jazda po ulicach). Ostatecznie zdecydowałem się na pierwszą, bo w ten sposób ścieżka jest krótsza i program pracuje marginalnie szybciej, a specyfika mojej gry sprawia że dość częste poruszanie się postaci po ukośnej nie jest problemem.