Euler angles/Code

From Knowino
Jump to: navigation, search

Fortran-77 subroutine to compute the Euler factorization of a proper orthogonal 3×3 matirx A. Routine returns Euler angles of the factorization

 \mathbf{A} = \mathbf{R}_z(\alpha) \mathbf{R}_y(\beta)\mathbf{R}_z(\gamma)
 Subroutine Euler(A, alpha,beta, gamma)

C     Determine Euler angles of the proper rotation matrix A.
C     All conventions according to Biedenharn & Louck
C     Author P.E.S. Wormer 1985

      implicit real*8(a-h,o-z)
      parameter (thresh = 1.d-50)
      real*8 A(3,3)

C--Check if matrix is orthogonal with unit determinant.
      Call Check(A)

C--Get polar angles of third column
      Call Polar(A(1,3), alpha, beta)

C--Compute gamma
      Call Comgamma(A, alpha, beta, gamma)

      end
      Subroutine Check(A)

C     Check if matrix is orthogonal with unit determinant.

      implicit real*8(a-h,o-z)
      parameter (thresh = 1.d-12)
      real*8 A(3,3)
      do i=1,3
         do j=1,i-1
            t = 0.d0
            do k=1,3
               t = t + A(i,k)*A(j,k)
            enddo
            if (abs(t) .gt. thresh) then
               write(*,'(1x,a,i1,a,i1,a,d12.5 )')
     1         ' Row ',i, ' and ', j, 'non-orthogonal',abs(t)
               stop 'non-orthogonal'
            endif
         enddo
         t = 0.d0
         do k=1,3
            t = t + A(i,k)*A(i,k)
         enddo
         if (abs(t-1.d0) .gt. thresh) then
            write(*,'(1x,a,i1,a,d25.15 )')
     1      ' Row ',i, ' non-normalized:' , t
            stop 'non-normalized'
         endif
      enddo

      t = det(A)
      if (abs(t-1.d0) .gt. thresh) then
         if (abs(t+1.d0) .lt. thresh) then
            write(*,'(//1x,a,1x,d14.6,a)')
     .     'Non-unit determinant:',t, ' will interchange column 1 and 2'
            do i=1,3
               T = A(i,2)
               A(i,2) = A(i,1)
               A(i,1) = T
            enddo
         else
         write(*,'(//1x,a,d20.10,a)')
     .      'Non-unit determinant:',t
            stop ' Determinant'
         endif
      endif

      end
      real*8 function det(A)

C     determinant of A

      implicit real*8(a-h,o-z)
      real*8 A(3,3), B(3)

      B(1) = A(2,2)*A(3,3) - A(3,2)*A(2,3)
      B(2) = A(3,2)*A(1,3) - A(1,2)*A(3,3)
      B(3) = A(1,2)*A(2,3) - A(2,2)*A(1,3)
      det = 0.d0
      do i=1,3
         det = det + A(i,1)*B(i)
      enddo
      end
      Subroutine Polar(A, alpha, beta)

C     Get polar angles of vector A.

      implicit real*8(a-h,o-z)
      parameter (thresh = 1.d-50)
      real*8 A(3)

      R = sqrt( A(1)**2 + A(2)**2 + A(3)**2 )
      if ( abs(R) .lt. thresh) then
         write(*,'(a)') ' zero vector'
         alpha = 0.d0
         beta  = 0.d0
         return
      endif

      beta = acos(A(3)/R)
      cb   = abs (A(3)/R)
      if ( abs(cb-1.d0) .lt. thresh ) then
         alpha = 0.d0
         return
      endif
      alpha = acos( A(1) / (sqrt( A(1)**2 + A(2)**2 ) ) )
      if ( A(2) .lt. 0.d0 ) then
         pi = acos(-1.d0)
         alpha = 2.d0*pi - alpha
      endif

      end

      Subroutine Comgamma(A, alpha,beta, gamma)

C     Compute third Euler angle gamma.

      implicit real*8(a-h,o-z)
      parameter (thresh = 1.d-50)
      real*8 A(3,3), B1(3), B2(3)

      cb = cos(beta)
      sb = sin(beta)
      ca = cos(alpha)
      sa = sin(alpha)
      B1(1) = ca*cb
      B1(2) = sa*cb
      B1(3) = -sb
      B2(1) = -sa
      B2(2) =  ca
      B2(3) =  0.d0
      cg = 0.d0
      sg = 0.d0
      do i=1,3
        cg = cg + B1(i)*A(i,1)
        sg = sg + B2(i)*A(i,1)
      enddo
      gamma = acos(cg)
      if ( sg  .lt. 0.d0 ) then
         pi = acos(-1.d0)
         gamma = 2.d0*pi - gamma
      endif

      end     
Personal tools
Variants
Actions
Navigation
Community
Toolbox