Visibility graph generation #27
Description
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.