GEANT321/ggeom/gsrotm.F

   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+2*N)*DEGRAD
  71         PHIRAD = Q(JR+10+2*N)*DEGRAD
  72         SINTHE = SIN(THERAD)
  73         Q(JR+3*N-2) = SINTHE * COS(PHIRAD)
  74         Q(JR+3*N-1) = SINTHE * SIN(PHIRAD)
  75         Q(JR+3*N  ) = COS(THERAD)
  76         CALL VUNIT (Q(JR+3*N-2),Q(JR+3*N-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(J*2-1) = ACOS(MAX(-ONE,MIN(ONE,ROTMAT(J*3))))*RADDEG
 147         ANGLES(J*2)   = ZERO
 148         IF(ROTMAT(J*3-1).NE.ZERO) THEN
 149           ANGLES(J*2) = ATAN2(ROTMAT(J*3-1),ROTMAT(J*3-2))*RADDEG
 150           IF(ANGLES(2*J).LT.0.0) ANGLES(2*J) = 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