C     DECOMP: DECOMPOSE MATRIX AND VECTOR REPRESENTATION TO 
C             ROTATION AROUND AXIS, PLUS TRANSLATION ALONG AXIS
C
C     Standard input: Transformation matrix and vector
C     Standard output: rotation axis, angle of rotation, etc.
C
C     Author: Randy Read
C
      REAL R1(3,3),V1(3)
      REAL DCOS(3),COSA,SINA,ANGLE,ANGOUT,PAR,P(3),Q(3),VEC(3),S(3)
      character*80 line
      logical demon,first
C
      first = .true.
C
C     Figure out whether this is DEMON style input or 4-line matrix+vector
C
      read(*,'(A80)') line
      read(line,*,end=10,err=10) iseq,(r1(1,j),j=1,3),v1(1)
      if (iseq .ne. 1) go to 10
      write(*,*) 'Input interpreted as DEMON-style input'
      demon = .true.
      go to 20
10    continue
      read(line,*,end=998,err=998) (r1(1,j),j=1,3)
      write(*,*) 'Input interpreted as 4-line matrix+vector'
      demon = .false.
C
20    continue
      do 22 i=1,3
        if ((i .ne. 1) .or. (.not. first)) then
          if (demon) then
            read(*,*,end=1000) iseq,(r1(i,j),j=1,3),v1(i)
            if (iseq .ne. i) go to 999
          else
            read(*,*,end=1000) (r1(i,j),j=1,3)
          endif
        endif
22    continue
      if (.not. demon) read(*,*,end=1000) v1
      first = .false.
C
      write(*,23) ((r1(i,j),j=1,3),i=1,3),v1
23    format(/,' =======================',//,' Input matrix/vector:',/,
     $  3(3f10.6,/),3f10.4,/)
      CALL DECOMP(R1,DCOS,COSA,SINA)
      ANGLE = ATAN2(SINA,COSA)
      ANGOUT = ANGLE*180.0/ACOS(-1.0)
      WRITE(*,25) DCOS,ANGOUT
25    FORMAT(' Direction cosines of rotation axis ',3F10.6,/,
     $       '     Rotation angle =',F10.4,' degrees',/)
C
C     COMPONENT OF V1 PARALLEL TO ROTATION AXIS IS GIVEN BY
C     P = (V1 DOT DCOS)*DCOS, AND COMPONENT PERPENDICULAR TO AXIS
C     IS THEN Q = V1 - P
C
      PAR = DOT(V1,DCOS)
      DO 30 I=1,3
        P(I) = PAR*DCOS(I)
        Q(I) = V1(I) - P(I)
30    CONTINUE
      WRITE(*,35) P,PAR
35    FORMAT(' Translation along axis=  ',3F10.4,/,
     $       '     corresponds to ',F10.4,' Angstroms')
C
C     DETERMINE LOCAL ORIGIN REQUIRED FOR ROTATION AXIS SO THAT
C     COMPONENT OF TRANSLATION PERPENDICULAR TO AXIS IS 0
C     SET UP BASIS VECTOR VEC, WHICH IS PERPENDICULAR TO ROTATION
C     AXIS AND OTHER BASIS VECTOR Q, TO DEFINE SHIFT VECTOR S.
C
      IF (ANGLE .GE. 0.001) GO TO 39
C
      WRITE(*,37)
37    FORMAT(/,' Since there is no rotational difference,',
     $       ' origin of rotation axis is undefined')
      go to 20
C
39    CONTINUE
      if (vecabs(q) .gt. 0.0) then
        CALL CROSS(DCOS,Q,VEC)
        call vnorm(vec)
        SABS = VECABS(Q)/(2.0*SIN(ANGLE/2.0))
        DO 40 I=1,3
          S(I) = Q(I)/2.0 + SABS*COS(ANGLE/2.0)*VEC(I)
40      CONTINUE
      else
        do 45 i=1,3
          s(i) = 0.0
45      continue
      endif
      WRITE(*,50) S
