Obstacle Avoidance and Navigation in the Real World by a Seeing Robot Rover, Hans Moravec, 1980
<-- Previous  Next -->

Appendix 8: Path Planning

Exact Shortest Path in Circle Space

The following test program demonstrates an algorithm which finds the guaranteed best tangent path. It was not used in the cart program because of its slow running time and large arrays. An approximate method given in the next section was chosen instead.

The positions and radii of N circular obstacles are given in arrays X, Y and R, indices 1 thru NX and Y also hold the desired starting and stopping co-ordinates in indices 0 and N+1.

The best path is recorded in arrays PATH_XI, PATH_YI, PATH_XJ, PATH_YJ and PATH_R, indices 1 thru PATH_N. For each index i, PATH_XI[i] and PATH_YI[i] are the starting co-ordinates of a tangential segment, PATH_XJ[i] and PATH_YJ[i] are the finish. PATH_R[i] is the radius of the circular arc which connects (and is tangent to) this straight run i, and the one given by i+1. If PATH_R is positive, the connecting circle is to be traversed in a counter-clockwise direction, or clockwise if negative. The center of the circular arc must be determined by analytic geometry from its tangents and radius.

In the procedure, each obstacle becomes two, one representing a counter-clockwise approach by a tangent (positive index), and the other a clockwise approach (negative index).

BEGIN "AVOID1" comment test program for shortest path alg;

REQUIRE "TYPHDR.SAI[GOD,HPM]" SOURCE_FILE;
DEFINE PI="3.14159265",TWOPI="(2*PI)";

DEFINE METER="3.2808399", XPOINTS=2, CART_RADIUS=3, TURN_RADIUS=9;
INTEGER I,J,K,N,NREAL,CH,FJ; REAL LEN;
REAL ARRAY X,Y,R[0:200];
INTEGER PATH_N;
REAL ARRAY PATH_XI,PATH_YI,PATH_XJ,PATH_YJ,PATH_RI[0:200];

PROCEDURE CONSOLIDATE;
   BEGIN
   INTEGER I,J;
   
   FOR I←1 STEP 1 UNTIL N-1 DO
      BEGIN
      REAL RC,XC,YC,RB,XB,YB,SEP;
      FOR J←I+1 STEP 1 UNTIL N DO
	 BEGIN
         RC←R[I]; XC←X[I]; YC←Y[I];
         RB←R[J]; XB←X[J]; YB←Y[J];
	 SEP←SQRT((XB-XC)^2+(YB-YC)^2);
	 IF RC<RB THEN BEGIN RC↔RB; XB↔XC; YB↔YC; END;
	 IF SEP>(RB+RC)*0.8 THEN comment no contact; ELSE
	 IF SEP<RC-RB THEN 
            BEGIN
            comment total overlap;
            X[I]←XC; Y[I]←YC; R[I]←RC;
            X[J]←X[N]; Y[J]←Y[N]; R[J]←R[N]; N←N-1; J←J-1;
            END
         ELSE
	    BEGIN            comment the hard part, a partial overlap;
	    REAL XD,YD,RD,K,POKE;
	    POKE←RB+SEP-RC;
	    K←1/2+(RC-RB)/(2*SEP);
	    XD←XC*K+XB*(1-K);
	    YD←YC*K+YB*(1-K);
	    RD←(RC + RB + SEP)/2 - POKE/8;
            X[I]←XD; Y[I]←YD; R[I]←RD;
            X[J]←X[N]; Y[J]←Y[N]; R[J]←R[N]; N←N-1; J←J-1;
	    END;
	 END;
      END;
   END;

