Added option to output paths, added boost graph pathing