Я бы порекомендовал JGraphT. Я использовал его для создания многоуровневых графиков в своей диссертации и в качестве основы программного обеспечения для GPS-маршрутизации, понимая, что происходит, я обнаружил, что что-то не так, но, посмотрев, как работает пакет алгоритмов, я обнаружил, что это довольно легко реализовать A * / D * эвристические алгоритмы. Для определения расстояния между узлами на графике я бы также порекомендовал посмотреть на функцию Хаверсайна, если это ваше дело.