You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
148 lines
4.6 KiB
148 lines
4.6 KiB
module subroutines
|
|
use parameters
|
|
use functions
|
|
implicit none
|
|
|
|
integer :: allostat, deallostat
|
|
public
|
|
contains
|
|
|
|
!This subroutine is just used to break the code and exit on an error
|
|
subroutine read_error_check(para, loc)
|
|
|
|
integer, intent(in) :: para
|
|
character(len=100), intent(in) :: loc
|
|
|
|
if (para > 0) then
|
|
print *, "Read error in ", trim(loc), " because of ", para
|
|
stop "Exit with error"
|
|
end if
|
|
|
|
end subroutine
|
|
|
|
subroutine matrix_inverse(a, n, a_inv)
|
|
|
|
integer :: i, j, k, piv_loc
|
|
integer, intent(in) :: n
|
|
real(kind = dp) :: coeff, sum_l, sum_u
|
|
real(kind = dp), dimension(n) :: b, x, y, b_piv
|
|
real(kind = dp), dimension(n, n) :: l, u, p
|
|
real(kind = dp), dimension(n, n), intent(in) :: a
|
|
real(kind = dp), dimension(n, n), intent(out) :: a_inv
|
|
real(kind = dp), allocatable :: v(:), u_temp(:), l_temp(:), p_temp(:)
|
|
|
|
l(:, :) = identity_mat(n)
|
|
u(:, :) = a(:, :)
|
|
p(:, :) = identity_mat(n)
|
|
!LU decomposition with partial pivoting
|
|
do j = 1, n-1
|
|
|
|
allocate(v(n-j+1), stat = allostat)
|
|
if(allostat /=0 ) then
|
|
print *, 'Fail to allocate v in matrix_inverse'
|
|
stop
|
|
end if
|
|
|
|
v(:) = u(j:n, j)
|
|
if(maxval(abs(v)) < lim_zero) then
|
|
print *, 'Fail to inverse matrix', a
|
|
stop
|
|
end if
|
|
|
|
piv_loc = maxloc(abs(v), 1)
|
|
deallocate(v, stat = deallostat)
|
|
|
|
if(deallostat /=0 ) then
|
|
print *, 'Fail to deallocate v in matrix_inverse'
|
|
stop
|
|
end if
|
|
!partial pivoting
|
|
if(piv_loc /= 1) then
|
|
|
|
allocate( u_temp(n-j+1), p_temp(n), stat = allostat)
|
|
if(allostat /=0 ) then
|
|
print *, 'Fail to allocate p_temp and/or u_temp in matrix_inverse'
|
|
stop
|
|
end if
|
|
|
|
u_temp(:) = u(j, j:n)
|
|
u(j, j:n) = u(piv_loc+j-1, j:n)
|
|
u(piv_loc+j-1, j:n) = u_temp(:)
|
|
p_temp(:) = p(j, :)
|
|
p(j, :) = p(piv_loc+j-1, :)
|
|
p(piv_loc+j-1, :) = p_temp(:)
|
|
|
|
deallocate( u_temp, p_temp, stat = deallostat)
|
|
if(deallostat /=0 ) then
|
|
print *, 'Fail to deallocate p_temp and/or u_temp in matrix_inverse'
|
|
stop
|
|
end if
|
|
|
|
if(j > 1) then
|
|
|
|
allocate( l_temp(j-1), stat = allostat)
|
|
if(allostat /= 0) then
|
|
print *, 'Fail to allocate l_temp in matrix_inverse'
|
|
stop
|
|
end if
|
|
|
|
l_temp(:) = l(j, 1:j-1)
|
|
l(j, 1:j-1) = l(piv_loc+j-1, 1:j-1)
|
|
l(piv_loc+j-1, 1:j-1) = l_temp(:)
|
|
|
|
deallocate( l_temp, stat = deallostat)
|
|
if(deallostat /=0 ) then
|
|
print *, 'Fail to deallocate l_temp in matrix_inverse'
|
|
stop
|
|
end if
|
|
end if
|
|
end if
|
|
|
|
!LU decomposition
|
|
do i = j+1, n
|
|
coeff = u(i, j)/u(j, j)
|
|
l(i, j) = coeff
|
|
u(i, j:n) = u(i, j:n)-coeff*u(j, j:n)
|
|
end do
|
|
|
|
end do
|
|
|
|
a_inv(:, :) = 0.0_dp
|
|
do j = 1, n
|
|
b(:) = 0.0_dp
|
|
b(j) = 1.0_dp
|
|
b_piv(:) = matmul(p, b)
|
|
!Now we have LUx = b_piv
|
|
!the first step is to solve y from Ly = b_piv
|
|
!forward substitution
|
|
do i = 1, n
|
|
if(i == 1) then
|
|
y(i) = b_piv(i)/l(i, i)
|
|
else
|
|
sum_l = 0
|
|
do k = 1, i-1
|
|
sum_l = sum_l+l(i, k)*y(k)
|
|
end do
|
|
y(i) = (b_piv(i)-sum_l)/l(i, i)
|
|
end if
|
|
end do
|
|
!then we solve x from ux = y
|
|
!backward subsitution
|
|
do i = n, 1, -1
|
|
if(i == n) then
|
|
x(i) = y(i)/u(i, i)
|
|
else
|
|
sum_u = 0
|
|
do k = i+1, n
|
|
sum_u = sum_u+u(i, k)*x(k)
|
|
end do
|
|
x(i) = (y(i)-sum_u)/u(i, i)
|
|
end if
|
|
end do
|
|
!put x into j column of a_inv
|
|
a_inv(:, j) = x(:)
|
|
end do
|
|
return
|
|
end subroutine matrix_inverse
|
|
|
|
end module subroutines |