WHILE TRUE DO
   BEGIN "TEST"
   INTEGER NO;

   FJ←FILJOB("DSK:AVOID1.GOD[TMP,HPM]");
   DDINIT; SCREEN(-.01,-.01,10.01,7.01); CH←-1;
   FNTSELECT(3,"METSBM");

   DDINIT; LITEN;
   FOR I←0 STEP 1 UNTIL 7 DO LINE(0,I,10,I); FOR J←0 STEP 1 UNTIL 10 DO LINE(J,0,J,7);

   N←0; X[0]←0; Y[0]←0;  R[0]←1.0@-8;  NO←30*RAN(0);

   FOR J←1 STEP 1 UNTIL NO DO
      BEGIN N←N+1; R[N]←(RAN(0)+.4)*.5; 
      X[N]←(10-2*R[N])*RAN(0)+R[N]; Y[N]←(7-2*R[N])*RAN(0)+R[N]; END;

   FOR J←1 STEP 1 UNTIL 10 DO
      BEGIN REAL P; N←N+1; R[N]←(RAN(0)+.4)*.5; 
      X[N]←(10-2*R[N])*(P←RAN(0))+R[N]; Y[N]←(7-2*R[N])*(P*.4+RAN(0)*.6)+R[N]; END;

   INVEN;

   NREAL←N;

   DRKEN;
   FOR I←1 STEP 1 UNTIL N DO
      ELLIPS(X[I]-R[I]+.05,Y[I]-R[I]+.05,X[I]+R[I]-.05,Y[I]+R[I]-.05);

   FOR I←1 STEP 1 UNTIL N DO
      BEGIN
      INVEN;
      ELLIPS(X[I]-R[I]+.05,Y[I]-R[I]+.05,X[I]+R[I]-.05,Y[I]+R[I]-.05);
      ELLIPS(X[I]-R[I]+.09,Y[I]-R[I]+.09,X[I]+R[I]-.09,Y[I]+R[I]-.09);
      LITEN;
      FNTPOS(X[I],Y[I]);
      DEPOSIT(0,0,CENTER(JTXT(3,CVS(I))));
      END;

   X[N+1]←10; Y[N+1]←7; R[N+1]←1.0@-8;

      BEGIN
      DEFINE INF="1.0@20";
      STRING PROCEDURE CVH(REAL X);
         RETURN(IF X≥INF THEN "INF" ELSE IF X≤-INF THEN "-INF" ELSE CVF(X));

      INTEGER L,PL;
      INTEGER ARRAY PERM,PATH[-N:N+1,-N:N+1];
      REAL ARRAY BEST,XI,YI,XJ,YJ,RI,DIST[-N-1:N+1,-N-1:N+1];  REAL IR;

      REAL PROCEDURE DISTA(INTEGER I,J);
      IF DIST[I,J]≠-INF THEN RETURN(DIST[I,J]) ELSE
      IF DIST[-J,-I]≠-INF THEN
         BEGIN
         DIST[I,J]←DIST[-J,-I];
         XJ[I,J]←XI[-J,-I];   YJ[I,J]←YI[-J,-I];
         XI[I,J]←XJ[-J,-I];   YI[I,J]←YJ[-J,-I];
         RETURN(DIST[I,J]);
         END
      ELSE IF ABS(I)=ABS(J) THEN RETURN(DIST[I,J]←INF) ELSE
	 BEGIN
         REAL IX,IY,JX,JY;
	 REAL A,D,D2,DX,DY,RA,RB,XA,XB,YA,YB;
	 XA←X[ABS(I)]; YA←Y[ABS(I)];  RA←R[ABS(I)]; IF I<0 THEN RA←-RA;
	 XB←X[ABS(J)]; YB←Y[ABS(J)];  RB←R[ABS(J)]; IF J<0 THEN RB←-RB;
	 DX←XB-XA; DY←YB-YA; D2←DX*DX+DY*DY; D←SQRT(D2); DX←DX/D; DY←DY/D;
	 IF D2=0 ∨ ABS(A←(RA-RB)/D)≥1 THEN RETURN(DIST[I,J]←INF) ELSE
	    BEGIN
	    REAL B,PAR,PER,DISTIJ;
	    B←SQRT(1-A*A); DISTIJ←D*B;    PAR←DX*A+DY*B; PER←DY*A-DX*B;
	    IX←XA+RA*PAR;  IY←YA+RA*PER;   JX←XB+RB*PAR;  JY←YB+RB*PER;
	    XI[I,J]←IX; YI[I,J]←IY; XJ[I,J]←JX; YJ[I,J]←JY;
	    DX←JX-IX; DY←JY-IY; D2←DX*DX+DY*DY; D←SQRT(D2); DX←DX/D; DY←DY/D;
	    FOR K←1 STEP 1 UNTIL (IF I=0∨J=0 THEN N ELSE NREAL) DO
	       BEGIN IF K≠ABS(I) ∧ K≠ABS(J) THEN
		  BEGIN  REAL XC,YC,RC,WAY,RC2,DYC,DXC;
		  XC←X[K]; YC←Y[K]; RC←R[K]; DXC←XC-IX; DYC←YC-IY;
		  IF ABS(DX*DYC-DY*DXC)≤RC THEN
		     BEGIN  WAY←DY*DYC+DX*DXC;  RC2←RC*RC;
		     IF (WAY≥0∧WAY≤D)∨DXC*DXC+DYC*DYC≤RC2∨(JX-XC)^2+(JY-YC)^2≤RC2
		     THEN RETURN(DIST[I,J]←INF); END; END; END;
	    RETURN(DIST[I,J]←DISTIJ);
	    END;
	 END;

      REAL PROCEDURE ARCLEN(INTEGER OBP,OBS,OBN);
	 BEGIN REAL TH1,TH2,DTH,XC,YC,RC,RET1,RET2,X1,Y1,X2,Y2; INTEGER O;
         IF OBS=0 OR OBS=N+1 THEN RETURN(0);
         O←ABS(OBS); RC←R[O];
         X1←XJ[OBP,OBS]; Y1←YJ[OBP,OBS];  X2←XI[OBS,OBN]; Y2←YI[OBS,OBN];
	 IR←RC*(IF OBS<0 THEN -1 ELSE 1); XC←X[O]; YC←Y[O];
	 TH2←ATAN2(Y2-YC,X2-XC);   TH1←ATAN2(Y1-YC,X1-XC);  IF IR<0 THEN TH1↔TH2;
	 DTH←TH2-TH1;  IF DTH<0 THEN DTH←DTH+TWOPI;
	 RET1←RC*DTH;  RET2←RC*(TWOPI-DTH);
	 TH2←TH2-TH1;
	 WHILE TH2<0 DO TH2←TH2+TWOPI; WHILE TH2≥TWOPI DO TH2←TH2-TWOPI;
	 FOR K←1 STEP 1 UNTIL NREAL DO
	    BEGIN "OBSTST"
	    IF K≠O THEN
	       BEGIN
	       REAL RB,XB,YB,SEP;
	       XB←X[K]; YB←Y[K]; RB←R[K];
	       SEP←SQRT((XB-XC)^2+(YB-YC)^2);
	       IF SEP<RB+RC ∧ SEP>RC-RB THEN        comment overlap;
	       IF SEP≤RB-RC THEN RETURN(INF) ELSE   comment total occlusion;
		  BEGIN            comment the hard part, a partial overlap;
		  REAL X2,Y2,DYX,DYXR,RT,XI1,YI1,XI2,YI2,TI1,TI2;
		  X2←XB-XC; Y2←YB-YC; DYX←X2^2+Y2^2; DYXR←DYX-RB^2+RC^2;
		  RT←4*DYX*RC^2-DYXR^2;
		  IF RT<0 THEN BEGIN PRINT("RT neg in ARCLEN ",RT,'15&'12); RT←0; END;
		  RT←SQRT(RT);
		  XI1←(DYXR*X2+Y2*RT)/(2*DYX)+XC; comment intersect two obstacles;
		  YI1←(DYXR*Y2-X2*RT)/(2*DYX)+YC;
		  XI2←(DYXR*X2-Y2*RT)/(2*DYX)+XC;
		  YI2←(DYXR*Y2+X2*RT)/(2*DYX)+YC;
		  TI1←ATAN2(YI1-YC,XI1-XC); TI2←ATAN2(YI2-YC,XI2-XC);
                  TI1←TI1-TH1; TI2←TI2-TH1;
		  WHILE TI1<0 DO TI1←TI1+TWOPI; WHILE TI1≥TWOPI DO TI1←TI1-TWOPI;
		  WHILE TI2<0 DO TI2←TI2+TWOPI; WHILE TI2≥TWOPI DO TI2←TI2-TWOPI;
		  IF TI2<TI1 THEN RET1←RET2←INF; comment occludes both directions;
		  IF TI1≤TH2 THEN RET1←INF; comment occludes forward direction;
		  IF TI2≥TH2 THEN RET2←INF; comment occludes reverse direction;
		  IF RET1=INF∨RET2=INF THEN BEGIN LITEN; LINE(XI1,YI1,XI2,YI2); END;
                  IF RET1=INF∧RET2=INF THEN DONE "OBSTST";
		  END;
	       END;
	    END "OBSTST";
	 IF RET2<RET1 THEN BEGIN RET2↔RET1; IR←-IR; END;
         RETURN(RET1);
	 END;

      ARRCLR(DIST,-INF);

      FOR I←-N STEP 1 UNTIL N+1 DO FOR J←-N STEP 1 UNTIL N+1 DO
	  BEGIN BEST[J,I]←INF; PERM[J,I]←FALSE; PATH[J,I]←0; END;
      FOR J←-N STEP 1 UNTIL N+1 DO
	 BEGIN BEST[J,0]←0; PERM[J,J]←PERM[J,0]←TRUE;
               XJ[J,0]←X[0]; YJ[J,0]←Y[0]; END;

      PL←0; L←0;
      WHILE L≠N+1 DO
	 BEGIN "ROUTE"  REAL SMV,BL;  BL←BEST[PL,L];
	 FOR I←-N STEP 1 UNTIL N+1 DO IF ¬PERM[L,I] THEN
	    BEGIN  REAL DLI,BI;
	    DLI←DISTA(L,I); BI←BEST[L,I]; DLI←BL+DLI;
	    IF DLI<BI ∧ (DLI←DLI+ARCLEN(PL,L,I))<BI THEN comment sets IR;
	       BEGIN RI[L,I]←IR; BEST[L,I]←DLI; PATH[L,I]←PL; END;

	    END;
	 SMV←INF;
         FOR I←-N STEP 1 UNTIL N+1 DO FOR J←-N STEP 1 UNTIL N+1 DO
            IF ¬PERM[I,J]∧BEST[I,J]<SMV THEN BEGIN SMV←BEST[I,J]; PL←I; L←J; END;
         IF SMV≥INF THEN BEGIN PRINT("No path!!!",'15&'12); DONE "ROUTE"; END;
	 PERM[PL,L]←TRUE;
	 END "ROUTE";
      
      PATH_N←0;
