Also ich würde hier auf den A* Algorithmus zurückgreifen,
der ist relativ einfach und funktioniert sehr gut.