[u/mrichter/AliRoot.git] / GEANT321 / ggeom / gsrotm.F

*
* $Id$
*
* $Log$
* Revision 1.1.1.1  1995/10/24 10:20:56  cernlib
* Geant
*
*
#include "geant321/pilot.h"
*CMZ :  3.21/02 02/07/94  18.24.47  by  S.Giani
*-- Author :
      SUBROUTINE GSROTM(NMAT,THETA1,PHI1,THETA2,PHI2,THETA3,PHI3)
C.
C.    ******************************************************************
C.    *                                                                *
C.    *       STORE ROTATION MATRICES                                  *
C.    *                                                                *
C.    *    ==>Called by : <USER>                                       *
C.    *         Author  R.Brun  *********                              *
C.    *                                                                *
C.    ******************************************************************
C.
#include "geant321/gcbank.inc"
#include "geant321/gcunit.inc"
#include "geant321/gconsp.inc"
#include "geant321/gcnum.inc"
      DIMENSION ANGLES(6),IP(3)
      SAVE SINMIN
#if defined(CERNLIB_SINGLE)
      DIMENSION ROTMAT(9)
      PARAMETER (ONE=1.0,ZERO=0.0)
      DATA SINMIN/1.00E-5/
#endif
#if !defined(CERNLIB_SINGLE)
      DOUBLE PRECISION ROTMAT(9),ONE,ZERO
      DOUBLE PRECISION PROD1,PROD2,PROD3,HMOD,SINMIN
      PARAMETER (ONE=1.D0,ZERO=0.D0)
      DATA SINMIN/1.00D-5/
#endif
C.
C.    ------------------------------------------------------------------
C.
      IF(NMAT.LE.0)GO TO 999
      IF(JROTM.LE.0)CALL MZBOOK(IXCONS,JROTM,JROTM,1,'ROTM',NROTM,NROTM,
     +0,3,0)
      IF(NMAT.GT.IQ(JROTM-2)) THEN
         NPUSH=NMAT-IQ(JROTM-2)+50
         CALL MZPUSH(IXCONS,JROTM,NPUSH,0,'I')
         NROTM=IQ(JROTM-2)
         JR1=0
      ELSE
         JR1=LQ(JROTM-NMAT)
         IF(JR1.GT.0)THEN
            WRITE(CHMAIL,1000)
            CALL GMAIL(1,0)
            CALL GPROTM(NMAT)
            CALL MZDROP(IXCONS,LQ(JROTM-NMAT),' ')
         ENDIF
      ENDIF
      CALL MZBOOK(IXCONS,JR,JROTM,-NMAT,'ROTM',0,0,16,3,0)
C
      Q(JR + 11) = THETA1
      Q(JR + 12) = PHI1
      Q(JR + 13) = THETA2
      Q(JR + 14) = PHI2
      Q(JR + 15) = THETA3
      Q(JR + 16) = PHI3
C
      DO  10 N = 1,3
        THERAD = Q(JR+ 9+2*N)*DEGRAD
        PHIRAD = Q(JR+10+2*N)*DEGRAD
        SINTHE = SIN(THERAD)
        Q(JR+3*N-2) = SINTHE * COS(PHIRAD)
        Q(JR+3*N-1) = SINTHE * SIN(PHIRAD)
        Q(JR+3*N  ) = COS(THERAD)
        CALL VUNIT (Q(JR+3*N-2),Q(JR+3*N-2),3)
  10  CONTINUE
C.
C.---       Test orthonormality
      DO 20 J=1,9
        ROTMAT(J)=Q(JR+J)
  20  CONTINUE
      PROD2=ZERO
C.
C.               X - Y
      PROD1=
     +ROTMAT(1)*ROTMAT(4)+ROTMAT(2)*ROTMAT(5)+ROTMAT(3)*ROTMAT(6)
      IF(ABS(PROD1).GT.SINMIN) GO TO 30
C.
C.               X - Z
      PROD2=
     +ROTMAT(1)*ROTMAT(7)+ROTMAT(2)*ROTMAT(8)+ROTMAT(3)*ROTMAT(9)
      IF(ABS(PROD2).GT.SINMIN) GO TO 30
C.
C.               Y - Z
      PROD3=
     +ROTMAT(7)*ROTMAT(4)+ROTMAT(8)*ROTMAT(5)+ROTMAT(9)*ROTMAT(6)
      IF(ABS(PROD3).LE.SINMIN) GO TO 110
  30  CONTINUE
