Уже правильно считается путь по А*. Надо только непрямолинейную отрисовку добавить и все будет чики-пики