Nav graph edits

Nav graph now uses BFS for filling out data points.

Figured out why I was getting those stray debug lines leading to
(0,0,0). The first index of NavigationGraph array wasn't getting set and
defaulted to 0. Kind of makes me wonder why this works at all. Fix this
in a later version.