C.
C.---       Orthonormalization needed
C.
C.          Assume X correct
      HMOD=ZERO
      DO 40 J=4,6
        ROTMAT(J)=ROTMAT(J)-ROTMAT(J-3)*PROD1
        HMOD=HMOD+ROTMAT(J)*ROTMAT(J)
  40  CONTINUE
      HMOD=ONE/SQRT(HMOD)
      DO 50 J=4,6
        ROTMAT(J)=ROTMAT(J)*HMOD
  50  CONTINUE
C.
C.          Y done, do Z
C.
*      IF(PROD2.EQ.ZERO) THEN
*        PROD2=
*     +  ROTMAT(1)*ROTMAT(7)+ROTMAT(2)*ROTMAT(8)+ROTMAT(3)*ROTMAT(9)
*      ENDIF
*      PROD3=
*     +ROTMAT(4)*ROTMAT(7)+ROTMAT(5)*ROTMAT(8)+ROTMAT(6)*ROTMAT(9)
*      HMOD = ZERO
*      DO 60 J=1,3
*        ROTMAT(J+6)=ROTMAT(J+6)-ROTMAT(J+3)*PROD3-ROTMAT(J)*PROD2
*        HMOD = HMOD+ROTMAT(J)*ROTMAT(J)
*  60  CONTINUE
* == AV ==
      ROTMAT(7) = ROTMAT(2)*ROTMAT(6) - ROTMAT(3)*ROTMAT(5)
      ROTMAT(8) = ROTMAT(3)*ROTMAT(4) - ROTMAT(1)*ROTMAT(6)
      ROTMAT(9) = ROTMAT(1)*ROTMAT(5) - ROTMAT(2)*ROTMAT(4)
      HMOD = ZERO + ROTMAT(7)*ROTMAT(7) + ROTMAT(8)*ROTMAT(8)
     &            + ROTMAT(9)*ROTMAT(9)
* == AV ==
      HMOD = ONE/SQRT(HMOD)
      DO 70 J=7,9
        ROTMAT(J) = ROTMAT(J)*HMOD
  70  CONTINUE
C.
C.          Put back the matrix in place
      DO 80 J=1,9
        Q(JR+J)=ROTMAT(J)
  80  CONTINUE
C.
C.          Now recompute the angles
      DO 90 J=1,3
        ANGLES(J*2-1) = ACOS(MAX(-ONE,MIN(ONE,ROTMAT(J*3))))*RADDEG
        ANGLES(J*2)   = ZERO
        IF(ROTMAT(J*3-1).NE.ZERO) THEN
          ANGLES(J*2) = ATAN2(ROTMAT(J*3-1),ROTMAT(J*3-2))*RADDEG
          IF(ANGLES(2*J).LT.0.0) ANGLES(2*J) = ANGLES(2*J)+360.0
        ENDIF
  90  CONTINUE
      WRITE(CHMAIL,2000) NMAT
      CALL GMAIL(1,2)
      WRITE(CHMAIL,2001) (Q(JR+10+J),J=1,6)
      CALL GMAIL(0,0)
      WRITE(CHMAIL,2002) ANGLES
      CALL GMAIL(0,1)
C.
C.          Put back the angles in place
      DO 100 J=1,6
        Q(JR+10+J) = ANGLES(J)
 100  CONTINUE
C.
C.---       Orthonormalization ended
 110  CONTINUE
C
      DO 130 J = 1,3
        IP(J)  = 3
        JJR=JR+J*3-3
C
        DO 120 I = 1,3
          IF(ABS(Q(JJR+I)).LT.0.99999) GO TO 120
C
          IP(J)  = I + 3
          IF(Q(JJR+I).GE.0.) GO TO 130
C
          IP(J)  = 3 - I
          GO TO 130
C
 120    CONTINUE
 130  CONTINUE
C
      Q(JR + 10) = IP(1) + 10* IP(2) + 100* IP(3)
C
      IF(JR1.GT.0) THEN
         CALL GPROTM(-NMAT)
      ENDIF
C
1000  FORMAT(' *** GSROTM ***: Warning, rotation matrix redefinition:')
2000  FORMAT(' *** GSROTM ***: ',
     +       'Parameters of matrix no. ',I4,' changed:')
2001  FORMAT(' Old values: ',6(F14.7,3X))
2002  FORMAT(' New values: ',6(F14.7,3X))
 999  RETURN
      END
