Fixed optimal path bug in path planning.

This new bug regarded not adding the goal node to the visibility graph
when computing the optimal path. More minor bugs were encountered along
the way mainly relating to incorrect path construction in corner cases
(single point path, etc.).