135 lines
2.2 KiB
Fortran
135 lines
2.2 KiB
Fortran
! Rotation matrix with the iterative method
|
|
|
|
! \begin{align*}
|
|
! \textbf{R} = \sum_{k=0}^{\infty} \frac{1}{k!} \textbf{X}^k
|
|
! \end{align*}
|
|
|
|
! !!! Doesn't work !!!
|
|
|
|
|
|
subroutine rotation_matrix_iterative(m,X,R)
|
|
|
|
implicit none
|
|
|
|
! in
|
|
integer, intent(in) :: m
|
|
double precision, intent(in) :: X(m,m)
|
|
|
|
! out
|
|
double precision, intent(out) :: R(m,m)
|
|
|
|
! internal
|
|
double precision :: max_elem, pre_factor
|
|
double precision :: t1,t2,t3
|
|
integer :: k,l,i,j
|
|
logical :: not_converged
|
|
double precision, allocatable :: RRT(:,:), A(:,:), B(:,:)
|
|
|
|
! Functions
|
|
integer :: factorial
|
|
|
|
print*,'---rotation_matrix_iterative---'
|
|
call wall_time(t1)
|
|
|
|
allocate(RRT(m,m),A(m,m),B(m,m))
|
|
|
|
! k = 0
|
|
R = 0d0
|
|
do i = 1, m
|
|
R(i,i) = 1d0
|
|
enddo
|
|
|
|
! k = 1
|
|
R = R + X
|
|
|
|
k = 2
|
|
|
|
not_converged = .True.
|
|
|
|
do while (not_converged)
|
|
|
|
pre_factor = 1d0/DBLE(factorial(k))
|
|
if (pre_factor < 1d-15) then
|
|
print*,'pre factor=', pre_factor,'< 1d-15, exit'
|
|
exit
|
|
endif
|
|
|
|
A = X
|
|
B = 0d0
|
|
do l = 1, k-1
|
|
call dgemm('N','N',m,m,m,1d0,X,size(X,1),A,size(A,1),0d0,B,size(B,1))
|
|
A = B
|
|
enddo
|
|
|
|
!print*,'B'
|
|
!do i = 1, m
|
|
! print*,B(i,:) * 1d0/DBLE(factorial(k))
|
|
!enddo
|
|
|
|
R = R + pre_factor * B
|
|
|
|
k = k + 1
|
|
call dgemm('T','N',m,m,m,1d0,R,size(R,1),R,size(R,1),0d0,RRT,size(RRT,1))
|
|
|
|
!print*,'R'
|
|
!do i = 1, m
|
|
! write(*,'(10(ES12.5))') R(i,:)
|
|
!enddo
|
|
|
|
do i = 1, m
|
|
RRT(i,i) = RRT(i,i) - 1d0
|
|
enddo
|
|
|
|
!print*,'RRT'
|
|
!do i = 1, m
|
|
! write(*,'(10(ES12.5))') RRT(i,:)
|
|
!enddo
|
|
|
|
max_elem = 0d0
|
|
do j = 1, m
|
|
do i = 1, m
|
|
if (dabs(RRT(i,j)) > max_elem) then
|
|
max_elem = dabs(RRT(i,j))
|
|
endif
|
|
enddo
|
|
enddo
|
|
|
|
print*, 'Iteration:', k
|
|
print*, 'Max error in R:', max_elem
|
|
|
|
if (max_elem < 1d-12) then
|
|
not_converged = .False.
|
|
endif
|
|
|
|
enddo
|
|
|
|
deallocate(RRT,A,B)
|
|
|
|
call wall_time(t2)
|
|
t3 = t2 - t1
|
|
print*,'Time in rotation matrix iterative:', t3
|
|
print*,'---End roration_matrix_iterative---'
|
|
|
|
|
|
print*,'Does not work yet, abort'
|
|
call abort
|
|
|
|
end
|
|
|
|
! Factorial
|
|
|
|
function factorial(n)
|
|
|
|
implicit none
|
|
|
|
integer, intent(in) :: n
|
|
integer :: factorial, k
|
|
|
|
factorial = 1
|
|
|
|
do k = 1, n
|
|
factorial = factorial * k
|
|
enddo
|
|
|
|
end
|