Sources Esope | Cast3M

rota3d
C ROTA3D    SOURCE    CHAT      05/01/13    03:04:51     5004
      SUBROUTINE ROTA3D(ROTATS,TATORS,ROTATE,TATORE,
     .                  VECX,VECY,VECZ,ANGLE)
      IMPLICIT INTEGER(I-N)
      IMPLICIT REAL*8 (A-H,O-Z)
      DIMENSION ROTATS(6,6),TATORS(6,6),
     .          ROTATE(6,6),TATORE(6,6),
     .          R(6,6),VECX(3),VECY(3),VECZ(3)
C
C  CETTE ROUTINE CALCUL LA MATRICE DE ROTATION 3D ROTATS ET SON
C  INVERSE TATORS POUR SIGMA (ROTATE ET TATORE POUR EPSILON)
C
C  EXEMPLE :          SIGFI = ROTATS * SIGIN
C                     SIGIN = TATORS * SIGFI
C
C  ENTREES :
C
C         VECX   : VECTEUR PROPRE DE LA DIRECTION X
C         VECY   : VECTEUR PROPRE DE LA DIRECTION Y
C         VECZ   : VECTEUR PROPRE DE LA DIRECTION Z
C         ANGLE  : ANGLE DANS LE PLAN X-Y AVEC LE VECTEUR X (RADIAN)
C
C  SORTIES :
C
C         ROTATS : MATRICE DE ROTATION EN 3D POUR SIGMA
C         TATORS : MATRICE INVERSE
C
C         ROTATE : MATRICE DE ROTATION EN 3D POUR EPSILON
C         TATORE : MATRICE INVERSE
C
C     INITIALISATION
C
      X1=VECX(1)
      X2=VECX(2)
      X3=VECX(3)
      Y1=VECY(1)
      Y2=VECY(2)
      Y3=VECY(3)
      Z1=VECZ(1)
      Z2=VECZ(2)
      Z3=VECZ(3)
C
      C=COS(ANGLE)
      S=SIN(ANGLE)
      CC=C*C
      SS=S*S
      CS=C*S
C
      X1X1=X1*X1
      X2X2=X2*X2
      X3X3=X3*X3
C
      Y1Y1=Y1*Y1
      Y2Y2=Y2*Y2
      Y3Y3=Y3*Y3
C
      Z1Z1=Z1*Z1
      Z2Z2=Z2*Z2
      Z3Z3=Z3*Z3
C
      X1X2=X1*X2
      X2X3=X2*X3
      X3X1=X3*X1
C
      Y1Y2=Y1*Y2
      Y2Y3=Y2*Y3
      Y3Y1=Y3*Y1
C
      Z1Z2=Z1*Z2
      Z2Z3=Z2*Z3
      Z3Z1=Z3*Z1
C
      X1Y1=X1*Y1
      X2Y2=X2*Y2
      X3Y3=X3*Y3
C
      Y1Z1=Y1*Z1
      Y2Z2=Y2*Z2
      Y3Z3=Y3*Z3
C
      Z1X1=Z1*X1
      Z2X2=Z2*X2
      Z3X3=Z3*X3
C
      X1Y2=X1*Y2
      Y2Z3=Y2*Z3
      Z3X1=Z3*X1
C
      X2Y3=X2*Y3
      Y3Z1=Y3*Z1
      Z1X2=Z1*X2
C
      X3Y1=X3*Y1
      Y1Z2=Y1*Z2
      Z2X3=Z2*X3
C
      X2Y1=X2*Y1
      Y3Z2=Y3*Z2
      Z1X3=Z1*X3
C
      X3Y2=X3*Y2
      Y1Z3=Y1*Z3
      Z2X1=Z2*X1
C
      X1Y3=X1*Y3
      Y2Z1=Y2*Z1
      Z3X2=Z3*X2
