DEFINE INITIALIZE_WORLD=       ! initialization;
⊂     BEGIN
      INTEGER I;
      CATALOG_N←0;		   ! # of features in total world model;
      TRAVEL_N←0;		   ! # of features in travel history;
      FEAT_WORLD_N←0;              ! # of features whose world co-ords are known;
      FEAT_CAMERA_N←0;             ! # of features in current camera frame;
      FEAT_FREE_N←FEAT_MAX;        ! # of free features;
      CART_XC←0;
      CART_YC←(52.5-10*SIN(CAM_TILT))/12;  ! height of camera above ground;
      CART_ZC←-10*COS(CAM_TILT)/12;        ! initial position of cart defines co-ord system;
      CART_AA←1;
      CART_BB←CART_CC←COS(CAM_TILT);   ! co-ordinate system;
      CART_AB←CART_AC←CART_BA←CART_CA←0;
      CART_BC←SIN(CAM_TILT);  CART_CB←-CART_BC;
      TRAVEL_N←TRAVEL_N+1;
      TRAVEL_XC[TRAVEL_N]←CART_XC;
      TRAVEL_YC[TRAVEL_N]←CART_YC;
      TRAVEL_ZC[TRAVEL_N]←CART_ZC;
      TRAVEL_AA[TRAVEL_N]←CART_AA;
      TRAVEL_AB[TRAVEL_N]←CART_AB;
      TRAVEL_AC[TRAVEL_N]←CART_AC;
      TRAVEL_BA[TRAVEL_N]←CART_BA;
      TRAVEL_BB[TRAVEL_N]←CART_BB;
      TRAVEL_BC[TRAVEL_N]←CART_BC;
      TRAVEL_CA[TRAVEL_N]←CART_CA;
      TRAVEL_CB[TRAVEL_N]←CART_CB;
      TRAVEL_CC[TRAVEL_N]←CART_CC;

      ARRCLR(FEAT_WORLD);
      ARRCLR(FEAT_CAMERA);
      FOR I←1 STEP 1 UNTIL FEAT_MAX DO FEAT_FREE[I]←I;
      END; ⊃;


   DEFINE GET_CALIBRATION_FILE=             ! Calibration;
