| 1 | ! Routines for the survey command in MADX / A. Verdier (started October 2001) |
|---|
| 2 | subroutine survey |
|---|
| 3 | |
|---|
| 4 | |
|---|
| 5 | |
|---|
| 6 | implicit none |
|---|
| 7 | !----------------------------------------------------------------------* |
|---|
| 8 | ! Purpose: * |
|---|
| 9 | ! Execute SURVEY command. * |
|---|
| 10 | ! Attributes, must be given in this order in the dictionary: * |
|---|
| 11 | ! X0 (real) Initial X position. * |
|---|
| 12 | ! Y0 (real) Initial Y position. * |
|---|
| 13 | ! Z0 (real) Initial Z position. * |
|---|
| 14 | ! THETA0 (real) Initial azimuthal angle. * |
|---|
| 15 | ! PHI0 (real) Initial elevation angle. * |
|---|
| 16 | ! PSI0 (real) Initial roll angle. * |
|---|
| 17 | !----------------------------------------------------------------------* |
|---|
| 18 | ! Modified: 01-APR-1999, M. Woodley (SLAC) * |
|---|
| 19 | ! If we're doing tape file output and there are LCAVITY elements in * |
|---|
| 20 | ! the current beamline, initialize ENER1 (in COMMON /OPTIC1/) using * |
|---|
| 21 | ! ENERGY from BEAM common, and call TMLCAV for each one to update * |
|---|
| 22 | ! ENERGY * |
|---|
| 23 | !----------------------------------------------------------------------* |
|---|
| 24 | |
|---|
| 25 | integer i,j,code,restart_sequ,advance_node,add_pass,passes,n_add_angle |
|---|
| 26 | integer angle_count, node_count, node_ref(100),set_cont_sequence |
|---|
| 27 | double precision dphi,dpsi,dtheta,phi,phi0,proxim,psi,psi0,sums, & |
|---|
| 28 | theta,theta0,v(3),v0(3),ve(3),w(3,3),w0(3,3),we(3,3),tx(3), & |
|---|
| 29 | node_value,el,suml,get_value,tilt,globaltilt,zero,add_angle(10), & |
|---|
| 30 | org_ang(100) |
|---|
| 31 | parameter(zero=0d0) |
|---|
| 32 | |
|---|
| 33 | !---- Retrieve command attributes. |
|---|
| 34 | v0(1)= get_value('survey ','x0 ') |
|---|
| 35 | v0(2)= get_value('survey ','y0 ') |
|---|
| 36 | v0(3)= get_value('survey ','z0 ') |
|---|
| 37 | theta0 = get_value('survey ','theta0 ') |
|---|
| 38 | phi0 = get_value('survey ','phi0 ') |
|---|
| 39 | psi0 = get_value('survey ','psi0 ') |
|---|
| 40 | |
|---|
| 41 | !---- Initialise the angles |
|---|
| 42 | theta = theta0 |
|---|
| 43 | phi = phi0 |
|---|
| 44 | psi = psi0 |
|---|
| 45 | |
|---|
| 46 | !---- Set up initial V and W. |
|---|
| 47 | suml = zero |
|---|
| 48 | sums = zero |
|---|
| 49 | call sumtrx(theta0, phi0, psi0, w0) |
|---|
| 50 | ! theta = theta0 |
|---|
| 51 | ! phi = phi0 |
|---|
| 52 | ! psi = psi0 |
|---|
| 53 | |
|---|
| 54 | |
|---|
| 55 | !---- (replaces SUCOPY) |
|---|
| 56 | do j = 1, 3 |
|---|
| 57 | v(j) = v0(j) |
|---|
| 58 | do i = 1, 3 |
|---|
| 59 | w(i,j) = w0(i,j) |
|---|
| 60 | enddo |
|---|
| 61 | enddo |
|---|
| 62 | 5 continue |
|---|
| 63 | |
|---|
| 64 | !---- loop over elements NO SYMMETRIC SUPERPERIOD ANYMORE! ******* |
|---|
| 65 | ! print *,"suml length theta(x) phi(y) psi(z) coord." |
|---|
| 66 | add_pass = get_value('sequence ','add_pass ') |
|---|
| 67 | ! multiple passes allowed |
|---|
| 68 | do passes = 0, add_pass |
|---|
| 69 | j = restart_sequ() |
|---|
| 70 | angle_count = 0 |
|---|
| 71 | node_count = 0 |
|---|
| 72 | 10 continue |
|---|
| 73 | node_count = node_count + 1 |
|---|
| 74 | if (passes .gt. 0) then |
|---|
| 75 | call get_node_vector('add_angle ',n_add_angle,add_angle) |
|---|
| 76 | if (n_add_angle .gt. 0 .and. add_angle(passes) .ne. 0.) then |
|---|
| 77 | if (passes .eq. 1) then |
|---|
| 78 | angle_count = angle_count + 1 |
|---|
| 79 | node_ref(angle_count) = node_count |
|---|
| 80 | org_ang(angle_count) = node_value('angle ') |
|---|
| 81 | endif |
|---|
| 82 | call store_node_value('angle ', add_angle(passes)) |
|---|
| 83 | endif |
|---|
| 84 | endif |
|---|
| 85 | code = node_value('mad8_type ') |
|---|
| 86 | if(code.eq.39) code=15 |
|---|
| 87 | if(code.eq.38) code=24 |
|---|
| 88 | ! print *,"code ", code |
|---|
| 89 | !**** el is the arc length for all bends ******** |
|---|
| 90 | el = node_value('l ') |
|---|
| 91 | call suelem(el, ve, we,tilt) |
|---|
| 92 | ! print *,"el, tilt", el, tilt |
|---|
| 93 | suml = suml + el |
|---|
| 94 | !** Compute the coordinates at each point |
|---|
| 95 | call sutrak(v, w, ve, we) |
|---|
| 96 | !** Compute globaltilt HERE : it's the value at the entrance |
|---|
| 97 | globaltilt=psi+tilt |
|---|
| 98 | !** Compute the survey angles at each point |
|---|
| 99 | call suangl(w, theta, phi, psi) |
|---|
| 100 | !** Fill the survey table |
|---|
| 101 | call sufill(suml,v, theta, phi, psi,globaltilt) |
|---|
| 102 | if (advance_node().ne.0) goto 10 |
|---|
| 103 | !---- end of loop over elements *********************************** |
|---|
| 104 | enddo |
|---|
| 105 | ! restore original angle to node if necessary |
|---|
| 106 | if (add_pass .gt. 0) then |
|---|
| 107 | j = restart_sequ() |
|---|
| 108 | angle_count = 1 |
|---|
| 109 | node_count = 0 |
|---|
| 110 | 20 continue |
|---|
| 111 | node_count = node_count+1 |
|---|
| 112 | if (node_ref(angle_count) .eq. node_count) then |
|---|
| 113 | call store_node_value('angle ', org_ang(angle_count)) |
|---|
| 114 | angle_count = angle_count+1 |
|---|
| 115 | endif |
|---|
| 116 | if (advance_node().ne.0) goto 20 |
|---|
| 117 | endif |
|---|
| 118 | if (set_cont_sequence() .ne. 0) goto 5 |
|---|
| 119 | |
|---|
| 120 | !---- Centre of machine. |
|---|
| 121 | do i = 1, 3 |
|---|
| 122 | tx(i) = v(i) - v0(i) |
|---|
| 123 | enddo |
|---|
| 124 | dtheta = theta - proxim(theta0, theta) |
|---|
| 125 | dphi = phi - proxim(phi0, phi) |
|---|
| 126 | dpsi = psi - proxim(psi0, psi) |
|---|
| 127 | ! print *,v, theta, phi, psi |
|---|
| 128 | end subroutine survey |
|---|
| 129 | !----------------- end of survey subroutine ------------------------- |
|---|
| 130 | |
|---|
| 131 | !*********************************************************************** |
|---|
| 132 | ! Subroutines necessary : suangl sutrak suelem |
|---|
| 133 | !********************************************************************** |
|---|
| 134 | |
|---|
| 135 | subroutine suangl(w, theta, phi, psi) |
|---|
| 136 | implicit none |
|---|
| 137 | !----------------------------------------------------------------------* |
|---|
| 138 | ! Purpose: * |
|---|
| 139 | ! Given a rotation matrix, compute the survey angles. * |
|---|
| 140 | ! Input: * |
|---|
| 141 | ! W(3,3) (real) Rotation matrix. * |
|---|
| 142 | ! Output: * |
|---|
| 143 | ! THETA (real) Azimuthal angle. * |
|---|
| 144 | ! PHI (real) Elevation angle. * |
|---|
| 145 | ! PSI (real) Roll angle. * |
|---|
| 146 | !----------------------------------------------------------------------* |
|---|
| 147 | double precision arg,theta,phi,psi,w(3,3),proxim |
|---|
| 148 | |
|---|
| 149 | arg = sqrt(w(2,1)**2 + w(2,2)**2) |
|---|
| 150 | phi = atan2(w(2,3), arg) |
|---|
| 151 | ! print *,"SUANGL: phi =",phi," arg=",arg," w23 =",w(2,3), |
|---|
| 152 | ! " w22 =",w(2,2)," w21 =",w(2,1) |
|---|
| 153 | |
|---|
| 154 | !***** old procedure commented as incompatiblr with YROT |
|---|
| 155 | ! if (arg .gt. 1.0e-20) then |
|---|
| 156 | theta = proxim(atan2(w(1,3), w(3,3)), theta) |
|---|
| 157 | psi = proxim(atan2(w(2,1), w(2,2)), psi) |
|---|
| 158 | ! else |
|---|
| 159 | ! theta = atan2(w(1,3), w(3,3)) |
|---|
| 160 | ! psi = proxim(atan2(-w(1,2), w(1,1))-theta, psi) |
|---|
| 161 | ! endif |
|---|
| 162 | end subroutine suangl |
|---|
| 163 | !----------------- end of suangl subroutine ------------------------- |
|---|
| 164 | ! |
|---|
| 165 | !********************************************************************** |
|---|
| 166 | |
|---|
| 167 | |
|---|
| 168 | subroutine sutrak(v, w, ve, we) |
|---|
| 169 | implicit none |
|---|
| 170 | !----------------------------------------------------------------------* |
|---|
| 171 | ! Purpose: * |
|---|
| 172 | ! Update global position. * |
|---|
| 173 | ! Input: * |
|---|
| 174 | ! V(3) (real) Global displacement before element. * |
|---|
| 175 | ! W(3,3) (real) Global rotation matrix before element. * |
|---|
| 176 | ! VE(3) (real) Displacement due to element. * |
|---|
| 177 | ! WE(3,3) (real) Rotation due to element. * |
|---|
| 178 | ! Output: * |
|---|
| 179 | ! V(3) (real) Global displacement after element. * |
|---|
| 180 | ! W(3,3) (real) Global rotation matrix after element. * |
|---|
| 181 | !----------------------------------------------------------------------* |
|---|
| 182 | integer i |
|---|
| 183 | double precision v(3),ve(3),w(3,3),we(3,3),wt1,wt2,wt3 |
|---|
| 184 | |
|---|
| 185 | do i = 1, 3 |
|---|
| 186 | v(i) = v(i) + w(i,1)*ve(1) + w(i,2)*ve(2) + w(i,3)*ve(3) |
|---|
| 187 | wt1 = w(i,1)*we(1,1) + w(i,2)*we(2,1) + w(i,3)*we(3,1) |
|---|
| 188 | wt2 = w(i,1)*we(1,2) + w(i,2)*we(2,2) + w(i,3)*we(3,2) |
|---|
| 189 | wt3 = w(i,1)*we(1,3) + w(i,2)*we(2,3) + w(i,3)*we(3,3) |
|---|
| 190 | w(i,1) = wt1 |
|---|
| 191 | w(i,2) = wt2 |
|---|
| 192 | w(i,3) = wt3 |
|---|
| 193 | enddo |
|---|
| 194 | end subroutine sutrak |
|---|
| 195 | !----------------- end of sutrak subroutine -------------------------- |
|---|
| 196 | ! |
|---|
| 197 | |
|---|
| 198 | !********************************************************************** |
|---|
| 199 | subroutine sufill(suml,v, theta, phi, psi,globaltilt) |
|---|
| 200 | |
|---|
| 201 | |
|---|
| 202 | use twtrrfi |
|---|
| 203 | implicit none |
|---|
| 204 | !----------------------------------------------------------------------* |
|---|
| 205 | ! Purpose: * |
|---|
| 206 | ! writes the survey output in the table "survey" * |
|---|
| 207 | ! Output: * |
|---|
| 208 | ! EL (real) Element length along design orbit. * |
|---|
| 209 | ! V(3) (real) Coordinate at the end of the element * |
|---|
| 210 | ! theta, phi, psi(real) : the survey angles * |
|---|
| 211 | !----------------------------------------------------------------------* |
|---|
| 212 | integer code,nn, i |
|---|
| 213 | double precision ang,el,v(3),theta,phi,psi,node_value,suml, & |
|---|
| 214 | normal(0:maxmul),globaltilt,tmp, surv_vect(7) |
|---|
| 215 | |
|---|
| 216 | el = node_value('l ') |
|---|
| 217 | call string_to_table_curr('survey ', 'name ', 'name ') |
|---|
| 218 | call double_to_table_curr('survey ', 's ',suml ) |
|---|
| 219 | call double_to_table_curr('survey ', 'l ',el ) |
|---|
| 220 | call double_to_table_curr('survey ', 'x ',v(1) ) |
|---|
| 221 | call double_to_table_curr('survey ', 'y ',v(2) ) |
|---|
| 222 | call double_to_table_curr('survey ', 'z ',v(3) ) |
|---|
| 223 | call double_to_table_curr('survey ', 'theta ',theta) |
|---|
| 224 | call double_to_table_curr('survey ', 'phi ',phi) |
|---|
| 225 | call double_to_table_curr('survey ', 'psi ',psi) |
|---|
| 226 | call double_to_table_curr('survey ', 'globaltilt ',globaltilt) |
|---|
| 227 | i = node_value('pass_flag ') |
|---|
| 228 | if (i .eq. 0) then |
|---|
| 229 | surv_vect(1) = v(1) |
|---|
| 230 | surv_vect(2) = v(2) |
|---|
| 231 | surv_vect(3) = v(3) |
|---|
| 232 | surv_vect(4) = theta |
|---|
| 233 | surv_vect(5) = phi |
|---|
| 234 | surv_vect(6) = psi |
|---|
| 235 | surv_vect(7) = suml |
|---|
| 236 | tmp = 1 |
|---|
| 237 | call store_node_value('pass_flag ', tmp) |
|---|
| 238 | i = 7 |
|---|
| 239 | call store_node_vector('surv_data ', i, surv_vect) |
|---|
| 240 | endif |
|---|
| 241 | code = node_value('mad8_type ') |
|---|
| 242 | if(code.eq.39) code=15 |
|---|
| 243 | if(code.eq.38) code=24 |
|---|
| 244 | if(code.eq.2.or.code.eq.3) then |
|---|
| 245 | ang = node_value('angle ')*node_value('other_bv ') |
|---|
| 246 | else if(code.eq.8) then |
|---|
| 247 | call get_node_vector('knl ',nn,normal) |
|---|
| 248 | if (nn .ne. 0) then |
|---|
| 249 | ang = normal(0) |
|---|
| 250 | else |
|---|
| 251 | ang = 0d0 |
|---|
| 252 | endif |
|---|
| 253 | else |
|---|
| 254 | ang = 0d0 |
|---|
| 255 | endif |
|---|
| 256 | call double_to_table_curr('survey ', 'angle ',ang) |
|---|
| 257 | |
|---|
| 258 | !---------------- -------------------------- |
|---|
| 259 | ! copy over the attributes 'mech_sep' and 'assembly_id' |
|---|
| 260 | ! FT 06.06.2008 |
|---|
| 261 | |
|---|
| 262 | tmp = node_value('slot_id ') |
|---|
| 263 | call double_to_table_curr('survey ', 'slot_id ',tmp) |
|---|
| 264 | |
|---|
| 265 | tmp = node_value('assembly_id ') |
|---|
| 266 | call double_to_table_curr('survey ', 'assembly_id ',tmp) |
|---|
| 267 | |
|---|
| 268 | tmp = node_value('mech_sep ') |
|---|
| 269 | call double_to_table_curr('survey ', 'mech_sep ',tmp) |
|---|
| 270 | |
|---|
| 271 | !== jln dealt with the new property v_pos as for mech_sep |
|---|
| 272 | tmp = node_value('v_pos ') |
|---|
| 273 | call double_to_table_curr('survey ', 'v_pos ',tmp) |
|---|
| 274 | !== |
|---|
| 275 | |
|---|
| 276 | call augment_count('survey ') |
|---|
| 277 | end subroutine sufill |
|---|
| 278 | !----------------- end of sufill subroutine -------------------------- |
|---|
| 279 | subroutine survtest |
|---|
| 280 | integer j, length, advance_node |
|---|
| 281 | double precision vector(7) |
|---|
| 282 | integer, external :: restart_sequ |
|---|
| 283 | ! test routine for USE with option SURVEY |
|---|
| 284 | call set_sequence('combseq ') |
|---|
| 285 | print *, "# sequence combseq" |
|---|
| 286 | j = restart_sequ() |
|---|
| 287 | 10 continue |
|---|
| 288 | call get_node_vector('surv_data ', length, vector) |
|---|
| 289 | print *, vector |
|---|
| 290 | if (advance_node() .ne. 0) goto 10 |
|---|
| 291 | call set_sequence('new3 ') |
|---|
| 292 | print *, "# sequence newcont" |
|---|
| 293 | j = restart_sequ() |
|---|
| 294 | 20 continue |
|---|
| 295 | call get_node_vector('surv_data ', length, vector) |
|---|
| 296 | print *, vector |
|---|
| 297 | if (advance_node() .ne. 0) goto 20 |
|---|
| 298 | end subroutine survtest |
|---|
| 299 | !----------------- end of surtest subroutine -------------------------- |
|---|
![(please configure the [header_logo] section in trac.ini)](/PSPA/chrome/site/pspa.gif)