PRINT("[0] ",BEST[0,0]," [",N+1,"] ",BEST[PL,L],'15&'12);
      DO   comment  record the best path;
	 BEGIN  INTEGER T; PATH_N←PATH_N+1;
	 PATH_XI[PATH_N]←XI[PL,L]; PATH_YI[PATH_N]←YI[PL,L];
	 PATH_XJ[PATH_N]←XJ[PL,L]; PATH_YJ[PATH_N]←YJ[PL,L];
	 PATH_RI[PATH_N]←RI[PL,L]; T←PATH[PL,L]; L←PL; PL←T; END
      UNTIL L=0;
      END;

   LITEN; LEN←0;
   FOR I←PATH_N STEP -1 UNTIL 1 DO
      BEGIN
      LINE(PATH_XI[I],PATH_YI[I],PATH_XJ[I],PATH_YJ[I],PATH_N+1-I);
      LEN←LEN+SQRT((PATH_XI[I]-PATH_XJ[I])^2+(PATH_YI[I]-PATH_YJ[I])^2);

      ELLIPS(PATH_XJ[I]-.02,PATH_YJ[I]-.02,PATH_XJ[I]+.02,PATH_YJ[I]+.02);
      IF I>1 THEN 
	 BEGIN
	 REAL X1,Y1,X2,Y2,DX1,DY1,DX2,DY2,XC,YC,R,XL,YL,DTH,SD,CD; INTEGER NS,IS;
	 X1←PATH_XJ[I];   Y1←PATH_YJ[I];     X2←PATH_XI[I-1]; Y2←PATH_YI[I-1];
	 DX1←PATH_XJ[I]-PATH_XI[I];  DY1←PATH_YJ[I]-PATH_YI[I];
	 DX2←PATH_XJ[I-1]-PATH_XI[I-1]; DY2←PATH_YJ[I-1]-PATH_YI[I-1];
	 XC←(DY1*(DY2*Y2+DX2*X2)-DY2*(DY1*Y1+DX1*X1))/(DX2*DY1-DX1*DY2);
	 YC←(DX2*(DY1*Y1+DX1*X1)-DX1*(DY2*Y2+DX2*X2))/(DX2*DY1-DX1*DY2);
	 DTH←ATAN2(Y2-YC,X2-XC)-ATAN2(Y1-YC,X1-XC);  R←PATH_RI[I-1];
	 IF R<0 THEN DTH←-DTH; IF DTH<0 THEN DTH←DTH+TWOPI;

   LEN←LEN+ABS(R*DTH);
	 NS←5*R*DTH; NS←NS+(IF NS<0 THEN -1 ELSE 1); SD←SIN(DTH/NS); CD←COS(DTH/NS);
	 XL←X1; YL←Y1;
	 FOR IS←1 STEP 1 UNTIL ABS(NS) DO
	    BEGIN REAL X3,Y3;
	    X3←XC+CD*(XL-XC)-SD*(YL-YC); Y3←YC+CD*(YL-YC)+SD*(XL-XC);
	    LINE(XL,YL,X3,Y3); XL←X3; YL←Y3;
	    END;
	 END;
      END;
   PRINT("LENGTH ",LEN);
   DPYUP(CH);
   KILJOB(FJ); FJ←DDJOB; GRAFIL("DSK:AVOID1.GOD[TMP,HPM]");
   INCHWL;
   KILJOB(FJ);
   END "TEST";