C
      R(1,1)=X1X1*CC+Y1Y1*SS+2.D0*X1Y1*CS
      R(1,2)=X1X1*SS+Y1Y1*CC-2.D0*X1Y1*CS
      R(1,3)=Z1Z1
      R(1,4)=-X1X1*CS+X1Y1*(CC-SS)+Y1Y1*CS
      R(1,5)=-Z1X1*S+Y1Z1*C
      R(1,6)=Z1X1*C+Y1Z1*S
C
      R(2,1)=X2X2*CC+Y2Y2*SS+2.D0*X2Y2*CS
      R(2,2)=X2X2*SS+Y2Y2*CC-2.D0*X2Y2*CS
      R(2,3)=Z2Z2
      R(2,4)=-X2X2*CS+X2Y2*(CC-SS)+Y2Y2*CS
      R(2,5)=-Z2X2*S+Y2Z2*C
      R(2,6)=Z2X2*C+Y2Z2*S
C
      R(3,1)=X3X3*CC+Y3Y3*SS+2.D0*X3Y3*CS
      R(3,2)=X3X3*SS+Y3Y3*CC-2.D0*X3Y3*CS
      R(3,3)=Z3Z3
      R(3,4)=-X3X3*CS+X3Y3*(CC-SS)+Y3Y3*CS
      R(3,5)=-Z3X3*S+Y3Z3*C
      R(3,6)=Z3X3*C+Y3Z3*S
C
      R(4,1)=X1X2*CC+Y1Y2*SS+X1Y2*CS+X2Y1*CS
      R(4,2)=X1X2*SS+Y1Y2*CC-X1Y2*CS-X2Y1*CS
      R(4,3)=Z1Z2
      R(4,4)=-2.D0*X1X2*CS+X1Y2*(CC-SS)+X2Y1*(CC-SS)+2.D0*Y1Y2*CS
      R(4,5)=-Z1X2*S-Z2X1*S+Y1Z2*C+Y2Z1*C
      R(4,6)=Z1X2*C+Z2X1*C+Y1Z2*S+Y2Z1*S
C
      R(5,1)=X2X3*CC+Y2Y3*SS+X2Y3*CS+X3Y2*CS
      R(5,2)=X2X3*SS+Y2Y3*CC-X2Y3*CS-X3Y2*CS
      R(5,3)=Z2Z3
      R(5,4)=-2.D0*X2X3*CS+X2Y3*(CC-SS)+X3Y2*(CC-SS)+2.D0*Y2Y3*CS
      R(5,5)=-Z2X3*S-Z3X2*S+Y2Z3*C+Y3Z2*C
      R(5,6)=Z2X3*C+Z3X2*C+Y2Z3*S+Y3Z2*S
C
      R(6,1)=X3X1*CC+Y3Y1*SS+X3Y1*CS+X1Y3*CS
      R(6,2)=X3X1*SS+Y3Y1*CC-X3Y1*CS-X1Y3*CS
      R(6,3)=Z3Z1
      R(6,4)=-2.D0*X3X1*CS+X3Y1*(CC-SS)+X1Y3*(CC-SS)+2.D0*Y3Y1*CS
      R(6,5)=-Z3X1*S-Z1X3*S+Y3Z1*C+Y1Z3*C
      R(6,6)=Z3X1*C+Z1X3*C+Y3Z1*S+Y1Z3*S
C
      DO 10 I=1,6
      DO 10 J=1,6
      ROTATS(I,J)=R(I,J)
      TATORS(I,J)=R(J,I)
   10 CONTINUE
C
      DO 20 I=1,3
      DO 20 J=4,6
      ROTATS(I,J)=2.D0*ROTATS(I,J)
      TATORS(I,J)=2.D0*TATORS(I,J)
   20 CONTINUE
C
C  CAS DU TENSEUR DE DEFORMATIONS
C
      DO 30 I=1,6
      DO 30 J=1,6
      ROTATE(I,J)=R(I,J)
      TATORE(I,J)=R(J,I)
   30 CONTINUE
C
      DO 40 I=4,6
      DO 40 J=1,3
      ROTATE(I,J)=2.0D0*ROTATE(I,J)
      TATORE(I,J)=2.0D0*TATORE(I,J)
   40 CONTINUE
C
      RETURN
      END