Suppose we have the set of obstacles as shown in our environment. We restrict the motion of the robot to a large bounding box B that contains the set of polygons. In other words, we add an extra infinitely large obstacle, which is the area outside B.

The algorithm is as follows:

Step 1 : Compute the trapezoidal map of the free space.

This step takes O(n log n) where n is the number of segments. This time complexity is due to sorting the vertices by x-coordinate before constructing the trapezoidal map.

Step 2 : Construct a road map from the trapezoidal map.

            We take the center of each trapezoid, and join it with the midpoint of its two neighboring vertical extension, as follows:

Step 3 : Use breadth-first search to compute a path from the starting point to the destination.

            The road map has one node per trapezoid, and node per vertical extension. Both the number of trapezoids and number of vertical extensions are linear in the total number of vertices of the obstacles. Therefore, breadth-first search itself takes O(n). Finding the trapezoids containing the starting point and the destination takes O(n) by linear search. Therefore, the overall algorithm takes O(n log n).

            The obvious observation is that this algorithm does not find the shortest path from the starting point to the destination. An algorithm which will find the shortest path, with the cost of greater time complexity, will be presented later. Let us first revisit the problem of polygonal robot vs. point robot.

            As described earlier, there are certain forbidden points in the work space which the reference point r of a robot cannot trespass. Otherwise, the robot will intersect with the obstacles. This is illustrate in the figure below.