経路探索は以前のスレでサンプル作ってくれた人がいたんで参考にしてみては？

今現在、本業のゲームプログラマさん的にも
リアルタイムで全探索する様なのは重すぎるから現実的ではなく
マップ内の通路にタグを打って、それに沿って移動していくのが一般的
みたいな話だたような