source: PSPA/madxPSPA/src/madx_ptc_eplacement.f90 @ 430

Last change on this file since 430 was 430, checked in by touze, 11 years ago

import madx-5.01.00
File size: 22.9 KB
Line 
1module madx_ptc_eplacement_module
2  !This module enables the user to PLACE an element in arbitrary position
3  use madx_keywords
4  use madx_ptc_intstate_module, only : getdebug
5  use madx_ptc_module, only: my_ring;
6  implicit none
7  save
8  private
9
10  !============================================================================================
11  !  PUBLIC INTERFACE
12  public                                      :: place_element
13  public                                      :: printlayout_rootm
14
15  !============================================================================================
16  !  PRIVATE
17  !    data structures
18  integer, parameter                          :: black  = 1
19  integer, parameter                          :: red    = 2
20  integer, parameter                          :: green  = 3
21  integer, parameter                          :: blue   = 4
22  integer, parameter                          :: yellow = 5
23  integer, parameter                          :: magenta= 6
24  integer, parameter                          :: cyan = 7
25  integer, parameter                          :: darkgreen = 8
26  integer, parameter                          :: violet = 9
27  integer, parameter                          :: color_n_sext = 46
28  integer, parameter                          :: color_s_sext = 30
29  integer, parameter                          :: dgrey = 14
30  integer, parameter                          :: grey = 16
31  integer, parameter                          :: lgrey = 18
32  integer, parameter                          :: color_of_ghost = 19
33
34  !    routines
35  private                                     :: rot
36  private                                     :: printfframes
37  !============================================================================================
38
39contains
40  !____________________________________________________________________________________________
41
42  subroutine place_element(elementidx,refframe)
43    implicit none
44    integer              :: elementidx
45    integer              :: refframe
46    integer              :: j, i
47    integer              :: restart_sequ,advance_node
48    real(kind(1d0))      :: get_value
49    type(fibre), pointer :: p
50    real(dp),target      :: a(3), ange(3)
51    real(dp)             :: phi, theta, eta
52    real(dp),target      :: idm(3,3),ent(3,3)
53    real(dp),pointer     :: base(:,:)
54    logical              :: onlyposition, onlyorientation, autoplace, surveyall
55
56    idm(:,:) = zero
57    idm(1,1) = one
58    idm(2,2) = one
59    idm(3,3) = one
60
61    a(:) = 0
62
63    j=restart_sequ()
64    j=0
65    p=>my_ring%start
66
67    elementidx = elementidx + 1
68    if (getdebug()>2) then
69       print*, "I am in placeelement: Element index is ", elementidx
70       print*, "refframe is", refframe
71    endif
72
73    if ( (elementidx .lt. 1) .and. (elementidx .gt. my_ring%n) ) then
74       call fort_warn("place_element","element out of range of the current layout")
75       return
76    endif
77
78    do
79       j=j+1
80
81       if (elementidx == j) then
82          exit
83       endif
84
85       if(advance_node().eq.0) then
86          return
87       endif
88
89       p=>p%next
90    enddo
91
92    if (getdebug() > 1 ) then
93       print*,"Found element no. ", elementidx," named ", p%mag%name, &
94            &" of kind ", p%mag%kind, mytype(p%mag%kind)
95    endif
96
97
98    onlyorientation = get_value('ptc_eplacement ','onlyorientation ') .ne. 0
99    if (onlyorientation .eqv. .false.) then
100       a(1) = get_value('ptc_eplacement ','x ')
101       a(2) = get_value('ptc_eplacement ','y ')
102       a(3) = get_value('ptc_eplacement ','z ')
103       if (getdebug() > 2 ) then
104          write(6,'(a, 3(f13.10,1x))') 'ptc_eplacement: Read position ',a
105       endif
106    endif
107
108    select case(refframe)
109    case(0)
110       if (getdebug() > 2) then
111          print*,"ptc_eplacement: Reference frame: Global Coordinate System"
112       endif
113       a = a - p%chart%f%a
114       base => idm
115    case(1)
116       !reference frame of the passed parameters is the current magnet position
117       if (getdebug() > 2) then
118          print*,"ptc_eplacement: Reference frame: the current magnet position"
119       endif
120
121       ent = p%chart%f%ent  !we need to make copy because it may be overwritten at the time of rotation
122       base => ent
123    case(2)
124       !reference frame of the passed parameters is the end face of the preceding magnet
125       if (getdebug() > 2) then
126          print*,"ptc_eplacement: Reference frame: the end face of the preceding magnet"
127       endif
128       base => P%previous%chart%f%exi
129    case default
130       refframe = 0
131       call fort_warn("ptc_eplacement","Such reference frame is not supported. Using global")
132       a = a - p%chart%f%a
133       base => idm
134    end select
135
136
137
138
139    onlyposition = get_value('ptc_eplacement ','onlyposition ') .ne. 0
140    if (onlyposition .eqv. .false.) then
141
142       phi   = get_value('ptc_eplacement ','phi ')
143       theta = get_value('ptc_eplacement ','theta ')
144       eta   = zero
145       if (getdebug() > 2 ) then
146          write(6,'(a20, 2f8.4)') 'Read rotations ',phi, theta
147       endif
148
149
150       if (refframe /=  1) then
151          !brings it to the ref system
152          CALL COMPUTE_ENTRANCE_ANGLE(p%chart%f%ent,base,ANGE)
153
154          if (getdebug() > 2 ) then
155             write(6,'(a, 3(f13.10,1x))') 'ptc_eplacement: R0 Computed entr angles ',ange
156          endif
157
158          CALL ROTATE_Fibre(p,p%chart%f%a,ange)
159
160       endif
161
162       ange(1) = theta
163       ange(2) = phi
164       ange(3) = eta
165
166       CALL ROTATE_Fibre(p,p%chart%f%a,ange,1,base)
167
168    endif
169
170    if (onlyorientation .eqv. .false.) then
171       if (refframe ==  2) then ! face translation of the moved one to the end of the previous one
172          CALL TRANSLATE_Fibre(p,p%previous%chart%f%b - p%chart%f%a)
173       endif
174       CALL TRANSLATE_Fibre(p,a,1,base)
175    endif
176
177
178    p%patch=-1
179    p%patch= 0
180    CALL FIND_PATCH(P%PREVIOUS,P,NEXT=my_true,ENERGY_PATCH=MY_FALSE)
181
182    autoplace = get_value('ptc_eplacement ','autoplacedownstream ') .ne. 0
183    if (autoplace) then
184       if (getdebug() > 2) then
185          print*,"ptc_eplacement: autoplacedownstream=true: running survey"
186       endif
187       call survey(my_ring,j+1,my_ring%n)
188    else
189       if (getdebug() > 2) then
190          print*,"ptc_eplacement: autoplacedownstream=false: finding patch"
191       endif
192       CALL FIND_PATCH(P,P%NEXT,NEXT=MY_FALSE,ENERGY_PATCH=MY_FALSE)
193    endif
194
195
196    j=j+1
197    p=>p%next
198
199    do i=j,my_ring%n-1
200
201       p%patch =-1
202       p%patch = 0
203       CALL FIND_PATCH(P,P%next,NEXT=MY_TRUE,ENERGY_PATCH=MY_FALSE)
204       P=>P%NEXT
205    ENDDO
206
207    surveyall = get_value('ptc_eplacement ','surveyall ') .ne. 0
208    if (surveyall) then
209       if (getdebug() > 2) then
210          print*,"ptc_eplacement: surveyall=true"
211       endif
212       call survey(my_ring)
213    endif
214
215    !    CALL FIND_PATCH(P,P%next,NEXT=my_false,ENERGY_PATCH=MY_FALSE)
216
217    !    print*, "Exiting placeelement"
218  end subroutine place_element
219
220
221  !____________________________________________________________________________________________
222  !____________________________________________________________________________________________
223  !____________________________________________________________________________________________
224
225  subroutine printlayout_rootm(filenameIA)
226    use twissafi
227    implicit none
228    integer   filenameIA(*)
229    integer       :: i  !iterator + tmp
230    integer       :: mf !macro file descriptor
231    integer       :: xmin, xmax, ymin, ymax
232    character(48) :: filename
233    character(48) :: fctname
234    TYPE(LAYOUT),pointer :: r
235    type(fibre), pointer :: p
236    real(dp)     :: a(3)
237    integer      :: nmul
238    character(48) charconv
239
240    r=>my_ring
241
242    filename = charconv(filenameIA)
243
244    !    print*, "I am in printlayout_rootm: Macro name is is ", filename
245
246    call kanalnummer(mf)
247    open(unit=mf,file=filename)
248
249    i=index(filename,'.C')
250    fctname = filename(1:i-1)
251
252    if (getdebug() > 2 ) then
253       print*, ".C found at ",i," function name is ", fctname
254    endif
255
256    write(mf,*) '#ifndef __MAKECINT__'
257    write(mf,*) ' #ifndef __CINT__'
258    write(mf,*) ''
259    write(mf,*) ' #include "TROOT.h"'
260    write(mf,*) ' #include "TCanvas.h"'
261    write(mf,*) ' #include "Riostream.h"'
262    write(mf,*) ''
263    write(mf,*) ' #include "TBRIK.h"'
264    write(mf,*) ' #include "TShape.h"'
265    write(mf,*) ' #include "TNode.h"'
266    write(mf,*) ' #include "TCanvas.h"'
267    write(mf,*) ' #include "TGLViewer.h"'
268    write(mf,*) ' #include "TPoints3DABC.h"'
269    write(mf,*) ' #include "TTUBE.h"'
270    write(mf,*) ' #include "TRotMatrix.h"'
271    write(mf,*) ''
272    write(mf,*) ' #endif'
273    write(mf,*) '#endif'
274    write(mf,*) ''
275    write(mf,*) ''
276    write(mf,*) "void ", fctname,'()'
277    write(mf,*) "{"
278    write(mf,*) 'TShape* s;'
279    write(mf,*) 'TNode* mn;'
280    write(mf,*) 'TNode* n;'
281    write(mf,*) 'Double_t rotmatrix[9];'
282    write(mf,*) 'TRotMatrix* m;'
283    write(mf,*) ''
284    write(mf,*) 'gSystem->Load("libGed");'
285    write(mf,*) 'gSystem->Load("libRGL");'
286    write(mf,*) ''
287    write(mf,*) 'TCanvas* c = new TCanvas("c","PTC Layout",10,10,800,600);'
288    write(mf,*) ''
289    write(mf,*) 's = new TBRIK("START","START","void",0.01,0.01,0.01);'
290    write(mf,*) 's->SetLineColor(2);'
291    write(mf,*) 'mn = new TNode("NODE1","NODE1","START");'
292    write(mf,*) 'mn->cd();'
293    write(mf,*) ''
294
295
296
297
298    xmin = 0
299    xmax = 0
300    ymin = 0
301    ymax = 0
302    p=>r%start
303    do i=1,r%n
304
305       if ( P%mag%p%f%a(1) .lt. xmin) xmin = P%mag%p%f%a(1)
306       if ( P%mag%p%f%b(1) .lt. xmin) xmin = P%mag%p%f%b(1)
307       if ( P%mag%p%f%a(3) .gt. ymax) ymax = P%mag%p%f%a(3)
308       if ( P%mag%p%f%b(3) .gt. ymax) ymax = P%mag%p%f%b(3)
309
310       P=>P%NEXT
311    enddo
312    xmin = xmin -1
313    ymin = ymin -1
314
315    xmax = xmax +1
316    ymax = ymax +1
317
318    write(mf,*) ''
319    write(mf,*) 'c->Range(', xmin, ',', ymin, ',', xmax, ',', ymax,');'
320    write(mf,*) ''
321!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
322    p=>r%start
323    do i=1,r%n
324       write(mf,*)
325       write(mf,*) '//cout<<',i,'<<" ',p%mag%name,'"<<endl;'
326
327       !print*, i,p%mag%name
328       if (getdebug() > 2) then
329          print*, i,p%mag%name,' of kind ',p%mag%kind
330          print*, 'Edges: ', p%mag%P%EDGE(1), p%mag%P%EDGE(2)
331       endif
332
333
334       if (p%mag%l == zero) then
335          !print*, i,p%mag%name(1:3), p%mag%kind
336          if(p%mag%name(1:3) == 'M__') then
337            call drawtube(p,mf,1.0_dp,lgrey)
338          endif
339
340          goto 100;
341       endif
342
343       select case(p%mag%kind)
344
345       case(kind1)
346          call drawtube(p,mf,0.05_dp,lgrey)
347
348       case(kind11) !monitor
349          call drawtube(p,mf,0.05_dp,magenta)
350
351       case(kind12) !H monitor
352          call drawtube(p,mf,0.05_dp,magenta)
353
354       case(kind13) !V monitor
355          call drawtube(p,mf,0.05_dp,magenta)
356
357       case(kind14) !instrument
358          call drawtube(p,mf,0.05_dp,magenta)
359
360       case(kind10) !SBEND
361          call drawsbend(p,mf)
362
363       case(32) !SBEND (Multipole) with model 1 ( Kick Drift Kick) and NON-exact
364          print*,"This is type 32"
365          call drawsbend(p,mf)
366
367       case(kind20) !stright exact bend
368          call drawboxm(p,mf,magenta)
369
370       case(kind15)!septum (electric)
371          call drawboxm(p,mf,cyan)
372
373       case(kind21) !TW CAV
374          call drawtube(p,mf,0.25_dp,yellow)
375
376       case(kind4) !TW CAV
377          call drawtube(p,mf,0.25_dp,yellow) !TW CAV
378
379       case(kind5) !solenoid
380          call drawtube(p,mf,0.25_dp,darkgreen)
381
382       case(kind16,kind7)
383          if ( p%mag%kind==kind16 .and. getdebug() > 3) then
384             print*, "KIND16: likemad is ", p%mag%k16%likemad
385             print*, "KIND16: bn(0) ", p%mag%bn(0), " bn(1)", p%mag%bn(1), " bn(2)", p%mag%bn(2)
386             print*, "KIND16: an(0) ", p%mag%an(0), " an(1)", p%mag%an(1), " an(2)", p%mag%an(2)
387          endif
388
389          nmul = p%mag%p%nmul
390          if (p%mag%bn(1) /= zero ) then
391             !BEND
392             a(1)= P%mag%p%f%ent(3,1)*p%mag%l/two + P%mag%p%f%a(1)
393             a(2)= P%mag%p%f%ent(3,2)*p%mag%l/two + P%mag%p%f%a(2)
394             a(3)= P%mag%p%f%ent(3,3)*p%mag%l/two + P%mag%p%f%a(3)
395             call drawbox(p,mf,P%mag%p%f%ent,a,blue)
396          elseif (nmul < 2)  then
397             !not powered element or very high order multipole
398             call drawboxm(p,mf,color_of_ghost)
399             cycle
400
401          elseif (   (p%mag%bn(2) /= zero) ) then
402             !QUAD
403             if (p%mag%bn(2) .gt. zero ) then
404                call drawboxm(p,mf,red)  !QUAD foc
405             else
406                call drawboxm(p,mf,green)!QUAD defoc
407             endif
408
409          elseif (nmul < 3)  then
410             !not powered element or very high order multipole
411             call drawboxm(p,mf,color_of_ghost)
412             goto 100
413
414          elseif ( (p%mag%bn(3) /= zero) .or. (p%mag%an(3) /= zero)  ) then
415             !SEXTUPOLE
416             call drawboxm(p,mf,color_n_sext)
417
418          elseif (nmul < 4)  then
419             !not powered element or very high order multipole
420             call drawboxm(p,mf,color_of_ghost)
421             goto 100
422          elseif ( (p%mag%bn(4) /= zero) .or. (p%mag%an(4) /= zero) ) then
423
424             !OCTUPOLE
425             print*,"OCTUPOLE ",p%mag%bn(4),p%mag%an(4)
426             call drawboxm(p,mf,color_s_sext)
427
428          else
429             !not powered element or very high order multipole
430             call drawboxm(p,mf,color_of_ghost)
431          endif
432
433
434       case default
435          print*, "################# "
436          print*, "################# "
437          print*, "Unrecognized kind ", p%mag%kind, mytype(p%mag%kind)
438          print*, "################# "
439          print*, "################# "
440       end select
441
442
443
444100    continue
445       P=>P%NEXT
446    enddo
447!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
448
449    write(mf,*) ''
450    write(mf,*) 'mn->Draw("ogl");'
451    write(mf,*) 'TGLViewer * v = (TGLViewer *)c->GetViewer3D();'
452
453    write(mf,*) ''
454    write(mf,*) "}"
455    close(mf)
456
457  contains
458
459    subroutine  drawsbend(p,mf)
460      implicit none
461      type(fibre), pointer :: p
462      integer      :: mf !macro file descriptor
463      integer      :: color = blue
464      character(10) :: fname
465      character(9) :: nodname
466      character(8) :: mtxname
467      real(dp)     :: x,y,z,r, phi
468      !To be finished - need more information about SBEND frames in PTC
469
470
471      if (P%mag%p%f%a(2) .ne. P%mag%p%f%b(2) ) then
472         print*, "Not able yet to draw horizonthally skewed SBEND. DRAWING AS RBEND"
473         call drawboxm(p,mf,color)
474         return
475      endif
476
477      x = P%mag%p%f%b(1) - P%mag%p%f%a(1)
478      y = P%mag%p%f%b(3) - P%mag%p%f%a(3)
479
480      z = (x*x + y*y)/four !square of the half of distance between ent and exi
481
482      x = P%mag%p%f%a(1) + x/two !x coord of the half of distance between ent and exi
483      y = P%mag%p%f%a(3) + y/two !z coord of the half of distance between ent and exi
484
485!!!!!!!!!!!!!!!!!!!
486
487      x = x - P%mag%p%f%o(1)
488      y = y - P%mag%p%f%o(3)
489
490      y = sqrt(x*x + y*y)
491
492      if (y == 0) then
493         print*,i,p%mag%name, "All three reference frames are inline. DRAWING AS RBEND"
494         call drawboxm(p,mf,color)
495         return
496      endif
497
498      r = (z + y*y)/(two*y)
499      if (r > 100000) then
500         print*,i,p%mag%name, "SBEND curvature is almost null. DRAWING AS RBEND"
501         call drawboxm(p,mf,color)
502         return
503      endif
504
505      phi = two*arcsin(z/r)
506      print*, "R is ", r," phi is ", phi," z is ", z," y ",y
507
508      write(fname,'(a5,i5.5)') 'SBEND',i
509      write(mf,*) 's = new TTUBS("',fname, '","',  fname,'","void",',&
510           &  r-0.25_dp,',',r+0.25_dp,',0.25,0,',phi,');'
511      write(mf,*) 's->SetLineColor(',color,');'
512
513      write(mtxname,'(a3,i5.5)') 'mtx',i
514      !      print*, fname, " ",mtxname
515
516      call setmatrix(P%mag%p%f%mid,mtxname,mf)
517
518      write(nodname,'(a4,i5.5)') 'NODE',i
519      write(mf,*) 'n = new TNode("',nodname,'","',  nodname,'",s,', &
520           & x,',',y, ',',z,',m);'
521
522    end subroutine  drawsbend
523
524    subroutine  drawbox(p,mf,m,a,color)
525      implicit none
526      type(fibre), pointer :: p
527      integer      :: mf !macro file descriptor
528      real(dp)     :: m(3,3)
529      real(dp)     :: a(3)
530      integer      :: color
531      character(10) :: fname
532      character(9) :: nodname
533      character(8) :: mtxname
534      real(dp)     :: x,y,z
535
536      write(fname,'(a5,i5.5)') 'RECTA',i
537      write(mf,*) 's = new TBRIK("',fname, '","',  fname,'","void",0.5,0.5,',p%mag%l/two,');'
538      write(mf,*) 's->SetLineColor(',color,');'
539
540      x = a(1)
541      y = a(2)
542      z = a(3)
543
544
545      write(mtxname,'(a3,i5.5)') 'mtx',i
546      !      print*, fname," ",mtxname
547
548      call setmatrix(m,mtxname,mf)
549
550      write(nodname,'(a4,i5.5)') 'NODE',i
551      write(mf,*) 'n = new TNode("',nodname,'","',  nodname,'",s,', &
552           & x,',',y, ',',z,',m);'
553
554    end subroutine  drawbox
555
556    subroutine  drawboxm(p,mf,color)
557      implicit none
558      type(fibre), pointer :: p
559      integer      :: mf !macro file descriptor
560      integer      :: color
561
562      call drawbox(p,mf, P%mag%p%f%mid, P%mag%p%f%o, color)
563
564    end subroutine  drawboxm
565
566
567    subroutine  drawtube(p,mf,r,color)
568      implicit none
569      type(fibre), pointer :: p
570      integer      :: mf !macro file descriptor
571      real(dp)     :: r
572      integer      :: color
573      character(10) :: fname
574      character(9) :: nodname
575      character(8) :: mtxname
576      real(dp)     :: x,y,z
577
578      write(fname,'(a5,i5.5)') 'DRIFT',i
579      write(mf,*) 's = new TTUBE("',fname, '","',  fname,'","void",',r,',',p%mag%l/two,');'
580      write(mf,*) 's->SetLineColor(',color,');'
581
582      x = P%mag%p%f%o(1)
583      y = P%mag%p%f%o(2)
584      z = P%mag%p%f%o(3)
585
586
587      write(mtxname,'(a3,i5.5)') 'mtx',i
588      !      print*, fname," ", mtxname
589
590      call setmatrix(P%mag%p%f%mid,mtxname,mf)
591
592      write(nodname,'(a4,i5.5)') 'NODE',i
593      write(mf,*) 'n = new TNode("',nodname,'","',  nodname,'",s,', &
594           & x,',',y, ',',z,',m);'
595
596
597
598    end subroutine  drawtube
599
600    subroutine setmatrix(m,name,mf)
601      implicit none
602      real(dp)     :: m(3,3)
603      character(8) :: name
604      integer      :: mf !macro file descriptor
605
606
607      write(mf,*) 'rotmatrix[0] = ', m(1,1),';'
608      write(mf,*) 'rotmatrix[1] = ', m(1,2),';'
609      write(mf,*) 'rotmatrix[2] = ', m(1,3),';'
610
611      write(mf,*) 'rotmatrix[3] = ', m(2,1),';'
612      write(mf,*) 'rotmatrix[4] = ', m(2,2),';'
613      write(mf,*) 'rotmatrix[5] = ', m(2,3),';'
614
615      write(mf,*) 'rotmatrix[6] = ', m(3,1),';'
616      write(mf,*) 'rotmatrix[7] = ', m(3,2),';'
617      write(mf,*) 'rotmatrix[8] = ', m(3,3),';'
618      write(mf,*) 'm = new TRotMatrix("',name,'","',name,'",rotmatrix);'
619
620    end subroutine setmatrix
621
622
623  end subroutine printlayout_rootm
624
625  subroutine printfframes(p)
626    implicit none
627    type(fibre), pointer :: p
628    write(6,*) " Actual magnet positioning  "
629    write(6,*) "  "
630    write(6,*) " Entrance origin A(3) "
631    write(6,*) P%mag%p%f%a
632    write(6,*) " Entrance frame (i,j,k) basis in the ent(3,3) array "
633    write(6,*) P%mag%p%f%ent(1,:)
634    write(6,*) P%mag%p%f%ent(2,:)
635    write(6,*) P%mag%p%f%ent(3,:)
636    write(6,*) " Middle origin O(3) "
637    write(6,*) P%mag%p%f%o
638    write(6,*) " Middle frame (i,j,k) basis in the ent(3,3) array "
639    write(6,*) P%mag%p%f%mid(1,:)
640    write(6,*) P%mag%p%f%mid(2,:)
641    write(6,*) P%mag%p%f%mid(3,:)
642    write(6,*) " Exit origin B(3) "
643    write(6,*) P%mag%p%f%B
644    write(6,*) " Exit frame (i,j,k) basis in the ent(3,3) array "
645    write(6,*) P%mag%p%f%exi(1,:)
646    write(6,*) P%mag%p%f%exi(2,:)
647    write(6,*) P%mag%p%f%exi(3,:)
648  end subroutine printfframes
649
650  function rot(m,a)
651    implicit none
652    real(dp)             :: rot(3)
653    real(dp)             :: a(3),m(3,3)
654
655    rot(1) = m(1,1)*a(1) + m(2,1)*a(2) + m(3,1)*a(3)
656    rot(2) = m(1,2)*a(1) + m(2,2)*a(2) + m(3,2)*a(3)
657    rot(3) = m(1,3)*a(1) + m(2,3)*a(2) + m(3,3)*a(3)
658
659    if (getdebug() > 3) then
660       write(6,*) " Vector Rotation  "
661       write(6,'(a26, f8.4)')             " ", a(1)
662       write(6,'(a26, f8.4)')             " ", a(2)
663       write(6,'(a26, f8.4)')             " ", a(3)
664       write(6,*) "     x "
665       write(6,'(3f8.4,a2,f8.4)') m(:,1), " ", rot(1)
666       write(6,'(3f8.4,a2,f8.4)') m(:,2), " ", rot(2)
667       write(6,'(3f8.4,a2,f8.4)') m(:,3), " ", rot(3)
668    endif
669
670  end function rot
671  !____________________________________________________________________________________________
672
673  function rotm(a,m)
674    implicit none
675    real(dp)             :: rotm(3,3)
676    real(dp)             :: a(3,3),m(3,3)
677
678    rotm(1,1) = a(1,1)*m(1,1) + a(1,2)*m(2,1) + a(1,3)*m(3,1)
679    rotm(2,1) = a(2,1)*m(1,1) + a(2,2)*m(2,1) + a(2,3)*m(3,1)
680    rotm(3,1) = a(3,1)*m(1,1) + a(3,2)*m(2,1) + a(3,3)*m(3,1)
681
682    rotm(1,2) = a(1,1)*m(1,2) + a(1,2)*m(2,2) + a(1,3)*m(3,2)
683    rotm(2,2) = a(2,1)*m(1,2) + a(2,2)*m(2,2) + a(2,3)*m(3,2)
684    rotm(3,2) = a(3,1)*m(1,2) + a(3,2)*m(2,2) + a(3,3)*m(3,2)
685
686    rotm(1,3) = a(1,1)*m(1,3) + a(1,2)*m(2,3) + a(1,3)*m(3,3)
687    rotm(2,3) = a(2,1)*m(1,3) + a(2,2)*m(2,3) + a(2,3)*m(3,3)
688    rotm(3,3) = a(3,1)*m(1,3) + a(3,2)*m(2,3) + a(3,3)*m(3,3)
689
690
691    if (getdebug() > 3) then
692       write(6,*) " 2 Rotation MATRIX product "
693       write(6,'(a26, 3f8.4)')             " ", a(:,1)
694       write(6,'(a26, 3f8.4)')             " ", a(:,2)
695       write(6,'(a26, 3f8.4)')             " ", a(:,3)
696       write(6,*) "     x "
697       write(6,'(3f8.4,a2,3f8.4)') m(:,1), " ", rotm(:,1)
698       write(6,'(3f8.4,a2,3f8.4)') m(:,2), " ", rotm(:,2)
699       write(6,'(3f8.4,a2,3f8.4)') m(:,3), " ", rotm(:,3)
700    endif
701
702  end function rotm
703  !____________________________________________________________________________________________
704
705  subroutine rotmatrixfromeuler(phi, theta, eta, ent)
706    implicit none
707    real(dp)             :: phi, theta, eta
708    real(dp)             :: ent(3,3)
709    real(dp)             :: sinphi, cosphi, sintheta, costheta
710
711    eta = zero ! we do not support rotations around the magnet axis yet
712
713
714    sinphi = sin(phi)
715    cosphi = cos(phi)
716    sintheta = sin(theta)
717    costheta = cos(theta)
718
719    ent(1,1) = cosphi
720    ent(1,2) = 0
721    ent(1,3) = sinphi
722
723    ent(2,1) =  sintheta*sinphi
724    ent(2,2) =  costheta
725    ent(2,3) = -sintheta*cosphi
726
727    ent(3,1) = -costheta*sinphi
728    ent(3,2) =  sintheta
729    ent(3,3) =  costheta*cosphi
730
731    write(6,*) " Rotation MATRIX  "
732    write(6,'(3f8.4)') ent(1,:)
733    write(6,'(3f8.4)') ent(2,:)
734    write(6,'(3f8.4)') ent(3,:)
735  end subroutine rotmatrixfromeuler
736
737
738end module madx_ptc_eplacement_module
739!____________________________________________________________________________________________
740!____________________________________________________________________________________________
741!____________________________________________________________________________________________
742!____________________________________________________________________________________________
743
744
745function w_ptc_getnelements()
746  use madx_ptc_module, only: my_ring;
747  implicit none
748  integer ::  w_ptc_getnelements
749  integer :: n
750
751  n = 0
752  if (associated(my_ring)) then
753     n = my_ring%n
754  endif
755
756  w_ptc_getnelements = n
757
758end function w_ptc_getnelements
759!____________________________________________________________________________________________
760
761subroutine w_ptc_getelname(n)
762  use madx_ptc_module, only: my_ring;
763  implicit none
764  integer n
765
766  if (associated(my_ring)) then
767     n = my_ring%n
768  endif
769
770end subroutine w_ptc_getelname
Note: See TracBrowser for help on using the repository browser.