END "AVOID1";

Approximate Shortest Path in Circle Space

A version of the following code was used by the cart program. It differs from the exact algorithm in that it does not search as many possibilities, and is sometimes fooled by the difference in distances caused by arrival at an obstacle from different directions. In typical 100 obstacle courses, it gives answers trivially different from the exact ones about 5% of the time.

BEGIN "AVOID" comment test program for shortest path alg;
REQUIRE "TYPHDR.SAI[GOD,HPM]" SOURCE_FILE;
DEFINE PI="3.14159265",TWOPI="(2*PI)";

DEFINE METER="3.2808399", XPOINTS=2, CART_RADIUS=3, TURN_RADIUS=9;
INTEGER I,J,K,N,NREAL,CH,FJ; REAL LEN;
REAL ARRAY X,Y,R[0:200];
INTEGER PATH_N;
REAL ARRAY PATH_XI,PATH_YI,PATH_XJ,PATH_YJ,PATH_RI[0:200];

PROCEDURE CONSOLIDATE;
   BEGIN
   INTEGER I,J;
   
   FOR I←1 STEP 1 UNTIL N-1 DO
      BEGIN
      REAL RC,XC,YC,RB,XB,YB,SEP;
      FOR J←I+1 STEP 1 UNTIL N DO
	 BEGIN
         RC←R[I]; XC←X[I]; YC←Y[I];
         RB←R[J]; XB←X[J]; YB←Y[J];
	 SEP←SQRT((XB-XC)^2+(YB-YC)^2);
	 IF RC<RB THEN BEGIN RC↔RB; XB↔XC; YB↔YC; END;
	 IF SEP>(RB+RC)*0.8 THEN comment no contact; ELSE
	 IF SEP<RC-RB THEN 
            BEGIN
            comment total overlap;
            X[I]←XC; Y[I]←YC; R[I]←RC;
            X[J]←X[N]; Y[J]←Y[N]; R[J]←R[N]; N←N-1; J←J-1;
            END
         ELSE
	    BEGIN            comment the hard part, a partial overlap;
	    REAL XD,YD,RD,K,POKE;
	    POKE←RB+SEP-RC;
	    K←1/2+(RC-RB)/(2*SEP);
	    XD←XC*K+XB*(1-K);
	    YD←YC*K+YB*(1-K);
	    RD←(RC + RB + SEP)/2 - POKE/8;
            X[I]←XD; Y[I]←YD; R[I]←RD;
            X[J]←X[N]; Y[J]←Y[N]; R[J]←R[N]; N←N-1; J←J-1;
	    END;
	 END;
      END;
   END;


