*-- Author : R. Henderson 24/10/94
SUBROUTINE FPFSTS(ID1,ID2,ID3,ISM,PCHI,CHISQ,NDF,SOL,EXYSXY)
C------------------------------------------------------------------
C
C--- ESPECIALLY MODIFIED TO FIT SECONDARY AND TERTIARY SEGMENTS
C
C------------------------------------------------------------------
C
C--- THIS ROUTINE PERFORMS A LEAST SQUARES FIT TO A STRAIGHT
C--- LINE FOR POINTS AT POSITIONS Z(I) MEASURED IN THE Y U V COORDINATE
C--- FRAMES WHERE U COORDINATES ARE AT ANGLE THETA(2) AND V ARE AT
C--- ANGLE THETA(3) TO Y. THETA IS ASSUMED TO BE MEASURED IN THE
C--- CLOCKWISE DIRECTION.
C
C--- INPUT :
C
C--- Y - POINTS FOR FITTING; 4 FROM Y COORDS, 4 FROM U, 4 FROM V.
C--- RESOL - RESOLUTION ON EACH DIGITIZING (IN PRINCIPLE PER WIRE)
C--- ZPLAN(36) - Z COORDINATE OF EACH PLANAR WIRE SET
C
C--- OUTPUT :
C
C--- PCHI PROBABILITY FROM CHISQUARE
C--- SOL(4) X0 Y0 DX/DZ DY/DZ
C--- EXYSXY(4,4) COVARIANCE MATRIX TO SOL
C
C------------------------------------------------------------------
C---
*KEEP,FPPRAM.
C
C--- MAXSEG is maximum number of segments per supermodule
C--- MAXCON is maximum number of amibiguous segments associatable with
C--- one segment
C--- LIMSTO is maximum number of 2 cluster planes intersections to be
C--- stored per supermodule
C--- MSEGLM is maximum number of clusters that can be found before
C--- connectivity considered
C--- MAXCLU is maximum number of clusters that can be found after
C--- forming non-connected set MUST BE 50 IF RUN WITH OLD RCW
C--- (cluster = 3/4 digits found in a straight line in one
C--- 4-wire orientation)
C
PARAMETER (MAXSEG = 200)
PARAMETER (MAXCON = 100)
PARAMETER (LIMSTO = 5000)
PARAMETER (MSEGLM = 150)
PARAMETER (MAXCLU = 50)
C---
*KEND.
C---
*KEEP,FPLSEG.
C---
COMMON /FPLSEG / PW(12,MAXSEG,3) , PWC(12,MAXSEG,3) ,
1 PRCHI(MAXSEG,3) , NFSEG(3) ,
2 XYDXY(4,MAXSEG,3) , EXYDXY(4,4,MAXSEG,3) ,
3 ZSEG(2,MAXSEG,3) ,
4 ASEGIN(MAXSEG,3) , ISEGIN(5,MAXSEG,3) ,
5 MASKSG(MAXSEG,3) , IDGISG(12,MAXSEG,3)
C---
*KEND.
C---
*KEEP,FPLGEO.
C---
COMMON /FPLGEO/ ZPLAN(36) , TP(9) , YP(26) , PLANE(3,9),
1 RMAX , RMIN , YSTART , YSPACE ,
2 X0 , Y0 , PZSTRU (8), STAGER ,
3 RESOL , ACUT , CTP(9) , STP(9)
C---
*KEND.
C---
*KEEP,FPCLUS.
COMMON /FPCLUS/ TC(3,9,MAXCLU) , NTC(9) , TOC(3,9,MAXCLU) ,
2 TCYUV(4,9,MAXCLU), TCYUVW(4,9,MAXCLU)
C---
*KEND.
C---
*KEEP,FPH1WRK.
PARAMETER (MAXHTS=200)
PARAMETER (NUMWPL=36)
PARAMETER (MAXTRK=200)
PARAMETER (MXTTRK=900)
PARAMETER (MAXTR3=200)
PARAMETER (MAXHPW=2)
PARAMETER (MAXDIG=2000)
PARAMETER (NUMRWR=1727)
PARAMETER (NUMPWR=1151)
LOGICAL DRMASK
COMMON /H1WORK/
C-- *KEEP,FPCSEG.
C---
3 TPNORM(3,9,MAXCLU), PCONST(9,MAXCLU) ,
4 SMLS(4,2,LIMSTO,3) ,
C---
C-- *KEEP,FPDIGI.
5 DRSTO(MSEGLM,4),NDRSTO(4),
6 IDIGST(4,MSEGLM),
7 SEGTAB(MSEGLM,MSEGLM),DRMASK(MSEGLM,4),
8 IDCELL(MSEGLM,4),
9 NSGTAB(MSEGLM),ASGTAB(MSEGLM),
A RESSTO(MSEGLM,4) ,
C---
C-- *KEEP,FPDGI.
B IDGIST(MSEGLM,4),IDGISM(4,9,MAXCLU)
C ,RCHI(MAXSEG,3) ,
C---
C-- *KEEP,FPSTID.
D IDRSTO(MSEGLM,4),IDYUV(4,9,MSEGLM),
E IDYUVS(12,MAXSEG,3),FREQ(MSEGLM+1) ,
C---
C-- *interface to real data
F NDPW(NUMWPL),DW(MAXHTS,NUMWPL),DWG(MAXHTS,NUMWPL),
G DRIWP(MAXHTS,NUMWPL),DRIWM(MAXHTS,NUMWPL),
G DRIW(MAXHTS,NUMWPL),IPHOLE(MAXHTS,NUMWPL),
H IPFRPE(MAXHTS,36), IPPIOS(MAXDIG,2)
C---.
*KEEP,BOSMDL.
C ------BOSMDL
LOGICAL BEGJOB,ENDRUN,BEGRUN,REVENT,ENDJOB,OTHDAT
COMMON/BOSMDL/BEGJOB,ENDRUN,BEGRUN,REVENT,ENDJOB,OTHDAT,
+ LCCRUN,NCCRUN,NEVENT,
+ IHA,IBS,IDB,IDATEL,LUP,ISN,JSN
SAVE /BOSMDL/
C ------
*KEEP,FPSTSG.
COMMON/FPSTSG/ NSTC(9),NFSSEG(3),NFTSEG(3)
C---
*KEND.
C---
DIMENSION W(12),Z(12)
DIMENSION WSUM(3),WZSUM(3),WZ2SUM(3)
DIMENSION WYSUM(3),WYZSUM(3)
DIMENSION B(4)
DIMENSION WORK(16)
DIMENSION WTEST(4,4)
DIMENSION MTRIXA(4,4)
DIMENSION Y(12),YC(12)
DIMENSION SOL(4),EXYSXY(4,4)
DIMENSION XSOL(2),YSOL(2),ZSOL(2)
DIMENSION COST(3),COS2T(3),TANT(3),TAN2T(3),THETA(3)
DIMENSION XT(12),YT(12)
DIMENSION COUNTS(500),CHISQA(12)
C---
REAL MTRIXA
DOUBLE PRECISION EXYSXY,SOL,B
REAL CHISQ
C---
SAVE
C DATA THETA/ 0.0 , - 1.0471976 , 1.0471976/
DATA IFIRST/1/
C---
C
C--- INITIALIZATION PER SUPERMODULE
C
IF( IFIRST .EQ. 1 .OR. ISM .NE. ISMLAS )THEN
IFIRST = 0
ISMLAS = ISM
C
C--- ROTATE THETA SO THETA(1) = 0.0
C
TZERO = - TP( 1 + (ISM-1)*3 )
DO 43 I = 1,3
THETA(I) = - TP( I + (ISM-1)*3 ) - TZERO
43 CONTINUE
CTZERO = COS(TZERO)
STZERO = SIN(TZERO)
C
C--- SETUP GEOMETRICAL CONSTANTS
C
DO 200 I = 1,3
COST(I) = COS( THETA(I) )
COS2T(I) = COST(I)**2
TANT(I) = TAN( THETA(I) )
TAN2T(I) = TANT(I)**2
200 CONTINUE
ENDIF
C
C--- SETUP Z ARRAY APPROPRIATE TO CURRENT SM
C
DO 210 I = 1,4
DO 211 J = 1,3
Z(I + (J-1)*4 ) = ZPLAN( (((ISM-1)*3 + J) - 1)*4 + I )
211 CONTINUE
210 CONTINUE
C
C--- CENTRE OF PLANES
C
ZSUM = 0.0
DO 215 I = 1,12
ZSUM = ZSUM + Z(I)
215 CONTINUE
ZMEAN = ZSUM/12.0
DO 216 I = 1,12
Z(I) = Z(I) - ZMEAN
216 CONTINUE
C
C--- FIND Y U V FOR THE CURRENT SEGMENT
C
IPLAN1 = MOD(ID1,10)
IPLAN2 = MOD(ID2,10)
IPLAN3 = MOD(ID3,10)
ITRCK1 = ID1 / 10
ITRCK2 = ID2 / 10
ITRCK3 = ID3 / 10
C
C--- FIND OFSETS SUCH THAT Y ARRAY IS ON ORDER YUV
C
IND = MOD(IPLAN1,3)
IF (IND .EQ. 0) IND = 3
IOFF1 = 2**IND
IF (IOFF1 .EQ. 2) IOFF1 = 0
C---
IND = MOD(IPLAN2,3)
IF (IND .EQ. 0) IND = 3
IOFF2 = 2**IND
IF (IOFF2 .EQ. 2) IOFF2 = 0
C---
IND = MOD(IPLAN3,3)
IF (IND .EQ. 0) IND = 3
IOFF3 = 2**IND
IF (IOFF3 .EQ. 2) IOFF3 = 0
C---
DO 10 I = 1,4
C
C--- Unpack segment Y values identifiers
C
Y(I+IOFF1) = TCYUV(I,IPLAN1,ITRCK1)
Y(I+IOFF2) = TCYUV(I,IPLAN2,ITRCK2)
IF(ITRCK3 .NE. 0)THEN
Y(I+IOFF3) = TCYUV(I,IPLAN3,ITRCK3)
ELSE
Y(I+IOFF3) = 0.0
ENDIF
10 CONTINUE
C
C--- SET WEIGHT MATRIX
C
DO 20 I=1,4
W(I+IOFF1) = TCYUVW(I,IPLAN1,ITRCK1)
W(I+IOFF2) = TCYUVW(I,IPLAN2,ITRCK2)
IF(ITRCK3 .NE. 0)THEN
W(I+IOFF3) = TCYUVW(I,IPLAN3,ITRCK3)
ELSE
W(I+IOFF3) = 0.0
ENDIF
C---
IF(W(I+IOFF1) .NE. 0)THEN
W(I+IOFF1) = 44.0
ENDIF
IF(W(I+IOFF2) .NE. 0)THEN
W(I+IOFF2) = 44.0
ENDIF
IF(W(I+IOFF3) .NE. 0)THEN
W(I+IOFF3) = 44.0
ENDIF
20 CONTINUE
C
C--- COUNT NUMBER OF DIGITIZINGS CONTRIBUTING
C
NDIG = 0
DO 21 I = 1,12
IF(W(I) .EQ. 0.0)GO TO 21
NDIG = NDIG+1
21 CONTINUE
C
C--- ZERO SUMS
C
DO 30 IO = 1,3
WSUM(IO) = 0.0
WZSUM(IO) = 0.0
WZ2SUM(IO) = 0.0
WYSUM(IO)=0.0
WYZSUM(IO) = 0.0
30 CONTINUE
C
C--- LOOP OVER Z POSITIONS TO FORM SUMS
C
DO 40 IZ = 1,12
IO = ((IZ-1)/4) + 1
C---
WSUM(IO) = WSUM(IO) + W(IZ)
WZSUM(IO) = WZSUM(IO) + W(IZ) * Z(IZ)
WZ2SUM(IO) = WZ2SUM(IO) + W(IZ) * Z(IZ)**2
WYSUM(IO) = WYSUM(IO) + W(IZ) * Y(IZ)
WYZSUM(IO) = WYZSUM(IO) + W(IZ) * Y(IZ) * Z(IZ)
C---
40 CONTINUE
C
C--- SCALE TERMS 2,3 BY COS2T
C
DO 50 I =2,3
WSUM(I) = COS2T(I) * WSUM(I)
WZSUM(I) = COS2T(I) * WZSUM(I)
WZ2SUM(I) = COS2T(I) * WZ2SUM(I)
50 CONTINUE
C
C--- NOW FORM MTRIXA
C
MTRIXA(1,1) = TAN2T(2) * WSUM(2) + TAN2T(3) * WSUM(3)
MTRIXA(2,2) = WSUM(1) + WSUM(2) + WSUM(3)
MTRIXA(3,3) = TAN2T(2) * WZ2SUM(2) + TAN2T(3) * WZ2SUM(3)
MTRIXA(4,4) = WZ2SUM(1) + WZ2SUM(2) + WZ2SUM(3)
C---
MTRIXA(1,2) = TANT(2) * WSUM(2) + TANT(3) * WSUM(3)
MTRIXA(2,1) = MTRIXA(1,2)
MTRIXA(1,3) = TAN2T(2) * WZSUM(2) + TAN2T(3) * WZSUM(3)
MTRIXA(3,1) = MTRIXA(1,3)
MTRIXA(1,4) = TANT(2) * WZSUM(2) + TANT(3) * WZSUM(3)
MTRIXA(4,1) = MTRIXA(1,4)
C---
MTRIXA(2,3) = TANT(2) * WZSUM(2) + TANT(3) * WZSUM(3)
MTRIXA(3,2) = MTRIXA(2,3)
MTRIXA(2,4) = WZSUM(1) + WZSUM(2) + WZSUM(3)
MTRIXA(4,2) = MTRIXA(2,4)
C---
MTRIXA(3,4) = TANT(2) * WZ2SUM(2) + TANT(3) * WZ2SUM(3)
MTRIXA(4,3) = MTRIXA(3,4)
C
C--- NOW CALCULATE ERROR MATRIX FOR XY,SLOPE XY
C
DO 60 IR=1,4
DO 70IC=1,4
EXYSXY(IR,IC) = MTRIXA(IR,IC)
70 CONTINUE
60 CONTINUE
C---
CALL DINV(4,EXYSXY,4,WORK,IFAIL)
C---
IF(IFAIL .NE. 0) THEN
CALL ERRLOG(215,'W:FPFSTS: YUV fit failed')
RETURN
ENDIF
C
C--- CALCULATE VECTOR B
C
B(1) = TANT(2)*COST(2) * WYSUM(2) + TANT(3)*COST(3) * WYSUM(3)
B(2) = WYSUM(1) + COST(2) * WYSUM(2) + COST(3) * WYSUM(3)
B(3) = TANT(2)*COST(2) * WYZSUM(2) + TANT(3)*COST(3) * WYZSUM(3)
B(4) = WYZSUM(1) + COST(2) * WYZSUM(2) + COST(3) * WYZSUM(3)
C
C--- NOW SOLVE FOR X,Y,SX,SY
C
DO 90 IR =1,4
SOL(IR) = 0.0
90 CONTINUE
DO 100 IR = 1,4
DO 110 IC = 1,4
SOL(IR) = SOL(IR) + EXYSXY(IR,IC)*B(IC)
110 CONTINUE
100 CONTINUE
C
C--- PUT ZERO BACK TO Z=0 FROM Z=ZMEAN
C
SOL(1) = SOL(1) - SOL(3)*ZMEAN
SOL(2) = SOL(2) - SOL(4)*ZMEAN
DO 632 I =1,12
Z(I) = Z(I) + ZMEAN
632 CONTINUE
C--
CALL FPPPTZ(EXYSXY,-ZMEAN)
C
C--- NOW CALCULATE RESULTANT Y U V
C
DO 130 IZ = 1,4
YC(IZ) = SOL(2) + SOL(4)*Z(IZ)
YC(IZ+4) = COST(2) * ( (SOL(2) + SOL(4)*Z(IZ+4) ) +
1 TANT(2) * (SOL(1) + SOL(3)*Z(IZ+4) ) )
YC(IZ+8) = COST(3) * ( (SOL(2) + SOL(4)*Z(IZ+8) ) +
1 TANT(3) * (SOL(1) + SOL(3)*Z(IZ+8) ) )
130 CONTINUE
C
C--- CALCULATE CHISQUARE
C
CHISQ = 0.0
C
C---
C
DO 140 IZ=1,12
CHISQA(IZ) = (Y(IZ)-YC(IZ))**2*W(IZ)
CHISQ = CHISQ + (Y(IZ)-YC(IZ))**2*W(IZ)
140 CONTINUE
NDF = NDIG-4
* Fix for v. small Chisq...
IF(CHISQ .LT. 0.001) THEN
PCHI = 0.9999999
ELSE
PCHI = PROB( ABS(CHISQ),NDF )
ENDIF
C
C--- PLSEG ARRAY FILLED HERE
C
IF (NFSEG(ISM) .GE. MAXSEG) THEN
CALL ERRLOG(216,'W:FPFSTS: .GT. MAXSEG segments found')
RETURN
ELSE
C
C--- INCREMENT FOUND SEGMENT COUNTER
C
IF(PCHI .GT. 0.0)THEN
NFSEG(ISM) = NFSEG(ISM) + 1
IF(ITRCK3 .GT. 0)THEN
NFSSEG(ISM) = NFSSEG(ISM) + 1
ELSE
NFTSEG(ISM) = NFTSEG(ISM) + 1
ENDIF
ELSE
RETURN
ENDIF
C
C--- Store away digitization pointers for found segment
C
DO 305 I=1,4
IDGISG(I+IOFF1,NFSEG(ISM),ISM) = IDGISM(I,IPLAN1,ITRCK1)
IDGISG(I+IOFF2,NFSEG(ISM),ISM) = IDGISM(I,IPLAN2,ITRCK2)
IF(ITRCK3 .GT. 0)THEN
IDGISG(I+IOFF3,NFSEG(ISM),ISM) = IDGISM(I,IPLAN3,ITRCK3)
ELSE
IDGISG(I+IOFF3,NFSEG(ISM),ISM) = 0
ENDIF
305 CONTINUE
C
C---
DO 300 I = 1,12
C
C--- PREPARE OUTPUT BANKS
C
PW(I,NFSEG(ISM),ISM) = Y(I)
PWC(I,NFSEG(ISM),ISM) = YC(I)
300 CONTINUE
PRCHI(NFSEG(ISM),ISM) = PCHI
C
C--- ROTATE BACK FROM THETA(1) = 0.0 TO X Y FRAME
C
IF(TZERO .NE. 0.0)THEN
S1 = CTZERO*SOL(1) + STZERO*SOL(2)
S2 = - STZERO*SOL(1) + CTZERO*SOL(2)
S3 = CTZERO*SOL(3) + STZERO*SOL(4)
S4 = - STZERO*SOL(3) + CTZERO*SOL(4)
SOL(1) = S1
SOL(2) = S2
SOL(3) = S3
SOL(4) = S4
ENDIF
DO 301 I = 1,4
XYDXY(I,NFSEG(ISM),ISM) = SOL(I)
DO 302 J = 1,4
EXYDXY(I,J,NFSEG(ISM),ISM) = EXYSXY(I,J)
302 CONTINUE
301 CONTINUE
ZSEG(1,NFSEG(ISM),ISM) = Z(1)
ZSEG(2,NFSEG(ISM),ISM) = Z(12)
C
C--- Store segments information for FPSGRF
C
ISEGIN(1,NFSEG(ISM),ISM)=ID1
ISEGIN(2,NFSEG(ISM),ISM)=ID2
ISEGIN(3,NFSEG(ISM),ISM)=ID3
ISEGIN(4,NFSEG(ISM),ISM)=NDF
ASEGIN( NFSEG(ISM),ISM)=CHISQ
C---
C---
ENDIF
END
*