Commit	Line	Data
fe4da5cc	1	*
	2	* $Id$
	3	*
	4	* $Log$
	5	* Revision 1.1.1.1 1995/10/24 10:20:56 cernlib
	6	* Geant
	7	*
	8	*
	9	#include "geant321/pilot.h"
	10	*CMZ : 3.21/02 02/07/94 18.24.47 by S.Giani
	11	*-- Author :
	12	SUBROUTINE GSROTM(NMAT,THETA1,PHI1,THETA2,PHI2,THETA3,PHI3)
	13	C.
	14	C. ******************************************************************
	15	C. * *
	16	C. * STORE ROTATION MATRICES *
	17	C. * *
	18	C. * ==>Called by : <USER> *
	19	C. * Author R.Brun ********* *
	20	C. * *
	21	C. ******************************************************************
	22	C.
	23	#include "geant321/gcbank.inc"
	24	#include "geant321/gcunit.inc"
	25	#include "geant321/gconsp.inc"
	26	#include "geant321/gcnum.inc"
	27	DIMENSION ANGLES(6),IP(3)
	28	SAVE SINMIN
	29	#if defined(CERNLIB_SINGLE)
	30	DIMENSION ROTMAT(9)
	31	PARAMETER (ONE=1.0,ZERO=0.0)
	32	DATA SINMIN/1.00E-5/
	33	#endif
	34	#if !defined(CERNLIB_SINGLE)
	35	DOUBLE PRECISION ROTMAT(9),ONE,ZERO
	36	DOUBLE PRECISION PROD1,PROD2,PROD3,HMOD,SINMIN
	37	PARAMETER (ONE=1.D0,ZERO=0.D0)
	38	DATA SINMIN/1.00D-5/
	39	#endif
	40	C.
	41	C. ------------------------------------------------------------------
	42	C.
	43	IF(NMAT.LE.0)GO TO 999
	44	IF(JROTM.LE.0)CALL MZBOOK(IXCONS,JROTM,JROTM,1,'ROTM',NROTM,NROTM,
	45	+0,3,0)
	46	IF(NMAT.GT.IQ(JROTM-2)) THEN
	47	NPUSH=NMAT-IQ(JROTM-2)+50
	48	CALL MZPUSH(IXCONS,JROTM,NPUSH,0,'I')
	49	NROTM=IQ(JROTM-2)
	50	JR1=0
	51	ELSE
	52	JR1=LQ(JROTM-NMAT)
	53	IF(JR1.GT.0)THEN
	54	WRITE(CHMAIL,1000)
	55	CALL GMAIL(1,0)
	56	CALL GPROTM(NMAT)
	57	CALL MZDROP(IXCONS,LQ(JROTM-NMAT),' ')
	58	ENDIF
	59	ENDIF
	60	CALL MZBOOK(IXCONS,JR,JROTM,-NMAT,'ROTM',0,0,16,3,0)
	61	C
	62	Q(JR + 11) = THETA1
	63	Q(JR + 12) = PHI1
	64	Q(JR + 13) = THETA2