WHILE TRUE DO
   BEGIN "TEST"
   INTEGER NO;

   FJ←FILJOB("DSK:AVOID.GOD[TMP,HPM]");
   DDINIT; SCREEN(-.01,-.01,10.01,7.01); CH←-1;
   FNTSELECT(3,"METSBM");
   DDINIT; LITEN;

   FOR I←0 STEP 1 UNTIL 7 DO LINE(0,I,10,I); FOR J←0 STEP 1 UNTIL 10 DO LINE(J,0,J,7);

   N←0;   X[0]←0; Y[0]←0;  R[0]←1.0@-8;  NO←30*RAN(0);

   FOR J←1 STEP 1 UNTIL NO DO
      BEGIN N←N+1; R[N]←(RAN(0)+.4)*.5; 
      X[N]←(10-2*R[N])*RAN(0)+R[N]; Y[N]←(7-2*R[N])*RAN(0)+R[N]; END;

   FOR J←1 STEP 1 UNTIL 10 DO
      BEGIN REAL P; N←N+1; R[N]←(RAN(0)+.4)*.5; 
      X[N]←(10-2*R[N])*(P←RAN(0))+R[N]; Y[N]←(7-2*R[N])*(P*.4+RAN(0)*.6)+R[N]; END;

   INVEN;

   NREAL←N;

   DRKEN;
   FOR I←1 STEP 1 UNTIL N DO
      ELLIPS(X[I]-R[I]+.05,Y[I]-R[I]+.05,X[I]+R[I]-.05,Y[I]+R[I]-.05);
   
   FOR I←1 STEP 1 UNTIL N DO
      BEGIN
      INVEN;
      ELLIPS(X[I]-R[I]+.05,Y[I]-R[I]+.05,X[I]+R[I]-.05,Y[I]+R[I]-.05);
      ELLIPS(X[I]-R[I]+.09,Y[I]-R[I]+.09,X[I]+R[I]-.09,Y[I]+R[I]-.09);
      LITEN;
      FNTPOS(X[I],Y[I]);
      DEPOSIT(0,0,CENTER(JTXT(3,CVS(I))));
      END;

   X[N+1]←10; Y[N+1]←7; R[N+1]←1.0@-8;

      BEGIN
      DEFINE INF="1.0@20";
      INTEGER LATEST;
      INTEGER ARRAY PERM,PATH[-N:N+1];
      REAL ARRAY BEST,XI,YI,XJ,YJ,RI[-N:N+1];
      REAL IX,IY,JX,JY,IR;

      REAL PROCEDURE DISTA(INTEGER I,J);
      IF ABS(I)=ABS(J) THEN RETURN(INF) ELSE
	 BEGIN
	 REAL A,D,D2,DX,DY,RA,RB,XA,XB,YA,YB;
	 XA←X[ABS(I)]; YA←Y[ABS(I)];  RA←R[ABS(I)]; IF I<0 THEN RA←-RA;
	 XB←X[ABS(J)]; YB←Y[ABS(J)];  RB←R[ABS(J)]; IF J<0 THEN RB←-RB;
	 DX←XB-XA; DY←YB-YA; D2←DX*DX+DY*DY; D←SQRT(D2); DX←DX/D; DY←DY/D;
	 IF D2=0 ∨ ABS(A←(RA-RB)/D)≥1 THEN RETURN(INF) ELSE
	    BEGIN
	    REAL B,PAR,PER,DISTIJ;
	    B←SQRT(1-A*A); DISTIJ←D*B;    PAR←DX*A+DY*B; PER←DY*A-DX*B;
	    IX←XA+RA*PAR;  IY←YA+RA*PER;   JX←XB+RB*PAR;  JY←YB+RB*PER;
	    DX←JX-IX; DY←JY-IY; D2←DX*DX+DY*DY; D←SQRT(D2); DX←DX/D; DY←DY/D;
	    FOR K←1 STEP 1 UNTIL (IF I=0∨J=0 THEN N ELSE NREAL) DO
	       BEGIN IF K≠ABS(I) ∧ K≠ABS(J) THEN
		  BEGIN  REAL XC,YC,RC,WAY,RC2,DYC,DXC;
		  XC←X[K]; YC←Y[K]; RC←R[K]; DXC←XC-IX; DYC←YC-IY;
		  IF ABS(DX*DYC-DY*DXC)≤RC THEN
		     BEGIN  WAY←DY*DYC+DX*DXC;  RC2←RC*RC;
		     IF (WAY≥0∧WAY≤D)∨DXC*DXC+DYC*DYC≤RC2∨(JX-XC)^2+(JY-YC)^2≤RC2
		     THEN RETURN(INF); END; END; END;
	    RETURN(DISTIJ);
	    END;
	 END;

      REAL PROCEDURE ARCLEN(REAL X1,Y1,X2,Y2; INTEGER OBS);
	 BEGIN REAL TH1,TH2,DTH,XC,YC,RC,RET1,RET2; INTEGER O; O←ABS(OBS); RC←R[O];
	 IR←RC*(IF OBS<0 THEN -1 ELSE 1); XC←X[O]; YC←Y[O];
	 TH2←ATAN2(Y2-YC,X2-XC);   TH1←ATAN2(Y1-YC,X1-XC);  IF IR<0 THEN TH1↔TH2;
	 DTH←TH2-TH1;  IF DTH<0 THEN DTH←DTH+TWOPI;
	 RET1←RC*DTH;  RET2←RC*(TWOPI-DTH);
	 TH2←TH2-TH1;
	 WHILE TH2<0 DO TH2←TH2+TWOPI; WHILE TH2≥TWOPI DO TH2←TH2-TWOPI;
	 FOR K←1 STEP 1 UNTIL NREAL DO IF K≠O THEN
	    BEGIN
	    REAL RB,XB,YB,SEP;
	    XB←X[K]; YB←Y[K]; RB←R[K];
	    SEP←SQRT((XB-XC)^2+(YB-YC)^2);
	    IF SEP<RB+RC ∧ SEP>RC-RB THEN        comment overlap;
	    IF SEP≤RB-RC THEN RETURN(INF) ELSE   comment total occlusion;
	       BEGIN            comment the hard part, a partial overlap;
	       REAL X2,Y2,DYX,DYXR,RT,XI1,YI1,XI2,YI2,TI1,TI2;
	       X2←XB-XC; Y2←YB-YC; DYX←X2^2+Y2^2; DYXR←DYX-RB^2+RC^2;
	       RT←4*DYX*RC^2-DYXR^2;
	       IF RT<0 THEN BEGIN PRINT("RT neg in ARCLEN ",RT,'15&'12); RT←0; END;
	       RT←SQRT(RT);
	       XI1←(DYXR*X2+Y2*RT)/(2*DYX)+XC; comment intersect two obstacles;
	       YI1←(DYXR*Y2-X2*RT)/(2*DYX)+YC;
	       XI2←(DYXR*X2-Y2*RT)/(2*DYX)+XC;
	       YI2←(DYXR*Y2+X2*RT)/(2*DYX)+YC;
	       TI1←ATAN2(YI1-YC,XI1-XC);
	       TI2←ATAN2(YI2-YC,XI2-XC);
               TI1←TI1-TH1; TI2←TI2-TH1;
	       WHILE TI1<0 DO TI1←TI1+TWOPI; WHILE TI1≥TWOPI DO TI1←TI1-TWOPI;
	       WHILE TI2<0 DO TI2←TI2+TWOPI; WHILE TI2≥TWOPI DO TI2←TI2-TWOPI;
	       IF TI2<TI1 THEN RET1←RET2←INF; comment occludes both directions;
	       IF TI1≤TH2 THEN RET1←INF; comment occludes forward direction;
	       IF TI2≥TH2 THEN RET2←INF; comment occludes reverse direction;
               IF RET1=INF∨RET2=INF THEN BEGIN LITEN; LINE(XI1,YI1,XI2,YI2); END;
	       END;
	    END;
	 IF RET2<RET1 THEN BEGIN RET2↔RET1; IR←-IR; END;  RETURN(RET1);
	 END;

      FOR I←-N STEP 1 UNTIL N+1 DO
	  BEGIN BEST[I]←INF; PERM[I]←FALSE; PATH[I]←0; END;
      BEST[0]←0; PERM[0]←TRUE; LATEST←0;  XJ[0]←X[0]; YJ[0]←Y[0];
      WHILE LATEST≠N+1 DO
	 BEGIN "ROUTE" INTEGER SMI; REAL SMV,XJL,YJL,BL;
	 SMI←0; SMV←INF;
	 BL←BEST[LATEST]; XJL←XJ[LATEST]; YJL←YJ[LATEST];
	 FOR I←-N STEP 1 UNTIL N+1 DO IF ¬PERM[I] THEN
	    BEGIN  REAL DLI,BI;
	    DLI←DISTA(LATEST,I); BI←BEST[I]; DLI←BL+DLI; comment sets IX,IY,JX,JY;
	    IF DLI<BI ∧ (DLI←DLI+ARCLEN(XJL,YJL,IX,IY,LATEST))<BI THEN comment IR sets;
	       BEGIN XI[I]←IX; YI[I]←IY; XJ[I]←JX; YJ[I]←JY; RI[I]←IR;
                     BEST[I]←BI←DLI; PATH[I]←LATEST; END;
	    IF BI<SMV THEN BEGIN SMV←BI; SMI←I; END;
	    END;
         IF SMV≥INF THEN BEGIN PRINT("No path!!!",'15&'12); DONE "ROUTE"; END;
	 LATEST←SMI; PERM[LATEST]←TRUE;
	 END "ROUTE";

      PATH_N←0;  K←N+1;
