[线性代数] 高斯-赛德尔迭代解线性方程组的问题

    subroutine GS_solver (A,b,n,x)
        implicit none
        
        integer,parameter :: step = 5000  !最大迭代步数
        real   ,parameter :: err  = 1e-7 !误差
        
        integer :: i,j,k,n
        real    :: A(n,n),b(n),x0(n),x(n)
        real    :: x1(n)
        real    :: s,dx
        
        x0 = 0
        x1 = x0
        x  = x0

        
        do k = 1,step
            dx = 0
            
            do i = 1,n
               x1 = x0
               x1(i) = 0
               s = 0 
               s = dot_product( A(i,:), x1 )
               
               x(i) = ( b(i) - s ) / A(i,i) 
               
               dx = dx + ( x0(i)-x(i) )**2
               x0(i) = x(i) 
            end do
            
            dx = sqrt( dx )
            if ( dx < err ) exit
            
            
            x0 = x

    end do
    
 end subroutine GS_solver

    subroutine GSeidelSolver (A,b,n,x)
        implicit none
        
        integer   ,parameter :: MaxStep = 9000   !最大迭代步数
        real(8)   ,parameter :: cov  = 1d-8      !误差
        integer  :: i, j, k, n, step
        real(8)  :: A(n,n), b(n), x(n)
        real(8)  :: x0(n), x1(n)
        real(8)  :: dx, err
        
        x0 = 0d0
        x1 = 0d0
        x  = 0d0
        
        LOOP: do step = 1, MaxStep
            
            dx = 0d0
            do i = 1,n

                x1 = x0
                
                x1(i) = 0
                
                k = 0d0
                k = dot_product( A(i,:), x1 )
                
                x(i) = ( b(i) - k ) / A(i,i)
                dx = dx + ( x(i)-x0(i) )**2
                
                x0(i) = x(i) 
         
           end do
         
        err = dsqrt( dx )
                        
       if ( err < cov )  then
                exit LOOP
        end if

        x0 = x          

  end do LOOP         

 end subroutine GSeidelSolver

    !> @details 下三角矩阵求逆（稠密存储）
    !> @author Jackdaw毛毛
    !> @date 2019 年 8 月 26 日
    subroutine LowerTriMatrixInv(mat,matInv)
        real(fp),intent(in)    :: mat(:,:)    !< 输入矩阵，下三角，稠密存储
        real(fp),intent(inout) :: matInv(:,:) !< 输出矩阵，下三角，稠密存储，与输入矩阵关系：互为逆矩阵
        integer :: i,j,n 
        n = size(mat,dim=1)  

        matInv = 0.0_fp 
        do i = 1, n 
            matInv(i,i) = 1.0_fp / mat(i,i) 
        end do 
        do i = 2, n 
            do j = 1, i-1
                !do k = j, i-1 
                !   matInv(i,j) = matInv(i,j) + mat(i,k)*matInv(k,j)
                !end do 
                matInv(i,j) = dot_product( mat(i,j:i-1),matInv(j:i-1,j) )
                matInv(i,j) = matInv(i,j) * (-matInv(i,i)) 
            end do 
        end do 
    end subroutine 
    !> @details 高斯赛德尔迭代法计算 \f$ Ax=b \f$，收敛速度比雅克比迭代更快。
    !> @author Jackdaw毛毛
    !> @date 2019 年 8 月 26 日
    subroutine useGaussSeidelIterSolveAx_eq_b(matrixA,vectorB,vectorX)
        real(fp),intent(in)    :: matrixA(:,:) !< 系数矩阵（迭代收敛的充分条件：严格对角占优或对称正定）
        real(fp),intent(in)    :: vectorB(:)   !< 右端项
        real(fp),intent(inout) :: vectorX(:)   !< 解向量 
        
        real(fp),allocatable   :: G(:,:),tmpM(:,:),tmpB(:),tmpX(:)
        integer ::i,n 
        
        n = size(matrixA,dim=1)
        allocate(G(n,n),tmpM(n,n),tmpX(n),tmpB(n))
        ! 迭代格式
        ! x(k+1) = D^-1(b-Ux(k)-Lx(k+1)) 
        ! x(k+1) = (L+D)^-1(-Ux(k) + b )
        ! 构造迭代矩阵
        tmpM = 0.0_fp 
        do i = 1, n 
            tmpM(i:n,i) = matrixA(i:n,i) ! L+D
        end do 
        call LowerTriMatrixInv(tmpM,G)   ! 求逆，存在G里面
        tmpM = 0.0_fp 
        do i = 1, n-1
            tmpM(i,i+1:n) = matrixA(i,i+1:n) ! U
        end do 
        tmpB = matmul(G,vectorB) ! 常向量
        G = matmul(G,tmpM) ! 迭代矩阵
        deallocate(tmpM) 
        do i = 1, 1000
            tmpX = tmpB - matmul(G,vectorX) 
            if( maxval(abs(vectorX-tmpX)) .lt. 1.e-10_fp ) then 
                vectorX = vectorX + tmpX
                vectorX = vectorX / 2.0_fp
                exit
            else
                vectorX = tmpX
            endif 
        end do 
        if(i .ge. 1000 ) write(*,*) "GaussSeidel-iteration failed."
        deallocate( G,tmpX,tmpB )
    end subroutine  
    
    !> @details 高斯赛德尔迭代法计算 \f$ Ax=b \f$，测试算例。
    !> @author Jackdaw毛毛
    !> @date 2019 年 8 月 26 日
    subroutine useGaussSeidelIterSolveAx_eq_b_TEST
        real(fp) :: a(2,2),b(2),x(2)
        a(1,:) = [ 3.0_fp, 1.0_fp ]
        a(2,:) = [ 1.0_fp, 2.0_fp ] 
        b      = [ 5.0_fp, 5.0_fp ]
        x      = 0.0_fp
        call useGaussSeidelIterSolveAx_eq_b(a,b,x) 
        write(*,*) "解析解 x1 = 1.0, x2 = 2.0"
        write(*,*) "数值解",x
    end subroutine useGaussSeidelIterSolveAx_eq_b_TEST