Point robot moving among polygonal obstacles in 2D: motion planning via the visibility graph

One of the fundamental problems in robotics is motion planning: given a robot R and an environment (or physical space), a start position p_start and an end position p_end, find a path so that the robot can move from start to end, without collisisons. Sometimes we might want the shortest path from p_start to p_end; other times just a path (not necessarily the shortest) may be sufficient.

General motion planning is difficult. We'll make the following (simplifying) assumptions:

2D: We'll assume that the environment consists of a set of 2D polygons that represent the obstacles. Furthermore, we assume that the robot is moving in a large bounding box B that contains all polygons.
Static: We'll assume that the environment is static (the obstacles are fixed).
Known: The environment is known, i.e. the robot knows the list of obstacles with their vertices and edges. That is, the robot does not rely on sensors to learn the environment, instead the environment is given.

We'll assume that the obstacles consist of n edges. So O(n) represents the complexiy of the obstacles (vertices plus edges).

Motion planning with a roadmap

The general idea of motion planning is to start by computing a decomposition of the free space (the space where the robot can move) into cells, and construct a road map of the free space to guide the motion between neighboring cells. The road map is essentially a graph of free space. Ideally we want to build the roadmap so that so that:

Any path in the road map corresponds to a collision-free path in the free space.
Any path in the free-space corresponds to a path in the road map.

If we are able to compute a road map of free space with these properties, then to move from s to t, we find a path in the road map from s to t. We know that any path in the road map is colision-free (by (1)), and we know that if a path in the road map does not exist, then no path exists (by (2)). Essentially we have reduced the motion planning problem to a path problem in a graph.

Different types of motion planning problems will require different types of roadmaps.

The simplest problem is a point robot moving among polygonal obstacles in 2D. Two types of roadmaps are used:

Trapezoidal decomposition: The free space can be decomposed as follows: from each vertex, shoot a vertical ray above and below, until it intersects with an edge (or the bounding box B). This is called a trapezoidal decomposition, and we have seen something very similar when we talked about polygon triangulation; in this case we have a set of polygons, not just one, but the idea is the same. This trapezoidal decomposition can be computed using standard techniques. Each trapezoid corresponds to a region of the free space. To guide movement between adjacent trapezoids, we can construct a road map as follows: we place one node in the center of each trapezoid, and we place one node in the middle of each vertical edge. There is an arc between two nodes if and only iff one node is in the center of a trapezoid and the other node is on the boundary of that same trapezoid. The figure below illustrates this (from 4M book). A path from start to end can be found via BFS in the road map. It can be shown that the road map has linear complexity O(n), which is good. But, paths obtained with this approach are not necessarily shortest. In summary, the roadmap idea is a useful thing to remember in general, but in the simple case of a point robot moving in 2D, we can do better.
The visibility graph: A different type of road map that can be used for finding shortest paths is the visibility graph: this is a graph whose vertices are the vertices of the obstacles, and its edges (u,v) are all the pair of vertices that can "see" each other, that is, segment uv does not intersect the interior of any obstacle. Below are some screenshots (the first screenshot is from http://www.cs.cmu.edu/afs/cs.cmu.edu/academic/class/16311/www/s06/lecture/lec8.html).

Shortest paths in 2D have the very nice and convenient property that they are straight lines, and they have to go through the vertices of the obstacles. This basically means that any shortest path will be contained in the VG. Once the visibility graph (VG) is computed, the shortest paths from start to end can be computed for e.g. using Dijkstra's algorithm.
Perspective: The problem with the VG approach is that the VG may have quadratic complexity in the worst case, and any approach that computes the whole VG is doomed to quadratic complexity. It would be nice to compute the shortest path from s to t on the fly, without using at least quadratic time and space to compute and store the VG.This was an open problem for a while; the quadratic barier was first broken by Joe Mitchell, and subsequently Hershberger and Suri (1993) described an optimal (n lg n) algorithm called the continuous Dijkstra algorithm.

This assignment

Generate a scene consisting of multiple polygonal obstacles and a start and end position. Have a pre-set scene when you start, but also allow the user to change the start and end position for the same scene, and to reset the whole scene and start from scratch entering polygons.
Compute and render the visibility graph.
Run Dijkstra's algorithm on the VG and render the resulting path (for e.g. in a different color and different line width).

Extra features

Implement a point-in-polygon method and use it to detect if start or end points are inside an obstacle.
Detect if a polygon is self-intersecting.
Detect if polygons are overlapping.
Draw the polygons filled. Note that openGL can only fill correctly convex polygons, so if you want it filled, you'll need to compute a triangulation.

What to turn in

Check in your code on GitHub.
Provide a README that describes the state of your code (how to run it, does it work on all test cases, do you know of any bugs, did you implement any extra features, etc). Basically what you want people to know about your code.
Fill in the self-reflection report and get it to me (either hard copy in class or DM on slack).

Some notes on grading

+50 points: find al edges in VG, no more, no less (no missing edges, no extraneous edges)
+20 points: compute shortest path and display it
+10: the user is able to click and select new start and end positions

Code Style: 20 points

+0-10 points: poor to medium code.
+10-20 points: medium to excellent code.