⊂     BEGIN
      INTEGER FLAG,CHN;   REAL XL,YL,XH,YH;
      CHN←GETCHAN;
      DO
	 BEGIN  "get calibration file"
	 INTEGER BRCHAR,EOF;
	 OUTSTR("CALIBRATION FILE:");
	 OPEN(CHN,"DSK",8,2,0,1,BRCHAR,EOF);
	 LOOKUP(CHN,INCHWL,FLAG);
	 END
      UNTIL ¬FLAG;
      CAM_YLO←WORDIN(CHN);
      CAM_XLO←WORDIN(CHN);
      CAL_HIG←WORDIN(CHN);
      CAL_WID←WORDIN(CHN);
      CAL_P←WORDIN(CHN); ARRYIN(CHN,CAL_G[1],44); ARRYIN(CHN,CAL_IG[1],44);
      CLOSE(CHN); RELEASE(CHN);
      PQINIT(CAL_P,0);
      PIC_HIG←CAL_HIG; PIC_WID←CAL_WID;

      XL←0;         YL←.5;  DISREM(1,-.01,CAL_G,XL,YL,XL,YL);
      XH←1;         YH←.5;  DISREM(1,-.01,CAL_G,XH,YH,XH,YH);
      CAL_PIXELX←(XH-XL)/PIC_WID;  ! average width of a pixel in undistorted coords;

      YL←1;         XL←.5;  DISREM(1,-.01,CAL_G,XL,YL,XL,YL);
      YH←0;         XH←.5;  DISREM(1,-.01,CAL_G,XH,YH,XH,YH);
      CAL_PIXELY←(YH-YL)/PIC_HIG;  ! average height of a pixel in undistorted coords;

      XL←0;         YL←0;  DISREM(1,-.01,CAL_G,XL,YL,XL,YL);
      XH←0;         YH←1;  DISREM(1,-.01,CAL_G,XH,YH,XH,YH);
      CAL_MINX←XL MIN XH;     ! leftmost boundary of the pic;

      XL←1;         YL←0;  DISREM(1,-.01,CAL_G,XL,YL,XL,YL);
      XH←1;         YH←1;  DISREM(1,-.01,CAL_G,XH,YH,XH,YH);
      CAL_MAXX←XL MAX XH;     ! rightmost boundary;

         BEGIN
         INTEGER IO,I;
         DO PRINT("INDOOR OR OUTDOOR:") UNTIL (I←INCHWL LAND '137)="I" ∨ I="O";
         IF I="I" THEN
            BEGIN
            PRINT("Indoor calibration",'15&'12);
            CART_RADIUS←2.5;
            TURN_RADIUS←9;
            OBS_MIN←0.8;
            OBS_MAX←6.5;
            CLIP_B←0.05;
            CLIP_T←0.05;
            ERROR_LIM←12;
            END
         ELSE
            BEGIN
            PRINT("Outdoor calibration",'15&'12);
            CART_RADIUS←3;
            TURN_RADIUS←9;
            OBS_MIN←1.5;
            OBS_MAX←15;
            CLIP_B←0.1;
            CLIP_T←0.1;
            ERROR_LIM←12;
            END;
         END;
      END; ⊃;


   DEFINE GET_DESTINATION=            ! Where do we want to go? ;
⊂     BEGIN
      DEFINE METER="3.2808399";
      INTEGER FOO;  STRING S;
      PRINT("Destination in meters (Right, Forward):");
      S←INCHWL;
      TRAVEL_DESTX←REALSCAN(S,FOO)*METER;
      TRAVEL_DESTZ←REALSCAN(S,FOO)*METER;
      END; ⊃;


   DEFINE GET_PICTURE_SOURCE=       ! Video source;
⊂     BEGIN
      INTEGER FOO,I;  STRING INST;
      CAM_PAUSE←0;  comment delay to avoid free storage crash of DIGTIZ;

      PRINT("# OF STEPS PER CAMERA MOVE:");  SCAN_STEPSIZE←CVD(INCHWL);
      PRINT("CAMERA TILT, IN DEGREES:"); CAM_TILT←PI*CVD(INCHWL)/180;
      SCAN_NPICS←2060%SCAN_STEPSIZE+1;
      SCAN_STEPTIME←SCAN_STEPSIZE/150;
      SCAN_MIDPIC←(SCAN_NPICS+1)%2;
      SCAN_EXTENT←SCAN_STEPSIZE*(SCAN_NPICS-1)/1200;

      DO
	 BEGIN "get TV numbers or file names"
	 OUTSTR("TV #(S) or FILES (E.G 44 45 or OBS(3:15)):");  INST←INCHWL;
	 CAM_N←0;
	 IF INST="4"∨INST="5" THEN
	    BEGIN "get TV numbers"
	    WHILE (CAM_ID[CAM_N←CAM_N+1]←INTSCAN(INST,FOO))≠0 DO; CAM_N←CAM_N-1;
	    FOR I←1 STEP 1 UNTIL CAM_N DO  ! convert to base 8 (urp);
	    CAM_ID[I]←
	       (CAM_ID[I]%100)*64+((CAM_ID[I] MOD 100)%10)*8+(CAM_ID[I] MOD 10);
	    FOR I←1 STEP 1 UNTIL CAM_N DO
               CLPADJ(CAM_ID[I],CAM_BCL[I],CAM_TCL[I],CLIP_B,CLIP_T);
            FOR I←1 STEP 1 UNTIL CAM_N DO
            PRINT("FOR CAMERA ",CVOS(CAM_ID[I]),", BCLIP=",
                       CAM_BCL[I]," TCLIP=",CAM_TCL[I],'15&'12);
	    END "get TV numbers"
	 ELSE
	    BEGIN "decode file specs"
	    INTEGER I,J;
	    IPIC_NAME←"";
	    WHILE INST≠"(" AND LENGTH(INST)>0 DO IPIC_NAME←IPIC_NAME&LOP(INST);
	    IF INST="(" THEN 
	       BEGIN
	       IPIC_LO←INTSCAN(INST,J);   IF J≠":" THEN CAM_N←-1;
	       IPIC_HI←INTSCAN(INST,J);   IF J≠")" THEN CAM_N←-1;
	       J←LOP(INST);  IPIC_NAME←IPIC_NAME&INST;
	       PRSFIL(""); PRSFIL(IPIC_NAME);
	       END;
	    END "decode file specs";
	 END 
      UNTIL CAM_N>0 ∨ (CAM_N=0 ∧ GETPFD("."&CVS(IPIC_LO),PIC_DIM[0])>0);

      IF CAM_N=0 THEN
	 BEGIN
	 PIC_HIG←PIC_DIM[PCLN];
	 PIC_WID←PIC_DIM[LNBY];
	 PIC_BIT←PIC_DIM[BYBI];
	 END
      ELSE
         BEGIN
	 IF CAL_HIG≠PIC_HIG ∨ CAL_WID≠PIC_WID THEN
	    OUTSTR("WARNING: CALIBRATION IMAGE SIZE ≠ DATA IMAGE SIZE"&'15&'12);
         PIC_BIT←6;			! bits per sample in digitized images;
         END;
      END; ⊃;


   DEFINE GET_OUTPUT_FILENAMES=         ! Output saved files;
⊂     BEGIN
      STRING INST; INTEGER I,J;
      IF CAM_N>0 THEN DO
	 BEGIN
	 OUTSTR("OUTPUT FILE SPEC (EG. FN(1:9)), IF ANY:");
	 INST←INCHWL;
	 I←0; OPIC_LO←1; OPIC_HI←0;
	 IF LENGTH(INST)>4 THEN
	    BEGIN "decode output file specs"
	    OPIC_NAME←"";
	    WHILE INST≠"(" AND LENGTH(INST)>0 DO OPIC_NAME←OPIC_NAME&LOP(INST);
	    IF INST="(" THEN 
	       BEGIN
	       OPIC_LO←INTSCAN(INST,J);
	       IF J≠":" THEN I←-1;
	       OPIC_HI←INTSCAN(INST,J);
	       IF J≠")" THEN I←-1;
	       J←LOP(INST);
               OPIC_NAME←OPIC_NAME&INST;
	       END;
	    END "decode output file specs";
	 END
      UNTIL I≥0;
      END; ⊃;


   DEFINE GET_DISPLAY_PARAMETERS=      ! Display parameters;
⊂     BEGIN
comment  DIS_DDJ←DDJOB;
      DIS_DDJ←FILJOB("DSK:FOO.GOD[TMP,HPM]");
IF FALSE THEN BEGIN
      OUTSTR("DISPLAY WIDTH (PICTURES):"); DIS_WID←CVD(INCHWL);

      IF (DIS_SYN← SYNMAP(2)>0) THEN
	 BEGIN
         INTEGER I;
	 OUTSTR("USE SYNTHESIZER?");
	 DIS_SYN←(INCHWL LAND '137)="Y";
         IF ¬DIS_SYN THEN FOR I←'30 STEP 1 UNTIL '37 DO RDDCHN(I);
	 END;

      IF DIS_SYN THEN
	 BEGIN
	 DIS_TCH←0; WHILE SYNMAP(DIS_TCH)>0∧SYNMAP(DIS_TCH)≠'33 DO DIS_TCH←DIS_TCH+1;
	 DIS_SCH←PIC_BIT MIN DIS_TCH;
	 DIS_TCH←DIS_TCH MIN (2*DIS_SCH);
	 IF DIS_SCH≤0 THEN DIS_SYN←FALSE;
	 END;

END;
      DIS_PIC←0;
      DIS_SCH←PIC_BIT;
      PRINT("DD CHAN ",CVOS(DIS_CH1←GDDCHN(-1)),'15&'12);
      SHOW(DIS_CH1,'402);  SHOW(DIS_CH1,'403);
      QUASH(DIS_DDJ);
      END; ⊃;


   DEFINE SCAN_THE_CAMERA=     ! take a picture sequence;
⊂     BEGIN "CAMSCAN"
      INTEGER I;
      INTEGER BESTCAM;

      IF CAM_N>0 THEN
         BEGIN
	 REAL MINOIS,NOIS;
	 DEFINE INF=100000000;
	 INTEGER ARRAY T[0:PIXDIM(100,100,4)];
	 comment determine best reciever and move camera to left edge;
         MAKPIX(100,100,4,T[0]);
	 MINOIS←INF;
	 DO
	    BEGIN
	    FOR I←1 STEP 1 UNTIL CAM_N DO
	    IF CAMPIX(CAM_ID[I],50,70,T[0],1,CAM_BCL[I],CAM_TCL[I],4)≥0 THEN
	    IF (NOIS←NOISE(T[0]))<MINOIS THEN
	       BEGIN
	       BESTCAM←I;
	       MINOIS←NOIS;
	       END;
	    IF MINOIS=INF THEN PRINT("EEP! COULDN'T GET A FRAME.",'15&'12);
	    END
	 UNTIL MINOIS<INF;
         SLIDER(SCAN_NPICS);
         PRINT("RECEIVER ",CVOS(CAM_ID[BESTCAM]));
         CLPADJ(CAM_ID[BESTCAM],CAM_BCL[BESTCAM],CAM_TCL[BESTCAM],CLIP_B,CLIP_T);
	 PRINT("  BCLIP=",CAM_BCL[BESTCAM]," TCLIP=",CAM_TCL[BESTCAM],'15&'12);
         INIT_DIGITIZER;
         END;

      FOR I←1 STEP 1 UNTIL SCAN_NPICS DO
	 BEGIN "SWEEP"
         IF CAM_N>0 THEN
	    BEGIN "Get a picture, from best receiver"
 
            ACTIVATE_DIGITIZER;
            MAKRED(PIC_HIG,PIC_WID,PIC_BIT,PIC[I,0]);
            READIN_DIGITIZER(<PIC[I,PIC[I,1]]>);
            PICRED(PIC[I,0]);
            SLIDER(-1);

            IF OPIC_LO≤OPIC_HI THEN
               BEGIN
               PRSFIL("");
               PRSFIL(OPIC_NAME); PRSFIL("."&CVS(OPIC_LO));
               PRINT(DEVPRS,":",FILPRS,'15&'12);
               PUTPFL(PIC[I,PIC[I,1]]," ");
               OPIC_LO←OPIC_LO+1;
               END;
	    END
         ELSE
            BEGIN
            INTEGER FSZ;
            IF IPIC_LO>IPIC_HI THEN CALL(0,"EXIT");
            DO
               BEGIN
               PRSFIL("");
               PRSFIL(IPIC_NAME); PRSFIL("."&CVS(IPIC_LO));
               FSZ←GETRDF(" ",PIC[I,0]);
	       IF FSZ≤0 THEN
                  BEGIN
                  PRINT("EEP! COULDN'T GET FRAME ",DEVPRS,":",FILPRS,'15&'12);
                  CALL(0,"EXIT"); INCHWL;
                  END;
               PRINT(IPIC_LO," ");
               IPIC_LO←IPIC_LO+1;
               END
            UNTIL FSZ>0∨IPIC_LO>IPIC_HI;
            END;
	 END "SWEEP";

      END "CAMSCAN"; ⊃;


   DEFINE INIT_DIGITIZER=     ! initialize DIGTIZ;
⊂     BEGIN "CAMINIT"
      STRING S; INTEGER I;
      EXTERNAL INTEGER _SKIP_; STRING POU;
      INTEGER ARRAY HD[1:2],MESSAGE[1:32];
      IF ¬CAM_PTY[1] THEN
	BEGIN "create a pty for DIGITIZ job"
	DO
	   BEGIN
	   CAM_PTY[1]←PTYGET;
	   IF _SKIP_=0 THEN
	      BEGIN PRINT(" ptyget failed "); CALL(5,"SLEEP"); END;
	   END
	UNTIL _SKIP_≠0;
	DO
	   BEGIN
	   CAM_PTY[2]←5;    comment LOGIN function;
	   START_CODE PTYUUO '16,ACCESS(CAM_PTY[1]); comment PTJOBX; END;
	   IF CAM_PTY[2]=0 THEN
	      BEGIN PRINT(" login failed "); CALL(5,"SLEEP"); END;
	   END
	UNTIL CAM_PTY[2]>0;
	PTOSTR(CAM_PTY[1],"R DIGTIZ"&'15&'12);
	POU←"";
	DO POU←POU&PTCHRW(CAM_PTY[1]) UNTIL 
	   EQU(POU[∞-4 TO ∞],"READY") ∨
	   EQU(POU[∞-4 TO ∞],"found");

	IF  EQU(POU[∞-4 TO ∞],"found") THEN BEGIN PRINT(POU,'15&'12);
	    PTOSTR(CAM_PTY[1],"K/F"&'15&'12); CALL(1,"SLEEP");
	    PTYREL(CAM_PTY[1]); CAM_PTY[1]←0; CALL(0,"EXIT"); END;

	END;

      MESSAGE[1]←CALL(0,"PJOB");
      MESSAGE[2]←PIC_HIG; MESSAGE[3]←PIC_WID; MESSAGE[4]←PIC_BIT;
      MESSAGE[5]←CAM_ID[BESTCAM]; MESSAGE[6]←CAM_YLO; MESSAGE[7]←CAM_XLO;
      MESSAGE[8]←CAM_BCL[BESTCAM]; MESSAGE[9]←CAM_TCL[BESTCAM]; MESSAGE[10]←CAM_SUM;
      MEMORY[LOCATION(MESSAGE[11]),REAL]←CAM_REDUN; MESSAGE[12]←CVSIX("DSK");
      MESSAGE[13]←CVSIX("CAR"); MESSAGE[14]←CVSIX("TMP");
      MESSAGE[15]←CVSIX("TMP"); MESSAGE[16]←CVSIX("HPM");
      MESSAGE[17]←CAM_PAUSE MIN 5;
      HD[1]←CAM_PTY[2];  HD[2]←LOCATION(MESSAGE[1]);
      DO BEGIN I←0;
	 START_CODE MAIL 0,ACCESS(HD[1]); comment SEND; SETOM I; END;
	 IF I≠0 THEN CALL(0,"SLEEP");
	 END UNTIL I=0;
      DO BEGIN
	 INTEGER STAT;
	 S←PTYALL(CAM_PTY[1]);
	 IF S[∞ TO ∞]≠"!" THEN
	    BEGIN
	    CALL(0,"SLEEP");
	    IF S[∞ TO ∞]="?" THEN PTOSTR(CAM_PTY[1],"ββ C 0"&'15&'12);
	    STAT←CALL(CAM_PTY[2],"JBTSTS");
	    IF (STAT LAND '400000000000) = 0 THEN
	       BEGIN PRINT("DIGTIZ died! Probably too many pictures requested.",'15&'12);
	       PTOSTR(CAM_PTY[1],"K/F"&'15&'12); CALL(1,"SLEEP");
	       PTYREL(CAM_PTY[1]); CAM_PTY[1]←0; CALL(0,"EXIT"); END;
	    END;
	 END UNTIL S[∞ TO ∞]="!";
      END "CAMINIT" ⊃;


   DEFINE ACTIVATE_DIGITIZER=     ! make DIGTIZ take a picture;
⊂     BEGIN "CAMGO"
      INTEGER ARRAY HD[1:2],MESSAGE[1:32]; INTEGER I; BOOLEAN NEED_RETRY;
      DO
	 BEGIN
	 MESSAGE[1]←-1;  HD[1]←CAM_PTY[2];  HD[2]←LOCATION(MESSAGE[1]);

	 DO BEGIN I←0;
	    START_CODE MAIL 0,ACCESS(HD[1]); comment SEND; SETOM I; END;
	    IF I≠0 THEN CALL(0,"SLEEP");
	    END UNTIL I=0;

	 NEED_RETRY←FALSE;

	 WHILE TRUE DO
	    BEGIN "MAIL WAIT"
	    INTEGER STAT; I←0;
	    START_CODE MAIL 2,ACCESS(MESSAGE[1]); comment SRCV; SETOM I; END;
	    IF I=0 THEN DONE "MAIL WAIT";  CALL(0,"SLEEP");
	    STAT←CALL(CAM_PTY[2],"JBTSTS");
	    IF (STAT LAND '400000000000) = 0 THEN
	       BEGIN PRINT("DIGTIZ died! (probably free storage limit) ");
	       PTOSTR(CAM_PTY[1],"K/F"&'15&'12); CALL(1,"SLEEP");
	       PTYREL(CAM_PTY[1]); CAM_PTY[1]←0;
	       NEED_RETRY←TRUE; DONE "MAIL WAIT"; END;
	    END "MAIL WAIT";

	 IF NEED_RETRY THEN
	    BEGIN
	    CAM_PAUSE←CAM_PAUSE+1;
            PRINT("retrying with ",CAM_PAUSE," sec pause ");
	    INIT_DIGITIZER;
            CALL(CAM_PAUSE,"SLEEP");
	    END
	 ELSE CAM_PAUSE←(CAM_PAUSE-1) MAX 0;
	 END
      UNTIL ¬NEED_RETRY;
      END "CAMGO"; ⊃;


   DEFINE READIN_DIGITIZER(PICTURE)=     ! get picture DIGTIZ made;
⊂     BEGIN "READIN"
      INTEGER ARRAY MESSAGE[1:32];
      START_CODE MAIL 1,ACCESS(MESSAGE[1]); comment WRCV; END;
      GETPFL("DSK:CAR.TMP[TMP,HPM]",PICTURE);
      END "READIN"; ⊃;


   DEFINE EXECUTE_PATH=    ! move forward;
⊂     BEGIN "RUN"
      DEFINE METER="3.2808399";
      INTEGER I,XGJ,TRYANG;  BOOLEAN ARRIVED;
      REAL DIST,TRYDIST,XF,YF,AF,XO,YO,AO;
      PRELOAD_WITH 0; OWN INTEGER ARRAY PCNT[1:1];

      ARRIVED←FALSE;
      PCNT[1]←PCNT[1]+1;
      XGJ←FILJOB("DSK:SIM."&CVS(PCNT[1])&"[DIA,HPM]");
      GRAFIL("U:PLAN."&CVS(PCNT[1])&"[DIA,HPM]");
      FOR TRYDIST←0.75*METER STEP 0.1*METER UNTIL 0.75*METER DO
	 BEGIN "TRY"
	 FOR TRYANG←-1,1 DO
	    BEGIN
	    DIST←0; I←PATH_N;
	    WHILE I≥1 DO
	       BEGIN "CREEP"
	       REAL DDIST,DX,DY;
	       DX←PATH_XJ[I]-PATH_XI[I]; DY←PATH_YJ[I]-PATH_YI[I];
	       DDIST←SQRT(DX^2+DY^2); DX←DX/DDIST; DY←DY/DDIST;
	       IF DIST+DDIST>TRYDIST THEN
		  BEGIN
		  XF←PATH_XI[I]+DX*(TRYDIST-DIST);
		  YF←PATH_YI[I]+DY*(TRYDIST-DIST);
		  AF←ATAN2(DY*TRYANG,DX*TRYANG);
		  DONE "CREEP";
		  END;
	       DIST←DIST+DDIST;

	       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;  BOOLEAN TRNFLG;
		  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;

		  NS←50*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);
		     DX←X3-XL; DY←Y3-YL;
		     DDIST←SQRT(DX^2+DY^2); DX←DX/DDIST; DY←DY/DDIST;
		     IF DIST+DDIST>TRYDIST THEN
			BEGIN
			XF←XL+DX*(TRYDIST-DIST);
			YF←YL+DY*(TRYDIST-DIST);
			AF←ATAN2(DY*TRYANG,DX*TRYANG);
			DONE "CREEP";
			END;
		     DIST←DIST+DDIST;
		     XL←X3; YL←Y3;
		     END;
		  END;
	       I←I-1;
	       END "CREEP";

	    IF I=0 THEN
	       BEGIN
	       STRING S; INTEGER FOO;
	       ARRIVED←TRUE;
	       PRINT('15&'12,"Let the bells ring out and the banners fly !!",
		     '15&'12,"I'm here, I'm here!",'15&'12);
	       PRINT("New Destination (in original co-ordinate system)",
		     '15&'12,"in meters (Right, Forward):");
	       S←INCHWL;
	       TRAVEL_DESTX←REALSCAN(S,FOO)*METER;
	       TRAVEL_DESTZ←REALSCAN(S,FOO)*METER;
	       CART_XPRED←CART_XC; CART_ZPRED←CART_ZC;
	       DONE "TRY";
	       END
	    ELSE
	       BEGIN
	       XO←CART_XC; YO←CART_ZC; AO←ATAN2(CART_CC,CART_CA);

		  BEGIN REAL DX,DY,SDT,CDT,IR,D1,D2;
		  SDT←SIN(AO); CDT←COS(AO);
		  DX←XF-XO; DY←YF-YO; AF←AF-AO; XF←DX*CDT+DY*SDT; YF←DY*CDT-DX*SDT;
                  SETFORMAT(0,4);
		  PRINT("GO TO ",XF,YF,AF*180/PI,'15&'12);
		  IF THRASH(XF,YF,AF,IR,D1,D2) THEN DONE "TRY";
		  END;
	       END;
            END;
         END "TRY";

PRINT(IF TRYANG>0 THEN "Forward!" ELSE "Run away, run away!",'15&'12);
IF TRYANG<0 THEN BACK_UP(.5*METER);

     IF ¬ARRIVED THEN
	 BEGIN
	 REAL X,Y,A,AL,VEL;

	 PROCEDURE DSIMUL(REAL DT; REFERENCE REAL X,Y,TH,AL,VEL);
	    BEGIN INTEGER B,I,J,K; REAL MOT,TRN,IR,VMAX,TDEC;
	    FOR B←1 STEP 1 UNTIL PNEXT-1 DO
	       BEGIN
	       I←(MESG[B] LSH -18)*TICK/DT+0.5;  DT←(MESG[B] LSH -18)*TICK/I;
	       MOT←CASE (MESG[B] LAND '3) OF (0, 0, 1, -1);
	       K←MESG[B] LAND '14;
	       TRN←(IF K='14 THEN 2 ELSE IF K='10 THEN 1 ELSE IF K='4 THEN -1 ELSE 0);
	       FOR J←1 STEP 1 UNTIL I DO
		  BEGIN  REAL SAL,CAL;
		  AL←((AL+(IF TRN=2 THEN (IF AL>0 THEN -DT*TURN_V ELSE DT*TURN_V)
				    ELSE DT*TRN*TURN_V)) MAX ALMIN) MIN ALMAX;
		  SAL←SIN(AL); CAL←COS(AL);
		  IR←SAL/(CART_SIZE*(CAL-SAL/2));
		  VMAX←TERM_V*DRAG_H/(ABS(IR)+DRAG_H);
		  TDEC←DECAY_T*DRAG_H/(ABS(IR)+DRAG_H);
		  VEL←VEL+(IF MOT=0 THEN -(DT/TDEC MIN 1)*VEL
				    ELSE (SIGN(VMAX,MOT)-VEL)*DT/ACCEL_T);
		  TH←TH+VEL*DT*IR;
		  LINE(X,Y,X←X+COS(TH)*VEL*DT,Y←Y+SIN(TH)*VEL*DT);
		  END;
	       END;
	    END;

	 X←XO; Y←YO; A←AO; AL←VEL←0;
	 LITEN; DSIMUL(1/30,X,Y,A,AL,VEL);
	 INVEN;
         FNTPOS(X,Y);
         FNTELL(-7,-7,7,7);
         IF CAM_N>0 THEN BEGIN MAIL; CART_XPRED←X; CART_ZPRED←Y; END
                    ELSE BEGIN CART_XPRED←X; CART_ZPRED←Y; END;
	 END;

      DPYUP(-1); KILJOB(XGJ);
      END "RUN"; ⊃;


   DEFINE DISPLAY_SCAN_SET=   ! display picture set;
⊂     BEGIN
      INTEGER I,J,K,JT;
      SCREEN(0,DIS_WID,DIS_WID,0);  MAPMON(1.0);
      FOR I←0 STEP 1 UNTIL 7 DO IF SYNMAP(I)>0 THEN ERASE(SYNMAP(I));
      FOR I←0 STEP 1 UNTIL PIC_BIT-1 DO
      IF SYNMAP(I)>0 THEN
	 BEGIN
	 DDINIT;
	 FOR K←1 STEP 1 UNTIL SCAN_NPICS DO
	    BEGIN
	    INTEGER II,JJ;
	    II←(K-1)%DIS_WID;  JJ←K-1-II*DIS_WID;
	    VID(JJ,II+1,JJ+1,II,PIC[K,PIC[K,1]],1 ASH (PIC_BIT-I-1));
	    FOR JT←1 STEP 1 UNTIL FEAT_CAMERA_N DO
	       BEGIN
               J←FEAT_CAMERA[JT];
	       LITEN;
               ELLIPS(JJ+IMAGX[K,J]-.01,II+IMAGY[K,J]-.01,JJ+IMAGX[K,J]+.01,II+IMAGY[K,J]+.01);
	       DRKEN; 
	       LINE(JJ+IMAGX[K,J]-.02,II+IMAGY[K,J]-.02,JJ+IMAGX[K,J]+.02,II+IMAGY[K,J]+.02);
	       LINE(JJ+IMAGX[K,J]-.02,II+IMAGY[K,J]+.02,JJ+IMAGX[K,J]+.02,II+IMAGY[K,J]-.02);
	       END;
	    END;
	 DPYUP(SYNMAP(I)); DPYUP(SYNMAP(I)); DPYUP(SYNMAP(I));
	 END;
      INCHWL;
      END; ⊃;


   DEFINE DISPLAY_PICTURE=     ! display single picture;
⊂     BEGIN
      INTEGER I,J,K;
      SCREEN(0,1,1,0);  MAPMON(1.0);
      FOR I←0 STEP 1 UNTIL 7 DO IF SYNMAP(I)>0 THEN ERASE(SYNMAP(I));
      FOR I←0 STEP 1 UNTIL PIC_BIT-1 DO
      IF SYNMAP(I+1)>0 THEN
	 BEGIN
	 DDINIT;
	 VID(0,1,1,0,PIC[SCAN_MIDPIC,PIC[SCAN_MIDPIC,1]],1 ASH (PIC_BIT-I-1));
	 DPYUP(SYNMAP(I+1));
	 DPYUP(SYNMAP(I+1));
	 DPYUP(SYNMAP(I+1));
	 END;
      END; ⊃;


   DEFINE FEAT_COMPACT=   ! procedure to coalesce remaining features;
⊂     BEGIN
      INTEGER I,J,WORLD,CAMERA;

      FEAT_WORLD_N←1;
      FEAT_CAMERA_N←1;
      FOR I←1 STEP 1 UNTIL FEAT_MAX DO
 	 BEGIN
	 IF FEAT_WORLD[I]>0 THEN FEAT_WORLD[I]↔FEAT_WORLD[FEAT_WORLD_N];
	 IF FEAT_WORLD[FEAT_WORLD_N]>0 THEN FEAT_WORLD_N←FEAT_WORLD_N+1;
	 IF FEAT_CAMERA[I]>0 THEN FEAT_CAMERA[I]↔FEAT_CAMERA[FEAT_CAMERA_N];
	 IF FEAT_CAMERA[FEAT_CAMERA_N]>0 THEN FEAT_CAMERA_N←FEAT_CAMERA_N+1;
	 END;

      FEAT_WORLD_N←FEAT_WORLD_N-1;
      FEAT_CAMERA_N←FEAT_CAMERA_N-1;
!     FEAT_FREE_N←FEAT_MAX-FEAT_WORLD_N-FEAT_CAMERA_N;
      END; ⊃;



   DEFINE REACQUIRE_POINTS=   ! correlator for between hops;
⊂     BEGIN
      INTEGER I,J,JT;
      FOR JT←1 STEP 1 UNTIL FEAT_WORLD_N DO
         BEGIN
         J←FEAT_WORLD[JT];
	 IMAGQ[SCAN_MIDPIC,J]←1;
         LOCATE(FEAT_IMAGE[J,0],PIC[SCAN_MIDPIC,0],
                IMAGY[SCAN_MIDPIC,J],IMAGX[SCAN_MIDPIC,J],0,0,1,1);
         REDWIN(PIC[SCAN_MIDPIC,0],IMAGY[SCAN_MIDPIC,J],
                                   IMAGX[SCAN_MIDPIC,J],FEAT_WIN,FEAT_IMAGE[J,0]);
	 DISREM (1,-.01,CAL_G,IMAGX[SCAN_MIDPIC,J],IMAGY[SCAN_MIDPIC,J],
                             IMAGX[SCAN_MIDPIC,J],IMAGY[SCAN_MIDPIC,J]);
         END;

      END; ⊃;


   DEFINE PRUNE_IN_3D= ! weed out features whose location in camera co-ords
     is inconsistent  with location in world co-ords;
⊂     BEGIN
DEFINE PRINT="COMMENT ";
      INTEGER II,JJ,I,J,THR,FW;
      INTEGER ARRAY FN,SRT[0:FEAT_MAX];
      REAL ARRAY ERR[0:FEAT_WORLD_N,0:FEAT_WORLD_N];
      REAL ARRAY RSM[0:FEAT_WORLD_N];

      comment measure distances between point pairs;
      ARRCLR(RSM);
      FOR II←1 STEP 1 UNTIL FEAT_WORLD_N DO FN[II]←FEAT_WORLD[II];
      FOR II←1 STEP 1 UNTIL FEAT_WORLD_N-1 DO
      FOR JJ←II+1 STEP 1 UNTIL FEAT_WORLD_N DO
         BEGIN
         REAL T;
	 J←FEAT_WORLD[JJ];  I←FEAT_WORLD[II];
         ERR[II,JJ]←ERR[JJ,II]←T←
         (SQRT((FEAT_X[I]-FEAT_X[J])^2+
                  (FEAT_Y[I]-FEAT_Y[J])^2+(FEAT_Z[I]-FEAT_Z[J])^2)
	 -SQRT((FX[I]-FX[J])^2+(FY[I]-FY[J])^2+(FZ[I]-FZ[J])^2))^2
            *2/((FER[I]+FER[J])^2+(FEAT_ERROR[I]+FEAT_ERROR[J])^2);
         RSM[II]←RSM[II]+T;   RSM[JJ]←RSM[JJ]+T;
         END;
      FOR II←1 STEP 1 UNTIL FEAT_WORLD_N DO SRT[II]←II; FW←FEAT_WORLD_N;

      SETFORMAT(0,3);

      WHILE FW>1 DO
         BEGIN "PRUNE"
	 PRINT('15&'12&'12);
	 FOR II←1 STEP 1 UNTIL FW-1 DO FOR JJ←II+1 STEP 1 UNTIL FW DO
            IF RSM[SRT[II]]>RSM[SRT[JJ]] THEN SRT[II]↔SRT[JJ];
	 FOR II←1 STEP 1 UNTIL FW DO PRINT(CVF(RSM[SRT[II]]/FW)," ");
         IF RSM[SRT[FW]]/FW<ERROR_LIM THEN DONE "PRUNE";
         FOR II←1 STEP 1 UNTIL FW-1 DO
             RSM[SRT[II]]←RSM[SRT[II]]-ERR[SRT[FW],SRT[II]];
         FW←FW-1;
         END "PRUNE";

      FOR I←1 STEP 1 UNTIL FEAT_WORLD_N DO
         BEGIN
         II←SRT[I];
	 IF I>FW THEN
	    FEAT_FREE[FEAT_FREE_N←FEAT_FREE_N+1]↔FEAT_WORLD[II]  ! free up bad feature;
	 ELSE IF FEAT_CATALOG[FEAT_WORLD[II]]=0 THEN  ! if not cataloged yet, log it;
	    BEGIN
	    FEAT_CATALOG[FEAT_WORLD[II]]←(CATALOG_N←CATALOG_N+1);
	    CATALOG_X[CATALOG_N]←FEAT_X[FEAT_WORLD[II]];
	    CATALOG_Y[CATALOG_N]←FEAT_Y[FEAT_WORLD[II]];
	    CATALOG_Z[CATALOG_N]←FEAT_Z[FEAT_WORLD[II]];
	    CATALOG_ERROR[CATALOG_N]←FEAT_ERROR[FEAT_WORLD[II]];
	    CATALOG_ERRORX[CATALOG_N]←FEAT_ERRORX[FEAT_WORLD[II]];
	    CATALOG_ERRORY[CATALOG_N]←FEAT_ERRORY[FEAT_WORLD[II]];
	    CATALOG_ERRORZ[CATALOG_N]←FEAT_ERRORZ[FEAT_WORLD[II]];
	    CATALOG_TRAVEL[CATALOG_N]←FEAT_TRAVEL[FEAT_WORLD[II]];
	    END;
         END;

      FEAT_COMPACT;
      END; ⊃;


   DEFINE RELATE_CAMERA_TO_WORLD= ! match 3d locations of world and camera
    positions of features to get a camera movement description;
⊂  IF FEAT_WORLD_N≥3 THEN
      BEGIN "fit"
  DEFINE PRINT="COMMENT ";
      REAL AA,AB,AC, BA,BB,BC, CA,CB,CC, DEN;  INTEGER I,II;
      REAL X1,Y1,Z1,X2,Y2,Z2,  X1X1,X1Y1,X1Z1,X1X2,X1Y2,X1Z2,
		Y1Y1,Y1Z1,Y1X2,Y1Y2,Y1Z2, Z1Z1,Z1X2,Z1Y2,Z1Z2;

      REAL PROCEDURE ERROR(REAL AA,AB,AC,BA,BB,BC,CA,CB,CC);
      comment mean square error for a given rotation matrix;
	 RETURN(
      2*CC*Z1*Z2+2*BC*Y1*Z2+2*AC*X1*Z2-2*CC*Z1Z2+CC^2*Z1Z1+
      CB^2*Z1Z1+CA^2*Z1Z1-2*CB*Z1Y2-2*CA*Z1X2-CC^2*Z1^2-CB^2*Z1^2-
      CA^2*Z1^2+2*CB*Y2*Z1-2*BC*CC*Y1*Z1-2*BB*CB*Y1*Z1-2*BA*CA*Y1*Z1+
      2*CA*X2*Z1-2*AC*CC*X1*Z1-2*AB*CB*X1*Z1-2*AA*CA*X1*Z1+
      2*BB*Y1*Y2+2*AB*X1*Y2-2*BC*Y1Z2+2*BC*CC*Y1Z1+2*BB*CB*Y1Z1+2*BA*CA*Y1Z1-
      2*BB*Y1Y2+BC^2*Y1Y1+BB^2*Y1Y1+BA^2*Y1Y1-2*BA*Y1X2-BC^2*Y1^2-BB^2*Y1^2-
      BA^2*Y1^2+2*BA*X2*Y1-2*AC*BC*X1*Y1-2*AB*BB*X1*Y1-2*AA*BA*X1*Y1+
      2*AA*X1*X2-2*AC*X1Z2+2*AC*CC*X1Z1+2*AB*CB*X1Z1+2*AA*CA*X1Z1-
      2*AB*X1Y2+2*AC*BC*X1Y1+2*AB*BB*X1Y1+2*AA*BA*X1Y1-2*AA*X1X2+AC^2*X1X1+
      AB^2*X1X1+AA^2*X1X1-AC^2*X1^2-AB^2*X1^2-AA^2*X1^2 );

      REAL PROCEDURE QERROR(REAL PX,PY,PZ);
	 BEGIN
	 REAL AA,AB,AC,BA,BB,BC,CA,CB,CC, P0;
	 P0←SQRT(0 MAX (1.0-PX^2-PY^2-PZ^2));
	 AA←2*PX*PX+2*P0*P0-1; AB←2*PY*PX+2*PZ*P0; AC←2*PZ*PX-2*PY*P0;
	 BA←2*PX*PY-2*PZ*P0; BB←2*PY*PY+2*P0*P0-1; BC←2*PZ*PY+2*PX*P0;
	 CA←2*PX*PZ+2*PY*P0; CB←2*PY*PZ-2*PX*P0; CC←2*PZ*PZ+2*P0*P0-1;
	 RETURN(ERROR(AA,AB,AC,BA,BB,BC,CA,CB,CC));
	 END;

      comment calculate cross product sums;
      DEN←X1←Y1←Z1←X2←Y2←Z2←X1X1←X1Y1←X1Z1←X1X2←X1Y2←X1Z2←
      Y1Y1←Y1Z1←Y1X2←Y1Y2←Y1Z2←Z1Z1←Z1X2←Z1Y2←Z1Z2←0;
      FOR II←1 STEP 1 UNTIL FEAT_WORLD_N,-1,0 DO 
         BEGIN REAL FAC,PX1,PY1,PZ1,PX2,PY2,PZ2;
         IF II>0 THEN
            BEGIN
	    I←FEAT_WORLD[II];
	    FAC←FQU[I]*FEAT_QUAL[I]/(FEAT_ERROR[I]^2+FER[I]^2);
	    PX1←FX[I]; PY1←FY[I]; PZ1←FZ[I];
	    PX2←FEAT_X[I]; PY2←FEAT_Y[I]; PZ2←FEAT_Z[I];
            END
         ELSE IF II=0 THEN
            BEGIN
            FAC←DEN/(10.0^8);
            PX1←0;       PY1←10.0^5*COS(CAM_TILT);  PZ1←-10.0^5*SIN(CAM_TILT);
            PX2←CART_XC; PY2←10.0^5;                PZ2←CART_ZC;
            END
         ELSE IF II=-1 THEN
            BEGIN
            FAC←DEN*3;
            PX1←0;   PY1←0;  PZ1←0;
            PX2←CART_XPRED;
            PY2←(52.5-10*SIN(CAM_TILT))/12;  ! height of camera above ground;
            PZ2←CART_ZPRED;
            END;
	 X1 ← X1 + PX1*FAC; Y1 ← Y1 + PY1*FAC; Z1 ← Z1 + PZ1*FAC;
	 X2 ← X2 + PX2*FAC; Y2 ← Y2 + PY2*FAC; Z2 ← Z2 + PZ2*FAC;
	 X1X1 ← X1X1 + PX1*PX1*FAC; X1Y1 ← X1Y1 + PX1*PY1*FAC;
	 X1Z1 ← X1Z1 + PX1*PZ1*FAC; X1X2 ← X1X2 + PX1*PX2*FAC;
	 X1Y2 ← X1Y2 + PX1*PY2*FAC; X1Z2 ← X1Z2 + PX1*PZ2*FAC;
	 Y1Y1 ← Y1Y1 + PY1*PY1*FAC; Y1Z1 ← Y1Z1 + PY1*PZ1*FAC;
	 Y1X2 ← Y1X2 + PY1*PX2*FAC; Y1Y2 ← Y1Y2 + PY1*PY2*FAC;
	 Y1Z2 ← Y1Z2 + PY1*PZ2*FAC; Z1Z1 ← Z1Z1 + PZ1*PZ1*FAC;
	 Z1X2 ← Z1X2 + PZ1*PX2*FAC; Z1Y2 ← Z1Y2 + PZ1*PY2*FAC;
	 Z1Z2 ← Z1Z2 + PZ1*PZ2*FAC; DEN ← DEN + FAC; END;
      X1←X1/DEN; Y1←Y1/DEN; Z1←Z1/DEN; X2←X2/DEN; Y2←Y2/DEN; Z2←Z2/DEN;
      X1X1 ← X1X1/DEN; X1Y1 ← X1Y1/DEN; X1Z1 ← X1Z1/DEN; X1X2 ← X1X2/DEN;
      X1Y2 ← X1Y2/DEN; X1Z2 ← X1Z2/DEN; Y1Y1 ← Y1Y1/DEN; Y1Z1 ← Y1Z1/DEN;
      Y1X2 ← Y1X2/DEN; Y1Y2 ← Y1Y2/DEN; Y1Z2 ← Y1Z2/DEN; Z1Z1 ← Z1Z1/DEN;
      Z1X2 ← Z1X2/DEN; Z1Y2 ← Z1Y2/DEN; Z1Z2 ← Z1Z2/DEN;

	 BEGIN "linear form"
	 comment presolved for XC,YC,ZC only;
	 REAL ARRAY A[1:9,1:9], B[1:9];
	 INTEGER I,J,K; REAL T;
	 ARRCLR(A);
	 A[1,1] ← A[2,2] ← A[3,3] ← 2*(X1X1-X1^2);
	 A[4,4] ← A[5,5] ← A[6,6] ← 2*(Y1Y1-Y1^2);
	 A[7,7] ← A[8,8] ← A[9,9] ← 2*(Z1Z1-Z1^2);
	 A[1,4] ← A[2,5] ← A[3,6] ← A[4,1] ← A[5,2] ← A[6,3] ← -2*(X1*Y1-X1Y1);
	 A[1,7] ← A[2,8] ← A[3,9] ← A[7,1] ← A[8,2] ← A[9,3] ← -2*(X1*Z1-X1Z1);
	 A[4,7] ← A[5,8] ← A[6,9] ← A[7,4] ← A[8,5] ← A[9,6] ← -2*(Y1*Z1-Y1Z1);
	 B[1] ← 2*(X1*X2-X1X2); B[2] ← 2*(X1*Y2-X1Y2); B[3] ← 2*(X1*Z2-X1Z2);
	 B[4] ← 2*(Y1*X2-Y1X2); B[5] ← 2*(Y1*Y2-Y1Y2); B[6] ← 2*(Y1*Z2-Y1Z2);
	 B[7] ← 2*(Z1*X2-Z1X2); B[8] ← 2*(Z1*Y2-Z1Y2); B[9] ← 2*(Z1*Z2-Z1Z2);
	 comment solve [AA, AB, AC, BA, BB, BC, CA, CB, CC] ← A^-1 x -B ;
	 FOR I←1 STEP 1 UNTIL 9 DO comment  upper echelon form;
	    BEGIN T←1./A[I,I]; FOR J←I+1 STEP 1 UNTIL 9 DO A[I,J]←T*A[I,J];
	    B[I]←T*B[I]; FOR K←I+1 STEP 1 UNTIL 9 DO
	       BEGIN T←A[K,I]; FOR J←I+1 STEP 1 UNTIL 9 DO
	       A[K,J]←A[K,J]-T*A[I,J]; B[K]←B[K]-T*B[I]; END; END;
	 FOR I←9 STEP -1 UNTIL 2 DO comment back substitution;
	    BEGIN T←B[I]; FOR K←1 STEP 1 UNTIL I-1 DO B[K]←B[K]-A[K,I]*T; END;
	 AA←-B[1];AB←-B[2];AC←-B[3];BA←-B[4];BB←-B[5];BC←-B[6];CA←-B[7];CB←-B[8];CC←-B[9];
	 END "linear form";
      SETFORMAT(0,4);
      PRINT("MR derived",'15&'12);
      PRINT("[ ",AA," ",AB," ",AC," ]",'15&'12);
      PRINT("[ ",BA," ",BB," ",BC," ] + (",
      X2-CA*Z1-BA*Y1-AA*X1,",",Y2-CB*Z1-BB*Y1-AB*X1,",",Z2-CC*Z1-BC*Y1-AC*X1,")",'15&'12);
      PRINT("[ ",CA," ",CB," ",CC," ] ",
      ERROR(AA,AB,AC,BA,BB,BC,CA,CB,CC),'15&'12&'12);

	 BEGIN "iterate"
	 INTEGER I,J,K,L;
	 REAL ARRAY A[1:3,1:3],B[1:3];
	 REAL T,PX,PY,PZ,P0, PX2,PY2,PZ2,PX3,PY3,PZ3,PE, OLER,NEER,MIER;

	 P0←(AA+BB+CC+1)/2;
	 PX2←SQRT(0 MAX ((AA+1-P0)/2));
	 PY2←SQRT(0 MAX ((BB+1-P0)/2));
	 PZ2←SQRT(0 MAX ((CC+1-P0)/2));
	 P0←SQRT(0 MAX (P0/2));
	 PE←SQRT(PX2^2+PY2^2+PZ2^2+P0^2);
	 P0←P0/PE; PX2←PX2/PE;  PY2←PY2/PE;  PZ2←PZ2/PE;
	 PE←1.0@6;
	 FOR PX←-PX2,PX2 DO FOR PY←-PY2,PY2 DO FOR PZ←-PZ2,PZ2 DO
	 IF (T←(2*PX*PY-2*PZ*P0-BA)^2+(2*PX*PZ+2*PY*P0-CA)^2+
	    (2*PY*PX+2*PZ*P0-AB)^2+(2*PY*PZ-2*PX*P0-CB)^2+
	    (2*PZ*PX-2*PY*P0-AC)^2+(2*PZ*PY+2*PX*P0-BC)^2 )< PE THEN
	    BEGIN PE←T; PX3←PX; PY3←PY; PZ3←PZ; END;
	 PX←PX3; PY←PY3; PZ←PZ3;

	 PRINT("trial matrix:",'15&'12);
	 PRINT("[",2*PX*PX+2*P0*P0-1," ",2*PY*PX+2*PZ*P0," ",2*PZ*PX-2*PY*P0,"]",'15&'12);
	 PRINT("[",2*PX*PY-2*PZ*P0," ",2*PY*PY+2*P0*P0-1," ",2*PZ*PY+2*PX*P0,
	 "] + (",X2-(X1*(2*PX*PX+2*P0*P0-1)+Y1*(2*PX*PY-2*PZ*P0)+Z1*(2*PX*PZ+2*PY*P0)),",",
         Y2-(X1*(2*PY*PX+2*PZ*P0)+Y1*(2*PY*PY+2*P0*P0-1)+Z1*(2*PY*PZ-2*PX*P0)),",",
         Z2-(X1*(2*PZ*PX-2*PY*P0)+Y1*(2*PZ*PY+2*PX*P0)+Z1*(2*PZ*PZ+2*P0*P0-1)),")",'15&'12);
	 PRINT("[",2*PX*PZ+2*PY*P0," ",2*PY*PZ-2*PX*P0," ",2*PZ*PZ+2*P0*P0-1,"] ",
	 QERROR(PX,PY,PZ),'15&'12);

	 MIER←OLER←QERROR(PX,PY,PZ);
	 PX←-PX; PY←-PY; PZ←-PZ;   ! because in the derivation p0→sqrt(p0^2)→-p0 ;
	 L←1;
	 DO
	    BEGIN
	    REAL T1,T2,T3,T4,T5,T6,T7,T8,T9,T10,T11,T12,T13,T14,T15,T16,T17,T18,
	       T19,T20,T21,T22,T23,T24,T25,T26,T27,T28,T29,T30,T31,T32,T33,T34;

            P0←SQRT(0 MAX (1.0-PX^2-PY^2-PZ^2));

	    T1←1/P0; T2←-PZ*X1Y2; T3←PY*X1Z2; T4←-PZ*X2*Y1; T5←PZ*Y1X2; T6←-PX*Y1Z2;
	    T7←PZ*X1*Y2; T8←PY*X2*Z1; T9←-PX*Y2*Z1; T10←-PY*Z1X2; T11←PX*Z1Y2;
	    T12←-PY*X1*Z2; T13←PX*Y1*Z2; T14←T13+T12+T11+T10+T9+T8+T7+T6+T5+T4+T3+T2;
	    T15←-X1Y2; T16←PX^2; T17←PY^2; T18←-X2*Y1; T19←X1*Y2;
	    T20←PX*PZ*Y1*Z2-PY*PZ*X1*Z2+PX*PZ*Z1Y2-PY*PZ*Z1X2-PX*PZ*Y2*Z1+PY*PZ*X2*Z1-
		T17*X1*Y2-T16*X1*Y2+T19-PX*PZ*Y1Z2-T17*Y1X2-T16*Y1X2+Y1X2+T17*X2*Y1+
		T16*X2*Y1+T18+PY*PZ*X1Z2+T17*X1Y2+T16*X1Y2+T15;
	    T21←2*Y1Y2; T22←-2*Y1*Y2; T23←2*Z1Z2; T24←-2*Z1*Z2; T25←1/P0^3;
	    T26←4*(T1*(PY*Y1*Z2-PX*X1*Z2+PY*Z1Y2-PX*Z1X2-PY*Y2*Z1+PX*X2*Z1-PY*Y1Z2+PX*X1Z2)+
		T25*PX*PY*T14+T19-Y1X2+X2*Y1+T15);
	    T27←-X1Z2; T28←X1*Z2; T29←-Y1Z2; T30←Y1*Z2;
	    T31←4*(T25*PX*T20+T1*PZ*(T30+Z1Y2-Y2*Z1+T29)+T28-Z1X2+X2*Z1+T27);
	    T32←2*X1X2; T33←-2*X1*X2;
	    T34←4*(T25*PY*T20-T1*PZ*(T28+Z1X2-X2*Z1+T27)+T30-Z1Y2+Y2*Z1+T29);
	    B[1]←4*(T1*PX*T14-2*PX*Z1*Z2-P0*Y1*Z2+PZ*X1*Z2+2*PX*Z1Z2-
	    P0*Z1Y2-PZ*Z1X2+P0*Y2*Z1+PZ*X2*Z1-2*PX*Y1*Y2+PY*X1*Y2+
	    P0*Y1Z2+2*PX*Y1Y2-PY*Y1X2+PY*X2*Y1-PZ*X1Z2-PY*X1Y2);
	    B[2]←4*(T1*PY*T14-2*PY*Z1*Z2+PZ*Y1*Z2+P0*X1*Z2+2*PY*Z1Z2-
	    PZ*Z1Y2+P0*Z1X2+PZ*Y2*Z1-P0*X2*Z1+PX*X1*Y2-PZ*Y1Z2-
	    PX*Y1X2+PX*X2*Y1-2*PY*X1*X2-P0*X1Z2-PX*X1Y2+2*PY*X1X2);
	    B[3]←4*(T1*T20+PY*Y1*Z2+PX*X1*Z2-PY*Z1Y2-PX*Z1X2+
	    PY*Y2*Z1+PX*X2*Z1-2*PZ*Y1*Y2-2*P0*X1*Y2-PY*Y1Z2+2*PZ*Y1Y2-
	    2*P0*Y1X2+2*P0*X2*Y1-2*PZ*X1*X2-PX*X1Z2+2*P0*X1Y2+2*PZ*X1X2);

	    A[1,1] ← 4*(T1*(3*PX*Y1*Z2+T12+3*PX*Z1Y2+T10-3*PX*Y2*Z1+T8+T7-3*PX*Y1Z2+T5+
			T4+T3+T2)+T25*T16*T14+T24+T23+T22+T21);
	    A[1,2] ← T26; A[1,3] ← T31; A[2,1] ← T26;
	    A[2,2] ← 4*(T25*T17*T14+T1*(T13-3*PY*X1*Z2+T11-3*PY*Z1X2+T9+3*PY*X2*Z1+T7+T6+
			T5+T4+3*PY*X1Z2+T2)+T24+T23+T33+T32);
	    A[2,3] ← T34; A[3,1] ← T31; A[3,2] ← T34;
	    A[3,3] ← 4*(-T25*(T17+T16-1)*T14+2*T1*PZ*(T19+Y1X2+T18+T15)+T22+T21+T33+T32);

	    FOR I←1 STEP 1 UNTIL 3 DO comment  upper echelon form;
	       BEGIN T←1./A[I,I]; FOR J←I+1 STEP 1 UNTIL 3 DO A[I,J]←T*A[I,J];  B[I]←T*B[I];
	       FOR K←I+1 STEP 1 UNTIL 3 DO
		  BEGIN T←A[K,I]; FOR J←I+1 STEP 1 UNTIL 3 DO A[K,J] ← A[K,J]-T*A[I,J];
		  B[K]←B[K]-T*B[I]; END; END;
	    FOR I←3 STEP -1 UNTIL 2 DO comment back substitution;
	      BEGIN T←B[I]; FOR K←1 STEP 1 UNTIL I-1 DO B[K]←B[K]-A[K,I]*T; END;

	    IF QERROR(-PX,-PY,-PZ)≤ MIER THEN
		 PRINT(L," ",-PX,-PY,-PZ,P0,QERROR(-PX,-PY,-PZ),'15&'12);
	    PX←PX-B[1]; PY←PY-B[2]; PZ←PZ-B[3]; PE←PX^2+PY^2+PZ^2;
	    IF PE>1 THEN BEGIN PE←SQRT(PE); PX←PX/PE; PY←PY/PE; PZ←PZ/PE; END;
	    NEER←QERROR(-PX,-PY,-PZ);
	    IF NEER<MIER THEN BEGIN PX3←-PX; PY3←-PY; PZ3←-PZ; MIER←NEER; END;

	    IF NEER>OLER THEN
	       BEGIN
               B[1]←10;
               DO BEGIN PX←2*RAN(0)-1; PY←2*RAN(0)-1; PZ←2*RAN(0)-1;
                  END UNTIL PX^2+PY^2+PZ^2<1;
	       OLER←QERROR(-PX,-PY,-PZ);
	       IF OLER<MIER THEN BEGIN PX3←-PX; PY3←-PY; PZ3←-PZ; MIER←OLER; END;
	       END
	    ELSE OLER←NEER;
	    END
	 UNTIL ABS(B[1]) MAX ABS(B[2]) MAX ABS(B[3]) < 0.00001 ∨ (L←L+1)>500;
	 PX←PX3; PY←PY3; PZ←PZ3;

         P0←SQRT(0 MAX (1.0-PX^2-PY^2-PZ^2));
	 CART_AA←2*PX*PX+2*P0*P0-1; CART_AB←2*PY*PX+2*PZ*P0; CART_AC←2*PZ*PX-2*PY*P0;
	 CART_BA←2*PX*PY-2*PZ*P0; CART_BB←2*PY*PY+2*P0*P0-1; CART_BC←2*PZ*PY+2*PX*P0;
	 CART_CA←2*PX*PZ+2*PY*P0; CART_CB←2*PY*PZ-2*PX*P0; CART_CC←2*PZ*PZ+2*P0*P0-1;

	 CART_XC←X2 - (X1*CART_AA + Y1*CART_BA + Z1*CART_CA);
	 CART_YC←Y2 - (X1*CART_AB + Y1*CART_BB + Z1*CART_CB);
	 CART_ZC←Z2 - (X1*CART_AC + Y1*CART_BC + Z1*CART_CC);

	 PRINT("iterated matrix:",'15&'12);
	 PRINT("[",CART_AA," ",CART_AB," ",CART_AC,"]",'15&'12);
	 PRINT("[",CART_BA," ",CART_BB," ",CART_BC,"] + (",
                      CART_XC,",",CART_YC,",",CART_ZC,")",'15&'12);
	 PRINT("[",CART_CA," ",CART_CB," ",CART_CC,"] ",
                      QERROR(PX,PY,PZ),'15&'12);
	 END "iterate";

      TRAVEL_N←TRAVEL_N+1;
      TRAVEL_XC[TRAVEL_N]←CART_XC;
      TRAVEL_YC[TRAVEL_N]←CART_YC;
      TRAVEL_ZC[TRAVEL_N]←CART_ZC;
      TRAVEL_AA[TRAVEL_N]←CART_AA;
      TRAVEL_AB[TRAVEL_N]←CART_AB;
      TRAVEL_AC[TRAVEL_N]←CART_AC;
      TRAVEL_BA[TRAVEL_N]←CART_BA;
      TRAVEL_BB[TRAVEL_N]←CART_BB;
      TRAVEL_BC[TRAVEL_N]←CART_BC;
      TRAVEL_CA[TRAVEL_N]←CART_CA;
      TRAVEL_CB[TRAVEL_N]←CART_CB;
      TRAVEL_CC[TRAVEL_N]←CART_CC;

      FOR II←1 STEP 1 UNTIL FEAT_WORLD_N DO ! if we have a better fix on a feature;
         BEGIN
         I←FEAT_WORLD[II];
	 IF FER[I]/FQU[I]<FEAT_ERROR[I]/FEAT_QUAL[I] THEN
	    BEGIN
	    FEAT_X[I]←CART_AA*FX[I]+CART_BA*FY[I]+CART_CA*FZ[I] + CART_XC;
	    FEAT_Y[I]←CART_AB*FX[I]+CART_BB*FY[I]+CART_CB*FZ[I] + CART_YC;
	    FEAT_Z[I]←CART_AC*FX[I]+CART_BC*FY[I]+CART_CC*FZ[I] + CART_ZC;
            FEAT_TRAVEL[I]←TRAVEL_N;    ! points to co-oord xfrm for this pnts cam;
	    FEAT_ERRORX[I]←FERX[I];
	    FEAT_ERRORY[I]←FERY[I];
	    FEAT_ERRORZ[I]←FERZ[I];
	    FEAT_ERROR[I]←FER[I];
 	    FEAT_QUAL[I]←FQU[I];
            IF FEAT_CATALOG[I]>0 THEN
               BEGIN
	       CATALOG_X[FEAT_CATALOG[I]]←FEAT_X[I];
	       CATALOG_Y[FEAT_CATALOG[I]]←FEAT_Y[I];
	       CATALOG_Z[FEAT_CATALOG[I]]←FEAT_Z[I];
               CATALOG_TRAVEL[FEAT_CATALOG[I]]←FEAT_TRAVEL[I];
	       CATALOG_ERROR[FEAT_CATALOG[I]]←FEAT_ERROR[I];
	       CATALOG_ERRORX[FEAT_CATALOG[I]]←FEAT_ERRORX[I];
	       CATALOG_ERRORY[FEAT_CATALOG[I]]←FEAT_ERRORY[I];
	       CATALOG_ERRORZ[FEAT_CATALOG[I]]←FEAT_ERRORZ[I];
               END;
	    END;
         END;

      END "fit";  ⊃;



   DEFINE DISPLAY_PLAN_VIEW_PIC=
⊂     BEGIN
      DEFINE METER="3.2808399";
      REAL ARRAY COX,COY,COZ,CNX,CNY,CNZ[1:7];
      REAL XL,YL,XH,YH;
      INTEGER II,I,J,K,JT; INTEGER XGJ;
      PRELOAD_WITH 0; OWN INTEGER ARRAY PCNT[1:1];
      PCNT[1]←PCNT[1]+1;
      SETFORMAT(0,0);
      XGJ←FILJOB("DSK:PLAN."&CVS(PCNT[1])&"[DIA,HPM]");

      DDINIT; 
      FNTSELECT(2,"METMBM");
      FNTSELECT(3,"METSBM");

      XL←-12*METER;  YL←0*METER;  XH←10*METER;  YH←32*METER;
      SCREEN(XL-.1,YL-.1,XH+.1,YH+.1);
      LITEN;

      FOR J←-12 STEP 2 UNTIL 10 DO LINE(J*METER,0*METER,J*METER,16*METER);
      FOR I←0 STEP 2 UNTIL 16 DO LINE(-8*METER,I*METER,10*METER,I*METER);
      FOR I←-8 STEP 2 UNTIL 8 DO
         BEGIN
         FNTPOS((I+.1)*METER,.1*METER);
         FNTEXT(0,0,2,CVS(I)&(IF I=2 THEN " m" ELSE ""));
         END;
      FOR I←0,16 DO LINE(-12*METER,I*METER,10*METER,I*METER,5);
      FOR I←-12,-8,10 DO LINE(I*METER,0*METER,I*METER,16*METER,5);

      FOR I←-10 STEP 1 UNTIL 10 DO
         LINE(-10*METER+I*METER/5,16*METER,-10*METER+I*METER/5,16*METER-.1*METER,3);
      FNTPOS(-10*METER+.5*METER,15.3*METER);
      FNTEXT(0,0,2,"cms");

      COX[1]←-1.5; COY[1]←0; COZ[1]←-1.5;
      COX[2]←-1.5; COY[2]←0; COZ[2]← 1.5;
      COX[3]← 1.5; COY[3]←0; COZ[3]← 1.5;
      COX[4]← 1.5; COY[4]←0; COZ[4]←-1.5;
      COX[5]← 0;   COY[5]←0; COZ[5]←0;
      COX[6]← CAL_MINX*200;   COY[6]←0; COZ[6]←200;
      COX[7]← CAL_MAXX*200;   COY[7]←0; COZ[7]←200;

      FOR I←1 STEP 1 UNTIL 7 DO
         BEGIN
         CNX[I]←CART_AA*COX[I]+CART_BA*COY[I]+CART_CA*COZ[I]+CART_XC;
         CNY[I]←CART_AB*COX[I]+CART_BB*COY[I]+CART_CB*COZ[I]+CART_YC;
         CNZ[I]←CART_AC*COX[I]+CART_BC*COY[I]+CART_CC*COZ[I]+CART_ZC;
         END;
      LITEN; comment POLYGO(4,CNX[1],CNZ[1]);
      FOR I←1 STEP 1 UNTIL 4 DO LINE(CNX[I],CNZ[I],CNX[(I MOD 4)+1],CNZ[(I MOD 4)+1],7);
      LINE(CNX[5],CNZ[5],CNX[6],CNZ[6]);
      LINE(CNX[5],CNZ[5],CNX[7],CNZ[7]);
      INVEN;

      FOR I←1 STEP 1 UNTIL TRAVEL_N DO
	 BEGIN
	 COX[1]←-1; COY[1]←0; COZ[1]←0;
	 COX[2]← 1; COY[2]←0; COZ[2]←0;
         FOR J←1 STEP 1 UNTIL 2 DO
	    BEGIN
	    CNX[J]←TRAVEL_AA[I]*COX[J]+TRAVEL_BA[I]*COY[J]+TRAVEL_CA[I]*COZ[J]+TRAVEL_XC[I];
	    CNY[J]←TRAVEL_AB[I]*COX[J]+TRAVEL_BB[I]*COY[J]+TRAVEL_CB[I]*COZ[J]+TRAVEL_YC[I];
	    CNZ[J]←TRAVEL_AC[I]*COX[J]+TRAVEL_BC[I]*COY[J]+TRAVEL_CC[I]*COZ[J]+TRAVEL_ZC[I];
	    END;

         LINE(CNX[1],CNZ[1],CNX[2],CNZ[2],5);
         IF I>1 THEN 
            BEGIN
            LINE(TRAVEL_XC[I],TRAVEL_ZC[I],TRAVEL_XC[I-1],TRAVEL_ZC[I-1],5);
            LINE((TRAVEL_YC[I]-(52.5-10*SIN(CAM_TILT))/12)*20-10*meter,
                  TRAVEL_ZC[I],
                 (TRAVEL_YC[I-1]-(52.5-10*SIN(CAM_TILT))/12)*20-10*meter,
                  TRAVEL_ZC[I-1],5);
            END;
	 END;
      LITEN;

      FOR I←PATH_N STEP -1 UNTIL 1 DO
	 BEGIN
	 LINE(PATH_XI[I],PATH_YI[I],PATH_XJ[I],PATH_YJ[I],3);

	 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;

	    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,3); XL←X3; YL←Y3;
	       END;
	    END;
	 END;

	 BEGIN REAL PXL,PYL,PXH,PYH;
	 PXL←XL; PXH←XH; PYL←16*METER; PYH←32*METER;
	 DRKEN; RECTAN(PXL,PYL,PXH,PYH); LITEN;
         SETFORMAT(0,0);
	 PICFIL(PXL,PYL,PXH,PYH,"U:TAKE50."&CVS(PCNT[1]*9-4)&"[DIA,HPM]");
	 LINE(PXL,PYL,PXH,PYL,5); LINE(PXH,PYH,PXH,PYL,5);
	 LINE(PXH,PYH,PXL,PYH,5); LINE(PXL,PYL,PXL,PYH,5);
 	 END;

      FOR I←1 STEP 1 UNTIL CATALOG_N DO
         BEGIN
         REAL ERXX,ERXZ,ERZX,ERZZ; INTEGER COOR;
         COOR←CATALOG_TRAVEL[I];
         ERXX←CATALOG_ERRORX[I]*TRAVEL_AA[COOR];
         ERXZ←CATALOG_ERRORX[I]*TRAVEL_AC[COOR];
         ERZX←CATALOG_ERRORZ[I]*TRAVEL_CA[COOR];
         ERZZ←CATALOG_ERRORZ[I]*TRAVEL_CC[COOR];
         LITEN;
         LINE(CATALOG_X[I]-ERXX,CATALOG_Z[I]-ERXZ,CATALOG_X[I]+ERXX,CATALOG_Z[I]+ERXZ);
         LINE(CATALOG_X[I]-ERZX,CATALOG_Z[I]-ERZZ,CATALOG_X[I]+ERZX,CATALOG_Z[I]+ERZZ);
         
         FOR J←1 STEP 1 UNTIL 10 DO
            LINE(CATALOG_X[I]-CATALOG_ERROR[I]*COS(J*PI/5),
                 CATALOG_Z[I]-CATALOG_ERROR[I]*SIN(J*PI/5),
                 CATALOG_X[I]-CATALOG_ERROR[I]*COS((J-1)*PI/5),
                 CATALOG_Z[I]-CATALOG_ERROR[I]*SIN((J-1)*PI/5),3);
    
	 SETFORMAT(0,0);
	 LINE(CATALOG_X[I],CATALOG_Z[I],CATALOG_X[I]+CATALOG_Y[I]/1.4,CATALOG_Z[I]+CATALOG_Y[I]/1.4,5);
	 FNTPOS(CATALOG_X[I]+CATALOG_Y[I]/1.4,CATALOG_Z[I]+CATALOG_Y[I]/1.4);
	 FNTEXT(0,0,3,CVS(I));
         END;

      FOR J←1 STEP 1 UNTIL CATALOG_N DO
	 BEGIN
         REAL X,Y;
         BOOLEAN PROCEDURE PROJCT(REAL XW,YW,ZW; REFERENCE REAL X,Y);
            BEGIN
            REAL FX,FY,FZ,FTX,FTY,FTZ,H,V;
	    REAL XO,YO;
            FTX←XW-CART_XC;
            FTY←YW-CART_YC;
            FTZ←ZW-CART_ZC;
	    FX←CART_AA*FTX+CART_AB*FTY+CART_AC*FTZ;
	    FY←CART_BA*FTX+CART_BB*FTY+CART_BC*FTZ;
	    FZ←CART_CA*FTX+CART_CB*FTY+CART_CC*FTZ;
	    XO←FX/FZ; YO←FY/FZ;
	    DISREM (1,-.01,CAL_IG,XO,YO,X,Y);
            RETURN(FZ>0∧ABS(FX/FZ)≤1.5∧ABS(FY/FZ)≤1.5);
            END;
	 IF PROJCT(CATALOG_X[J],CATALOG_Y[J],CATALOG_Z[J],X,Y) THEN
	    BEGIN
            REAL XM,YM;
	    FNTPOS(XM←(22*X-12)*METER,YM←(32-16*Y)*METER);
	    DRKEN;
	    FNTLIN(-15,-15,-15, 15,7);  FNTLIN( 15, 15,-15, 15,7);
	    FNTLIN( 15, 15, 15,-15,7);  FNTLIN(-15,-15, 15,-15,7);
            DEPOSIT(-2,-2,CENTER(JTXT(3,J))); DEPOSIT( 2,-2,CENTER(JTXT(3,J)));
            DEPOSIT( 2, 2,CENTER(JTXT(3,J))); DEPOSIT(-2, 2,CENTER(JTXT(3,J)));
	    LITEN;
	    FNTLIN(-15,-15,-15, 15,3);  FNTLIN( 15, 15,-15, 15,3);
	    FNTLIN( 15, 15, 15,-15,3);  FNTLIN(-15,-15, 15,-15,3);
            DEPOSIT(0,0,CENTER(JTXT(3,CVS(J))));
            PROJCT(CATALOG_X[J],0,CATALOG_Z[J],X,Y);
            X←(22*X-12)*METER; Y←(32-16*Y)*METER;
            LITEN; LINE(XM,YM,X,Y,1);
            FNTPOS(X,Y);
            FNTELL(-6,-6,6,6);
	    END;
	 END;

      KILJOB(XGJ);

      END; ⊃;


   DEFINE PICK_NEW_POINTS= ! interest operator, add new features to the
                             existing list;
⊂     IF FEAT_FREE_N>0 THEN
	 BEGIN
	 INTEGER I,IT,J,K,FN;
	 REAL ARRAY FYT,FXT[1:FEAT_FREE_N];
	 FN←FEAT_FREE_N;
	 FINDIN(PIC[SCAN_MIDPIC,0],INTOP_WIN,FN,FYT[1],FXT[1]);
	 DISREM (FN,-.01,CAL_G,FXT[1],FYT[1],FXT[1],FYT[1]); ! undistort;

	 FOR I←1 STEP 1 UNTIL FN DO
	    BEGIN    ! remove newly found features which are near existing ones;
	    K←1;
	    WHILE K≤FEAT_WORLD_N ∧
	       (ABS(IMAGX[SCAN_MIDPIC,K]-FXT[I]) MAX
		ABS(IMAGY[SCAN_MIDPIC,K]-FYT[I]))>0.07 DO K←K+1;
	    IF K≤FEAT_WORLD_N THEN
	       BEGIN
	       FXT[I]←FXT[FN];
	       FYT[I]←FYT[FN];
	       FN←FN-1;
	       I←I-1;
	       END ELSE
	    END;

	 FOR I←1 STEP 1 UNTIL FN DO
	    BEGIN
	    REAL XD,YD;
	    FEAT_CAMERA[FEAT_CAMERA_N←FEAT_CAMERA_N+1]↔FEAT_FREE[FEAT_FREE_N];
	    J←FEAT_CAMERA[FEAT_CAMERA_N];
	    FEAT_FREE_N←FEAT_FREE_N-1;
	    DISREM (1,-.01,CAL_IG,FXT[I],FYT[I],XD,YD); ! REdistort;
	    REDWIN(PIC[SCAN_MIDPIC,0],YD,XD,FEAT_WIN,FEAT_IMAGE[J,0]);
	    IMAGX[SCAN_MIDPIC,J]←FXT[I];
	    IMAGY[SCAN_MIDPIC,J]←FYT[I];
	    IMAGQ[SCAN_MIDPIC,J]←1;
	    END;

	 END; ⊃;


   DEFINE MATCH_POINTS(FEAT)=   ! correlator for picture set at each position;
⊂     BEGIN
      DEFINE FEAT_N="FEAT"&"_N";

      INTEGER I,J,JT;  REAL X,Y;
      FOR JT←1 STEP 1 UNTIL FEAT_N DO
	 BEGIN
	 J←FEAT[JT];
	 DISREM (1,-.01,CAL_IG,IMAGX[SCAN_MIDPIC,J],IMAGY[SCAN_MIDPIC,J],X,Y);
! SETFORMAT(0,4);
IF FALSE THEN PRINT("F[",J,",",1,",",SCAN_MIDPIC,"]←",X,"; ",
      "F[",J,",",2,",",SCAN_MIDPIC,"]←",Y,";",'15&'12);
	 FOR I←1 STEP 1 UNTIL SCAN_NPICS DO IF I≠SCAN_MIDPIC THEN
	    BEGIN
	    IMAGQ[I,J]←LOCATE(FEAT_IMAGE[J,0],PIC[I,0],IMAGY[I,J],IMAGX[I,J],
			   Y-.05,X-.2,Y+.05,X+.2);
IF FALSE THEN PRINT("F[",J,",",1,",",I,"]←",IMAGX[I,J],"; ",
      "F[",J,",",2,",",I,"]←",IMAGY[I,J],";",'15&'12);
	    DISREM(1,-.01,CAL_G,IMAGX[I,J],IMAGY[I,J],IMAGX[I,J],IMAGY[I,J]);
	    END;
	 END;
      END; ⊃;


   DEFINE CONVERT_TO_WORLD_COORDS= ⊂! convert 3d location in camera co-ords,
 					 to world co-ords;
      BEGIN
      INTEGER J,JT;
      FOR JT←1 STEP 1 UNTIL FEAT_CAMERA_N DO
	 BEGIN
         J←FEAT_CAMERA[JT];
	 FEAT_X[J]←CART_AA*FX[J]+CART_BA*FY[J]+CART_CA*FZ[J]+CART_XC;
	 FEAT_Y[J]←CART_AB*FX[J]+CART_BB*FY[J]+CART_CB*FZ[J]+CART_YC;
	 FEAT_Z[J]←CART_AC*FX[J]+CART_BC*FY[J]+CART_CC*FZ[J]+CART_ZC;
	 FEAT_ERROR[J]←FER[J];
	 FEAT_ERRORX[J]←FERX[J];
	 FEAT_ERRORY[J]←FERY[J];
	 FEAT_ERRORZ[J]←FERZ[J];
         FEAT_TRAVEL[J]←TRAVEL_N;
	 FEAT_QUAL[J]←FQU[J];
         FEAT_CATALOG[J]←0;
         FEAT_WORLD[FEAT_WORLD_N←FEAT_WORLD_N+1]↔FEAT_CAMERA[JT];
	 END;
      FEAT_CAMERA_N←0;
      END; ⊃;


DEFINE HIST_RESOL=5000;



   DEFINE CALCULATE_3D_POSITIONS(FEAT)= ! calculate 3d location in camera co-ords,
 					 at a pause;
⊂     BEGIN
      DEFINE PRINT="COMMENT ";
      DEFINE FEAT_N="FEAT"&"_N";
      INTEGER II,I,J,JT,N,K;
      REAL DX,DY,DP, SD, SIG,EXO;
      REAL ARRAY DIST[-HIST_RESOL/25:HIST_RESOL*26/25];
      SD←HIST_RESOL*CAL_PIXELX/(CAL_MAXX-CAL_MINX);
      FOR JT←1 STEP 1 UNTIL FEAT_N DO
	 BEGIN
         J←FEAT[JT];
	 ARRCLR(DIST);
	 FOR II←1 STEP 1 UNTIL SCAN_NPICS-1 DO
	 FOR I←II+1 STEP 1 UNTIL SCAN_NPICS DO IF I≠II ∧
	    (N←(DX←-IMAGX[I,J]+IMAGX[II,J])*HIST_RESOL/(I-II))≥-1 THEN
            BEGIN
	    DY←IMAGY[II,J]-IMAGY[I,J];
	    DP←ABS(DX)/SQRT(DX^2+DY^2);
	    DP←DP*IMAGQ[I,J]*IMAGQ[II,J];
	    SIG←SD/ABS(I-II);
	    FOR K←-5*SIG STEP 1 UNTIL 5*SIG DO
 	    IF (EXO←-0.5*(K/SIG)^2)>-80 THEN 
	       DIST[K+N]←DIST[K+N]+DP*EXP(EXO)/(SIG*SQRT(2*PI));
            END;
         N←0;  FOR K←1 STEP 1 UNTIL HIST_RESOL DO IF DIST[K]>DIST[N] THEN N←K;
         IF DIST[N]*SD<10 THEN
            BEGIN "flush a feature that doesn't correlate well"
            FEAT_FREE[FEAT_FREE_N←FEAT_FREE_N+1]↔FEAT[JT];
            FEAT[JT]↔FEAT[FEAT_N];
            FEAT_N←FEAT_N-1;
            JT←JT-1;
            END
         ELSE
	    BEGIN "calculate its 3d position"
            REAL DEN,ERRX,ERRY,ERRZ;
	    FZ[J] ←(SCAN_EXTENT/(SCAN_NPICS-1))/(N/HIST_RESOL);
	    FX[J] ←IMAGX[SCAN_MIDPIC,J]*FZ[J];
	    FY[J] ←IMAGY[SCAN_MIDPIC,J]*FZ[J];
            DEN←SCAN_EXTENT/FZ[J];
            ERRZ←0.5*(SCAN_EXTENT/DEN-SCAN_EXTENT/(DEN+CAL_PIXELX));
            ERRX←0.5*CAL_PIXELX*FZ[J];
            ERRY←0.5*CAL_PIXELY*FZ[J];
            SETFORMAT(0,4);
            PRINT("ERROR(X,Y,Z) ",ERRX,ERRY,ERRZ,'15&'12);
            FERX[J]←ERRX;
            FERY[J]←ERRY;
            FERZ[J]←ERRZ;

            FER[J]←(ERRX+ERRY+ERRZ)/3;
!	    FER[J]←SQRT(ERRX^2+ERRY^2+ERRZ^2);
!	    FER[J]←(ERRX*ERRY*ERRZ)^(1/3);
            FQU[J]←DIST[N]*SD;
	    END;
	 END;
      END; ⊃;


   DEFINE DISPLAY_DISTANCES(FEAT)=   ! display the calculated distances;
⊂     BEGIN
      DEFINE FEAT_N="FEAT"&"_N";
      INTEGER II,I,J,JT,K,N,XGJ; BOOLEAN RUNFLAG;
      REAL XL,YL,XH,YH;
      XL←0; YL←1;  DISREM(1,-.01,CAL_G,XL,YL,XL,YL);
      XH←1; YH←0;  DISREM(1,-.01,CAL_G,XH,YH,XH,YH);

      LITEN;
      RUNFLAG←FALSE;

      SETFORMAT(0,0);
      FOR JT←1 STEP 1 UNTIL FEAT_N DO
	 BEGIN
	 REAL DX1,DY1,DX2,DY2;
         XGJ←FILJOB("DSK:DIST"&CVS(JT)&".GOD[NAS,TOB]");
         J←FEAT[JT];
	 DDINIT; SCREEN(0,1,9,-4);
         FOR I←1 STEP 1 UNTIL 9 DO
            PICFIL(I-1,0,I,1,"DSK:SEPT."&CVS(I)&"[NAS,TOB]");

	 INVEN;
	 DX1←IMAGX[SCAN_MIDPIC,J]-.2; DY1←IMAGY[SCAN_MIDPIC,J];
	 DX2←IMAGX[SCAN_MIDPIC,J]; DY2←IMAGY[SCAN_MIDPIC,J];
	 DISREM(1,-.01,CAL_IG,DX1,DY1,DX1,DY1);
	 DISREM(1,-.01,CAL_IG,DX2,DY2,DX2,DY2);
	 LINE(DX1+SCAN_MIDPIC-1,DY1,DX2+SCAN_MIDPIC-1,DY2);

	 DX1←IMAGX[SCAN_MIDPIC,J]; DY1←IMAGY[SCAN_MIDPIC,J];
	 DX2←IMAGX[SCAN_MIDPIC,J]+.2; DY2←IMAGY[SCAN_MIDPIC,J];
	 DISREM(1,-.01,CAL_IG,DX1,DY1,DX1,DY1);
	 DISREM(1,-.01,CAL_IG,DX2,DY2,DX2,DY2);
	 LINE(DX1+SCAN_MIDPIC-1,DY1,DX2+SCAN_MIDPIC-1,DY2);

	 DX1←IMAGX[SCAN_MIDPIC,J]; DY1←IMAGY[SCAN_MIDPIC,J]-.03;
	 DX2←IMAGX[SCAN_MIDPIC,J]; DY2←IMAGY[SCAN_MIDPIC,J]+.03;
	 DISREM(1,-.01,CAL_IG,DX1,DY1,DX1,DY1);
	 DISREM(1,-.01,CAL_IG,DX2,DY2,DX2,DY2);
	 LINE(DX1+SCAN_MIDPIC-1,DY1,DX2+SCAN_MIDPIC-1,DY2);

	 FOR I←1 STEP 1 UNTIL SCAN_NPICS DO
	    BEGIN
	    DX1←IMAGX[I,J]-.01; DY1←IMAGY[I,J]-.01;
	    DX2←IMAGX[I,J]+.01; DY2←IMAGY[I,J]+.01;
	    DISREM(1,-.01,CAL_IG,DX1,DY1,DX1,DY1);
	    DISREM(1,-.01,CAL_IG,DX2,DY2,DX2,DY2);
	    ELLIPS(DX1+I-1,DY1,DX2+I-1,DY2);
	    END;

	 DDINIT; SCREEN(XL,YL,XH,YH);

	    BEGIN
            REAL DX,DY,DP, SD, SIG, EXO;
	    REAL ARRAY DIST[-HIST_RESOL/25:HIST_RESOL*26/25];
	    ARRCLR(DIST);

            SD←HIST_RESOL*CAL_PIXELX/(CAL_MAXX-CAL_MINX);
            LITEN; SCREEN(0,-2/(SQRT(2*PI)*SD),80.1,20/(SQRT(2*PI)*SD));
	    FOR II←1 STEP 1 UNTIL SCAN_NPICS-1 DO
	    FOR I←II+1 STEP 1 UNTIL SCAN_NPICS DO IF I≠II ∧
	       (N←(DX←-IMAGX[I,J]+IMAGX[II,J])*HIST_RESOL/(I-II))≥-1 THEN
	       BEGIN
               REAL INC,ONC;
	       DY←IMAGY[II,J]-IMAGY[I,J];
	       DP←ABS(DX)/SQRT(DX^2+DY^2);
               DP←DP*IMAGQ[I,J]*IMAGQ[II,J];
               SIG←SD/ABS(I-II);
               FOR K←-5*SIG STEP 1 UNTIL 5*SIG DO
               IF (EXO←-0.5*(K/SIG)^2)>-80 THEN
                  BEGIN
                  INC←DP*EXP(EXO)/(SIG*SQRT(2*PI));
                  DIST[K+N]←DIST[K+N]+INC;
                  IF K>-HIST_RESOL/100 THEN
                  LINE((SCAN_STEPSIZE/1200)/((K+N-1)/HIST_RESOL),ONC,
                       (SCAN_STEPSIZE/1200)/((K+N)/HIST_RESOL),INC);
                  ONC←INC;
                  END;
	       END;
            FOR K←2 STEP 1 UNTIL HIST_RESOL DO
               LINE((SCAN_STEPSIZE/1200)/((K-1)/HIST_RESOL),DIST[K-1]*2/SCAN_NPICS,
                    (SCAN_STEPSIZE/1200)/(K/HIST_RESOL),DIST[K]*2/SCAN_NPICS);
            N←0;  FOR K←1 STEP 1 UNTIL HIST_RESOL DO IF DIST[K]>DIST[N] THEN N←K;
            SCREEN(0,-.2,80.1,2);
            LINE((SCAN_STEPSIZE/1200)/(N/HIST_RESOL),-.02,
                 (SCAN_STEPSIZE/1200)/(N/HIST_RESOL),-.2);
            TXTPOS(70,1,1,1);
            SETFORMAT(0,3);
            FNTEXT(0,0,2,CVF(DIST[N]*SD)&'15&'12&CVF(FQU[J]));
	    END;

         SCREEN(0,-.2,80.1,2);
         FOR I←0 STEP 1 UNTIL 80 DO
         LINE(I,0,I,
            IF (I MOD 5)=0 THEN IF (I MOD 10)=0 THEN -.2 ELSE -.1 ELSE -.05);
         FOR I←0 STEP 10 UNTIL 80 DO
            BEGIN
            FNTPOS(I+.7,-.18,1,1);
            FNTEXT(0,0,2,CVS(I));
            END;
         LINE(0,0,80,0);

	 DPYUP(DIS_CH1);
	 SHOW(DIS_CH1,'402);
	 SHOW(DIS_CH1,'403);

         KILJOB(XGJ);   DETJOB(DDJOB,"DISTA.TMP[TMP,HPM]");
 	 IF ¬RUNFLAG THEN RUNFLAG←(INCHRW="*");
	 END;

      END; ⊃;


   DEFINE HARD_DISPLAY_DISTANCES(FEAT)=   ! display the calculated distances;
⊂     BEGIN
      DEFINE FEAT_N="FEAT"&"_N";
      DEFINE METER="3.2808399";
      INTEGER II,I,J,JT,K,N,XGJ,DDJ,KLO,KHI;
      REAL XL,YL,XH,YH; STRING GRFN;
      REAL ARRAY PXP,PYP[1:9];
      DEFINE MAXX=50;

      FOR I←0 STEP 1 UNTIL 2 DO FOR J←0 STEP 1 UNTIL 2 DO
         BEGIN PXP[3*I+J+1]←J; PYP[3*I+J+1]←I; END;

      XL←0; YL←1;  DISREM(1,-.01,CAL_G,XL,YL,XL,YL);
      XH←1; YH←0;  DISREM(1,-.01,CAL_G,XH,YH,XH,YH);

      FOR JT←1 STEP 1 UNTIL FEAT_N DO
	 BEGIN
	 REAL DX1,DY1,DX2,DY2;
         J←FEAT[JT];

         SETFORMAT(0,0);
         XGJ←FILJOB(GRFN←"DSK:DIST"&CVS(JT)&".GOD[DIA,HPM]");

	 DDINIT; SCREEN(0,3+10/7,3,0); LITEN;
         FNTSEL(2,"METMBM");

	 FOR I←1 STEP 1 UNTIL SCAN_NPICS DO
	    BEGIN
            PICFIL(PXP[I],PYP[I]+1,PXP[I]+1,PYP[I],"U:TAKE50."&CVS(I+9)&"[DIA,HPM]");
            LINE(PXP[I],PYP[I]+1,PXP[I],PYP[I],2);
            LINE(PXP[I]+1,PYP[I],PXP[I],PYP[I],2);
            LINE(PXP[I]+1,PYP[I],PXP[I]+1,PYP[I]+1,2);
            LINE(PXP[I],PYP[I]+1,PXP[I]+1,PYP[I]+1,2);
	    DX1←IMAGX[I,J]; DY1←IMAGY[I,J];
	    DISREM(1,-.01,CAL_IG,DX1,DY1,DX1,DY1);
            FNTPOS(PXP[I]+DX1,PYP[I]+DY1);
	    DRKEN;
	    FNTLIN(-20,-20,-20, 20,7);  FNTLIN( 20, 20,-20, 20,7);
	    FNTLIN( 20, 20, 20,-20,7);  FNTLIN(-20,-20, 20,-20,7);
	    FNTLIN(0,-30,0,-10,7);  FNTLIN(0, 30,0, 10,7);
	    FNTLIN(-30,0,-10,0,7);  FNTLIN( 30,0, 10,0,7);
	    LITEN;
	    FNTLIN(-20,-20,-20, 20,3);  FNTLIN( 20, 20,-20, 20,3);
	    FNTLIN( 20, 20, 20,-20,3);  FNTLIN(-20,-20, 20,-20,3);
	    FNTLIN(-30,0,-20,0,3);  FNTLIN( 30,0, 20,0,3);
	    FNTLIN(0,-30,0,-10,3);  FNTLIN(0, 30,0, 10,3);
	    FNTLIN(-30,0,-10,0,3);  FNTLIN( 30,0, 10,0,3);
	    END;

	    BEGIN
            REAL DX,DY,DP, SD, SIG, EXO;
	    REAL ARRAY DIST[-HIST_RESOL/25:HIST_RESOL*26/25];
	    ARRCLR(DIST);

            SD←HIST_RESOL*CAL_PIXELX/(CAL_MAXX-CAL_MINX);
            LITEN; SCREEN(XL←1/(3*METER),YL←-2/(SQRT(2*PI)*SD),
                          XH←1/(MAXX*METER),YH←50/(SQRT(2*PI)*SD));

            KLO←XL*(SCAN_STEPSIZE/1200)*HIST_RESOL;
            KHI←XH*(SCAN_STEPSIZE/1200)*HIST_RESOL;

	    FOR II←1 STEP 1 UNTIL SCAN_NPICS-1 DO
	    FOR I←II+1 STEP 1 UNTIL SCAN_NPICS DO IF I≠II ∧
	       (N←(DX←-IMAGX[I,J]+IMAGX[II,J])*HIST_RESOL/(I-II))≥-1 THEN
	       BEGIN
               REAL INC,ONC;
	       DY←IMAGY[II,J]-IMAGY[I,J];
	       DP←ABS(DX)/SQRT(DX^2+DY^2);
               DP←DP*IMAGQ[I,J]*IMAGQ[II,J];
               SIG←SD/ABS(I-II);
               FOR K←-5*SIG STEP 1 UNTIL 5*SIG DO
               IF (EXO←-0.5*(K/SIG)^2)>-(MAXX*METER) THEN
                  BEGIN
                  INC←DP*EXP(EXO)/(SIG*SQRT(2*PI));
                  DIST[K+N]←DIST[K+N]+INC;
                  IF K>-HIST_RESOL/100 THEN
                  IF (K+N)≥KHI ∧ (K+N)≤KLO THEN
                  LINE(1/((SCAN_STEPSIZE/1200)/((K+N-1)/HIST_RESOL)),ONC,
                       1/((SCAN_STEPSIZE/1200)/((K+N)/HIST_RESOL)),INC);
                  ONC←INC;
                  END;
	       END;

            FOR K←KHI STEP 1 UNTIL KLO DO
               BEGIN
               LINE(1/((SCAN_STEPSIZE/1200)/((K-1)/HIST_RESOL)),DIST[K-1]*2/SCAN_NPICS,
                    1/((SCAN_STEPSIZE/1200)/(K/HIST_RESOL)),DIST[K]*2/SCAN_NPICS,3);
               ELLIPS(
                    1/((SCAN_STEPSIZE/1200)/(K/HIST_RESOL))-(XH-XL)/300,
                    DIST[K]*2/SCAN_NPICS-(YH-YL)/300,
                    1/((SCAN_STEPSIZE/1200)/(K/HIST_RESOL))+(XH-XL)/300,
                    DIST[K]*2/SCAN_NPICS+(YH-YL)/300);
               END;
            N←0;  FOR K←1 STEP 1 UNTIL HIST_RESOL DO IF DIST[K]>DIST[N] THEN N←K;
            SCREEN(XL,-.2,XH,5);
            LINE(1/((SCAN_STEPSIZE/1200)/(N/HIST_RESOL)),-.07,
                 1/((SCAN_STEPSIZE/1200)/(N/HIST_RESOL))-(XH-XL)/180,-.12,3);
            LINE(1/((SCAN_STEPSIZE/1200)/(N/HIST_RESOL)),-.07,
                 1/((SCAN_STEPSIZE/1200)/(N/HIST_RESOL))+(XH-XL)/180,-.12,3);
            LINE(1/((SCAN_STEPSIZE/1200)/(N/HIST_RESOL))-(XH-XL)/180,-.12,
                 1/((SCAN_STEPSIZE/1200)/(N/HIST_RESOL))+(XH-XL)/180,-.12,3);
	    END;

         SCREEN(XL,-.2,XH,5);

         FOR I←3,5 STEP 5 UNTIL 30 DO
         IF I<20 ∨ (I MOD 10)=0 THEN
            BEGIN
            FNTPOS(1/(I*METER)+.2*(XH-XL)/90,-.18,1,1);
            FNTEXT(2,-2,2,CVS(I)&(IF I≠3 THEN "" ELSE "  meters"));
	    LINE(1/(I*METER),0,1/(I*METER),
	       IF (I MOD 10)=0 THEN -.2 ELSE -.1,5);
            END;
         FOR I←1 STEP 1 UNTIL MAXX DO LINE(1/(I*METER),0,1/(I*METER),-.05,1);
         LINE(1,0,0,0,5);

	 DPYUP(DIS_CH1);
         KILJOB(XGJ);   
PRINT(" ",JT);
         INCITE(DIS_DDJ);  GRAFIL(GRFN); QUASH(DIS_DDJ);
         DETJOB(XGPJOB,"DISTA.TMP[TMP,HPM]");
         
	 END;

      END; ⊃;


   DEFINE OBSTACLE_AVOID=       ! path planning;
⊂     BEGIN
      DEFINE METER="3.2808399", XPOINTS=2;
      INTEGER I,J,K,N,NREAL,FAK1,FAK2;
      REAL ARRAY X,Y,R[0:CATALOG_N+XPOINTS+1];

      X[0]←CART_XC; Y[0]←CART_ZC;  R[0]←1.0@-8;  N←0;
      FOR I←1 STEP 1 UNTIL CATALOG_N DO
      IF CATALOG_Y[I]>(OBS_MIN+CATALOG_ERROR[I])∧
         CATALOG_Y[I]<(OBS_MAX-CATALOG_ERROR[I]) THEN 
         BEGIN 
         N←N+1; X[N]←CATALOG_X[I]; Y[N]←CATALOG_Z[I];
         R[N]←CATALOG_ERROR[I]+CART_RADIUS;
         IF (X[0]-X[N])^2+(Y[0]-Y[N])^2<(R[0]+R[N]+1)^2 ∨
            (TRAVEL_DESTX-X[N])^2+(TRAVEL_DESTZ-Y[N])^2<(R[N]+1)^2 THEN N←N-1;
         END;

      NREAL←N;

      N←N+1;
      X[N]←TRAVEL_AA[TRAVEL_N]*TURN_RADIUS+TRAVEL_XC[TRAVEL_N];
      Y[N]←TRAVEL_AC[TRAVEL_N]*TURN_RADIUS+TRAVEL_ZC[TRAVEL_N];
      R[N]←0.9999*SQRT((X[0]-X[N])^2+(Y[0]-Y[N])^2);
      N←N+1;
      X[N]←-TRAVEL_AA[TRAVEL_N]*TURN_RADIUS+TRAVEL_XC[TRAVEL_N];
      Y[N]←-TRAVEL_AC[TRAVEL_N]*TURN_RADIUS+TRAVEL_ZC[TRAVEL_N];
      R[N]←0.9999*SQRT((X[0]-X[N])^2+(Y[0]-Y[N])^2);

      X[N+1]←TRAVEL_DESTX; Y[N+1]←TRAVEL_DESTZ; R[N+1]←1.0@-8;

	 BEGIN "Shortest path (sort of)"
	 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 ∨ TRUE 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
	       BEGIN "ROUTE"
	       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 DONE "ROUTE";
		     END;
		  END;
	       END "ROUTE";
	    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 THEN
		  BEGIN
		  DLI←DLI+ARCLEN(XJL,YJL,IX,IY,LATEST); comment sets IR;
		  IF DLI<BI THEN
		     BEGIN XI[I]←IX; YI[I]←IY; XJ[I]←JX; YJ[I]←JY; RI[I]←IR;
		     BEST[I]←BI←DLI; PATH[I]←LATEST; END;
		  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;
	 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 "Shortest path (sort of)";
      END; ⊃;


   DEFINE MAKE_MOVIE_FRAME= ⊂! convert 3d location of world points to scene co-ords;
      BEGIN
      INTEGER J,JT,K;
      INTEGER ARRAY PC[0:PIXDIM(PIC_HIG,PIC_WID,PIC_BIT)];
      MAKPIX(PIC_HIG,PIC_WID,PIC_BIT,PC[0]);
      FOR K←1 STEP 1 UNTIL SCAN_NPICS DO
         BEGIN
	 REAL DX;
         COPPIC(PIC[K,PIC[K,1]],PC[0]);
	 DX←(K-(SCAN_NPICS+1)/2)*SCAN_STEPSIZE/1200;
	 FOR JT←1 STEP 1 UNTIL FEAT_WORLD_N DO
	    BEGIN
	    REAL FX,FY,FZ,FTX,FTY,FTZ,H,V;
	    J←FEAT_WORLD[JT];
	    FTX←FEAT_X[J]-CART_XC; FTY←FEAT_Y[J]-CART_YC; FTZ←FEAT_Z[J]-CART_ZC;
	    FX←CART_AA*FTX+CART_AB*FTY+CART_AC*FTZ;
	    FY←CART_BA*FTX+CART_BB*FTY+CART_BC*FTZ;
	    FZ←CART_CA*FTX+CART_CB*FTY+CART_CC*FTZ;
	    IF FZ>0∧ABS((FX-DX)/FZ)≤1.5∧ABS(FY/FZ)≤1.5 THEN
 	       BEGIN
	       REAL XO,YO,X,Y; INTEGER I0,J0,IC,JC;
	       XO←(FX-DX)/FZ; YO←FY/FZ;
	       DISREM (1,-.01,CAL_IG,XO,YO,X,Y);
               J0←X*PC[LNBY]; I0←Y*PC[PCLN];
               FOR JC←-3 STEP 1 UNTIL 3 DO
               FOR IC←ABS(JC)-4 STEP 1 UNTIL 4-ABS(JC) DO
               IF ABS(JC)+ABS(IC)>2 THEN
               IF J0+JC≥0 ∧ J0+JC<PC[LNBY] ∧ I0+IC≥0 ∧ I0+IC<PC[PCLN] THEN
                  PUTEL(PC[0],I0+IC,J0+JC,
                       IF PIXEL(PC[0],I0+IC,J0+JC)>PC[BMAX]%2 THEN 0 ELSE PC[BMAX]);
	       END;
            END;
         SETFORMAT(0,0);
         PUTPFL(PC[0],"U:MOVIE."&CVS(IPIC_LO-SCAN_NPICS+K-1)&"[TMP,HPM]");
	 END;
      END; ⊃;

   DEFINE MAKE_FEATURE_FRAME(FILENAME)=
  ⊂! convert 3d location of world points to scene co-ords;
      BEGIN
      INTEGER J,JT,K,FJ;
      SETFORMAT(0,0);
      FJ←FILJOB("DSK:"&FILENAME&CVS(IPIC_LO-5)&".GOD[DIA,HPM]");
      DDINIT;
      SCREEN(0,1,1,0);
      PRSFIL(""); PRSFIL(IPIC_NAME); PRSFIL("."&CVS(IPIC_LO-5));
      PRSFIL("U:[DIA,HPM]");
      PICFIL(0,1,1,0,DEVPRS&":"&FILPRS);
      LITEN;
      LINE(0,0,0,1,5); LINE(1,1,0,1,5);
      LINE(1,1,1,0,5); LINE(0,0,1,0,5);
      K←5;
         BEGIN
	 REAL DX;
	 DX←(K-(SCAN_NPICS+1)/2)*SCAN_STEPSIZE/1200;
	 FOR JT←1 STEP 1 UNTIL FEAT_WORLD_N DO
	    BEGIN
	    REAL FX,FY,FZ,FTX,FTY,FTZ,H,V;
	    J←FEAT_WORLD[JT];
	    FTX←FEAT_X[J]-CART_XC; FTY←FEAT_Y[J]-CART_YC; FTZ←FEAT_Z[J]-CART_ZC;
	    FX←CART_AA*FTX+CART_AB*FTY+CART_AC*FTZ;
	    FY←CART_BA*FTX+CART_BB*FTY+CART_BC*FTZ;
	    FZ←CART_CA*FTX+CART_CB*FTY+CART_CC*FTZ;
	    IF FZ>0∧ABS((FX-DX)/FZ)≤1.5∧ABS(FY/FZ)≤1.5 THEN
 	       BEGIN
	       REAL XO,YO,X,Y;
	       XO←(FX-DX)/FZ; YO←FY/FZ;
	       DISREM (1,-.01,CAL_IG,XO,YO,X,Y);
               FNTPOS(X,Y);
               DRKEN;
               FNTLIN(-15,-15,-15, 15,7);  FNTLIN( 15, 15,-15, 15,7);
               FNTLIN( 15, 15, 15,-15,7);  FNTLIN(-15,-15, 15,-15,7);
               LITEN;
               FNTLIN(-15,-15,-15, 15,3);  FNTLIN( 15, 15,-15, 15,3);
               FNTLIN( 15, 15, 15,-15,3);  FNTLIN(-15,-15, 15,-15,3);
	       END;
            END;
	 END;
      DPYUP(-1);
      KILJOB(FJ);
      END; ⊃;