diff --git a/src/modules/module_interfaces.f90 b/src/modules/module_interfaces.f90 index 7f0ea3e52..3ad88a7a2 100644 --- a/src/modules/module_interfaces.f90 +++ b/src/modules/module_interfaces.f90 @@ -1568,6 +1568,17 @@ FUNCTION util_kahan_sum(xsum_current, xi, xerror) END FUNCTION END INTERFACE + interface + function util_solve_linear_system(n,A,b) result(x) + use swiftest_globals + implicit none + integer(I4B), intent(in) :: n + real(DP), dimension(:,:), intent(in) :: A + real(DP), dimension(:), intent(in) :: b + real(DP), dimension(n) :: x + end function util_solve_linear_system + end interface + INTERFACE FUNCTION collresolve_resolve(model,m1,m2,r1,r2,p1,p2,v1,v2,n,mres,rres,pres,vres) USE swiftest_globals diff --git a/src/symba/symba_frag_pos.f90 b/src/symba/symba_frag_pos.f90 index f2eba05ac..271292a35 100644 --- a/src/symba/symba_frag_pos.f90 +++ b/src/symba/symba_frag_pos.f90 @@ -308,7 +308,7 @@ subroutine symba_frag_pos_ang_mtm(nfrag, xcom, vcom, L_orb, L_spin, m_frag, rad_ end do b(1:3) = -L_lin_others(:) b(4:6) = L_orb_old(:) - L_orb_others(:) - v_t_mag(1:6) = solve_wbs(ge_wpp(A, b)) + v_t_mag(1:6) = util_solve_linear_system(6, A, b) do i = 1, 6 v_frag(:, i) = v_t_mag(i) * v_t_unit(:, i) end do @@ -573,55 +573,7 @@ subroutine symba_frag_pos_energy_calc(npl, symba_plA, lexclude, ke_orbit, ke_spi return end subroutine symba_frag_pos_energy_calc - function solve_wbs(u) result(x) ! solve with backward substitution - !! Based on code available on Rosetta Code: https://rosettacode.org/wiki/Gaussian_elimination#Fortran - implicit none - ! Arguments - real(DP), intent(in), dimension(:,:), allocatable :: u - ! Result - real(DP), dimension(:), allocatable :: x - ! Internals - integer(I4B) :: i,n - - n = size(u,1) - allocate(x(n)) - do i = n,1,-1 - x(i) = (u(i, n + 1) - sum(u(i, i + 1:n) * x(i + 1:n))) / u(i, i) - end do - return - end function solve_wbs - - function ge_wpp(a, b) result(u) ! gaussian eliminate with partial pivoting - !! Solve Ax=b using Gaussian elimination then backwards substitution. - !! A being an n by n matrix. - !! x and b are n by 1 vectors. - !! Based on code available on Rosetta Code: https://rosettacode.org/wiki/Gaussian_elimination#Fortran - implicit none - ! Arguments - real(DP), dimension(:,:), intent(in) :: a - real(DP), dimension(:), intent(in) :: b - ! Result - real(DP), dimension(:,:), allocatable :: u - ! Internals - integer(I4B) :: i,j,n,p - real(DP) :: upi - - n = size(a, 1) - allocate(u(n, (n + 1))) - u = reshape([a, b], [n, n + 1]) - do j = 1, n - p = maxloc(abs(u(j:n, j)), 1) + j - 1 ! maxloc returns indices between (1, n - j + 1) - if (p /= j) u([p, j], j) = u([j, p], j) - u(j + 1:, j) = u(j + 1:, j) / u(j, j) - do i = j + 1, n + 1 - upi = u(p, i) - if (p /= j) u([p, j], i) = u([j, p], i) - u(j + 1:n, i) = u(j + 1:n, i) - upi * u(j + 1:n, j) - end do - end do - return - end function ge_wpp diff --git a/src/util/util_solve_linear_system.f90 b/src/util/util_solve_linear_system.f90 new file mode 100644 index 000000000..cd42a5578 --- /dev/null +++ b/src/util/util_solve_linear_system.f90 @@ -0,0 +1,77 @@ +function util_solve_linear_system(n,A,b) result(x) + !! Author: David A. Minton + !! + !! Solves the linear equation of the form A*x = b for x. + !! A is an (n,n) arrays + !! x and b are (n) arrays + !! Uses Gaussian elimination, so will have issues if system is ill-conditioned. + !! Uses quad precision intermidiate values, so works best on small arrays. + use swiftest + use module_interfaces, EXCEPT_THIS_ONE => util_solve_linear_system + implicit none + ! Arguments + integer(I4B), intent(in) :: n + real(DP), dimension(:,:), intent(in) :: A + real(DP), dimension(:), intent(in) :: b + ! Result + real(DP), dimension(n) :: x + ! Internals + real(QP), dimension(:), allocatable :: qx + + qx = solve_wbs(ge_wpp(real(A, kind=QP), real(b, kind=QP))) + x = real(qx, kind=DP) + return + + + contains + + function solve_wbs(u) result(x) ! solve with backward substitution + !! Based on code available on Rosetta Code: https://rosettacode.org/wiki/Gaussian_elimination#Fortran + implicit none + ! Arguments + real(QP), intent(in), dimension(:,:), allocatable :: u + ! Result + real(QP), dimension(:), allocatable :: x + ! Internals + integer(I4B) :: i,n + + n = size(u,1) + allocate(x(n)) + do i = n,1,-1 + x(i) = (u(i, n + 1) - sum(u(i, i + 1:n) * x(i + 1:n))) / u(i, i) + end do + return + end function solve_wbs + + function ge_wpp(A, b) result(u) ! gaussian eliminate with partial pivoting + !! Solve Ax=b using Gaussian elimination then backwards substitution. + !! A being an n by n matrix. + !! x and b are n by 1 vectors. + !! Based on code available on Rosetta Code: https://rosettacode.org/wiki/Gaussian_elimination#Fortran + implicit none + ! Arguments + real(QP), dimension(:,:), intent(in) :: A + real(QP), dimension(:), intent(in) :: b + ! Result + real(QP), dimension(:,:), allocatable :: u + ! Internals + integer(I4B) :: i,j,n,p + real(QP) :: upi + + n = size(a, 1) + allocate(u(n, (n + 1))) + u = reshape([A, b], [n, n + 1]) + do j = 1, n + p = maxloc(abs(u(j:n, j)), 1) + j - 1 ! maxloc returns indices between (1, n - j + 1) + if (p /= j) u([p, j], j) = u([j, p], j) + u(j + 1:, j) = u(j + 1:, j) / u(j, j) + do i = j + 1, n + 1 + upi = u(p, i) + if (p /= j) u([p, j], i) = u([j, p], i) + u(j + 1:n, i) = u(j + 1:n, i) - upi * u(j + 1:n, j) + end do + end do + return + end function ge_wpp + + end function util_solve_linear_system \ No newline at end of file