| 1 |
loizides |
1.1 |
c $Id:$
|
| 2 |
|
|
|
| 3 |
|
|
C VAX/DEC CMS REPLACEMENT HISTORY, Element DIUAPP.CDF
|
| 4 |
|
|
C *1 25-MAR-1992 13:06:59 SANSONI "calculates crude secondary vtx position"
|
| 5 |
|
|
C VAX/DEC CMS REPLACEMENT HISTORY, Element DIUAPP.CDF
|
| 6 |
|
|
SUBROUTINE DIUAPP(PAR0,XSV,YSV,ZSV,DLZ,IERR)
|
| 7 |
|
|
C ============================================
|
| 8 |
|
|
C
|
| 9 |
|
|
C Initial approximation to the secondary vertex position
|
| 10 |
|
|
C This subroutine intersects the two circles that are
|
| 11 |
|
|
C the projections of the helix trajectories of the
|
| 12 |
|
|
C tracks with the given input parameters. If the circles intersect,
|
| 13 |
|
|
C the program choses the one with the smaller z difference
|
| 14 |
|
|
C between the 2 helices. If the circles do not intersect
|
| 15 |
|
|
C it simply exits.
|
| 16 |
|
|
C
|
| 17 |
|
|
C Author: John Marriner
|
| 18 |
|
|
C Original creation: Feb 22, 1991
|
| 19 |
|
|
C
|
| 20 |
|
|
C===============================================================================
|
| 21 |
|
|
|
| 22 |
|
|
C $$IMPLICIT
|
| 23 |
|
|
|
| 24 |
|
|
INTEGER IERR,I
|
| 25 |
|
|
DOUBLE PRECISION R1, X1, Y1, R2, X2, Y2, DX, DY, S, Z1, Z2
|
| 26 |
|
|
DOUBLE PRECISION A, B, C, RAD, SRAD, XA, XB
|
| 27 |
|
|
DOUBLE PRECISION XSVI(2), YSVI(2), ZSVI(2), DZSVI(2)
|
| 28 |
|
|
REAL CKA, CKB, DZMAX, PI, TANG, XHAT, YHAT, PAR0(5,2)
|
| 29 |
|
|
REAL XSV, YSV, ZSV, DLZ
|
| 30 |
|
|
|
| 31 |
|
|
IERR = -1
|
| 32 |
|
|
XSV =0
|
| 33 |
|
|
YSV =0
|
| 34 |
|
|
ZSV =0
|
| 35 |
|
|
DLZ =0
|
| 36 |
|
|
|
| 37 |
|
|
DZMAX = 1.E+4 ! Maximum acceptable z difference for a solution
|
| 38 |
|
|
PI = 4.0*ATAN(1.0) ! Pi(the constant)
|
| 39 |
|
|
R1 = 1.0/(2.0*PAR0(2,1)) ! Radius of first track (with sign)
|
| 40 |
|
|
X1 = -(R1+PAR0(4,1))*SIN(PAR0(5,1)) ! Center (x) of the first track
|
| 41 |
|
|
Y1 = (R1+PAR0(4,1))*COS(PAR0(5,1)) ! Center (y) of the first track
|
| 42 |
|
|
R2 = 1.0/(2.0*PAR0(2,2)) ! Radius of the second track
|
| 43 |
|
|
X2 = -(R2+PAR0(4,2))*SIN(PAR0(5,2)) ! Center (x) of the second track
|
| 44 |
|
|
Y2 = (R2+PAR0(4,2))*COS(PAR0(5,2)) ! Center (y) of the second track
|
| 45 |
|
|
DX = X2 - X1 ! Center to center difference in x
|
| 46 |
|
|
DY = Y2 - Y1 ! Center to center difference in y
|
| 47 |
|
|
C
|
| 48 |
|
|
C Vertex y is the solution of A x y**2 + B x y + C = 0
|
| 49 |
|
|
C
|
| 50 |
|
|
A = 4.0*(DX**2+DY**2)
|
| 51 |
|
|
B = 4.0*DY*(R2*R2-R1*R1-DX*DX-DY*DY)
|
| 52 |
|
|
C = (R2*R2-R1*R1)**2 + (DX*DX+DY*DY)**2 -
|
| 53 |
|
|
- 2.0*DX*DX*(R2*R2+R1*R1) - 2.0*DY*DY*(R2*R2-R1*R1)
|
| 54 |
|
|
RAD = B*B - 4.0*A*C ! Argument of square root
|
| 55 |
|
|
|
| 56 |
|
|
C RAD<0 means circles don't intersect.It occurs for parallel trajectories only
|
| 57 |
|
|
C in which case the whole strategy of the approximation may not make sense.
|
| 58 |
|
|
|
| 59 |
|
|
IF (RAD .LT. 0.) RETURN
|
| 60 |
|
|
SRAD = DSQRT(RAD)
|
| 61 |
|
|
|
| 62 |
|
|
C Get y at circle crossings
|
| 63 |
|
|
YSVI(1) = (-B+SRAD)/(2.0*A)
|
| 64 |
|
|
YSVI(2) = (-B-SRAD)/(2.0*A)
|
| 65 |
|
|
|
| 66 |
|
|
C Loop over 2 potential vertices
|
| 67 |
|
|
DO 100 I=1,2
|
| 68 |
|
|
|
| 69 |
|
|
C The solution for X involves a SQRT with a sign ambiguity.
|
| 70 |
|
|
C The following code resolves the ambiguity
|
| 71 |
|
|
|
| 72 |
|
|
C XA = x if plus sign is correct
|
| 73 |
|
|
XA = DSQRT(R1*R1-YSVI(I)*YSVI(I)) + X1
|
| 74 |
|
|
C XB = x if minus sign is correct
|
| 75 |
|
|
XB = -DSQRT(R1*R1-YSVI(I)*YSVI(I)) + X1
|
| 76 |
|
|
C Translate Y to standard coordinates
|
| 77 |
|
|
YSVI(I) = YSVI(I) + Y1
|
| 78 |
|
|
C CKA = 0, if XA is correct
|
| 79 |
|
|
CKA = R2**2 - (XA-X2)**2 - (YSVI(I)-Y2)**2
|
| 80 |
|
|
C CKB = 0, if XB is correct
|
| 81 |
|
|
CKB = R2**2 - (XB-X2)**2 - (YSVI(I)-Y2)**2
|
| 82 |
|
|
C Choose vertex x according to whether CKA or CKB is smaller
|
| 83 |
|
|
IF (ABS(CKA).LT.ABS(CKB)) THEN
|
| 84 |
|
|
XSVI(I) = XA
|
| 85 |
|
|
ELSE
|
| 86 |
|
|
XSVI(I) = XB
|
| 87 |
|
|
ENDIF
|
| 88 |
|
|
|
| 89 |
|
|
100 CONTINUE ! Continue loop on Y solutions
|
| 90 |
|
|
C
|
| 91 |
|
|
C The following choses which of the two circle crossings in the "best"
|
| 92 |
|
|
C based on the z difference of the trajectories at that point.
|
| 93 |
|
|
C
|
| 94 |
|
|
C Loop over crossings
|
| 95 |
|
|
|
| 96 |
|
|
DO 200 I=1,2
|
| 97 |
|
|
|
| 98 |
|
|
DZSVI(I) = DZMAX ! Set z difference = big as a flag
|
| 99 |
|
|
|
| 100 |
|
|
C Xhat, Yhat = unit vector from circle center to vertex
|
| 101 |
|
|
XHAT = (XSVI(I) - X1)/R1
|
| 102 |
|
|
YHAT = (Y1 - YSVI(I))/R1
|
| 103 |
|
|
|
| 104 |
|
|
C Turning angle. Angular change from distance of closest
|
| 105 |
|
|
C approach (x,y=0) and vertex
|
| 106 |
|
|
TANG = ATAN2(XHAT,YHAT) - PAR0(5,1)
|
| 107 |
|
|
IF (TANG .GT. PI) TANG = TANG - 2.*PI ! Put -Pi < TANG < Pi
|
| 108 |
|
|
IF (TANG .LT. -PI) TANG = TANG + 2.*PI
|
| 109 |
|
|
C Don't allow turning angles beyond 81 degrees
|
| 110 |
|
|
IF (ABS(TANG) .GT. 0.45*PI) GO TO 200
|
| 111 |
|
|
|
| 112 |
|
|
S = R1*TANG ! Projected distance along the track
|
| 113 |
|
|
Z1 = PAR0(3,1) + S*PAR0(1,1) ! Track z at this vertex
|
| 114 |
|
|
C
|
| 115 |
|
|
C Xhat, Yhat = unit vector from circle center to vertex
|
| 116 |
|
|
XHAT = (XSVI(I) - X2)/R2
|
| 117 |
|
|
YHAT = (Y2 - YSVI(I))/R2
|
| 118 |
|
|
|
| 119 |
|
|
C Turning angle. Angular change from distance of closest
|
| 120 |
|
|
C approach (x,y=0) and vertex
|
| 121 |
|
|
TANG = ATAN2(XHAT,YHAT) - PAR0(5,2)
|
| 122 |
|
|
IF (TANG .GT. PI) TANG = TANG - 2.*PI ! Put -Pi < TANG < Pi
|
| 123 |
|
|
IF (TANG .LT. -PI) TANG = TANG + 2.*PI
|
| 124 |
|
|
C Don't allow turning angles beyond 81 degrees
|
| 125 |
|
|
IF (ABS(TANG) .GT. 0.45*PI) GO TO 200
|
| 126 |
|
|
|
| 127 |
|
|
S = R2*TANG ! Projected distance along the track
|
| 128 |
|
|
Z2 = PAR0(3,2) + S*PAR0(1,2) ! Track z at this vertex
|
| 129 |
|
|
|
| 130 |
|
|
C DZSVI = z difference between the two trajectories figure of merit
|
| 131 |
|
|
DZSVI(I) = Z2 - Z1
|
| 132 |
|
|
ZSVI(I) = 0.5*(Z2+Z1) ! Initial z = halfway between tracks
|
| 133 |
|
|
|
| 134 |
|
|
200 CONTINUE ! Continue loop on crossings
|
| 135 |
|
|
|
| 136 |
|
|
C Set I=best solution (smaller z difference)
|
| 137 |
|
|
IF (ABS(DZSVI(2)) .LT. ABS(DZSVI(1))) THEN
|
| 138 |
|
|
I = 2
|
| 139 |
|
|
ELSE
|
| 140 |
|
|
I = 1
|
| 141 |
|
|
ENDIF
|
| 142 |
|
|
|
| 143 |
|
|
C Done, if chosen solution is acceptable set IERR to 0
|
| 144 |
|
|
|
| 145 |
|
|
IF (ABS(DZSVI(I)) .LT. DZMAX) THEN
|
| 146 |
|
|
IERR=0
|
| 147 |
|
|
XSV = XSVI(I)
|
| 148 |
|
|
YSV = YSVI(I)
|
| 149 |
|
|
ZSV = ZSVI(I)
|
| 150 |
|
|
DLZ = DZSVI(I)
|
| 151 |
|
|
ELSE
|
| 152 |
|
|
IERR=-1
|
| 153 |
|
|
XSV = 0
|
| 154 |
|
|
YSV = 0
|
| 155 |
|
|
ZSV = 0
|
| 156 |
|
|
DLZ = 0
|
| 157 |
|
|
END IF
|
| 158 |
|
|
|
| 159 |
|
|
RETURN
|
| 160 |
|
|
|
| 161 |
|
|
END
|