50    FORMAT(/,' Origin of rotation axis = ',3F10.4)
C    
C     check for consistency by applying transformation to rotation
C     axis origin.  But just use q = component of the translation
C     perpendicular to the rotation axis so that the origin is not
C     expected to move even with a screw operation.
C      
      call trnsfm(R1,s,q,vec)
      chk = (s(1)-vec(1))**2 + (s(2)-vec(2))**2 + (s(3)-vec(3))**2
      chk = sqrt(chk)
      if (chk .gt. 0.01) then
        write(*,99) vec,chk
   99   format(//,' FAILURE IN CONSISTENCY CHECK!!',/,
     $            ' Rotated origin =',10x,3f10.4,/,
     $            ' moving by',f10.4,' Angstroms!',//)
      endif
      go to 20
998   continue
      write(*,*) ' Error attempting to read vector+matrix input'
      stop
999   continue
      write(*,*) ' Error reading DEMON-style input!'
      stop
1000  continue
      stop
      END
      SUBROUTINE TRNSFM(RMAT,XOLD,T,XNEW)
      REAL RMAT(3,3),XOLD(3),T(3),XNEW(3)
      DO 10 I=1,3
        XNEW(I) = T(I)
        DO 10 J=1,3
          XNEW(I) = XNEW(I) + RMAT(I,J)*XOLD(J)
10    CONTINUE
      RETURN
      END
      SUBROUTINE DECOMP(R,DCOS,COSA,SINA)
      REAL R(3,3),DCOS(3),COSA,SINA
C
C     COSINE OF ROTATION ANGLE IS (TRACE - 1)/2
C
      COSA = (R(1,1) + R(2,2) + R(3,3) - 1.0)/2.0
      SIN2 = 1.0 - COSA**2
      IF (SIN2 .LE. 0.0) SINA = 0.0
      IF (SIN2 .GT. 0.0) SINA = SQRT(SIN2)
      RNORM = 1.0/SQRT(COSA**2 + SINA**2)
      COSA = RNORM*COSA
      SINA = RNORM*SINA
C
C     IF SINA .NE. 0, THEN GENERAL CASE
C
      IF (SINA .LT. 0.001) GO TO 20
C
C     GENERAL CASE--SEE INT TAB VOL II, PAGE 63, WHICH GIVES
C     TRANSPOSE OF MATRIX WE ARE USING, EXPRESSED IN TERMS OF
C     DIRECTION COSINES.
C
      DCOS(1) = (R(3,2) - R(2,3)) / (2.0*SINA)
      DCOS(2) = (R(1,3) - R(3,1)) / (2.0*SINA)
      DCOS(3) = (R(2,1) - R(1,2)) / (2.0*SINA)
      GO TO 50
20    CONTINUE
C
C     SPECIAL CASE--ROTATION ANGLE = 0 OR 180 DEGREES
C
      IF (COSA .LT. 0.0) GO TO 30
C
C     COSA APPROXIMATELY 1.0, IE. ANGLE = 0, THEREFORE
C     DIRECTION COSINES ARE ARBITRARY.
C
      DCOS(1) = 1.0
      DCOS(2) = 0.0
      DCOS(3) = 0.0
      RETURN
30    CONTINUE
C
C     COSA APPROXIMATELY -1.0, IE. ANGLE = 180, THEREFORE
C     SIGN OF 1 DIRECTION COSINE IS ARBITRARY
C
      DCOS(1) = SQRT((R(1,1) + 1.0)/2.0)
      DCOS(2) = SQRT((R(2,2) + 1.0)/2.0) * SIGN(1.0,R(1,2))
      DCOS(3) = SQRT((R(3,3) + 1.0)/2.0) * SIGN(1.0,R(1,3))
50    CONTINUE
C
C     NORMALIZE DIRECTION COSINES, TO CORRECT FOR NUMERICAL ERROR
C
      CALL VNORM(DCOS)
      RETURN
      END
      SUBROUTINE CROSS(VEC1,VEC2,VEC3)
      REAL VEC1(3),VEC2(3),VEC3(3)
      VEC3(1) = VEC1(2)*VEC2(3) - VEC1(3)*VEC2(2)
      VEC3(2) = VEC1(3)*VEC2(1) - VEC1(1)*VEC2(3)
      VEC3(3) = VEC1(1)*VEC2(2) - VEC1(2)*VEC2(1)
      RETURN
      END
      SUBROUTINE VNORM(VECTOR)
      REAL VECTOR(3),RNORM
      RNORM = VECABS(VECTOR)
      IF (RNORM .NE. 0.0) GO TO 5
C
      WRITE(*,2)
2     FORMAT(/,' ATTEMPT TO NORMALIZE VECTOR OF ZERO LENGTH',/,
     $       ' EXECUTION SUPPRESSED')
      STOP
C
5     CONTINUE
      DO 10 I=1,3
        VECTOR(I) = VECTOR(I)/RNORM
10    CONTINUE
      RETURN
      END
      FUNCTION DOT(VECT1,VECT2)
C
C     CALCULATE THE DOT PRODUCT BETWEEN 2 THREE DIMENSIONAL VECTORS
C
      REAL VECT1(3),VECT2(3)
        DOT=0.0
        DO 10 I=1,3
        DOT=DOT+VECT1(I)*VECT2(I)
10      CONTINUE
      RETURN
      END
      FUNCTION VECABS(VECTOR)
      REAL VECTOR(3)
      VECABS = SQRT(DOT(VECTOR,VECTOR))
      RETURN
      END