65	Q(JR + 14) = PHI2
66	Q(JR + 15) = THETA3
67	Q(JR + 16) = PHI3
68	C
69	DO 10 N = 1,3
70	THERAD = Q(JR+ 9+2N)DEGRAD
71	PHIRAD = Q(JR+10+2N)DEGRAD
72	SINTHE = SIN(THERAD)
73	Q(JR+3N-2) = SINTHE COS(PHIRAD)
74	Q(JR+3N-1) = SINTHE SIN(PHIRAD)
75	Q(JR+3*N ) = COS(THERAD)
76	CALL VUNIT (Q(JR+3N-2),Q(JR+3N-2),3)
77	10 CONTINUE
78	C.
79	C.--- Test orthonormality
80	DO 20 J=1,9
81	ROTMAT(J)=Q(JR+J)
82	20 CONTINUE
83	PROD2=ZERO
84	C.
85	C. X - Y
86	PROD1=
87	+ROTMAT(1)ROTMAT(4)+ROTMAT(2)ROTMAT(5)+ROTMAT(3)*ROTMAT(6)
88	IF(ABS(PROD1).GT.SINMIN) GO TO 30
89	C.
90	C. X - Z
91	PROD2=
92	+ROTMAT(1)ROTMAT(7)+ROTMAT(2)ROTMAT(8)+ROTMAT(3)*ROTMAT(9)
93	IF(ABS(PROD2).GT.SINMIN) GO TO 30
94	C.
95	C. Y - Z
96	PROD3=
97	+ROTMAT(7)ROTMAT(4)+ROTMAT(8)ROTMAT(5)+ROTMAT(9)*ROTMAT(6)
98	IF(ABS(PROD3).LE.SINMIN) GO TO 110
99	30 CONTINUE
100	C.
101	C.--- Orthonormalization needed
102	C.
103	C. Assume X correct
104	HMOD=ZERO
105	DO 40 J=4,6
106	ROTMAT(J)=ROTMAT(J)-ROTMAT(J-3)*PROD1
107	HMOD=HMOD+ROTMAT(J)*ROTMAT(J)
108	40 CONTINUE
109	HMOD=ONE/SQRT(HMOD)
110	DO 50 J=4,6
111	ROTMAT(J)=ROTMAT(J)*HMOD
112	50 CONTINUE
113	C.
114	C. Y done, do Z
115	C.
116	* IF(PROD2.EQ.ZERO) THEN
117	* PROD2=
118	* + ROTMAT(1)ROTMAT(7)+ROTMAT(2)ROTMAT(8)+ROTMAT(3)*ROTMAT(9)
119	* ENDIF
120	* PROD3=
121	* +ROTMAT(4)ROTMAT(7)+ROTMAT(5)ROTMAT(8)+ROTMAT(6)*ROTMAT(9)
122	* HMOD = ZERO
123	* DO 60 J=1,3
124	* ROTMAT(J+6)=ROTMAT(J+6)-ROTMAT(J+3)PROD3-ROTMAT(J)PROD2
125	* HMOD = HMOD+ROTMAT(J)*ROTMAT(J)
126	* 60 CONTINUE
127	* == AV ==
128	ROTMAT(7) = ROTMAT(2)ROTMAT(6) - ROTMAT(3)ROTMAT(5)
129	ROTMAT(8) = ROTMAT(3)ROTMAT(4) - ROTMAT(1)ROTMAT(6)
130	ROTMAT(9) = ROTMAT(1)ROTMAT(5) - ROTMAT(2)ROTMAT(4)
131	HMOD = ZERO + ROTMAT(7)ROTMAT(7) + ROTMAT(8)ROTMAT(8)
132	& + ROTMAT(9)*ROTMAT(9)
133	* == AV ==
134	HMOD = ONE/SQRT(HMOD)
135	DO 70 J=7,9
136	ROTMAT(J) = ROTMAT(J)*HMOD
137	70 CONTINUE
138	C.
139	C. Put back the matrix in place
140	DO 80 J=1,9
141	Q(JR+J)=ROTMAT(J)
142	80 CONTINUE
143	C.
144	C. Now recompute the angles
145	DO 90 J=1,3
146	ANGLES(J2-1) = ACOS(MAX(-ONE,MIN(ONE,ROTMAT(J3))))*RADDEG
147	ANGLES(J*2) = ZERO
148	IF(ROTMAT(J*3-1).NE.ZERO) THEN
149	ANGLES(J2) = ATAN2(ROTMAT(J3-1),ROTMAT(J3-2))RADDEG
150	IF(ANGLES(2J).LT.0.0) ANGLES(2J) = ANGLES(2*J)+360.0
151	ENDIF
152	90 CONTINUE
153	WRITE(CHMAIL,2000) NMAT
154	CALL GMAIL(1,2)
155	WRITE(CHMAIL,2001) (Q(JR+10+J),J=1,6)
156	CALL GMAIL(0,0)
157	WRITE(CHMAIL,2002) ANGLES
158	CALL GMAIL(0,1)
159	C.
160	C. Put back the angles in place
161	DO 100 J=1,6
162	Q(JR+10+J) = ANGLES(J)
163	100 CONTINUE
164	C.
165	C.--- Orthonormalization ended
166	110 CONTINUE
167	C
168	DO 130 J = 1,3
169	IP(J) = 3
170	JJR=JR+J*3-3
171	C
172	DO 120 I = 1,3
173	IF(ABS(Q(JJR+I)).LT.0.99999) GO TO 120
174	C
175	IP(J) = I + 3
176	IF(Q(JJR+I).GE.0.) GO TO 130
177	C
178	IP(J) = 3 - I
179	GO TO 130
180	C
181	120 CONTINUE
182	130 CONTINUE
183	C
184	Q(JR + 10) = IP(1) + 10* IP(2) + 100* IP(3)
185	C
186	IF(JR1.GT.0) THEN
187	CALL GPROTM(-NMAT)
188	ENDIF
189	C
190	1000 FORMAT(' * GSROTM *: Warning, rotation matrix redefinition:')
191	2000 FORMAT(' * GSROTM *: ',
192	+ 'Parameters of matrix no. ',I4,' changed:')
193	2001 FORMAT(' Old values: ',6(F14.7,3X))
194	2002 FORMAT(' New values: ',6(F14.7,3X))
195	999 RETURN
196	END