Files
FemLite/src/solver/fem_linear_solver_cg.f90
T
2026-06-24 17:29:36 +08:00

180 lines
4.7 KiB
Fortran

module fem_linear_solver_cg
use fem_precision
use fem_error
use fem_sparse_csr
implicit none
private
public :: linear_cg_solve
contains
subroutine linear_cg_solve(a, b, x, max_iter, tol, iter, res_norm, ierr)
type(csr_matrix), intent(in) :: a
real(rk), intent(in) :: b(:)
real(rk), intent(inout) :: x(:)
integer, intent(in) :: max_iter
real(rk), intent(in) :: tol
integer, intent(out) :: iter
real(rk), intent(out) :: res_norm
integer, intent(out) :: ierr
real(rk), allocatable :: r(:)
real(rk), allocatable :: z(:)
real(rk), allocatable :: p(:)
real(rk), allocatable :: ap(:)
real(rk), allocatable :: diag_inv(:)
real(rk) :: alpha
real(rk) :: beta
real(rk) :: denom
real(rk) :: norm_b
real(rk) :: rsold
real(rk) :: rsnew
real(rk) :: threshold
integer :: n
integer :: stat
iter = 0
res_norm = ZERO
if (a%nrow .le. 0 .or. a%ncol .le. 0) then
call error_set(FEM_ERR_SOLVER, 'CG solver received an empty matrix', ierr)
return
end if
if (a%nrow .ne. a%ncol) then
call error_set(FEM_ERR_SOLVER, 'CG solver requires a square matrix', ierr)
return
end if
if (size(b) .ne. a%nrow .or. size(x) .ne. a%ncol) then
call error_set(FEM_ERR_SOLVER, 'CG solver vector size mismatch', ierr)
return
end if
if (max_iter .le. 0) then
call error_set(FEM_ERR_SOLVER, 'CG solver max_iter must be positive', ierr)
return
end if
if (tol .le. ZERO) then
call error_set(FEM_ERR_SOLVER, 'CG solver tolerance must be positive', ierr)
return
end if
n = a%nrow
allocate(r(n), z(n), p(n), ap(n), diag_inv(n), stat=stat)
if (stat .ne. 0) then
call error_set(FEM_ERR_SOLVER, 'failed to allocate CG workspace', ierr)
return
end if
diag_inv = ONE
call build_jacobi_inverse(a, diag_inv, ierr)
if (ierr .ne. FEM_SUCCESS) go to 900
call csr_matvec(a, x, ap, ierr)
if (ierr .ne. FEM_SUCCESS) go to 900
r = b - ap
call apply_jacobi_inverse(diag_inv, r, z)
p = z
norm_b = sqrt(dot_product(b, b))
threshold = tol * max(ONE, norm_b)
rsold = dot_product(r, r)
res_norm = sqrt(rsold)
if (res_norm .le. threshold) then
ierr = FEM_SUCCESS
go to 900
end if
rsold = dot_product(r, z)
if (rsold .le. tiny(ONE)) then
call error_set(FEM_ERR_SOLVER, 'CG solver detected invalid preconditioned residual', ierr)
go to 900
end if
do iter = 1, max_iter
call csr_matvec(a, p, ap, ierr)
if (ierr .ne. FEM_SUCCESS) go to 900
denom = dot_product(p, ap)
if (denom .le. tiny(ONE)) then
call error_set(FEM_ERR_SOLVER, 'CG solver detected non-positive curvature', ierr)
go to 900
end if
alpha = rsold / denom
x = x + alpha * p
r = r - alpha * ap
rsnew = dot_product(r, r)
res_norm = sqrt(rsnew)
if (res_norm .le. threshold) then
ierr = FEM_SUCCESS
go to 900
end if
call apply_jacobi_inverse(diag_inv, r, z)
rsnew = dot_product(r, z)
if (rsnew .le. tiny(ONE)) then
call error_set(FEM_ERR_SOLVER, 'CG solver detected invalid preconditioned update', ierr)
go to 900
end if
beta = rsnew / rsold
p = z + beta * p
rsold = rsnew
end do
iter = max_iter
call error_set(FEM_ERR_SOLVER, 'CG solver did not converge', ierr)
900 continue
if (allocated(r)) deallocate(r)
if (allocated(z)) deallocate(z)
if (allocated(p)) deallocate(p)
if (allocated(ap)) deallocate(ap)
if (allocated(diag_inv)) deallocate(diag_inv)
end subroutine linear_cg_solve
subroutine build_jacobi_inverse(a, diag_inv, ierr)
type(csr_matrix), intent(in) :: a
real(rk), intent(inout) :: diag_inv(:)
integer, intent(out) :: ierr
real(rk) :: diag
integer :: row
integer :: k
diag_inv = ONE
if (size(diag_inv) .ne. a%nrow) then
call error_set(FEM_ERR_SOLVER, 'Jacobi preconditioner size mismatch', ierr)
return
end if
do row = 1, a%nrow
diag = ZERO
do k = a%row_ptr(row), a%row_ptr(row+1) - 1
if (a%col_ind(k) .eq. row) then
diag = a%val(k)
exit
end if
end do
if (abs(diag) .gt. tiny(ONE)) then
diag_inv(row) = ONE / abs(diag)
end if
end do
ierr = FEM_SUCCESS
end subroutine build_jacobi_inverse
subroutine apply_jacobi_inverse(diag_inv, r, z)
real(rk), intent(in) :: diag_inv(:)
real(rk), intent(in) :: r(:)
real(rk), intent(out) :: z(:)
integer :: i
integer :: n
n = min(size(z), min(size(diag_inv), size(r)))
z = ZERO
do i = 1, n
z(i) = diag_inv(i) * r(i)
end do
end subroutine apply_jacobi_inverse
end module fem_linear_solver_cg