Added tether crossing checks, obstacle expansion.

Obstacle expansion is achieved by expanding in the direction of the
normals of each edge and approximating arcs by a sequence of discrete
connected points at convex vertices of the obstacle.