Вычесление пути от точки А до точки В

[Anton911]

Всем привет, при создании игры столкнулся с такой проблемой (Внимание на рисунок), есть некое колличество точек с координатами (x,y), нужно найти ближайший путь от точки А до точки В. На рисунке Черным цветом отмечены непроходимые точки. Линиями обозначены возможные пути.
На картинке есть четыре варианта пути, три из них я отметил цифрами: зеленого, синего и красного цвета. Синие и красные пути имеют 6 шагов, зеленый путь имеет 7 шагов (Он больше чем синий и красный).
В общем моя задача написать функцию, которая находит кротчайший путь от точки А до точки В, и вернуть первую точку после точки А, которая ведет к точке В.
Плиз Хелп.
(На рисунке вместо 3 написал 4 зеленую, касяк, не ругаться!)

[Utkin]

Алгоритм Дейкстры есть в Вики. Вот тут http://www.algolib.narod.ru/Graph/Path.html немного. И вообще в гугле полно инфы по запросу алгоритмы поиска пути...

[_-Re@l-_]

Ну как бы это просто граф, не так ли? Есть куча алгоритмов, алгоритм Дейкстры, алгоритм Беллмана-Форда, алгоритм Флойда-Уоршелла, алгоритм Джонсона, ...
Алгоритмы

[Anton911]

Всем спасибо, оказывается я был близок к написанию подобного алгаритма, но чтото сошел с дистанции. Теперь разобрался)