// 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; }