source: PSPA/madxPSPA/libs/ptc/src/Sd_frame.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: 20.7 KB
Line 
1!The Polymorphic Tracking Code
2!Copyright (C) Etienne Forest and CERN
3module S_FRAME
4  use S_euclidean
5  IMPLICIT NONE
6  public
7  PRIVATE ZERO_CHART,COPY_CHART,COPY_CHART1   !,GEO_ROTA,GEO_ROTB
8  PRIVATE COPY_PATCH,COPY_PATCH1,ZERO_PATCH,FIND_PATCH_b
9  Private alloc_f,dealloc_f,equal_f
10  private make_rot_x,make_rot_y,make_rot_z   !,GEO_ROTAB_no_vec,GEO_ROTA_no_vec
11  REAL(DP), public :: GLOBAL_FRAME(3,3)= RESHAPE((/1,0,0  ,0,1,0  ,0,0,1/),(/3,3/))
12  REAL(DP), public :: GLOBAL_origin(3)= (/0,0,0/)
13  integer :: ccc=0
14  !  include "a_def_frame_patch_chart.inc"   ! sept 2007
15
16
17  INTERFACE assignment (=)
18     MODULE PROCEDURE ZERO_CHART
19     MODULE PROCEDURE ZERO_PATCH
20  end  INTERFACE
21
22  INTERFACE EQUAL
23     MODULE PROCEDURE COPY_CHART
24     MODULE PROCEDURE COPY_PATCH
25  end  INTERFACE
26  INTERFACE copy
27     MODULE PROCEDURE COPY_CHART1
28     MODULE PROCEDURE COPY_PATCH1
29  end  INTERFACE
30
31  INTERFACE GEO_ROT
32     MODULE PROCEDURE GEO_ROTA      !(ENT,V,A,I)
33     MODULE PROCEDURE GEO_ROTA_no_vec  !(ENT,A,I)
34     MODULE PROCEDURE GEO_ROTAB_no_vec   ! (ENT,Exi,A_X2,A_X1,A_xy)
35     MODULE PROCEDURE GEO_ROTB      ! (ENT,EXI,A,B,A_X2,A_X1,A_XY)
36  end  INTERFACE
37
38
39
40  INTERFACE FIND_PATCH
41     MODULE PROCEDURE FIND_PATCH_b
42  end  INTERFACE
43
44  INTERFACE alloc
45     MODULE PROCEDURE alloc_f
46  END INTERFACE
47
48  INTERFACE kill
49     MODULE PROCEDURE dealloc_f
50  END INTERFACE
51
52  INTERFACE assignment (=)
53     MODULE PROCEDURE equal_f
54  end  INTERFACE
55
56
57CONTAINS
58
59  SUBROUTINE  NULL_f(p)
60    implicit none
61    type (MAGNET_frame), pointer:: P
62
63    nullify(P%O);nullify(P%MID);
64    nullify(P%A);nullify(P%ENT);
65    nullify(P%B);nullify(P%EXI);
66  end subroutine NULL_f
67
68  SUBROUTINE  alloc_f(p)
69    implicit none
70    type (MAGNET_frame), pointer:: P
71    nullify(p)
72    allocate(p)
73    CALL NULL_f(p)
74    ALLOCATE(P%O(3));ALLOCATE(P%MID(3,3));
75    ALLOCATE(P%A(3));ALLOCATE(P%ENT(3,3));
76    ALLOCATE(P%B(3));ALLOCATE(P%EXI(3,3));
77    P%O=global_origin
78    P%a=global_origin
79    P%b=global_origin
80    P%ENT=global_FRAME
81    P%MID=global_FRAME
82    P%EXI=global_FRAME
83
84  end subroutine alloc_f
85
86  SUBROUTINE  NULL_af(p)
87    implicit none
88    type (affine_frame), pointer:: P
89    nullify(P%ANGLE);nullify(P%D);
90    nullify(P%A);nullify(P%ENT);
91    nullify(P%b);nullify(P%EXI);
92  end subroutine NULL_af
93
94  SUBROUTINE  NULL_gs(gs)
95    implicit none
96    integer i
97    type (girder_siamese) :: gs(:)
98    do i=1,size(gs)
99       nullify(gs(i)%mag)
100    enddo
101  end subroutine NULL_gs
102
103  SUBROUTINE  alloc_af(p,girder)
104    implicit none
105    type (affine_frame), pointer:: P
106    logical(lp), optional :: girder
107    logical(lp) gird
108    gird=.false.
109    if(present(girder)) gird=girder
110
111    nullify(p)
112    allocate(p)
113    CALL NULL_af(p)
114    if(gird) then
115       ALLOCATE(P%A(3));ALLOCATE(P%ENT(3,3));
116       ALLOCATE(P%B(3));ALLOCATE(P%EXI(3,3));
117       P%a=global_origin
118       P%ENT=global_FRAME
119       P%B=global_origin
120       P%EXI=global_FRAME
121    else
122       ALLOCATE(P%ANGLE(3));ALLOCATE(P%D(3));
123       P%D=0.0_dp
124       P%ANGLE=0.0_dp
125    endif
126
127  end subroutine alloc_af
128
129  SUBROUTINE  kill_af(p)
130    implicit none
131    type (affine_frame), pointer:: P
132
133    if(associated(p%D)) DEALLOCATE(P%D);
134    if(associated(p%ANGLE)) DEALLOCATE(P%ANGLE);
135    if(associated(p%a)) DEALLOCATE(P%A);
136    if(associated(p%a)) DEALLOCATE(P%A);
137    if(associated(p%ENT)) DEALLOCATE(P%ENT);
138    if(associated(p%B)) DEALLOCATE(P%B);
139    if(associated(p%EXI)) DEALLOCATE(P%EXI);
140
141  end subroutine kill_af
142
143  SUBROUTINE  dealloc_f(p)
144    implicit none
145    type (MAGNET_frame), pointer:: P
146
147    if(associated(p%a)) then
148       DEALLOCATE(P%A);DEALLOCATE(P%ENT);
149       DEALLOCATE(P%O);DEALLOCATE(P%MID);
150       DEALLOCATE(P%B);DEALLOCATE(P%EXI);
151    endif
152
153  end SUBROUTINE  dealloc_f
154
155  SUBROUTINE  equal_f(elp,el)
156    implicit none
157    type (MAGNET_frame),INTENT(inOUT)::elP
158    type (MAGNET_frame),INTENT(IN)::el
159
160    if(associated(ELP%O).and.associated(EL%O)) then
161       ELP%O=EL%O
162       ELP%A=EL%A
163       ELP%B=EL%B
164       ELP%MID=EL%MID;ELP%ENT=EL%ENT;ELP%EXI=EL%EXI;
165    endif
166  end SUBROUTINE  equal_f
167
168  SUBROUTINE COPY_PATCH(B,A)  ! B<-A
169    IMPLICIT  NONE
170    TYPE(PATCH), INTENT(inOUT):: B
171    TYPE(PATCH), INTENT(IN):: A
172    INTEGER I
173
174    if(associated(b%PATCH).and.associated(a%PATCH)) then
175       B%PATCH=A%PATCH
176       B%ENERGY=A%ENERGY
177       B%TIME=A%TIME
178
179       B%A_T=A%A_T
180       B%B_T=A%B_T
181       B%A_X2=A%A_X2
182       B%B_X1=A%B_X1
183
184       DO I=1,3
185          B%A_D(I)=A%A_D(I)
186          B%B_D(I)=A%B_D(I)
187          B%A_ANG(I)=A%A_ANG(I)
188          B%B_ANG(I)=A%B_ANG(I)
189       ENDDO
190    endif
191
192  END SUBROUTINE COPY_PATCH
193
194  SUBROUTINE COPY_CHART(B,A)  ! B<-A
195    IMPLICIT  NONE
196    TYPE(CHART), INTENT(inOUT):: B
197    TYPE(CHART), INTENT(IN):: A
198    INTEGER I
199
200    if(associated(b%f).and.associated(a%f)) then
201       !   B%A_XY=A%A_XY
202       !   B%L=A%L
203       !   B%ALPHA=A%ALPHA
204
205       DO I=1,3
206          B%D_in(I)=A%D_in(I)
207          B%ang_in(I)=A%ang_in(I)
208          B%D_out(I)=A%D_out(I)
209          B%ang_out(I)=A%ang_out(I)
210       ENDDO
211    endif
212    if(associated(a%f)) then
213       b%f=a%f
214    endif
215
216
217  END SUBROUTINE COPY_CHART
218
219  SUBROUTINE COPY_CHART1(a,b)   ! a->b
220    IMPLICIT  NONE
221    TYPE(CHART), INTENT(inOUT):: B
222    TYPE(CHART), INTENT(IN):: A
223
224    call equal(b,a)
225
226  END SUBROUTINE COPY_CHART1
227
228  SUBROUTINE COPY_PATCH1(a,b)  ! a->b
229    IMPLICIT  NONE
230    TYPE(PATCH), INTENT(inOUT):: B
231    TYPE(PATCH), INTENT(IN):: A
232
233    call equal(b,a)
234
235  END SUBROUTINE COPY_PATCH1
236
237
238
239
240  SUBROUTINE ZERO_PATCH(F,R)   !R=0 nullifies and allocates ; R=-1 deallocates
241    IMPLICIT  NONE
242    TYPE(PATCH), INTENT(INOUT):: F
243    INTEGER, INTENT(IN):: R
244    !    if(r==1) then
245    !    write(6,*) "i =0 "
246    !    read(5,*) i
247    !    i=1/i
248    !   endif
249
250    IF(R==0.or.R==1) THEN    !
251       NULLIFY(F%A_T,F%B_T,F%A_D,  F%B_D,   F%A_ANG,F%B_ANG)
252       NULLIFY(F%A_X1,F%A_X2,F%B_X1,F%B_X2)
253
254       NULLIFY(F%TIME,F%ENERGY,F%PATCH)
255       ALLOCATE(F%A_D(3),F%B_D(3),F%A_ANG(3),F%B_ANG(3))
256       ALLOCATE(F%A_T,F%B_T)
257       ALLOCATE(F%A_X1,F%A_X2,F%B_X1,F%B_X2)
258       ALLOCATE(F%TIME,F%ENERGY,F%PATCH)
259       F%A_T=0.0_dp
260       F%B_T=0.0_dp
261       F%A_X1=1; F%A_X2=1; F%B_X1=1; F%B_X2=1;
262       F%A_D=0.0_dp
263       F%B_D=0.0_dp
264       F%A_ANG=0.0_dp
265       F%B_ANG=0.0_dp
266       f%patch=0
267       f%ENERGY=0
268       f%TIME=0
269    ELSEIF(R==-1) THEN
270       DEALLOCATE(F%A_D,F%B_D,F%A_ANG,F%B_ANG)
271       DEALLOCATE(F%A_T,F%B_T)
272       DEALLOCATE(F%A_X1,F%A_X2,F%B_X1,F%B_X2)
273       DEALLOCATE(F%TIME,F%ENERGY,F%PATCH)
274       nullify(F%A_D,F%B_D,F%A_ANG,F%B_ANG)
275       nullify(F%A_T,F%B_T)
276       nullify(F%A_X1,F%A_X2,F%B_X1,F%B_X2)
277       nullify(F%TIME,F%ENERGY,F%PATCH)
278    ELSE
279       w_p=1
280       w_p%nc=1
281       w_p%fc='(1((1X,a72)))'
282       write(w_p%c(1),'(a5,1x,i4,a30)') " R = ",R ," NOT DEFINED IN ZERO_CHART (1)"
283       ! call !write_e(-1)
284    ENDIF
285  END SUBROUTINE ZERO_PATCH
286
287
288
289  SUBROUTINE ZERO_CHART(F,R)   !R=0 nullifies and allocates ; R=-1 deallocates
290    IMPLICIT  NONE
291    TYPE(CHART), INTENT(INOUT):: F
292    INTEGER, INTENT(IN):: R
293    !    if(r==1) then
294    !    write(6,*) "i =0 "
295    !    read(5,*) i
296    !    i=1/i
297    !   endif
298    IF(R==0.or.R==1) THEN          !
299       nullify(f%f)
300       !       NULLIFY(    F%A_XY, F%L,    F%ALPHA)
301       NULLIFY( F%d_in, F%ang_in,F%d_out,F%ang_out)
302       call alloc(f%f)
303       ALLOCATE(F%d_in(3),F%ang_in(3),F%d_out(3),F%ang_out(3))
304       !      ALLOCATE(F%A_XY,F%L,F%ALPHA)
305       !         F%L=zero
306       !         F%ALPHA=zero
307       !         F%A_XY=zero
308       F%Ang_in=0.0_dp
309       F%d_in=0.0_dp
310       F%Ang_out=0.0_dp
311       F%d_out=0.0_dp
312       IF(associated(f%f)) THEN   ! R==1.and.
313          F%f%ENT=global_frame
314          F%f%EXI=global_frame
315          F%f%MID=global_frame
316          F%f%ENT=global_frame
317          F%f%EXI=global_frame
318          F%f%MID=global_frame
319          F%f%A=GLOBAL_origin
320          F%f%B=GLOBAL_origin
321          F%f%O=GLOBAL_origin
322       ENDIF
323    ELSEIF(R==-1) THEN
324       DEALLOCATE(F%d_in,F%ang_in,F%d_out,F%ang_out)
325       !       DEALLOCATE(F%A_XY,F%L,F%ALPHA)
326       if(associated(f%f)) then
327          call kill(f%f)
328          deallocate(f%f)
329       endif
330       NULLIFY(F%d_in,F%ang_in,F%d_out,F%ang_out,f%f)
331       !       NULLIFY(F%A_XY,F%L,F%ALPHA)
332    ELSEIF(R==2) THEN   ! SPECIAL TRYING TO FIX FRANK'S MEMORY IN EL_Q
333       DEALLOCATE(F%d_in,F%ang_in,F%d_out,F%ang_out)
334       !       DEALLOCATE(F%A_XY,F%L,F%ALPHA)
335       if(associated(f%f)) then
336          call kill(f%f)
337          deallocate(f%f)
338       endif
339       NULLIFY(F%d_in,F%ang_in,F%d_out,F%ang_out,f%f)
340       call alloc(f%f)
341       ALLOCATE(F%d_in(3),F%ang_in(3),F%d_out(3),F%ang_out(3))
342       !      ALLOCATE(F%A_XY,F%L,F%ALPHA)
343       !         F%L=zero
344       !         F%ALPHA=zero
345       !         F%A_XY=zero
346       F%Ang_in=0.0_dp
347       F%d_in=0.0_dp
348       F%Ang_out=0.0_dp
349       F%d_out=0.0_dp
350       IF(associated(f%f)) THEN   ! R==1.and.
351          F%f%ENT=global_frame
352          F%f%EXI=global_frame
353          F%f%MID=global_frame
354          F%f%ENT=global_frame
355          F%f%EXI=global_frame
356          F%f%MID=global_frame
357          F%f%A=GLOBAL_origin
358          F%f%B=GLOBAL_origin
359          F%f%O=GLOBAL_origin
360       ENDIF
361    ELSE
362       w_p=1
363       w_p%nc=1
364       w_p%fc='(1((1X,a72)))'
365       write(w_p%c(1),'(a5,1x,i4,a30)') " R = ",R ," NOT DEFINED IN ZERO_CHART (2)"
366       ! call !write_e(-1)
367    ENDIF
368
369  END SUBROUTINE ZERO_CHART
370
371
372  SUBROUTINE COPY_VECTOR(R,F)   ! R%ENT,R%MID,R%EXI -> F%ENT,F%MID,F%EXI
373    IMPLICIT  NONE
374    TYPE(CHART), INTENT(inOUT):: F
375    TYPE(CHART), INTENT(IN):: R
376    INTEGER I,J
377    if(associated(r%f).and.associated(f%f)) then
378       DO I=1,3
379          DO J=1,3
380             F%f%MID(I,J)=R%f%MID(I,J)
381             F%f%EXI(I,J)=R%f%EXI(I,J)
382             F%f%ENT(I,J)=R%f%ENT(I,J)
383          ENDDO
384       ENDDO
385    endif
386  END SUBROUTINE COPY_VECTOR
387  ! Geometry
388
389  SUBROUTINE change_basis(A,ENT,B,EXI)
390    implicit none
391    real(dp), INTENT(IN):: A(3),ENT(3,3)
392    real(dp), INTENT(INOUT):: EXI(3,3),B(3)
393    real(dp) T(3)
394    INTEGER L,K,N
395    T=A
396    B=0.0_dp
397    DO L=1,3
398       DO N=1,3
399          DO K=1,3
400             B(N)=T(L)*ENT(L,K)*EXI(N,K)+B(N)
401          ENDDO
402       ENDDO
403    ENDDO
404  END SUBROUTINE change_basis
405
406
407
408  SUBROUTINE GEO_TRA(A,ENT,D,I)
409    ! Adds/subtracts D to A where D is expressed in the ENT frame
410    implicit none
411    real(dp), INTENT(INOUT):: A(3)
412    real(dp), INTENT(IN):: ENT(3,3)
413    real(dp), INTENT(IN):: D(3)
414    INTEGER, INTENT(IN):: I
415    real(dp) B(3)
416    INTEGER J,K
417    B=0.0_dp
418    DO J=1,3
419       DO K=1,3
420          B(K)=D(J)*ENT(J,K)+B(K)
421       ENDDO
422    ENDDO
423    A=A+I*B
424  END SUBROUTINE GEO_TRA
425
426
427  SUBROUTINE GEO_ROTA_no_vec(ENT,ANG,I,basis)
428    ! Rotates frame ENT by A(3) in the PTC or reverse
429    !PTC order using global frame for angle definition
430    implicit none
431    real(dp), INTENT(INOUT):: ENT(3,3)
432    real(dp), INTENT(IN):: ANG(3)
433    INTEGER, INTENT(IN):: I
434    real(dp) V(3)
435    real(dp), optional, INTENT(IN):: basis(3,3)
436
437    v=0.0_dp
438    CALL GEO_ROT(ENT,V,ANG,i,basis)
439
440  END SUBROUTINE GEO_ROTA_no_vec
441
442  SUBROUTINE GEO_ROTA(ENT,A,ANG,I,basis)
443    ! Rotates frame ENT by Ang(3) in the PTC or reverse PTC order
444    !using global frame for angle definition
445    implicit none
446    real(dp), INTENT(INOUT):: ENT(3,3),A(3)
447    real(dp), INTENT(IN):: ANG(3)
448    INTEGER, INTENT(IN):: I
449    real(dp), optional, INTENT(IN):: basis(3,3)
450    REAL(DP) AT(3),AA(3),ent0(3,3)
451    INTEGER J
452
453    ! PTC order X first, than y and finally Z , here defined on global variables
454    aa=a
455    ent0=ent
456    IF(I==1) THEN
457       CALL GEO_ROT(ENT0,ENT,Aa,A,ANG,basis)
458    ELSE
459       DO J=1,3
460          AT=0.0_dp;AT(J)=-ANG(J);
461          CALL GEO_ROT(ENT0,ENT,Aa,A,AT,basis)
462       ENDDO
463    ENDIF
464
465  END SUBROUTINE GEO_ROTA
466
467
468
469  SUBROUTINE GEO_ROTAB_no_vec(ENT,Exi,Ang,basis) ! Used in survey stuff A_X2,A_X1,A_xy,
470    implicit none
471    real(dp), INTENT(INOUT):: ENT(3,3),exi(3,3)
472    real(dp), INTENT(IN):: ANG(3)
473    real(dp) v(3),vv(3)
474    real(dp), optional, INTENT(IN):: basis(3,3)
475
476
477    v=0.0_dp
478    vv=0.0_dp
479
480    CALL GEO_ROT(ENT,Exi,VV,V,ANG,basis)    !A_X2,A_X1,A_xy,
481
482  END SUBROUTINE GEO_ROTAB_no_vec
483
484  SUBROUTINE  ROTATE_FRAME(R,OMEGA,ANG,ORDER,BASIS)
485    ! ROTATES A FRAME R AROUND OMEGA BY ANG(3)  IN STANDARD PTC ORDER
486    ! INVERSE => ORDER=-1   USING GLOBAL FRAME
487    IMPLICIT NONE
488    TYPE (MAGNET_FRAME),TARGET,INTENT(INOUT):: R
489    REAL(DP),INTENT(IN):: OMEGA(3),ANG(3)
490    TYPE(MAGNET_FRAME), POINTER::P
491    REAL(DP) D(3),OMEGAT(3)
492    INTEGER IORDER
493    INTEGER, OPTIONAL :: ORDER
494    REAL(DP), OPTIONAL, INTENT(IN):: BASIS(3,3)
495    REAL(DP)  BASIST(3,3)
496    OMEGAT=OMEGA
497    P=>R
498    IORDER=1
499    IF(PRESENT(ORDER)) IORDER=ORDER
500
501    BASIST=GLOBAL_FRAME            ! NECESSARY SINCE BASIS CAN CHANGE DURING THE CALCULATION ASSUMING A POINTER IS PASSED
502    IF(PRESENT(BASIS)) BASIST=BASIS
503
504    ! THEY WILL ROTATE MORE THAN ONCE
505    D=P%A-OMEGAT
506    CALL GEO_ROT(P%ENT,D,ANG,IORDER,BASIST)
507    P%A=OMEGAT+D
508
509    D=P%O-OMEGAT
510    CALL GEO_ROT(P%MID,D,ANG,IORDER,BASIST)
511    P%O=OMEGAT+D
512
513    D=P%B-OMEGAT
514    CALL GEO_ROT(P%EXI,D,ANG,IORDER,BASIST)
515    P%B=OMEGAT+D
516
517
518  END SUBROUTINE ROTATE_FRAME
519
520
521  SUBROUTINE  TRANSLATE_FRAME(R,D,ORDER,BASIS) ! TRANSLATES A FRAME
522    IMPLICIT NONE
523    TYPE (MAGNET_FRAME),TARGET, INTENT(INOUT):: R
524    REAL(DP),INTENT(IN):: D(3)
525    REAL(DP), OPTIONAL :: BASIS(3,3)
526    INTEGER IORDER
527    INTEGER, OPTIONAL, INTENT(IN) :: ORDER
528    REAL(DP) DD(3)
529    TYPE(MAGNET_FRAME), POINTER :: P
530    ! THIS ROUTINE TRANSLATE THE ENTIRE LINE BY A(3) IN STANDARD ORDER USING THE
531    ! GLOBAL FRAME TO DEFINE D
532
533    P=>R
534
535    IORDER=1
536    DD=D
537    IF(PRESENT(ORDER)) IORDER=ORDER
538    IF(PRESENT(BASIS)) THEN
539       CALL CHANGE_BASIS(D,BASIS,DD,GLOBAL_FRAME)
540    ENDIF
541
542
543
544    P%A=P%A+IORDER*DD
545    P%B=P%B+IORDER*DD
546    P%O=P%O+IORDER*DD
547
548
549
550  END SUBROUTINE TRANSLATE_FRAME
551
552
553
554  SUBROUTINE make_rot_x(r,a)
555    implicit none
556    real(dp), INTENT(INOUT):: r(3,3)
557    real(dp), INTENT(IN):: a
558    r=0.0_dp
559
560    r(1,1)= 1.0_dp
561
562    r(2,2)=cos(a)
563    r(3,3)=cos(a)
564    r(2,3)=sin(a)   ! defined acting on vectors, so y goes into z
565    r(3,2)=-sin(a)
566
567  end SUBROUTINE make_rot_x
568
569  SUBROUTINE make_rot_y(r,a)
570    implicit none
571    real(dp), INTENT(INOUT):: r(3,3)
572    real(dp), INTENT(IN):: a
573    r=0.0_dp
574
575    r(2,2)= 1.0_dp
576
577    r(1,1)=cos(a)
578    r(3,3)=cos(a)
579    r(1,3)=sin(a)   ! defined acting on vectors, so x goes into z
580    r(3,1)=-sin(a)
581
582  end SUBROUTINE make_rot_y
583
584  SUBROUTINE make_rot_z(r,a)
585    implicit none
586    real(dp), INTENT(INOUT):: r(3,3)
587    real(dp), INTENT(IN):: a
588    r=0.0_dp
589
590    r(3,3)= 1.0_dp
591
592    r(1,1)=cos(a)
593    r(2,2)=cos(a)
594    r(1,2)=sin(a)   ! defined acting on vectors, so x goes into y
595    r(2,1)=-sin(a)
596
597  end SUBROUTINE make_rot_z
598
599
600  SUBROUTINE GEO_ROTB(ENT,EXI,A,B,ANG,BASIS)
601    ! Rotates ENT into EXI using global frame otherwise uses basis
602    implicit none
603    real(dp), INTENT(INOUT):: ENT(3,3),EXI(3,3),A(3),B(3)
604    real(dp), INTENT(IN):: ANG(3)
605    real(dp) TEMP(3,3),exii(3,3),enti(3,3),T(3),R(3,3),basist(3,3)
606    real(dp), optional, INTENT(IN):: basis(3,3)
607    INTEGER I,j,k
608    ! Definition
609    !  ENT(1,i) is the local x-vector
610    !  ENT(2,i) is the local y-vector
611    !  ENT(3,i) is the local z-vector
612    ! Standardized to correspound to the standard PTC order
613    ! Original order now changed was xy,xz,yz
614    ! TRANSPOSED OF GEO_ROTB
615
616    exii(:,:)=0.0_dp
617    enti(:,:)=0.0_dp
618    exii(:,:)=0.0_dp
619    temp(:,:)=0.0_dp
620
621
622    call make_rot_z(enti,ANG(3))
623    call make_rot_y(r,ANG(2))
624    if(present(basis)) then
625       basist=basis
626    else
627       basist=0.0_dp
628       do i=1,3
629          basist(i,i)=1.0_dp
630       enddo
631    endif
632
633    DO I=1,3
634       DO j=1,3
635          DO k=1,3
636             TEMP(i,k)=  basist(j,i)*enti(j,k)+TEMP(i,k)   ! basis^-1 * r(a(3))
637          ENDDO
638       ENDDO
639    ENDDO
640    enti=temp
641    temp(:,:)=0.0_dp
642
643
644    DO I=1,3
645       DO j=1,3
646          DO k=1,3
647             TEMP(i,k)=  enti(i,j)*r(j,k)+TEMP(i,k)     ! basis^-1 * r(a(3)) *r(a(2))
648          ENDDO
649       ENDDO
650    ENDDO
651
652    call make_rot_x(r,ANG(1))
653
654    DO I=1,3
655       DO j=1,3
656          DO k=1,3
657             exii(i,k)=  TEMP(i,j)*r(j,k)+exii(i,k)   ! basis^-1 * r(a(3)) *r(a(2)) *r(a(1))
658          ENDDO
659       ENDDO
660    ENDDO
661
662
663    temp=0.0_dp
664    DO I=1,3
665       DO j=1,3
666          DO k=1,3
667             temp(i,k)=  exii(i,j)*basist(j,k)+temp(i,k)  ! basis^-1 * r(a(3)) *r(a(2)) *r(a(1)) *basis
668          ENDDO
669       ENDDO
670    ENDDO
671    exii=temp
672
673
674    T(:)=0.0_dp
675
676    do i=1,3; do j=1,3;
677       T(i)=A(j)*exii(j,i)+T(i)
678    enddo;enddo;
679
680    B=T
681
682    temp(:,:)=0.0_dp
683
684    do i=1,3; do j=1,3;do k=1,3
685       temp(i,k)=ent(i,j)*exii(j,k)+temp(i,k)
686    enddo;enddo;enddo;
687
688    exi=temp
689
690    call check_frame(exi,t)
691
692  END SUBROUTINE GEO_ROTB
693
694  subroutine check_frame(ent,n)
695    implicit none
696    real(dp) ent(3,3),n(3),s,ss
697    integer i,j
698
699    n=0.0_dp
700    do i=1,3
701       do j=1,3
702          n(i)=ent(i,j)**2+n(i)
703
704       enddo
705
706       n(i)=sqrt(n(i))
707    enddo
708
709    do i=1,3
710       do j=1,3
711          ent(i,j)=ent(i,j)/n(i)
712       enddo
713    enddo
714
715    call COMPUTE_SCALAR(ent,1,ent,2,S)
716
717    do j=1,3
718       ent(2,j)=ent(2,j)-s*ent(1,j)
719    enddo
720
721    call COMPUTE_SCALAR(ent,2,ent,2,S)
722
723    do j=1,3
724       ent(2,j)=ent(2,j)/s
725    enddo
726
727
728    call COMPUTE_SCALAR(ent,1,ent,3,S)
729    call COMPUTE_SCALAR(ent,2,ent,3,SS)
730
731    do j=1,3
732       ent(3,j)=ent(3,j)-s*ent(1,j)-SS*ENT(2,J)
733    enddo
734
735    call COMPUTE_SCALAR(ent,3,ent,3,S)
736
737    do j=1,3
738       ent(3,j)=ent(3,j)/s
739    enddo
740
741
742
743  end subroutine check_frame
744
745  subroutine make_normal(n,s)
746    implicit none
747    real(dp) n(3),s
748    integer i
749
750    s=0.0_dp
751    do i=1,3
752       s=s+n(i)**2
753    enddo
754
755    if(s>EPS_FITTED) then
756       s=sqrt(s)
757       n=n/s
758    else
759       s=0.0_dp
760    endif
761  end subroutine make_normal
762
763  SUBROUTINE COMPUTE_ENTRANCE_ANGLE(ENTL,ENTB,A)
764    ! COMPUTES PTC'S ANGLES IN FRAME OF ENTL
765    IMPLICIT NONE
766    REAL(DP),INTENT(IN):: ENTL(3,3), ENTB(3,3)
767    REAL(DP), INTENT(OUT) :: A(3)
768    REAL(DP) D(3),T1(3,3),T10(3,3),T2(3,3),S_IJ,S_JJ
769    REAL(DP) AT(3)
770
771    T1=ENTL
772    T10=ENTL
773    T2=ENTL
774    D=0.0_dp
775
776    CALL COMPUTE_SCALAR(T1,2,ENTB,3,S_IJ)
777    CALL COMPUTE_SCALAR(T1,3,ENTB,3,S_JJ)
778
779    if(S_IJ==0.0_dp.and.S_JJ==0.0_dp) then
780       A(1)=0.0_dp
781    else
782       A(1)=ATAN2(-S_IJ,S_JJ)
783    endif
784
785    !   A(1)=ATAN2(-S_IJ,S_JJ)
786    AT=0.0_dp;AT(1)=A(1);
787
788    CALL GEO_ROT(T10,T1,AT,T2)
789    T2=T1
790    T10=T1
791
792    CALL COMPUTE_SCALAR(T1,1,ENTB,3,S_IJ)
793    CALL COMPUTE_SCALAR(T1,3,ENTB,3,S_JJ)
794
795    if(S_IJ==0.0_dp.and.S_JJ==0.0_dp) then
796       A(2)=0.0_dp
797    else
798       A(2)=ATAN2(-S_IJ,S_JJ)
799    endif
800
801    !    A(2)=ATAN2(-S_IJ,S_JJ)
802    AT=0.0_dp;AT(2)=A(2);
803
804    CALL GEO_ROT(T10,T1,AT,T2)
805    T2=T1
806    T10=T1
807
808    CALL COMPUTE_SCALAR(T1,2,ENTB,1,S_IJ)
809    CALL COMPUTE_SCALAR(T1,1,ENTB,1,S_JJ)
810
811    if(S_IJ==0.0_dp.and.S_JJ==0.0_dp) then
812       A(3)=0.0_dp
813    else
814       A(3)=ATAN2(S_IJ,S_JJ)
815    endif
816    !    A(3)=ATAN2(S_IJ,S_JJ)
817    AT=0.0_dp;AT(3)=A(3);
818
819    CALL GEO_ROT(T10,T1,AT,T2)
820    !   T2=T1
821    !   write(16,*) t2-entb
822
823
824  END SUBROUTINE COMPUTE_ENTRANCE_ANGLE
825
826  SUBROUTINE  COMPUTE_SCALAR(ENTL,I,ENTB,J,S_IJ) ! Adjusts frames of magnet_chart on the basis of the misalignments
827    IMPLICIT NONE
828    real(dp),INTENT(in):: ENTL(3,3), ENTB(3,3)
829    INTEGER,INTENT(in):: I,J
830    real(dp),INTENT(inOUT) :: S_IJ
831    INTEGER K
832
833    S_IJ=0.0_dp
834    DO K=1,3
835       S_IJ=ENTL(I,K)*ENTB(J,K)+S_IJ
836    ENDDO
837
838  END SUBROUTINE  COMPUTE_SCALAR
839
840
841
842  SUBROUTINE FIND_PATCH_B(A,ENT,B,EXI,D,ANG)
843    ! FINDS PATCH BETWEEN ENT AND EXI : INTERFACED LATER FOR FIBRES
844    IMPLICIT NONE
845    REAL(DP), INTENT(INOUT):: ENT(3,3),EXI(3,3)
846    REAL(DP), INTENT(INOUT):: A(3),B(3),D(3),ANG(3)
847    REAL(DP) dd(3)
848
849    CALL COMPUTE_ENTRANCE_ANGLE(ENT,EXI,ANG)
850
851
852    D=B-A
853    dd=d
854    CALL CHANGE_BASIS(Dd,GLOBAL_FRAME,D,EXI)
855
856
857  END SUBROUTINE FIND_PATCH_B
858
859  SUBROUTINE INVERSE_FIND_PATCH(A,ENT,D,ANG,B,EXI) !  used in misalignments of siamese
860    IMPLICIT NONE
861    REAL(DP), INTENT(INOUT):: ENT(3,3),EXI(3,3)
862    REAL(DP), INTENT(INOUT):: A(3),B(3),D(3),ANG(3)
863    REAL(DP) DD(3)
864
865    EXI=ENT
866    CALL GEO_ROT(EXI,ANG,1,basis=ENT)
867    !    CALL GEO_ROTA_no_vec(EXI,ANG,1,basis=ENT)
868    CALL CHANGE_BASIS(D,EXI,DD,GLOBAL_FRAME)
869    B=A+DD
870
871  END SUBROUTINE INVERSE_FIND_PATCH
872
873
874end module S_FRAME
Note: See TracBrowser for help on using the repository browser.