Как вариант: найди реализацию алгоритма волновой трассировки и допиши под себя.
Задача на графы же...