SUBROUTINE TFITXY(NHIT,IWIRE,TIME,LHIT,PAR,PRINT, & ERRPAR,CHIXY,XYPCA,DOCA,PHIVU,PHIDCA) C C This is a routine to do an r-phi fit to an array of drift-times. C C Basically this is just a copy of Paul Padley's TRACK_XY.CDF with C things reorganized a bit for the sake of modularity. This was C found desirable for example for determining the alignment. C C Author: C ======= C G.Redlinger C C History: C ======== C 06-May-93 (GR) Born C 12-Oct-93 (GR) Take B field value from utc_xy_pat.cmn C 19-Oct-93 (GR) Use DINV instead of DSINV. C Clean up drift angle mess. C 29-Nov-93 (GR) Added straight track fitting C 18-Mar-94 (GR) Return more information, concerning C residuals etc. C Also added the option of fitting to C a subset of the hits, calculating C residuals for all hits C 05-May-95 (GR) Update call to utc_posres C 14-Jun-97 (GR) Modified call to utc_posres C C Call parameters C =============== C NHIT (I*4) Number of hits C IWIRE (I*4) Array of wire numbers C TIME (R*4) Array of drift times (TDC counts) C LHIT (L*4) Array indicating whether or not to C use the hit in the fit C PAR (R*4) Array of approximate fit parameters: C rho,xcen,ycen C PRINT (L*4) .True. ==> diagnostic printing turned on C C Return parameters C ================= C PAR (R*4) Array of fit parameters C ERRPAR (R*4) Error matrix (not filled for now) C CHIXY (R*4) Chi-square/dof of the fit (set to -99 C if the track fit fails) C XYPCA (R*4) X and Y coords of the PCA of the track C to each wire in IWIRE C DOCA (R*4) Distance of closest approach to each wire C PHIVU (R*4) Angle between track and r-hat direction C for each wire C PHIDCA (R*4) Angle within the cell of the PCA for C each wire C C Global Declarations C =================== $$IMPLICIT $$INCLUDE 'KOFIA$INCLUDE:INFO.CMN' $$INCLUDE 'KOFIA$INCLUDE:LUNS.CMN' $$INCLUDE 'KOFIA$INCLUDE:UTC_GEOM.CMN' $$INCLUDE 'KOFIA$INCLUDE:UTC_WIRES.CMN' $$INCLUDE 'KOFIA$INCLUDE:UTC_XY_PAT.CMN' $$INCLUDE 'KOFIA$INCLUDE:UTC_XY_CUTS.CMN' C C Local Declarations C ================== INTEGER*4 NHIT,IWIRE(*) REAL*4 PAR(3),ERRPAR(6),CHIXY,TIME(*) REAL*4 XYPCA(2,*),DOCA(*),PHIVU(*),PHIDCA(*) LOGICAL*4 PRINT,LHIT(*) INTEGER*4 IFLAG,IRETRY,IBOX INTEGER*4 ICANTRY INTEGER*4 I,J,IG,JG,IPARM INTEGER*4 ICELL,ILAY,ISLAY,NLAYER INTEGER*4 IFAIL INTEGER*4 NUSED LOGICAL*4 CIRCLE REAL*4 PI,TWOPI,HALFPI,ROOT2 PARAMETER (PI=3.141592654) PARAMETER (TWOPI=2.0*PI) PARAMETER (HALFPI=PI/2.0) PARAMETER (ROOT2=1.4142136) REAL*4 PORIG(3) REAL*4 DELTAI(3,26) REAL*4 DRANG,DRIFT,DMAX REAL*4 CELLFRAC REAL*4 PXYFIT REAL*4 TRKPAR(3) REAL*8 XCC,YCC,RAD,P(3),PNEW(3),POLD(3),PMIN(3) REAL*8 CHIMIN,CHISQ_OLD,CHISQ REAL*8 G(3,3),Y(3),DERIV(3),GG(2,2) REAL*8 XS,YS,XL,YL REAL*8 DIST,DISTC,DCTOW REAL*8 SIGN REAL*8 RES,RESSQ REAL*8 TEMP,TMP REAL*8 DELTA(3) REAL*8 WORK(3,3) REAL*8 D0,PHI0,SINP,COSP C Functions C ========= REAL*4 UTC_POSRES REAL*4 DRDIST C C Data statements C =============== DATA ICANTRY/1/ DATA DELTAI/-1.,-1.,-1., -1.,-1.,1., -1.,1.,-1., 1.,-1.,-1., & 1.,1.,-1., 1.,-1.,1., -1.,1.,1., 1.,1.,1., 1.,1.,0., 1.,0.,1., & 0.,1.,1., 0.,0.,1., 0.,1.,0., 1.,0.,0., -1.,1.,0., -1.,0.,1., & -1.,0.,0., 1.,-1.,0., 0.,-1.,1., 0.,-1.,0., 1.,0.,-1., & 0.,1.,-1., 0.,0.,-1., -1.,-1.,0., -1.,0.,-1., & 0.,-1.,-1./ SAVE C C Start of executable code C ============================================================= C-- Initialize P(1) = DBLE(PAR(1)) P(2) = DBLE(PAR(2)) P(3) = DBLE(PAR(3)) IF(P(1).NE.0.0D0) THEN CIRCLE = .TRUE. ELSE CIRCLE = .FALSE. ENDIF IF(CIRCLE) THEN IF(NHIT.LT.4) THEN WRITE(LOUT,*) ' Less than 4 hits on the track. Bailing out' WRITE(LLOG,*) ' Less than 4 hits on the track. Bailing out' CHIXY = -99. RETURN ENDIF ELSE IF(NHIT.LT.3) THEN WRITE(LOUT,*) ' Less than 3 hits on the track. Bailing out' WRITE(LLOG,*) ' Less than 3 hits on the track. Bailing out' CHIXY = -99. RETURN ENDIF ENDIF PORIG(1) = PAR(1) PORIG(2) = PAR(2) PORIG(3) = PAR(3) IFLAG = 1 IRETRY = 1 IBOX = 0 CHIMIN = 999999999.0D0 CHISQ_OLD = 999999999.0D0 PMIN(1) = 0.D0 PMIN(2) = 0.D0 PMIN(3) = 0.D0 IF(PRINT) THEN IF(CIRCLE) THEN WRITE(LOUT,1001)NRUN,NEVT,P(1),P(2),P(3),NHIT,ICANTRY WRITE(LLOG,1001)NRUN,NEVT,P(1),P(2),P(3),NHIT,ICANTRY 1001 FORMAT(X,/,' TFITXY CALLED FOR RUN ',I10, & ' EVENT ',I10,/, & ' RAD,X,Y =',3E12.5,' WITH ',I2,' HITS',/, & ' TRY UP TO ', I3,' ITERATIONS ') ELSE WRITE(LOUT,1002) NRUN,NEVT,P(2),P(3),NHIT WRITE(LLOG,1002) NRUN,NEVT,P(2),P(3),NHIT 1002 FORMAT(X,/,' TFITXY CALLED FOR RUN ',I10, & ' EVENT ',I10,/, & ' D0,PHI0 =',2E12.5,' WITH ',I2,' HITS') ENDIF WRITE(LOUT,1003) WRITE(LLOG,1003) 1003 FORMAT(X,/,' Wire number Drift time Use hit?') WRITE(LOUT,1004) (IWIRE(I),TIME(I),LHIT(I),I=1,NHIT) WRITE(LLOG,1004) (IWIRE(I),TIME(I),LHIT(I),I=1,NHIT) 1004 FORMAT(X,I6,11X,F7.2,4X,L5) ENDIF C-- Initialize things for chisq and derivative matrix calculations 1 CHISQ = 0.D0 RAD = P(1) XCC = P(2) YCC = P(3) TRKPAR(1) = RAD TRKPAR(2) = XCC TRKPAR(3) = YCC IF(TRKPAR(1).EQ.0.) THEN D0 = P(2) PHI0 = P(3) SINP = SIN(PHI0) COSP = COS(PHI0) ENDIF IF(IFLAG.EQ.1)THEN DO I = 1,3 Y(I) = 0.0D0 DO J = 1,3 G(I,J) = 0.0D0 ENDDO ENDDO ENDIF NUSED = 0 C-- Loop over the hits forming the derivative matrix and other necesary C-- quantities. If iflag =1 then do full calculation, if iflag = 2 C-- then just calculate chisq DO 100 I = 1, NHIT ICELL = ICELL_NUM(IWIRE(I)) ILAY = ILAY_NUM(IWIRE(I)) ISLAY = ISLAY_NUM(IWIRE(I)) NLAYER = (ISLAY-1)/2 * NLAY_MAX + ILAY ! Runs from 1 to 12 XS = DBLE( XWPOS( ICELL, ILAY, ISLAY) ) YS = DBLE( YWPOS( ICELL, ILAY, ISLAY) ) C-- Distance of closest approach CALL CALCDCA(ICELL,ILAY,ISLAY, & TRKPAR,XYPCA(1,I),DOCA(I),PHIVU(I),PHIDCA(I)) DCTOW = DBLE(DOCA(I)) SIGN = DCTOW/ABS(DCTOW) DRANG = PHIDCA(I) C-- Drift distance DRIFT = DRDIST(TIME(I),DRANG,NLAYER) C-- Residual DIST = ABS(DCTOW) - DBLE(ABS(DRIFT)) C-- Calculate resolution CELLFRAC = DRIFT/WCELL(ILAY,ISLAY) RES = DBLE( UTC_POSRES(CELLFRAC,DRANG,IWIRE(I))) IF(RES.NE.0.0D0)THEN RESSQ = RES*RES ELSE RESSQ = 0.01D0 ! in this case use a large 0.1mm resolution ENDIF C-- Calculate chisq and derivatives only if we are including this C-- hit in the fit IF(.NOT.LHIT(I)) GO TO 100 CHISQ = CHISQ + DIST**2/RESSQ NUSED = NUSED+1 C-- Calculate the derivatives if iflag = 1 IF(IFLAG.EQ.1)THEN IF(TRKPAR(1).NE.0.) THEN XL = XS - XCC YL = YS - YCC DISTC = DSQRT(XL**2 + YL**2) DERIV(1) = SIGN DERIV(2) = SIGN*(XS - XCC)/DISTC DERIV(3) = SIGN*(YS - YCC)/DISTC C C-- Form the matrices G and Y that we will need, take care to do C-- in a time efficient way. Is it possible to optimize this step? C DO IG = 1,3 TEMP = DERIV(IG)/RESSQ Y(IG) = Y(IG) + TEMP*DIST DO JG = 1,3 G(JG,IG) = G(JG,IG) + TEMP*DERIV(JG) ENDDO ENDDO ELSE DERIV(2) = SIGN DERIV(3) = -SIGN*(XS*COSP+YS*SINP) DO IG=2,3 TEMP = DERIV(IG)/RESSQ Y(IG) = Y(IG)+TEMP*DIST DO JG=2,3 G(IG,JG) = G(IG,JG) + TEMP*DERIV(JG) END DO END DO ENDIF ENDIF 100 CONTINUE C-- Check number of hits again, this time counting only the layers C-- that were used in the fit IF(CIRCLE) THEN IF(NUSED.LT.4) THEN WRITE(LOUT,*) & ' Less than 4 USED hits on the track. Bailing out' WRITE(LLOG,*) & ' Less than 4 USED hits on the track. Bailing out' CHIXY = -99. RETURN ENDIF ELSE IF(NUSED.LT.3) THEN WRITE(LOUT,*) & ' Less than 3 USED hits on the track. Bailing out' WRITE(LLOG,*) & ' Less than 3 USED hits on the track. Bailing out' CHIXY = -99. RETURN ENDIF ENDIF C-- Now invert the matrix G. IFAIL = 0 IF(IFLAG.EQ.1)THEN IF(CIRCLE) THEN CALL DINV(3,G,3,WORK,IFAIL) IF(IFAIL.NE.0)THEN CALL KERROR(-3,0,'TFITXY','Could not invert matrix') CHIXY = -99. RETURN ENDIF C C The new values of the parameters are C DO I = 1, 3 TMP = 0.0D0 DO J = 1,3 TMP = TMP + G(I,J)*Y(J) ENDDO PNEW(I) = P(I) - TMP ENDDO ELSE GG(1,1) = G(2,2) GG(2,1) = G(3,2) GG(1,2) = G(2,3) GG(2,2) = G(3,3) CALL DINV(2,GG,2,WORK,IFAIL) IF(IFAIL.NE.0) THEN CALL KERROR(-3,0,'TRACK_XY','Could not invert matrix') CHIXY = -99. RETURN ENDIF DO I=1,2 TMP = 0.0D0 DO J = 1,2 TMP = TMP + GG(I,J)*Y(J+1) END DO PNEW(I+1) = P(I+1) - TMP END DO ENDIF ENDIF C--Check for convergence, DO I=1, 3 POLD(I) = P(I) P(I) = PNEW(I) ENDDO IF(CHISQ.LT.CHISQ_OLD.AND.ABS(CHISQ-CHISQ_OLD).GT.0.01D0)THEN CHISQ_OLD = CHISQ IF(IFLAG.NE.2)GO TO 1 ELSEIF(CHISQ.GT.CHISQ_OLD)THEN DO I=1, 3 P(I) = POLD(I) ENDDO ENDIF IF(CIRCLE) THEN CHIXY = REAL(CHISQ_OLD)/FLOAT(NUSED - 3) ELSE CHIXY = REAL(CHISQ_OLD)/FLOAT(NUSED - 2) ENDIF IF(PRINT) THEN WRITE(LOUT,400)CHIXY,P WRITE(LLOG,400)CHIXY,P ENDIF 400 FORMAT(' CHISQ=',F15.5,' P =',3F12.5) IF(CHISQ.LT.CHIMIN)THEN PMIN(1) = P(1) PMIN(2) = P(2) PMIN(3) = P(3) CHIMIN = CHISQ ENDIF C-- If at this point the chisq is lousy, then perhaps we have hit C-- a local minimum and so lets try stepping off it. C-- DON'T DO THIS FOR STRAIGHT TRACKS. Just calculate the chisq for C-- the best solution IF(.NOT.CIRCLE) THEN IF(IFLAG.EQ.1) THEN P(1) = PMIN(1) P(2) = PMIN(2) P(3) = PMIN(3) IFLAG = 2 GO TO 1 ELSE GO TO 7734 ENDIF ENDIF IBOX = IBOX + 1 IF(CHIXY.GT.CHIRETRY.AND.IRETRY.LE.ICANTRY.AND.IBOX.LE.26)THEN IF(PRINT) THEN WRITE(LOUT,4001)IBOX, IRETRY WRITE(LLOG,4001)IBOX, IRETRY ENDIF 4001 FORMAT(' BAD CHISQ, TRY TO STEP OFF LOCAL MIN. BOX', I3, & ' RETRY ',I3) DELTA(1) = DELTAI(1,IBOX) * 5.0 * FLOAT(IRETRY) DELTA(2) = DELTAI(2,IBOX) * 5.0 * FLOAT(IRETRY) DELTA(3) = DELTAI(3,IBOX) * 5.0 * FLOAT(IRETRY) P(1) = DBLE(PAR(1)) + DELTA(1) P(2) = DBLE(PAR(2)) + DELTA(2) P(3) = DBLE(PAR(3)) + DELTA(3) IF(PRINT) THEN WRITE(LOUT,4002)(DELTAI(IPARM,IBOX),IPARM=1,3),P WRITE(LLOG,4002)(DELTAI(IPARM,IBOX),IPARM=1,3),P ENDIF 4002 FORMAT(' TRY ',F4.1,1X,F4.1,1X,F4.1,3X,3F12.5) CHISQ_OLD = 999999999.0D0 IFLAG = 1 GO TO 1 ELSEIF(IBOX.GE.26.AND.IRETRY.LE.ICANTRY)THEN IRETRY = IRETRY + 1 IBOX = 0 PAR(1) = PMIN(1) PAR(2) = PMIN(2) PAR(3) = PMIN(3) IFLAG = 1 GO TO 1 ELSEIF(IFLAG.EQ.1)THEN C-- Calculate chisq for best solution P(1) = PMIN(1) P(2) = PMIN(2) P(3) = PMIN(3) IFLAG = 2 GO TO 1 ENDIF C-- Now have the current best possible result so save the results 7734 CONTINUE PAR(1) = REAL(PMIN(1)) ! final results PAR(2) = REAL(PMIN(2)) PAR(3) = REAL(PMIN(3)) PXYFIT = 0.29979 * UTBFLD * PAR(1) IF(PRINT) THEN IF(CIRCLE) THEN WRITE(LOUT,4050)CHIXY,PAR(1),PAR(2),PAR(3),PXYFIT,NUSED WRITE(LLOG,4050)CHIXY,PAR(1),PAR(2),PAR(3),PXYFIT,NUSED 4050 FORMAT(' CHIXY=',F7.3,' RADIUS=',F7.3,' XC=',F7.3, & ' YC=',F7.3,' PXY=',F7.3,' NPTS=',I3) ELSE WRITE(LOUT,4051) CHIXY,PAR(2),PAR(3),NUSED WRITE(LLOG,4051) CHIXY,PAR(2),PAR(3),NUSED 4051 FORMAT(' CHIXY=',F7.3,' D0= ',F7.3, & ' PHI0= ',F7.3,' NPTS= ',I3) ENDIF ENDIF 999 RETURN END