COMMENT ⊗   VALID 00009 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	BEGIN "TEST1"
C00004 00003	α IRON TRIANGLE - CAMERA LOCUS SOLVER
C00005 00004	ITG SUBR LSCAM (ITG I,K,J)
C00006 00005	REAL ARRAY XCAM,YCAM,ZCAM[1:40]		α CAMERA LOCUS SOLUTIONS
C00007 00006	α IF THE SOLUTION IS IN THE BALL PARK...
C00008 00007	SUBR CAMINIT
C00011 00008	SUBR TRANSFORM
C00013 00009	α MAIN LOOP
C00014 ENDMK
C⊗;
BEGIN "TEST1"
	REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;
	REQUIRE "DPYIII[SYS,BGB]" SOURCE_FILE;
	REQUIRE "SAITRG[SYS,BGB]" SOURCE_FILE;
	SAFE ITG ARRAY DPYBUF[0:999];

α CAMERA;
	REAL PAN,TILT,SWING;
	REAL CX,CY,CZ;
	REAL IX,IY,IZ;
	REAL JX,JY,JZ;
	REAL KX,KY,KZ;
	REAL SX,SY,FOCAL,MAG;	α RASTER SCALES;
	REAL LDX,LDY,PDX,PDY;	α LOGICAL AND PHYSICAL RASTERS;
	REAL ROWPA,COLPA;	α PRINCIPLE AXIS LOCUS IN PP;

α THREE INCH CUBE;

	PRELOAD_WITH 0,3,3,3,0,0;SAFE REAL ARRAY XWC[1:6];
	PRELOAD_WITH 0,0,0,3,3,3;SAFE REAL ARRAY YWC[1:6];
	PRELOAD_WITH 0,0,3,3,3,0;SAFE REAL ARRAY ZWC[1:6];
	PRELOAD_WITH '216,'176,'122,'105,'124,'200;SAFE ITG ARRAY ROW[1:6];
	PRELOAD_WITH '220,'247,'251,'210,'160,'160;SAFE ITG ARRAY COL[1:6];

	SAFE REAL ARRAY XPP,YPP,ZPP,XDC,YDC[1:6];
	SAFE REAL ARRAY XXPP,YYPP,XXDC,YYDC[1:6];
	SAFE REAL ARRAY XCC,YCC,ZCC[1:6];

α IRON TRIANGLE - CAMERA LOCUS SOLVER;

	REAL ARRAY P1,P2,P3,COSANG[1:3],V[1:10,1:3];
	REQUIRE "LS1V3P.REL" LOAD_MODULE;
	EXTERNAL ITG PROCEDURE LS1V3P(REAL ARRAY V,P1,P2,P3,COSANG);

REAL SUBR DOTVEC(ITG I,J);
BEGIN "DOTVEC"
	REAL X1,Y1,Z1,X2,Y2,Z2,R1,R2,ZCOS;
	X1 ← XCC[I]; Y1 ← YCC[I]; Z1 ← ZCC[I];
	X2 ← XCC[J]; Y2 ← YCC[J]; Z2 ← ZCC[J];
	R1 ← SQRT(X1*X1 + Y1*Y1 + Z1*Z1);
	R2 ← SQRT(X2*X2 + Y2*Y2 + Z2*Z2);
	ZCOS←(X1*X2 + Y1*Y2 + Z1*Z2)  /  (R1*R2);
	RETURN(ZCOS);
END "DOTVEC";

ITG SUBR LSCAM (ITG I,K,J);
BEGIN "LSCAM"
	ITG M,N; REAL C1,C2,C3;

α IRON TRIANGLE - KNOWN WORLD LOCI;
	P1[1]←XWC[I]; P2[1]←XWC[J]; P3[1]←XWC[K];
	P1[2]←YWC[I]; P2[2]←YWC[J]; P3[2]←YWC[K];
	P1[3]←ZWC[I]; P2[3]←ZWC[J]; P3[3]←ZWC[K];

α IRON TRIPOD - KNOW ANGLES BETWEEN  CAMERA RAYS;
	C1←COSANG[1] ← DOTVEC(J,K);
	C2←COSANG[2] ← DOTVEC(I,K);
	C3←COSANG[3] ← DOTVEC(I,J);
	IF ABS(C1-C2)≤1@-3 ∨ ABS(C2-C3)≤1@-3 ∨ ABS(C3-C1)≤1@-3 THEN RETURN(0);

α THROW THE SHIT AT THE FAN;
	M ← LS1V3P(V,P1,P2,P3,COSANG);
	RETURN(M);
END "LSCAM";
REAL ARRAY XCAM,YCAM,ZCAM[1:40];		α CAMERA LOCUS SOLUTIONS;
SUBR CAMLS1;
BEGIN "CAMLS1"
	REAL X,Y,Z,XSUM,YSUM,ZSUM,DEL;
	REAL XCCC,YCCC,ZCCC;
	ITG I,J,K,M,N,CNT;
	CNT ← 0;XSUM←YSUM←ZSUM←0;
	DPYSET(DPYBUF);
α FOR ALL COMBINATIONS OF THREE POINTS OF THE SIX POINTS;
	FOR I←1 TIL 4 DO
	FOR J←I+1 TIL 5 DO
	FOR K←J+1 TIL 6 DO
