Shortest Path Through A Concave Polygon With Holes

Shortest Path Through A Concave Polygon With Holes
©2006 Darel Rex Finley. This complete article, unmodified, may be freely distributed for educational purposes.

Inspired by Jonathan Walther’s RoBoat project.

The shortest path between two points inside a polygon may be a straight line:

polygon shortest path

Or, it may have to go around an obstacle:

polygon shortest path

Often, the shortest path will hug the wall of the polygon for part of its journey:

polygon shortest path

In fact, the shortest path will always consist of the startpoint and the endpoint, connected by a list of corners from the polygon. (The above examples use 0, 1, and 4 corners of their polygons, respectively.)

If the polygon has “holes” — interior polygons of exclusion — then the shortest path may have to dodge them:

Note that in this example, the shortest path is not the simplest. The green line represents a simpler way through the obstacles, but the blue path is shorter.

How can we find the shortest path for any polygon (with or without holes) and any startpoint and endpoint? As far as I know, an exhaustive search is required. However, the search can be optimized a bit so that it doesn’t take as long as testing every possible path.

The technique used in the below code sample is to grow a spanning tree of points, starting at the startpoint, where every point in the tree is connected back to the startpoint by the shortest possible path. When this tree reaches the endpoint, the solution is found.

Each tree-growing step adds not the closest connectible point, but rather the point that can be connected back to the startpoint by the shortest possible total path. For example, the red point is closer to the tree than the green point, but the green point gives a shorter total path back to the startpoint. Adding nodes to the spanning tree in this manner ensures that every point is connected back to the startpoint by the shortest possible path, and therefore that when we reach the endpoint, we will have reached it by the shortest possible path.

Code Sample

This code is C, but is based on structs that need to be defined. I have never compiled it, so if you discover a bug, please do let me know!

//  Public-domain code by Darel Rex Finley,
2006.





//  (This function automatically knows that
enclosed polygons are "no-go"

//  areas.)



boolean pointInPolygonSet(double testX, double testY, polySet allPolys) {



  bool  oddNodes=NO ;

  int   polyI, i, j ;



  for (polyI=0; polyI<allPolys.count; polyI++) {

    for (i=0;    i< allPolys.poly[polyI].corners; i++) {

      j=i+1; if (j==allPolys.poly[polyI].corners) j=0;

      if   ( allPolys.poly[polyI].y[i]< testY

      &&     allPolys.poly[polyI].y[j]>=testY

      ||     allPolys.poly[polyI].y[j]< testY

      &&     allPolys.poly[polyI].y[i]>=testY) {

        if ( allPolys.poly[polyI].x[i]+(testY-allPolys.poly[polyI].y[i])

        /   (allPolys.poly[polyI].y[j]      
-allPolys.poly[polyI].y[i])

        *   (allPolys.poly[polyI].x[j]      
-allPolys.poly[polyI].x[i])<testX) {

          oddNodes=!oddNodes; }}}}



  return oddNodes; }





//  This function should be called with the full set of *all* relevant polygons.

//  (The algorithm automatically knows that enclosed polygons are “no-go”

//  areas.)

//

//  Note:  As much as possible, this algorithm tries to return YES when the

//         test line-segment is exactly on the border of the polygon,
particularly

//         if the test line-segment *is* a side of a polygon.



bool lineInPolygonSet(

double testSX, double testSY, double testEX, double testEY, polySet allPolys) {



  double  theCos, theSin, dist, sX, sY, eX, eY, rotSX, rotSY, rotEX, rotEY, crossX ;

  int     i, j, polyI ;



  testEX-=testSX;

  testEY-=testSY; dist=sqrt(testEX*testEX+testEY*testEY);

  theCos =testEX/ dist;

  theSin =testEY/ dist;



  for (polyI=0; polyI<allPolys.count; polyI++) {

    for (i=0;    i< allPolys.poly[polyI].corners; i++) {

      j=i+1; if (j==allPolys.poly[polyI].corners) j=0;



      sX=allPolys.poly[polyI].x[i]-testSX;

      sY=allPolys.poly[polyI].y[i]-testSY;

      eX=allPolys.poly[polyI].x[j]-testSX;

      eY=allPolys.poly[polyI].y[j]-testSY;

      if (sX==0. && sY==0. && eX==testEX && eY==testEY

      ||  eX==0. && eY==0. && sX==testEX && sY==testEY) {

        return YES; }



      rotSX=sX*theCos+sY*theSin;

      rotSY=sY*theCos-sX*theSin;

      rotEX=eX*theCos+eY*theSin;

      rotEY=eY*theCos-eX*theSin;

      if (rotSY<0. && rotEY>0.

      ||  rotEY<0. && rotSY>0.) {

        crossX=rotSX+(rotEX-rotSX)*(0.-rotSY)/(rotEY-rotSY);

        if (crossX>=0. && crossX<=dist) return NO; }



      if ( rotSY==0.   && rotEY==0.

      &&  (rotSX>=0.   || rotEX>=0.  )

      &&  (rotSX<=dist || rotEX<=dist)

      &&  (rotSX< 0.   || rotEX< 0.

      ||   rotSX> dist || rotEX> dist)) {

        return NO; }}}



  return pointInPolygonSet(testSX+testEX/2.,testSY+testEY/2.,allPolys); }





