Visibility graph generation

[stevenwaslander]

Speed up collision checking, generate large visibility graphs quickly, only check edges that could be a part of a shortest path through the environment. Test scalability and make code functionalized and efficient.