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 N. X 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";
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 -->