802 lines
23 KiB
Plaintext
Executable File
802 lines
23 KiB
Plaintext
Executable File
|
|
!
|
|
! This program may be freely redistributed under the
|
|
! condition that the copyright notices (including this
|
|
! entire header) are not removed, and no compensation
|
|
! is received through use of the software. Private,
|
|
! research, and institutional use is free. You may
|
|
! distribute modified versions of this code UNDER THE
|
|
! CONDITION THAT THIS CODE AND ANY MODIFICATIONS MADE
|
|
! TO IT IN THE SAME FILE REMAIN UNDER COPYRIGHT OF THE
|
|
! ORIGINAL AUTHOR, BOTH SOURCE AND OBJECT CODE ARE
|
|
! MADE FREELY AVAILABLE WITHOUT CHARGE, AND CLEAR
|
|
! NOTICE IS GIVEN OF THE MODIFICATIONS. Distribution
|
|
! of this code as part of a commercial system is
|
|
! permissible ONLY BY DIRECT ARRANGEMENT WITH THE
|
|
! AUTHOR. (If you are not directly supplying this
|
|
! code to a customer, and you are instead telling them
|
|
! how they can obtain it for free, then you are not
|
|
! required to make any arrangement with me.)
|
|
!
|
|
! Disclaimer: Neither I nor: Columbia University, the
|
|
! National Aeronautics and Space Administration, nor
|
|
! the Massachusetts Institute of Technology warrant
|
|
! or certify this code in any way whatsoever. This
|
|
! code is provided "as-is" to be used at your own risk.
|
|
!
|
|
!
|
|
|
|
!
|
|
! INV.h90: block-wise solution of small linear systems.
|
|
!
|
|
! Darren Engwirda
|
|
! 25-Mar-2019
|
|
! de2363 [at] columbia [dot] edu
|
|
!
|
|
!
|
|
|
|
pure subroutine inv_2x2(amat,adim,ainv,vdim, &
|
|
& adet)
|
|
|
|
implicit none
|
|
|
|
!------------------------------------------- arguments !
|
|
integer, intent( in) :: adim
|
|
real*8 , intent( in) :: amat(adim,*)
|
|
integer, intent( in) :: vdim
|
|
real*8 , intent(out) :: ainv(vdim,*)
|
|
real*8 , intent(out) :: adet
|
|
|
|
!------------------------------------------- form A^-1 !
|
|
|
|
adet = amat(1,1) * amat(2,2) &
|
|
- amat(1,2) * amat(2,1)
|
|
|
|
ainv(1,1) = amat(2,2)
|
|
ainv(1,2) = - amat(1,2)
|
|
ainv(2,1) = - amat(2,1)
|
|
ainv(2,2) = amat(1,1)
|
|
|
|
return
|
|
|
|
end subroutine
|
|
|
|
pure subroutine inv_3x3(amat,adim,ainv,vdim, &
|
|
& adet)
|
|
|
|
implicit none
|
|
|
|
!------------------------------------------- arguments !
|
|
integer, intent( in) :: adim
|
|
real*8 , intent( in) :: amat(adim,*)
|
|
integer, intent( in) :: vdim
|
|
real*8 , intent(out) :: ainv(vdim,*)
|
|
real*8 , intent(out) :: adet
|
|
|
|
!------------------------------------------- variables !
|
|
real*8 :: &
|
|
aa2233,aa2332,aa2133,aa2331,aa2132,&
|
|
aa2231,aa1233,aa1332,aa1223,aa1322,&
|
|
aa1133,aa1331,aa1123,aa1321,aa1132,&
|
|
aa1231,aa1122,aa1221
|
|
|
|
!------------------------------------------- form A^-1 !
|
|
|
|
aa2233 = amat(2,2) * amat(3,3)
|
|
aa2332 = amat(2,3) * amat(3,2)
|
|
aa2133 = amat(2,1) * amat(3,3)
|
|
aa2331 = amat(2,3) * amat(3,1)
|
|
aa2132 = amat(2,1) * amat(3,2)
|
|
aa2231 = amat(2,2) * amat(3,1)
|
|
|
|
adet = &
|
|
amat(1,1) * (aa2233 - aa2332) - &
|
|
amat(1,2) * (aa2133 - aa2331) + &
|
|
amat(1,3) * (aa2132 - aa2231)
|
|
|
|
aa1233 = amat(1,2) * amat(3,3)
|
|
aa1332 = amat(1,3) * amat(3,2)
|
|
aa1223 = amat(1,2) * amat(2,3)
|
|
aa1322 = amat(1,3) * amat(2,2)
|
|
aa1133 = amat(1,1) * amat(3,3)
|
|
aa1331 = amat(1,3) * amat(3,1)
|
|
aa1123 = amat(1,1) * amat(2,3)
|
|
aa1321 = amat(1,3) * amat(2,1)
|
|
aa1132 = amat(1,1) * amat(3,2)
|
|
aa1231 = amat(1,2) * amat(3,1)
|
|
aa1122 = amat(1,1) * amat(2,2)
|
|
aa1221 = amat(1,2) * amat(2,1)
|
|
|
|
ainv(1,1) = (aa2233 - aa2332)
|
|
ainv(1,2) = -(aa1233 - aa1332)
|
|
ainv(1,3) = (aa1223 - aa1322)
|
|
|
|
ainv(2,1) = -(aa2133 - aa2331)
|
|
ainv(2,2) = (aa1133 - aa1331)
|
|
ainv(2,3) = -(aa1123 - aa1321)
|
|
|
|
ainv(3,1) = (aa2132 - aa2231)
|
|
ainv(3,2) = -(aa1132 - aa1231)
|
|
ainv(3,3) = (aa1122 - aa1221)
|
|
|
|
return
|
|
|
|
end subroutine
|
|
|
|
pure subroutine mul_2x2(amat,adim,bmat,bdim, &
|
|
& scal,cmat,cdim)
|
|
|
|
implicit none
|
|
|
|
!------------------------------------------- arguments !
|
|
integer, intent(in) :: adim
|
|
real*8 , intent(in) :: amat(adim,*)
|
|
integer, intent(in) :: bdim
|
|
real*8 , intent(in) :: bmat(bdim,*)
|
|
real*8 , intent(in) :: scal
|
|
integer, intent(in) :: cdim
|
|
real*8 , intent(inout) :: cmat(cdim,*)
|
|
|
|
!-------------------------------- C = C + scal * A * B !
|
|
|
|
if (scal .eq. +1.e0_8) then
|
|
|
|
cmat(1,1) = cmat(1,1) &
|
|
+ ( amat(1,1) * bmat(1,1) &
|
|
+ amat(1,2) * bmat(2,1) )
|
|
cmat(2,1) = cmat(2,1) &
|
|
+ ( amat(2,1) * bmat(1,1) &
|
|
+ amat(2,2) * bmat(2,1) )
|
|
|
|
cmat(1,2) = cmat(1,2) &
|
|
+ ( amat(1,1) * bmat(1,2) &
|
|
+ amat(1,2) * bmat(2,2) )
|
|
cmat(2,2) = cmat(2,2) &
|
|
+ ( amat(2,1) * bmat(1,2) &
|
|
+ amat(2,2) * bmat(2,2) )
|
|
|
|
else &
|
|
if (scal .eq. -1.e0_8) then
|
|
|
|
cmat(1,1) = cmat(1,1) &
|
|
- ( amat(1,1) * bmat(1,1) &
|
|
+ amat(1,2) * bmat(2,1) )
|
|
cmat(2,1) = cmat(2,1) &
|
|
- ( amat(2,1) * bmat(1,1) &
|
|
+ amat(2,2) * bmat(2,1) )
|
|
|
|
cmat(1,2) = cmat(1,2) &
|
|
- ( amat(1,1) * bmat(1,2) &
|
|
+ amat(1,2) * bmat(2,2) )
|
|
cmat(2,2) = cmat(2,2) &
|
|
- ( amat(2,1) * bmat(1,2) &
|
|
+ amat(2,2) * bmat(2,2) )
|
|
|
|
else
|
|
|
|
cmat(1,1) = cmat(1,1) + &
|
|
scal * ( amat(1,1) * bmat(1,1) &
|
|
+ amat(1,2) * bmat(2,1) )
|
|
cmat(2,1) = cmat(2,1) + &
|
|
scal * ( amat(2,1) * bmat(1,1) &
|
|
+ amat(2,2) * bmat(2,1) )
|
|
|
|
cmat(1,2) = cmat(1,2) + &
|
|
scal * ( amat(1,1) * bmat(1,2) &
|
|
+ amat(1,2) * bmat(2,2) )
|
|
cmat(2,2) = cmat(2,2) + &
|
|
scal * ( amat(2,1) * bmat(1,2) &
|
|
+ amat(2,2) * bmat(2,2) )
|
|
|
|
end if
|
|
|
|
return
|
|
|
|
end subroutine
|
|
|
|
pure subroutine mul_3x3(amat,adim,bmat,bdim, &
|
|
& scal,cmat,cdim)
|
|
|
|
implicit none
|
|
|
|
!------------------------------------------- arguments !
|
|
integer, intent(in) :: adim
|
|
real*8 , intent(in) :: amat(adim,*)
|
|
integer, intent(in) :: bdim
|
|
real*8 , intent(in) :: bmat(bdim,*)
|
|
real*8 , intent(in) :: scal
|
|
integer, intent(in) :: cdim
|
|
real*8 , intent(inout) :: cmat(cdim,*)
|
|
|
|
!-------------------------------- C = C + scal * A * B !
|
|
|
|
if (scal .eq. +1.e0_8) then
|
|
|
|
cmat(1,1) = cmat(1,1) &
|
|
+ ( amat(1,1) * bmat(1,1) &
|
|
+ amat(1,2) * bmat(2,1) &
|
|
+ amat(1,3) * bmat(3,1) )
|
|
cmat(2,1) = cmat(2,1) &
|
|
+ ( amat(2,1) * bmat(1,1) &
|
|
+ amat(2,2) * bmat(2,1) &
|
|
+ amat(2,3) * bmat(3,1) )
|
|
cmat(3,1) = cmat(3,1) &
|
|
+ ( amat(3,1) * bmat(1,1) &
|
|
+ amat(3,2) * bmat(2,1) &
|
|
+ amat(3,3) * bmat(3,1) )
|
|
|
|
cmat(1,2) = cmat(1,2) &
|
|
+ ( amat(1,1) * bmat(1,2) &
|
|
+ amat(1,2) * bmat(2,2) &
|
|
+ amat(1,3) * bmat(3,2) )
|
|
cmat(2,2) = cmat(2,2) &
|
|
+ ( amat(2,1) * bmat(1,2) &
|
|
+ amat(2,2) * bmat(2,2) &
|
|
+ amat(2,3) * bmat(3,2) )
|
|
cmat(3,2) = cmat(3,2) &
|
|
+ ( amat(3,1) * bmat(1,2) &
|
|
+ amat(3,2) * bmat(2,2) &
|
|
+ amat(3,3) * bmat(3,2) )
|
|
|
|
cmat(1,3) = cmat(1,3) &
|
|
+ ( amat(1,1) * bmat(1,3) &
|
|
+ amat(1,2) * bmat(2,3) &
|
|
+ amat(1,3) * bmat(3,3) )
|
|
cmat(2,3) = cmat(2,3) &
|
|
+ ( amat(2,1) * bmat(1,3) &
|
|
+ amat(2,2) * bmat(2,3) &
|
|
+ amat(2,3) * bmat(3,3) )
|
|
cmat(3,3) = cmat(3,3) &
|
|
+ ( amat(3,1) * bmat(1,3) &
|
|
+ amat(3,2) * bmat(2,3) &
|
|
+ amat(3,3) * bmat(3,3) )
|
|
|
|
else &
|
|
if (scal .eq. -1.e0_8) then
|
|
|
|
cmat(1,1) = cmat(1,1) &
|
|
- ( amat(1,1) * bmat(1,1) &
|
|
+ amat(1,2) * bmat(2,1) &
|
|
+ amat(1,3) * bmat(3,1) )
|
|
cmat(2,1) = cmat(2,1) &
|
|
- ( amat(2,1) * bmat(1,1) &
|
|
+ amat(2,2) * bmat(2,1) &
|
|
+ amat(2,3) * bmat(3,1) )
|
|
cmat(3,1) = cmat(3,1) &
|
|
- ( amat(3,1) * bmat(1,1) &
|
|
+ amat(3,2) * bmat(2,1) &
|
|
+ amat(3,3) * bmat(3,1) )
|
|
|
|
cmat(1,2) = cmat(1,2) &
|
|
- ( amat(1,1) * bmat(1,2) &
|
|
+ amat(1,2) * bmat(2,2) &
|
|
+ amat(1,3) * bmat(3,2) )
|
|
cmat(2,2) = cmat(2,2) &
|
|
- ( amat(2,1) * bmat(1,2) &
|
|
+ amat(2,2) * bmat(2,2) &
|
|
+ amat(2,3) * bmat(3,2) )
|
|
cmat(3,2) = cmat(3,2) &
|
|
- ( amat(3,1) * bmat(1,2) &
|
|
+ amat(3,2) * bmat(2,2) &
|
|
+ amat(3,3) * bmat(3,2) )
|
|
|
|
cmat(1,3) = cmat(1,3) &
|
|
- ( amat(1,1) * bmat(1,3) &
|
|
+ amat(1,2) * bmat(2,3) &
|
|
+ amat(1,3) * bmat(3,3) )
|
|
cmat(2,3) = cmat(2,3) &
|
|
- ( amat(2,1) * bmat(1,3) &
|
|
+ amat(2,2) * bmat(2,3) &
|
|
+ amat(2,3) * bmat(3,3) )
|
|
cmat(3,3) = cmat(3,3) &
|
|
- ( amat(3,1) * bmat(1,3) &
|
|
+ amat(3,2) * bmat(2,3) &
|
|
+ amat(3,3) * bmat(3,3) )
|
|
|
|
else
|
|
|
|
cmat(1,1) = cmat(1,1) + &
|
|
scal * ( amat(1,1) * bmat(1,1) &
|
|
+ amat(1,2) * bmat(2,1) &
|
|
+ amat(1,3) * bmat(3,1) )
|
|
cmat(2,1) = cmat(2,1) + &
|
|
scal * ( amat(2,1) * bmat(1,1) &
|
|
+ amat(2,2) * bmat(2,1) &
|
|
+ amat(2,3) * bmat(3,1) )
|
|
cmat(3,1) = cmat(3,1) + &
|
|
scal * ( amat(3,1) * bmat(1,1) &
|
|
+ amat(3,2) * bmat(2,1) &
|
|
+ amat(3,3) * bmat(3,1) )
|
|
|
|
cmat(1,2) = cmat(1,2) + &
|
|
scal * ( amat(1,1) * bmat(1,2) &
|
|
+ amat(1,2) * bmat(2,2) &
|
|
+ amat(1,3) * bmat(3,2) )
|
|
cmat(2,2) = cmat(2,2) + &
|
|
scal * ( amat(2,1) * bmat(1,2) &
|
|
+ amat(2,2) * bmat(2,2) &
|
|
+ amat(2,3) * bmat(3,2) )
|
|
cmat(3,2) = cmat(3,2) + &
|
|
scal * ( amat(3,1) * bmat(1,2) &
|
|
+ amat(3,2) * bmat(2,2) &
|
|
+ amat(3,3) * bmat(3,2) )
|
|
|
|
cmat(1,3) = cmat(1,3) + &
|
|
scal * ( amat(1,1) * bmat(1,3) &
|
|
+ amat(1,2) * bmat(2,3) &
|
|
+ amat(1,3) * bmat(3,3) )
|
|
cmat(2,3) = cmat(2,3) + &
|
|
scal * ( amat(2,1) * bmat(1,3) &
|
|
+ amat(2,2) * bmat(2,3) &
|
|
+ amat(2,3) * bmat(3,3) )
|
|
cmat(3,3) = cmat(3,3) + &
|
|
scal * ( amat(3,1) * bmat(1,3) &
|
|
+ amat(3,2) * bmat(2,3) &
|
|
+ amat(3,3) * bmat(3,3) )
|
|
|
|
end if
|
|
|
|
return
|
|
|
|
end subroutine
|
|
|
|
pure subroutine slv_2x2(amat,adim,vrhs,vdim, &
|
|
& nrhs,fEPS,okay)
|
|
|
|
implicit none
|
|
|
|
!------------------------------------------- arguments !
|
|
integer, intent(in) :: adim
|
|
real*8 , intent(in) :: amat(adim,*)
|
|
integer, intent(in) :: vdim
|
|
real*8 , intent(inout) :: vrhs(vdim,*)
|
|
integer, intent(in) :: nrhs
|
|
real*8 , intent(in) :: fEPS
|
|
logical, intent(inout) :: okay
|
|
|
|
!------------------------------------------- variables !
|
|
real*8 :: ainv(2,2)
|
|
real*8 :: adet
|
|
real*8 :: vtmp( 2)
|
|
integer :: irhs
|
|
|
|
integer, parameter :: LDIM = 2
|
|
|
|
!---------------------------------------- calc. inv(A) !
|
|
|
|
call inv_2x2(amat,adim,ainv,LDIM,&
|
|
adet)
|
|
|
|
okay = (abs(adet) .gt. fEPS)
|
|
|
|
if (okay.eqv..false.) return
|
|
|
|
!---------------------------------------- v = A^-1 * v !
|
|
|
|
do irhs = 1, nrhs
|
|
|
|
vtmp(1) = &
|
|
+ ( &
|
|
ainv(1, 1) * vrhs(1,irhs) &
|
|
+ ainv(1, 2) * vrhs(2,irhs) &
|
|
) / adet
|
|
|
|
vtmp(2) = &
|
|
+ ( &
|
|
ainv(2, 1) * vrhs(1,irhs) &
|
|
+ ainv(2, 2) * vrhs(2,irhs) &
|
|
) / adet
|
|
|
|
vrhs(1,irhs) = vtmp(1)
|
|
vrhs(2,irhs) = vtmp(2)
|
|
|
|
end do
|
|
|
|
return
|
|
|
|
end subroutine
|
|
|
|
pure subroutine slv_3x3(amat,adim,vrhs,vdim, &
|
|
& nrhs,fEPS,okay)
|
|
|
|
implicit none
|
|
|
|
!------------------------------------------- arguments !
|
|
integer, intent(in) :: adim
|
|
real*8 , intent(in) :: amat(adim,*)
|
|
integer, intent(in) :: vdim
|
|
real*8 , intent(inout) :: vrhs(vdim,*)
|
|
integer, intent(in) :: nrhs
|
|
real*8 , intent(in) :: fEPS
|
|
logical, intent(inout) :: okay
|
|
|
|
!------------------------------------------- variables !
|
|
real*8 :: ainv(3,3)
|
|
real*8 :: adet
|
|
real*8 :: vtmp( 3)
|
|
integer :: irhs
|
|
|
|
integer, parameter :: LDIM = 3
|
|
|
|
!---------------------------------------- calc. inv(A) !
|
|
|
|
call inv_3x3(amat,adim,ainv,LDIM,&
|
|
adet)
|
|
|
|
okay = (abs(adet) .gt. fEPS)
|
|
|
|
if (okay.eqv..false.) return
|
|
|
|
!---------------------------------------- v = A^-1 * v !
|
|
|
|
do irhs = 1, nrhs
|
|
|
|
vtmp(1) = &
|
|
+ ( &
|
|
ainv(1, 1) * vrhs(1,irhs) &
|
|
+ ainv(1, 2) * vrhs(2,irhs) &
|
|
+ ainv(1, 3) * vrhs(3,irhs) &
|
|
) / adet
|
|
|
|
vtmp(2) = &
|
|
+ ( &
|
|
ainv(2, 1) * vrhs(1,irhs) &
|
|
+ ainv(2, 2) * vrhs(2,irhs) &
|
|
+ ainv(2, 3) * vrhs(3,irhs) &
|
|
) / adet
|
|
|
|
vtmp(3) = &
|
|
+ ( &
|
|
ainv(3, 1) * vrhs(1,irhs) &
|
|
+ ainv(3, 2) * vrhs(2,irhs) &
|
|
+ ainv(3, 3) * vrhs(3,irhs) &
|
|
) / adet
|
|
|
|
vrhs(1,irhs) = vtmp(1)
|
|
vrhs(2,irhs) = vtmp(2)
|
|
vrhs(3,irhs) = vtmp(3)
|
|
|
|
end do
|
|
|
|
return
|
|
|
|
end subroutine
|
|
|
|
pure subroutine slv_4x4(amat,adim,vrhs,vdim, &
|
|
& nrhs,fEPS,okay)
|
|
|
|
implicit none
|
|
|
|
!------------------------------------------- arguments !
|
|
integer, intent(in) :: adim
|
|
real*8 , intent(in) :: amat(adim,*)
|
|
integer, intent(in) :: vdim
|
|
real*8 , intent(inout) :: vrhs(vdim,*)
|
|
integer, intent(in) :: nrhs
|
|
real*8 , intent(in) :: fEPS
|
|
logical, intent(inout) :: okay
|
|
|
|
!------------------------------------------- variables !
|
|
real*8 :: ainv(2,2)
|
|
real*8 :: lmat(2,2)
|
|
real*8 :: umat(2,2)
|
|
real*8 :: smat(2,2)
|
|
real*8 :: sinv(2,2)
|
|
real*8 :: adet,sdet
|
|
real*8 :: vtmp( 2)
|
|
integer :: irhs
|
|
|
|
integer, parameter :: LDIM = 2
|
|
|
|
!---------------------- form a block LDU factorisation !
|
|
|
|
call inv_2x2(amat(1,1),adim,ainv,LDIM, &
|
|
adet)
|
|
|
|
okay = (abs(adet) .gt. fEPS)
|
|
|
|
if (okay.eqv..false.) return
|
|
|
|
!---------------------------------------- L = C * A^-1 !
|
|
|
|
lmat(1,1) = +0.e0_8
|
|
lmat(1,2) = +0.e0_8
|
|
lmat(2,1) = +0.e0_8
|
|
lmat(2,2) = +0.e0_8
|
|
|
|
call mul_2x2(amat(3,1),adim,ainv,LDIM, &
|
|
+1.e0_8,lmat,LDIM)
|
|
|
|
!---------------------------------------- U = A^-1 * B !
|
|
|
|
umat(1,1) = +0.e0_8
|
|
umat(1,2) = +0.e0_8
|
|
umat(2,1) = +0.e0_8
|
|
umat(2,2) = +0.e0_8
|
|
|
|
call mul_2x2(ainv,LDIM,amat(1,3),adim, &
|
|
+1.e0_8,umat,LDIM)
|
|
|
|
!-------------------------------- S = D - C * A^-1 * B !
|
|
|
|
smat(1,1) = amat(3,3)
|
|
smat(1,2) = amat(3,4)
|
|
smat(2,1) = amat(4,3)
|
|
smat(2,2) = amat(4,4)
|
|
|
|
call mul_2x2(lmat,LDIM,amat(1,3),adim, &
|
|
-1.e0_8/adet,smat,LDIM)
|
|
|
|
call inv_2x2(smat,LDIM,sinv,LDIM,sdet)
|
|
|
|
okay = (abs(adet) .gt. fEPS)
|
|
|
|
if (okay.eqv..false.) return
|
|
|
|
!-------------------------------- back-solve LDU = rhs !
|
|
|
|
do irhs = 1, nrhs
|
|
|
|
!---------------------------------------- solve L part !
|
|
|
|
vrhs(3,irhs) = vrhs(3,irhs) &
|
|
- ( &
|
|
lmat(1, 1) * vrhs(1,irhs) &
|
|
+ lmat(1, 2) * vrhs(2,irhs) &
|
|
) / adet
|
|
|
|
vrhs(4,irhs) = vrhs(4,irhs) &
|
|
- ( &
|
|
lmat(2, 1) * vrhs(1,irhs) &
|
|
+ lmat(2, 2) * vrhs(2,irhs) &
|
|
) / adet
|
|
|
|
!---------------------------------------- solve D part !
|
|
|
|
vtmp(1) = &
|
|
+ ( &
|
|
ainv(1, 1) * vrhs(1,irhs) &
|
|
+ ainv(1, 2) * vrhs(2,irhs) &
|
|
) / adet
|
|
|
|
vtmp(2) = &
|
|
+ ( &
|
|
ainv(2, 1) * vrhs(1,irhs) &
|
|
+ ainv(2, 2) * vrhs(2,irhs) &
|
|
) / adet
|
|
|
|
vrhs(1,irhs) = vtmp(1)
|
|
vrhs(2,irhs) = vtmp(2)
|
|
|
|
vtmp(1) = &
|
|
+ ( &
|
|
sinv(1, 1) * vrhs(3,irhs) &
|
|
+ sinv(1, 2) * vrhs(4,irhs) &
|
|
) / sdet
|
|
|
|
vtmp(2) = &
|
|
+ ( &
|
|
sinv(2, 1) * vrhs(3,irhs) &
|
|
+ sinv(2, 2) * vrhs(4,irhs) &
|
|
) / sdet
|
|
|
|
vrhs(3,irhs) = vtmp(1)
|
|
vrhs(4,irhs) = vtmp(2)
|
|
|
|
!---------------------------------------- solve U part !
|
|
|
|
vrhs(1,irhs) = vrhs(1,irhs) &
|
|
- ( &
|
|
umat(1, 1) * vrhs(3,irhs) &
|
|
+ umat(1, 2) * vrhs(4,irhs) &
|
|
) / adet
|
|
|
|
vrhs(2,irhs) = vrhs(2,irhs) &
|
|
- ( &
|
|
umat(2, 1) * vrhs(3,irhs) &
|
|
+ umat(2, 2) * vrhs(4,irhs) &
|
|
) / adet
|
|
|
|
end do
|
|
|
|
return
|
|
|
|
end subroutine
|
|
|
|
pure subroutine slv_6x6(amat,adim,vrhs,vdim, &
|
|
& nrhs,fEPS,okay)
|
|
|
|
implicit none
|
|
|
|
!------------------------------------------- arguments !
|
|
integer, intent(in) :: adim
|
|
real*8 , intent(in) :: amat(adim,*)
|
|
integer, intent(in) :: vdim
|
|
real*8 , intent(inout) :: vrhs(vdim,*)
|
|
integer, intent(in) :: nrhs
|
|
real*8 , intent(in) :: fEPS
|
|
logical, intent(inout) :: okay
|
|
|
|
!------------------------------------------- variables !
|
|
real*8 :: ainv(3,3)
|
|
real*8 :: lmat(3,3)
|
|
real*8 :: umat(3,3)
|
|
real*8 :: smat(3,3)
|
|
real*8 :: sinv(3,3)
|
|
real*8 :: adet,sdet
|
|
real*8 :: vtmp( 3)
|
|
integer :: irhs
|
|
|
|
integer, parameter :: LDIM = 3
|
|
|
|
!---------------------- form a block LDU factorisation !
|
|
|
|
call inv_3x3(amat(1,1),adim,ainv,LDIM, &
|
|
adet)
|
|
|
|
okay = (abs(adet) .gt. fEPS)
|
|
|
|
if (okay.eqv..false.) return
|
|
|
|
!---------------------------------------- L = C * A^-1 !
|
|
|
|
lmat(1,1) = +0.e0_8
|
|
lmat(1,2) = +0.e0_8
|
|
lmat(1,3) = +0.e0_8
|
|
lmat(2,1) = +0.e0_8
|
|
lmat(2,2) = +0.e0_8
|
|
lmat(2,3) = +0.e0_8
|
|
lmat(3,1) = +0.e0_8
|
|
lmat(3,2) = +0.e0_8
|
|
lmat(3,3) = +0.e0_8
|
|
|
|
call mul_3x3(amat(4,1),adim,ainv,LDIM, &
|
|
+1.e0_8,lmat,LDIM)
|
|
|
|
!---------------------------------------- U = A^-1 * B !
|
|
|
|
umat(1,1) = +0.e0_8
|
|
umat(1,2) = +0.e0_8
|
|
umat(1,3) = +0.e0_8
|
|
umat(2,1) = +0.e0_8
|
|
umat(2,2) = +0.e0_8
|
|
umat(2,3) = +0.e0_8
|
|
umat(3,1) = +0.e0_8
|
|
umat(3,2) = +0.e0_8
|
|
umat(3,3) = +0.e0_8
|
|
|
|
call mul_3x3(ainv,LDIM,amat(1,4),adim, &
|
|
+1.e0_8,umat,LDIM)
|
|
|
|
!-------------------------------- S = D - C * A^-1 * B !
|
|
|
|
smat(1,1) = amat(4,4)
|
|
smat(1,2) = amat(4,5)
|
|
smat(1,3) = amat(4,6)
|
|
smat(2,1) = amat(5,4)
|
|
smat(2,2) = amat(5,5)
|
|
smat(2,3) = amat(5,6)
|
|
smat(3,1) = amat(6,4)
|
|
smat(3,2) = amat(6,5)
|
|
smat(3,3) = amat(6,6)
|
|
|
|
call mul_3x3(lmat,LDIM,amat(1,4),adim, &
|
|
-1.e0_8/adet,smat,LDIM)
|
|
|
|
call inv_3x3(smat,LDIM,sinv,LDIM,sdet)
|
|
|
|
okay = (abs(adet) .gt. fEPS)
|
|
|
|
if (okay.eqv..false.) return
|
|
|
|
!-------------------------------- back-solve LDU = rhs !
|
|
|
|
do irhs = 1, nrhs
|
|
|
|
!---------------------------------------- solve L part !
|
|
|
|
vrhs(4,irhs) = vrhs(4,irhs) &
|
|
- ( &
|
|
lmat(1, 1) * vrhs(1,irhs) &
|
|
+ lmat(1, 2) * vrhs(2,irhs) &
|
|
+ lmat(1, 3) * vrhs(3,irhs) &
|
|
) / adet
|
|
|
|
vrhs(5,irhs) = vrhs(5,irhs) &
|
|
- ( &
|
|
lmat(2, 1) * vrhs(1,irhs) &
|
|
+ lmat(2, 2) * vrhs(2,irhs) &
|
|
+ lmat(2, 3) * vrhs(3,irhs) &
|
|
) / adet
|
|
|
|
vrhs(6,irhs) = vrhs(6,irhs) &
|
|
- ( &
|
|
lmat(3, 1) * vrhs(1,irhs) &
|
|
+ lmat(3, 2) * vrhs(2,irhs) &
|
|
+ lmat(3, 3) * vrhs(3,irhs) &
|
|
) / adet
|
|
|
|
!---------------------------------------- solve D part !
|
|
|
|
vtmp(1) = &
|
|
+ ( &
|
|
ainv(1, 1) * vrhs(1,irhs) &
|
|
+ ainv(1, 2) * vrhs(2,irhs) &
|
|
+ ainv(1, 3) * vrhs(3,irhs) &
|
|
) / adet
|
|
|
|
vtmp(2) = &
|
|
+ ( &
|
|
ainv(2, 1) * vrhs(1,irhs) &
|
|
+ ainv(2, 2) * vrhs(2,irhs) &
|
|
+ ainv(2, 3) * vrhs(3,irhs) &
|
|
) / adet
|
|
|
|
vtmp(3) = &
|
|
+ ( &
|
|
ainv(3, 1) * vrhs(1,irhs) &
|
|
+ ainv(3, 2) * vrhs(2,irhs) &
|
|
+ ainv(3, 3) * vrhs(3,irhs) &
|
|
) / adet
|
|
|
|
vrhs(1,irhs) = vtmp(1)
|
|
vrhs(2,irhs) = vtmp(2)
|
|
vrhs(3,irhs) = vtmp(3)
|
|
|
|
vtmp(1) = &
|
|
+ ( &
|
|
sinv(1, 1) * vrhs(4,irhs) &
|
|
+ sinv(1, 2) * vrhs(5,irhs) &
|
|
+ sinv(1, 3) * vrhs(6,irhs) &
|
|
) / sdet
|
|
|
|
vtmp(2) = &
|
|
+ ( &
|
|
sinv(2, 1) * vrhs(4,irhs) &
|
|
+ sinv(2, 2) * vrhs(5,irhs) &
|
|
+ sinv(2, 3) * vrhs(6,irhs) &
|
|
) / sdet
|
|
|
|
vtmp(3) = &
|
|
+ ( &
|
|
sinv(3, 1) * vrhs(4,irhs) &
|
|
+ sinv(3, 2) * vrhs(5,irhs) &
|
|
+ sinv(3, 3) * vrhs(6,irhs) &
|
|
) / sdet
|
|
|
|
vrhs(4,irhs) = vtmp(1)
|
|
vrhs(5,irhs) = vtmp(2)
|
|
vrhs(6,irhs) = vtmp(3)
|
|
|
|
!---------------------------------------- solve U part !
|
|
|
|
vrhs(1,irhs) = vrhs(1,irhs) &
|
|
- ( &
|
|
umat(1, 1) * vrhs(4,irhs) &
|
|
+ umat(1, 2) * vrhs(5,irhs) &
|
|
+ umat(1, 3) * vrhs(6,irhs) &
|
|
) / adet
|
|
|
|
vrhs(2,irhs) = vrhs(2,irhs) &
|
|
- ( &
|
|
umat(2, 1) * vrhs(4,irhs) &
|
|
+ umat(2, 2) * vrhs(5,irhs) &
|
|
+ umat(2, 3) * vrhs(6,irhs) &
|
|
) / adet
|
|
|
|
vrhs(3,irhs) = vrhs(3,irhs) &
|
|
- ( &
|
|
umat(3, 1) * vrhs(4,irhs) &
|
|
+ umat(3, 2) * vrhs(5,irhs) &
|
|
+ umat(3, 3) * vrhs(6,irhs) &
|
|
) / adet
|
|
|
|
end do
|
|
|
|
return
|
|
|
|
end subroutine
|
|
|
|
|
|
|