PRINT("[0] ",BEST[0]," [N+1] ",BEST[N+1],'15&'12);
      WHILE K≠0 DO   comment  record the best path;
	 BEGIN INTEGER L; L←PATH[K];  PATH_N←PATH_N+1;
	 PATH_XI[PATH_N]←XI[K]; PATH_YI[PATH_N]←YI[K];
	 PATH_XJ[PATH_N]←XJ[K]; PATH_YJ[PATH_N]←YJ[K];
	 PATH_RI[PATH_N]←RI[K];  K←L; END;
      END;

   LITEN; LEN←0;
   FOR I←PATH_N STEP -1 UNTIL 1 DO
      BEGIN
      LINE(PATH_XI[I],PATH_YI[I],PATH_XJ[I],PATH_YJ[I],PATH_N+1-I);
      LEN←LEN+SQRT((PATH_XI[I]-PATH_XJ[I])^2+(PATH_YI[I]-PATH_YJ[I])^2);

      ELLIPS(PATH_XJ[I]-.02,PATH_YJ[I]-.02,PATH_XJ[I]+.02,PATH_YJ[I]+.02);
      IF I>1 THEN 
	 BEGIN
	 REAL X1,Y1,X2,Y2,DX1,DY1,DX2,DY2,XC,YC,R,XL,YL,DTH,SD,CD; INTEGER NS,IS;
	 X1←PATH_XJ[I];   Y1←PATH_YJ[I];     X2←PATH_XI[I-1]; Y2←PATH_YI[I-1];
	 DX1←PATH_XJ[I]-PATH_XI[I];  DY1←PATH_YJ[I]-PATH_YI[I];
	 DX2←PATH_XJ[I-1]-PATH_XI[I-1]; DY2←PATH_YJ[I-1]-PATH_YI[I-1];
	 XC←(DY1*(DY2*Y2+DX2*X2)-DY2*(DY1*Y1+DX1*X1))/(DX2*DY1-DX1*DY2);
	 YC←(DX2*(DY1*Y1+DX1*X1)-DX1*(DY2*Y2+DX2*X2))/(DX2*DY1-DX1*DY2);
	 DTH←ATAN2(Y2-YC,X2-XC)-ATAN2(Y1-YC,X1-XC);  R←PATH_RI[I-1];
	 IF R<0 THEN DTH←-DTH; IF DTH<0 THEN DTH←DTH+TWOPI;

   LEN←LEN+ABS(R*DTH);
	 NS←5*R*DTH; NS←NS+(IF NS<0 THEN -1 ELSE 1); SD←SIN(DTH/NS); CD←COS(DTH/NS);
	 XL←X1; YL←Y1;
	 FOR IS←1 STEP 1 UNTIL ABS(NS) DO
	    BEGIN REAL X3,Y3;
	    X3←XC+CD*(XL-XC)-SD*(YL-YC); Y3←YC+CD*(YL-YC)+SD*(XL-XC);
	    LINE(XL,YL,X3,Y3); XL←X3; YL←Y3;
	    END;
	 END;
      END;
   PRINT("LENGTH ",LEN);
   DPYUP(CH);
   KILJOB(FJ); FJ←DDJOB; GRAFIL("DSK:AVOID.GOD[TMP,HPM]");
   INCHWL;
   KILJOB(FJ);
   END "TEST";
END "AVOID";

<-- Previous  Next -->