Compute the shortest path around polygons in C++