BEGIN "TRIANGLES"
	M ← LSCAM(I,J,K);
	FOR  N←1 TIL M DO
BEGIN "SOLUTIONS"
	X ← V[N,1];	Y ← V[N,2];	Z ← V[N,3];
	DEL ← SQRT((X-CX)↑2 + (Y-CY)↑2 + (Z-CZ)↑2);

α IF THE SOLUTION IS IN THE BALL PARK...;
	IF DEL≤ 12 THEN
BEGIN "GOOD"
	CNT ← CNT+1;
	XSUM←XSUM+X;	YSUM←YSUM+Y;	ZSUM←ZSUM+Z;
	XCAM[CNT]←X;	YCAM[CNT]←Y;	ZCAM[CNT]←Z;
	X ← X-CX;	Y ← Y-CY;	Z ← Z-CZ;
	XCCC ← X*IX + Y*IY + Z*IZ;
	YCCC ← X*JX + Y*JY + Z*JZ;
	ZCCC ← X*KX + Y*KY + Z*KZ;
	AIVECT(-200,0);AVECT(15*XCCC-200,15*YCCC);
	AIVECT(+200,0);AVECT(15*XCCC+200,15*ZCCC);
	DPYOUT(1);
	OUTSTR(9&CVG(XCCC)&9&CVG(YCCC)&9&CVG(ZCCC)&↓);INCHRW;
END "GOOD";
END "SOLUTIONS";
END "TRIANGLES";
	OUTSTR(9&CVG(XSUM/CNT)&9&CVG(YSUM/CNT)&9&CVG(ZSUM/CNT)&↓);
END "CAMLS1";
SUBR CAMINIT;
BEGIN "CAMINIT"
	REAL RR;
	REAL C_PAN,S_PAN,C_TILT,S_TILT,C_SWING,S_SWING;

	CX ← -28.5;	CY ← -19;	CZ ← +17.4;
	PAN ← -π/4;	TILT ← 60*π/180; MAG ← 32/9;

	FOCAL ← 25;
	PDX ← 10.6;	LDX ← 288;
	PDY ←  8.6;	LDY ← 216;

	SX ← -FOCAL*LDX/PDX;
	SY ← -FOCAL*LDY/PDY;

	C_PAN ← COS(PAN); S_PAN ← SIN(PAN);
	C_TILT ← COS(TILT); S_TILT ← SIN(TILT);
	C_SWING ← COS(SWING); S_SWING ← SIN(SWING);

	IX ←  C_PAN*C_SWING - S_PAN*C_TILT*S_SWING;
	IY ←  S_PAN*C_SWING + C_PAN*C_TILT*S_SWING;
	IZ ←  S_TILT*S_SWING;

	JX ← -C_PAN*S_SWING - S_PAN*C_TILT*C_SWING;
	JY ← -S_PAN*S_SWING + C_PAN*C_TILT*C_SWING;
	JZ ←  S_TILT*C_SWING;

	KX ←  S_PAN*S_TILT;
	KY ← -C_PAN*S_TILT;
	KZ ←       C_TILT;
END "CAMINIT";
SUBR TRANSFORM;
BEGIN "TRANSFORM"
	ITG I;
FOR I←1 TIL 6 DO
BEGIN
	REAL X,Y,Z;
α WC → CC WORLD LOCII;
	X ← XWC[I] - CX;Y ← YWC[I] - CY;Z ← ZWC[I] - CZ;
	XCC[I] ← X*IX + Y*IY + Z*IZ;
	YCC[I] ← X*JX + Y*JY + Z*JZ;
	ZCC[I] ← X*KX + Y*KY + Z*KZ;
α CC → PP;
 	XPP[I] ← SX * XCC[I] / ZCC[I];
	YPP[I] ← SY * YCC[I] / ZCC[I];
α PP → RC;
α	ROW[I] ← 108 - YPP[I];
α	COL[I] ← XPP[I] + 144;
α RC → PP;
α	XPP[I] ← COL[I] - 144;
α	YPP[I] ← 108 - ROW[I];
α PP → CC IMAGE PLANE LOCII;
	XCC[I] ← (COL[I]-COLPA)*PDX;
	YCC[I] ← (ROWPA-ROW[I])*PDY;
	ZCC[I] ← -FOCAL;
α PP → DC;
	XDC[I] ← MAG * XPP[I];
	YDC[I] ← MAG * YPP[I];
 	XXDC[I] ← MAG * (COL[I]-144);
 	YYDC[I] ← MAG * (108-ROW[I]);
END;
END "TRANSFORM";

SUBR SHOW;
BEGIN "SHOW"
	ITG I;
	DPYSET(DPYBUF);
	AIVECT(-511,-384);
	AVECT(+511,-384);
	AVECT(+511,+384);
	AVECT(-511,+384);
	AVECT(-511,-384);

	AIVECT(XDC[6],YDC[6]);
	FOR I←1 TIL 6 DO AVECT(XDC[I],YDC[I]);
	AIVECT(XXDC[6],YYDC[6]);
	FOR I←1 TIL 6 DO AVECT(XXDC[I],YYDC[I]);
	DPYOUT(1);
END "SHOW";

α MAIN LOOP;
	CAMINIT;
	TRANSFORM;
	SHOW;INCHRW;

	CAMLS1;

	WHILE TRUE DO IF INCHRW="X" THEN DONE;
END "TEST1";