double calcDist(double sX, double sY, double eX, double eY) {

  eX-=sX; eY-=sY; return sqrt(eX*eX+eY*eY); }





void swapPoints(point *a, point *b) {

  point swap=*a; *a=*b; *b=swap; }





//  Finds the shortest path from sX,sY to eX,eY that stays within the polygon set.

//

//  Note:  To be safe, the solutionX and solutionY arrays should be large enough

//         to accommodate all the corners of your polygon set (although it is

//         unlikely that anywhere near that many elements will ever be
needed).

//

//  Returns YES if the optimal solution was found, or NO if there is no solution.

//  If a solution was found, solutionX and solutionY will contain the coordinates

//  of the intermediate nodes of the path, in order.  (The startpoint and endpoint

//  are assumed, and will not be included in the solution.)



bool shortestPath(double sX, double sY, double eX, double eY, polySet allPolys,

double *solutionX, double *solutionY, int *solutionNodes) {



  #define  INF  9999999.     //  (larger than total
solution dist could ever be)



  point  pointList[1000] ;   //  (enough for all polycorners plus
two)

  int    pointCount      ;



  int     treeCount, polyI, i, j, bestI, bestJ ;

  double  bestDist, newDist ;



  //  Fail if either the startpoint or endpoint is outside the polygon set.

  if (!pointInPolygonSet(sX,sY,allPolys)

  ||  !pointInPolygonSet(eX,eY,allPolys)) {

    return NO; }



  //  If there is a straight-line solution, return with it immediately.

  if (lineInPolygonSet(sX,sY,eX,eY,allPolys)) {

    (*solutionNodes)=0; return YES; }



  //  Build a point list that refers to the corners of the

  //  polygons, as well as to the startpoint and endpoint.

  pointList[0].x=sX;

  pointList[0].y=sY; pointCount=1;

  for (polyI=0; polyI<allPolys.count; polyI++) {

    for (i=0; i<allPolys.poly[polyI].corners; i++) {

      pointList[pointCount].x=allPolys.poly[polyI].x[i];

      pointList[pointCount].y=allPolys.poly[polyI].y[i]; pointCount++; }}

  pointList[pointCount].x=eX;

  pointList[pointCount].y=eY; pointCount++;



  //  Initialize the shortest-path tree to include just the startpoint.

  treeCount=1; pointList[0].totalDist=0.;



  //  Iteratively grow the shortest-path tree until it reaches the endpoint

  //  -- or until it becomes unable to grow, in which case exit with failure.

  bestJ=0;

  while (bestJ<pointcount-1) {

    bestDist=INF;

    for (i=0; i<treeCount; i++) {

      for (j=treeCount; j<pointCount; j++) {

        if (lineInPolygonSet(

        pointList[i].x,pointList[i].y,

        pointList[j].x,pointList[j].y,allPolys)) {

          newDist=pointList[i].totalDist+calcDist(

          pointList[i].x,pointList[i].y,

          pointList[j].x,pointList[j].y);

          if (newDist<bestDist) {

            bestDist=newDist; bestI=i; bestJ=j; }}}}

    if (bestDist==INF) return NO;   //  (no solution)

    pointList[bestJ].prev     =bestI   ;

    pointList[bestJ].totalDist=bestDist;

    swapPoints(&pointList[bestJ],&pointList[treeCount]); treeCount++; }



  //  Load the solution arrays.

  (*solutionNodes)= -1; i=treeCount-1;

  while (i> 0) {

    i=pointList[i].prev; (*solutionNodes)++; }

  j=(*solutionNodes)-1; i=treeCount-1;

  while (j>=0) {

    i=pointList[i].prev;

    solutionX[j]=pointList[i].x;

    solutionY[j]=pointList[i].y; j--; }



  //  Success.

  return YES; }

Shortest Path Around Obstacles

What if you want to find the shortest path around (or through/inbetween) one or more polygon-shaped obstacles? You can use the same method. Just include one extra polygon: a big rectangle (or any other shape) that surrounds all the other polygons and the startpoint and endpoint. Then, the obstacles become “holes” in that polygon, and the shortest-path-through-polygon method (described above) will find the solution.

To find the shortest path around these polygons...

...put a rectangular polygon around the whole problem, and it becomes a shortest-path-through-polygon problem as discussed earlier in this page.

Dijkstra’s Algorithm

Siavash Mortazavi has notified me that the algorithm described in my page is a special case of Dijkstra’s algorithm. If you connect all corners of the polygon, plus the start- and end-points, with every possible connection that doesn’t go outside the polygon, and weight each connection proportionate to its length (distance), then apply Dijkstra, you’re probably doing exactly what I describe in this page!

Send me an e-mail!

Does the brace style in the above code sample freak you out? Click here to see it explained in a new window.