1001 lines
24 KiB
FortranFixed
1001 lines
24 KiB
FortranFixed
subroutine inv_det_matix(a,n,np,a_inv,
|
|
& lnabsdet,signunit,ierr)
|
|
implicit none
|
|
!
|
|
! Invert a matrix and calculate ln(dabs(determinant)) and the assoaciated
|
|
! sign
|
|
!
|
|
!-------------Inputs-------------------------------
|
|
!a: matrix to be inverted
|
|
!n: the actual dimension of a
|
|
!np: the declared dimension of a
|
|
!
|
|
!-------------Outputs------------------------------
|
|
!a_inv: inverse matrix of a
|
|
!lnabsdet: the ln(|det|a||)
|
|
!signunit: det|a| = signunit*exp(lnabsdet)
|
|
!ierr: =0, ok; =1, matrix a is singular
|
|
|
|
|
|
integer n,np,indx(np),i,j,ierr,numneg
|
|
double precision a(np,np),a_inv(np,np),
|
|
& copya(np,np),d,signunit,lnabsdet
|
|
|
|
ierr=0
|
|
do i=1,n
|
|
do j=1,n
|
|
copya(i,j)=a(i,j)
|
|
a_inv(i,j)=0.0d0
|
|
enddo
|
|
a_inv(i,i)=1.0d0
|
|
enddo
|
|
call ludcmp(copya,n,np,indx,d,ierr)
|
|
numneg=0
|
|
lnabsdet=0.0d0
|
|
do j=1,n
|
|
if(copya(j,j).eq.0.0d0)then
|
|
! singular matrix and the determinant is zero
|
|
ierr=1
|
|
signunit=-1.0d+100
|
|
lnabsdet=-1.0d+100
|
|
return
|
|
endif
|
|
if(copya(j,j).lt.0.0d0)then
|
|
numneg=numneg+1
|
|
endif
|
|
lnabsdet=lnabsdet+dlog(dabs(copya(j,j)))
|
|
enddo
|
|
if(d.lt.0.0d0)then
|
|
numneg=numneg+1
|
|
endif
|
|
if(mod(numneg,2).eq.0)then
|
|
signunit=1.0d0
|
|
else
|
|
signunit=-1.0d0
|
|
endif
|
|
do j=1,n
|
|
call lubksb(copya,n,np,indx,a_inv(1,j))
|
|
enddo
|
|
return
|
|
end
|
|
|
|
subroutine svd_inv_det_matix(a,n,np,a_inv,
|
|
& lnabsdet,signunit,ierr)
|
|
implicit none
|
|
!
|
|
! Invert a matrix and calculate ln(dabs(determinant)) and the assoaciated
|
|
! sign
|
|
!
|
|
!-------------Inputs-------------------------------
|
|
!a: matrix to be inverted
|
|
!n: the actual dimension of a
|
|
!np: the declared dimension of a
|
|
!
|
|
!-------------Outputs------------------------------
|
|
!a_inv: inverse matrix of a
|
|
!lnabsdet: the ln(|det|a||)
|
|
!signunit: det|a| = signunit*exp(lnabsdet)
|
|
!ierr: =0, ok; =1, matrix a is singular
|
|
|
|
integer n,np,indx(np),i,j,ierr
|
|
double precision a(np,np),a_inv(np,np),
|
|
& copya(np,np),w(np),v(np,np),
|
|
& d,signunit,signunitu,signunitv,
|
|
& lnabsdet,lnabsdetu,lnabsdetv,copyatrans(np,np),
|
|
& vinw(np,np)
|
|
|
|
ierr=0
|
|
do i=1,n
|
|
do j=1,n
|
|
copya(i,j)=a(i,j)
|
|
enddo
|
|
enddo
|
|
call svdcmp(copya(1:n,1:n),n,n,np,np,w,v(1:n,1:n),ierr)
|
|
do i=1,n
|
|
do j=1,n
|
|
vinw(i,j)=v(i,j)/w(j)
|
|
enddo
|
|
enddo
|
|
|
|
call matrixtranspose(n,n,copya(1:n,1:n),
|
|
& copyatrans(1:n,1:n))
|
|
call matrixproduct(n,n,vinw(1:n,1:n),n,
|
|
& copyatrans(1:n,1:n),a_inv(1:n,1:n))
|
|
|
|
call det_matix(copya(1:n,1:n),n,n,lnabsdetu,signunitu,ierr)
|
|
call det_matix(v(1:n,1:n),n,n,lnabsdetv,signunitv,ierr)
|
|
|
|
lnabsdet=0.0d0
|
|
do j=1,n
|
|
lnabsdet=lnabsdet+dlog(w(j))
|
|
enddo
|
|
|
|
signunit=signunitu*signunitv
|
|
|
|
return
|
|
end
|
|
|
|
|
|
subroutine det_matix(a,n,np,lnabsdet,signunit,ierr)
|
|
implicit none
|
|
!
|
|
! calculate ln(dabs(determinant)) and the assoaciated sign
|
|
!
|
|
!-------------Inputs-------------------------------
|
|
!a: matrix to be inverted
|
|
!n: the actual dimension of a
|
|
!np: the declared dimension of a
|
|
!
|
|
!-------------Outputs------------------------------
|
|
!lnabsdet: the ln(|det|a||)
|
|
!signunit: det|a| = signunit*exp(lnabsdet)
|
|
!ierr: =1, ok; =0, matrix a is singular
|
|
|
|
|
|
integer n,np,indx(np),i,j,ierr,numneg
|
|
double precision a(np,np),a_inv(np,np),
|
|
& copya(np,np),d,signunit,lnabsdet
|
|
|
|
ierr=1
|
|
do i=1,n
|
|
do j=1,n
|
|
copya(i,j)=a(i,j)
|
|
enddo
|
|
enddo
|
|
|
|
call ludcmp(copya(1:n,1:n),n,np,indx,d,ierr)
|
|
numneg=0
|
|
lnabsdet=0.0d0
|
|
do j=1,n
|
|
if(copya(j,j).eq.0.0d0)then
|
|
! singular matrix and the determinant is zero
|
|
ierr=1
|
|
signunit=-1.0d+100
|
|
lnabsdet=-1.0d+100
|
|
return
|
|
endif
|
|
if(copya(j,j).lt.0.0d0)then
|
|
numneg=numneg+1
|
|
endif
|
|
lnabsdet=lnabsdet+dlog(dabs(copya(j,j)))
|
|
enddo
|
|
if(d.lt.0.0d0)then
|
|
numneg=numneg+1
|
|
endif
|
|
if(mod(numneg,2).eq.0)then
|
|
signunit=1.0d0
|
|
else
|
|
signunit=-1.0d0
|
|
endif
|
|
return
|
|
end
|
|
|
|
!%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
|
double precision function tracematrix(m,a)
|
|
implicit none
|
|
integer m,i
|
|
double precision a(m,m)
|
|
|
|
tracematrix=0.0d0
|
|
do i=1,m
|
|
tracematrix=tracematrix+a(i,i)
|
|
enddo
|
|
return
|
|
end
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
subroutine matrixproduct(ma,n,a,nb,b,c)
|
|
implicit none
|
|
!
|
|
! matrix product c=ab
|
|
!
|
|
integer ma,n,nb
|
|
double precision a(ma,n),b(n,nb),c(ma,nb)
|
|
integer i,j,k
|
|
do i=1,ma
|
|
do j=1,nb
|
|
c(i,j)=0.0d0
|
|
do k=1,n
|
|
c(i,j)=c(i,j)+a(i,k)*b(k,j)
|
|
enddo
|
|
enddo
|
|
enddo
|
|
return
|
|
end
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
subroutine matrixsum(m,n,a,b,c)
|
|
implicit none
|
|
!
|
|
! matrix sum c=a+b
|
|
!
|
|
integer m,n
|
|
double precision a(m,n),b(m,n),c(m,n)
|
|
integer i,j
|
|
do i=1,m
|
|
do j=1,n
|
|
c(i,j)=a(i,j)+b(i,j)
|
|
enddo
|
|
enddo
|
|
return
|
|
end
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
subroutine matrixdif(m,n,a,b,c)
|
|
implicit none
|
|
!
|
|
! matrix difference c=a-b
|
|
!
|
|
integer m,n
|
|
double precision a(m,n),b(m,n),c(m,n)
|
|
integer i,j
|
|
do i=1,m
|
|
do j=1,n
|
|
c(i,j)=a(i,j)-b(i,j)
|
|
enddo
|
|
enddo
|
|
return
|
|
end
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
subroutine matrixtranspose(m,n,a,b)
|
|
implicit none
|
|
!
|
|
! matrix transpose b=a^T
|
|
!
|
|
integer m,n
|
|
double precision a(m,n),b(n,m)
|
|
integer i,j
|
|
do i=1,m
|
|
do j=1,n
|
|
b(j,i)=a(i,j)
|
|
enddo
|
|
enddo
|
|
return
|
|
end
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
SUBROUTINE svbksb(u,w,v,m,n,mp,np,b,x)
|
|
implicit none
|
|
INTEGER m,mp,n,np,NMAX
|
|
double precision b(mp),u(mp,np),v(np,np),w(np),x(np)
|
|
PARAMETER (NMAX=1500)
|
|
INTEGER i,j,jj
|
|
double precision s,tmp(NMAX)
|
|
do 12 j=1,n
|
|
s=0.0d0
|
|
if(w(j).ne.0.0d0)then
|
|
do 11 i=1,m
|
|
s=s+u(i,j)*b(i)
|
|
11 continue
|
|
s=s/w(j)
|
|
endif
|
|
tmp(j)=s
|
|
12 continue
|
|
do 14 j=1,n
|
|
s=0.0d0
|
|
do 13 jj=1,n
|
|
s=s+v(j,jj)*tmp(jj)
|
|
13 continue
|
|
x(j)=s
|
|
14 continue
|
|
return
|
|
END
|
|
C (C) Copr. 1986-92 Numerical Recipes Software v%1jw#<0(9p#3.
|
|
|
|
SUBROUTINE svdcmp(a,m,n,mp,np,w,v,ierr)
|
|
implicit none
|
|
INTEGER m,mp,n,np,NMAX,ierr
|
|
double precision a(mp,np),v(np,np),w(np)
|
|
PARAMETER (NMAX=1500)
|
|
CU USES pythag
|
|
INTEGER i,its,j,jj,k,l,nm
|
|
double precision anorm,c,f,g,h,s,scaling,x,y,z,
|
|
& rv1(NMAX),pythag
|
|
g=0.0d0
|
|
scaling=0.0d0
|
|
anorm=0.0d0
|
|
do 25 i=1,n
|
|
l=i+1
|
|
rv1(i)=scaling*g
|
|
g=0.0d0
|
|
s=0.0d0
|
|
scaling=0.0d0
|
|
if(i.le.m)then
|
|
do 11 k=i,m
|
|
scaling=scaling+dabs(a(k,i))
|
|
11 continue
|
|
if(scaling.ne.0.0d0)then
|
|
do 12 k=i,m
|
|
a(k,i)=a(k,i)/scaling
|
|
s=s+a(k,i)*a(k,i)
|
|
12 continue
|
|
f=a(i,i)
|
|
g=-dsign(dsqrt(s),f)
|
|
h=f*g-s
|
|
a(i,i)=f-g
|
|
do 15 j=l,n
|
|
s=0.0d0
|
|
do 13 k=i,m
|
|
s=s+a(k,i)*a(k,j)
|
|
13 continue
|
|
f=s/h
|
|
do 14 k=i,m
|
|
a(k,j)=a(k,j)+f*a(k,i)
|
|
14 continue
|
|
15 continue
|
|
do 16 k=i,m
|
|
a(k,i)=scaling*a(k,i)
|
|
16 continue
|
|
endif
|
|
endif
|
|
w(i)=scaling*g
|
|
g=0.0d0
|
|
s=0.0d0
|
|
scaling=0.0d0
|
|
if((i.le.m).and.(i.ne.n))then
|
|
do 17 k=l,n
|
|
scaling=scaling+dabs(a(i,k))
|
|
17 continue
|
|
if(scaling.ne.0.0d0)then
|
|
do 18 k=l,n
|
|
a(i,k)=a(i,k)/scaling
|
|
s=s+a(i,k)*a(i,k)
|
|
18 continue
|
|
f=a(i,l)
|
|
g=-dsign(dsqrt(s),f)
|
|
h=f*g-s
|
|
a(i,l)=f-g
|
|
do 19 k=l,n
|
|
rv1(k)=a(i,k)/h
|
|
19 continue
|
|
do 23 j=l,m
|
|
s=0.0d0
|
|
do 21 k=l,n
|
|
s=s+a(j,k)*a(i,k)
|
|
21 continue
|
|
do 22 k=l,n
|
|
a(j,k)=a(j,k)+s*rv1(k)
|
|
22 continue
|
|
23 continue
|
|
do 24 k=l,n
|
|
a(i,k)=scaling*a(i,k)
|
|
24 continue
|
|
endif
|
|
endif
|
|
anorm=dmax1(anorm,(dabs(w(i))+dabs(rv1(i))))
|
|
25 continue
|
|
do 32 i=n,1,-1
|
|
if(i.lt.n)then
|
|
if(g.ne.0.0d0)then
|
|
do 26 j=l,n
|
|
v(j,i)=(a(i,j)/a(i,l))/g
|
|
26 continue
|
|
do 29 j=l,n
|
|
s=0.0d0
|
|
do 27 k=l,n
|
|
s=s+a(i,k)*v(k,j)
|
|
27 continue
|
|
do 28 k=l,n
|
|
v(k,j)=v(k,j)+s*v(k,i)
|
|
28 continue
|
|
29 continue
|
|
endif
|
|
do 31 j=l,n
|
|
v(i,j)=0.0d0
|
|
v(j,i)=0.0d0
|
|
31 continue
|
|
endif
|
|
v(i,i)=1.0d0
|
|
g=rv1(i)
|
|
l=i
|
|
32 continue
|
|
do 39 i=min(m,n),1,-1
|
|
l=i+1
|
|
g=w(i)
|
|
do 33 j=l,n
|
|
a(i,j)=0.0d0
|
|
33 continue
|
|
if(g.ne.0.0d0)then
|
|
g=1.0d0/g
|
|
do 36 j=l,n
|
|
s=0.0d0
|
|
do 34 k=l,m
|
|
s=s+a(k,i)*a(k,j)
|
|
34 continue
|
|
f=(s/a(i,i))*g
|
|
do 35 k=i,m
|
|
a(k,j)=a(k,j)+f*a(k,i)
|
|
35 continue
|
|
36 continue
|
|
do 37 j=i,m
|
|
a(j,i)=a(j,i)*g
|
|
37 continue
|
|
else
|
|
do 38 j= i,m
|
|
a(j,i)=0.0d0
|
|
38 continue
|
|
endif
|
|
a(i,i)=a(i,i)+1.0d0
|
|
39 continue
|
|
do 49 k=n,1,-1
|
|
do 48 its=1,30
|
|
do 41 l=k,1,-1
|
|
nm=l-1
|
|
if((dabs(rv1(l))+anorm).eq.anorm) goto 2
|
|
if((dabs(w(nm))+anorm).eq.anorm) goto 1
|
|
41 continue
|
|
1 c=0.0d0
|
|
s=1.0d0
|
|
do 43 i=l,k
|
|
f=s*rv1(i)
|
|
rv1(i)=c*rv1(i)
|
|
if((dabs(f)+anorm).eq.anorm) goto 2
|
|
g=w(i)
|
|
h=pythag(f,g)
|
|
w(i)=h
|
|
h=1.0d0/h
|
|
c= (g*h)
|
|
s=-(f*h)
|
|
do 42 j=1,m
|
|
y=a(j,nm)
|
|
z=a(j,i)
|
|
a(j,nm)=(y*c)+(z*s)
|
|
a(j,i)=-(y*s)+(z*c)
|
|
42 continue
|
|
43 continue
|
|
2 z=w(k)
|
|
if(l.eq.k)then
|
|
if(z.lt.0.0d0)then
|
|
w(k)=-z
|
|
do 44 j=1,n
|
|
v(j,k)=-v(j,k)
|
|
44 continue
|
|
endif
|
|
goto 3
|
|
endif
|
|
if(its.eq.30)then
|
|
ierr=0
|
|
return
|
|
endif
|
|
x=w(l)
|
|
nm=k-1
|
|
y=w(nm)
|
|
g=rv1(nm)
|
|
h=rv1(k)
|
|
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0d0*h*y)
|
|
g=pythag(f,1.0d0)
|
|
f=((x-z)*(x+z)+h*((y/(f+dsign(g,f)))-h))/x
|
|
c=1.0d0
|
|
s=1.0d0
|
|
do 47 j=l,nm
|
|
i=j+1
|
|
g=rv1(i)
|
|
y=w(i)
|
|
h=s*g
|
|
g=c*g
|
|
z=pythag(f,h)
|
|
rv1(j)=z
|
|
c=f/z
|
|
s=h/z
|
|
f= (x*c)+(g*s)
|
|
g=-(x*s)+(g*c)
|
|
h=y*s
|
|
y=y*c
|
|
do 45 jj=1,n
|
|
x=v(jj,j)
|
|
z=v(jj,i)
|
|
v(jj,j)= (x*c)+(z*s)
|
|
v(jj,i)=-(x*s)+(z*c)
|
|
45 continue
|
|
z=pythag(f,h)
|
|
w(j)=z
|
|
if(z.ne.0.0d0)then
|
|
z=1.0d0/z
|
|
c=f*z
|
|
s=h*z
|
|
endif
|
|
f= (c*g)+(s*y)
|
|
x=-(s*g)+(c*y)
|
|
do 46 jj=1,m
|
|
y=a(jj,j)
|
|
z=a(jj,i)
|
|
a(jj,j)= (y*c)+(z*s)
|
|
a(jj,i)=-(y*s)+(z*c)
|
|
46 continue
|
|
47 continue
|
|
rv1(l)=0.0d0
|
|
rv1(k)=f
|
|
w(k)=x
|
|
48 continue
|
|
3 continue
|
|
49 continue
|
|
return
|
|
END
|
|
C (C) Copr. 1986-92 Numerical Recipes Software v%1jw#<0(9p#3.
|
|
|
|
double precision FUNCTION pythag(a,b)
|
|
double precision a,b
|
|
double precision absa,absb
|
|
absa=dabs(a)
|
|
absb=dabs(b)
|
|
if(absa.gt.absb)then
|
|
pythag=absa*dsqrt(1.0d0+(absb/absa)**2)
|
|
else
|
|
if(absb.eq.0.0d0)then
|
|
pythag=0.0d0
|
|
else
|
|
pythag=absb*dsqrt(1.0d0+(absa/absb)**2)
|
|
endif
|
|
endif
|
|
return
|
|
END
|
|
C (C) Copr. 1986-92 Numerical Recipes Software v%1jw#<0(9p#3.
|
|
|
|
|
|
subroutine xmprove(N,NP,a,b,x,mark)
|
|
implicit none
|
|
INTEGER i,j,idum,N,NP,indx(N),mark
|
|
double precision d,a(NP,NP),b(N),x(N),aa(NP,NP)
|
|
|
|
do 12 i=1,N
|
|
x(i)=b(i)
|
|
do 11 j=1,N
|
|
aa(i,j)=a(i,j)
|
|
11 continue
|
|
12 continue
|
|
call ludcmp(aa,N,NP,indx,d,mark)
|
|
if (mark .eq. 0) goto 20
|
|
call lubksb(aa,N,NP,indx,x)
|
|
call mprove(a,aa,N,NP,indx,b,x)
|
|
20 continue
|
|
return
|
|
END
|
|
|
|
|
|
SUBROUTINE mprove(a,alud,n,np,indx,b,x)
|
|
implicit none
|
|
INTEGER n,np,indx(n),NMAX
|
|
double precision a(np,np),alud(np,np),b(n),x(n)
|
|
PARAMETER (NMAX=500)
|
|
CU USES lubksb
|
|
INTEGER i,j
|
|
double precision r(NMAX)
|
|
DOUBLE PRECISION sdp
|
|
do 12 i=1,n
|
|
sdp=-b(i)
|
|
do 11 j=1,n
|
|
sdp=sdp+(a(i,j))*(x(j))
|
|
11 continue
|
|
r(i)=sdp
|
|
12 continue
|
|
call lubksb(alud,n,np,indx,r)
|
|
do 13 i=1,n
|
|
x(i)=x(i)-r(i)
|
|
13 continue
|
|
return
|
|
END
|
|
|
|
SUBROUTINE ludcmp(a,n,np,indx,d,mark)
|
|
implicit none
|
|
INTEGER n,np,indx(n),NMAX
|
|
double precision d,a(np,np),TINY
|
|
PARAMETER (NMAX=500,TINY=1.0d-20)
|
|
INTEGER i,imax,j,k,mark
|
|
double precision aamax,dum,sum,vv(NMAX)
|
|
mark=1
|
|
|
|
d=1.0d0
|
|
do 12 i=1,n
|
|
aamax=0.0d0
|
|
do 11 j=1,n
|
|
if (dabs(a(i,j)).gt.aamax) aamax=dabs(a(i,j))
|
|
11 continue
|
|
if (aamax.eq.0.0d0) then
|
|
! singular matrix
|
|
mark=0
|
|
return
|
|
end if
|
|
vv(i)=1.0d0/aamax
|
|
12 continue
|
|
do 19 j=1,n
|
|
do 14 i=1,j-1
|
|
sum=a(i,j)
|
|
do 13 k=1,i-1
|
|
sum=sum-a(i,k)*a(k,j)
|
|
13 continue
|
|
a(i,j)=sum
|
|
14 continue
|
|
aamax=0.0d0
|
|
do 16 i=j,n
|
|
sum=a(i,j)
|
|
do 15 k=1,j-1
|
|
sum=sum-a(i,k)*a(k,j)
|
|
15 continue
|
|
a(i,j)=sum
|
|
dum=vv(i)*dabs(sum)
|
|
if (dum.ge.aamax) then
|
|
imax=i
|
|
aamax=dum
|
|
endif
|
|
16 continue
|
|
if (j.ne.imax)then
|
|
do 17 k=1,n
|
|
dum=a(imax,k)
|
|
a(imax,k)=a(j,k)
|
|
a(j,k)=dum
|
|
17 continue
|
|
d=-d
|
|
vv(imax)=vv(j)
|
|
endif
|
|
indx(j)=imax
|
|
if(a(j,j).eq.0.0d0)a(j,j)=TINY
|
|
if(j.ne.n)then
|
|
dum=1.0d0/a(j,j)
|
|
do 18 i=j+1,n
|
|
a(i,j)=a(i,j)*dum
|
|
18 continue
|
|
endif
|
|
19 continue
|
|
return
|
|
END
|
|
|
|
SUBROUTINE lubksb(a,n,np,indx,b)
|
|
implicit none
|
|
INTEGER n,np,indx(n)
|
|
double precision a(np,np),b(n)
|
|
INTEGER i,ii,j,ll
|
|
double precision sum
|
|
ii=0
|
|
do 12 i=1,n
|
|
ll=indx(i)
|
|
sum=b(ll)
|
|
b(ll)=b(i)
|
|
if (ii.ne.0)then
|
|
do 11 j=ii,i-1
|
|
sum=sum-a(i,j)*b(j)
|
|
11 continue
|
|
else if (sum.ne.0.0d0) then
|
|
ii=i
|
|
endif
|
|
b(i)=sum
|
|
12 continue
|
|
do 14 i=n,1,-1
|
|
sum=b(i)
|
|
do 13 j=i+1,n
|
|
sum=sum-a(i,j)*b(j)
|
|
13 continue
|
|
b(i)=sum/a(i,i)
|
|
14 continue
|
|
return
|
|
END
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
subroutine svdlinsys(n,np,a,b,x)
|
|
implicit none
|
|
!
|
|
! Solving a linear system ax=b through singular value decomposition
|
|
integer n,np
|
|
double precision a(np,np),b(np),x(np)
|
|
integer i,j,ierr
|
|
double precision u(np,np),w(np),v(np,np),
|
|
& wmax,ftol,wmin
|
|
parameter(ftol=1.0d-7)
|
|
|
|
do i=1,n
|
|
do j=1,n
|
|
u(i,j)=a(i,j)
|
|
enddo
|
|
enddo
|
|
call svdcmp(u(1:n,1:n),n,n,np,np,w,v(1:n,1:n),ierr)
|
|
wmax=0.0d0
|
|
do j=1,n
|
|
if(w(j).gt.wmax)wmax=w(j)
|
|
enddo
|
|
wmin=wmax*ftol
|
|
do j=1,n
|
|
if(w(j).lt.wmin)w(j)=0.0d0
|
|
enddo
|
|
call svbksb(u(1:n,1:n),w,v(1:n,1:n),n,n,np,np,b,x)
|
|
return
|
|
end
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
|
|
SUBROUTINE qrdcmp(a,n,np,c,d,sing)
|
|
implicit none
|
|
INTEGER n,np
|
|
DOUBLE PRECISION a(np,np),c(n),d(n)
|
|
LOGICAL sing
|
|
INTEGER i,j,k
|
|
DOUBLE PRECISION scaling,sigma,sum,tau
|
|
sing=.false.
|
|
do 17 k=1,n-1
|
|
scaling=0.0d0
|
|
do 11 i=k,n
|
|
scaling=dmax1(scaling,dabs(a(i,k)))
|
|
11 continue
|
|
if(scaling.eq.0.0d0)then
|
|
sing=.true.
|
|
c(k)=0.0d0
|
|
d(k)=0.0d0
|
|
else
|
|
do 12 i=k,n
|
|
a(i,k)=a(i,k)/scaling
|
|
12 continue
|
|
sum=0.0d0
|
|
do 13 i=k,n
|
|
sum=sum+a(i,k)**2
|
|
13 continue
|
|
sigma=dsign(dsqrt(sum),a(k,k))
|
|
a(k,k)=a(k,k)+sigma
|
|
c(k)=sigma*a(k,k)
|
|
d(k)=-scaling*sigma
|
|
do 16 j=k+1,n
|
|
sum=0.0d0
|
|
do 14 i=k,n
|
|
sum=sum+a(i,k)*a(i,j)
|
|
14 continue
|
|
tau=sum/c(k)
|
|
do 15 i=k,n
|
|
a(i,j)=a(i,j)-tau*a(i,k)
|
|
15 continue
|
|
16 continue
|
|
endif
|
|
17 continue
|
|
d(n)=a(n,n)
|
|
if(d(n).eq.0.0d0)sing=.true.
|
|
return
|
|
END
|
|
c
|
|
SUBROUTINE qrupdt(r,qt,n,np,u,v)
|
|
implicit none
|
|
INTEGER n,np
|
|
DOUBLE PRECISION r(np,np),qt(np,np),u(np),v(np)
|
|
CU USES rotate
|
|
INTEGER i,j,k
|
|
do 11 k=n,1,-1
|
|
if(u(k).ne.0.0d0)goto 1
|
|
11 continue
|
|
k=1
|
|
1 do 12 i=k-1,1,-1
|
|
call rotate(r,qt,n,np,i,u(i),-u(i+1))
|
|
if(u(i).eq.0.0d0)then
|
|
u(i)=dabs(u(i+1))
|
|
else if(dabs(u(i)).gt.dabs(u(i+1)))then
|
|
u(i)=dabs(u(i))*dsqrt(1.0d0+(u(i+1)/u(i))**2)
|
|
else
|
|
u(i)=dabs(u(i+1))*dsqrt(1.0d0+(u(i)/u(i+1))**2)
|
|
endif
|
|
12 continue
|
|
do 13 j=1,n
|
|
r(1,j)=r(1,j)+u(1)*v(j)
|
|
13 continue
|
|
do 14 i=1,k-1
|
|
call rotate(r,qt,n,np,i,r(i,i),-r(i+1,i))
|
|
14 continue
|
|
return
|
|
END
|
|
c
|
|
SUBROUTINE rsolv(a,n,np,d,b)
|
|
implicit none
|
|
INTEGER n,np
|
|
DOUBLE PRECISION a(np,np),b(n),d(n)
|
|
INTEGER i,j
|
|
DOUBLE PRECISION sum
|
|
b(n)=b(n)/d(n)
|
|
do 12 i=n-1,1,-1
|
|
sum=0.0d0
|
|
do 11 j=i+1,n
|
|
sum=sum+a(i,j)*b(j)
|
|
11 continue
|
|
b(i)=(b(i)-sum)/d(i)
|
|
12 continue
|
|
return
|
|
END
|
|
c
|
|
SUBROUTINE rotate(r,qt,n,np,i,a,b)
|
|
implicit none
|
|
INTEGER n,np,i
|
|
DOUBLE PRECISION a,b,r(np,np),qt(np,np)
|
|
INTEGER j
|
|
DOUBLE PRECISION c,fact,s,w,y
|
|
if(a.eq.0.0d0)then
|
|
c=0.0d0
|
|
s=dsign(1.0d0,b)
|
|
else if(dabs(a).gt.dabs(b))then
|
|
fact=b/a
|
|
c=dsign(1.0d0/dsqrt(1.0d0+fact**2),a)
|
|
s=fact*c
|
|
else
|
|
fact=a/b
|
|
s=dsign(1.0d0/dsqrt(1.0d0+fact**2),b)
|
|
c=fact*s
|
|
endif
|
|
do 11 j=i,n
|
|
y=r(i,j)
|
|
w=r(i+1,j)
|
|
r(i,j)=c*y-s*w
|
|
r(i+1,j)=s*y+c*w
|
|
11 continue
|
|
do 12 j=1,n
|
|
y=qt(i,j)
|
|
w=qt(i+1,j)
|
|
qt(i,j)=c*y-s*w
|
|
qt(i+1,j)=s*y+c*w
|
|
12 continue
|
|
return
|
|
END
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
subroutine symmatindex(ndim,icolrow,indexi)
|
|
implicit none
|
|
|
|
integer ndim,icolrow,indexi(ndim)
|
|
|
|
! this subroutine determines the sequence number of the elements of the row i
|
|
! or column i of a symmetric matrix that is stored in compact form (diagonal included)
|
|
! The compact form stores the upper triangle of the matrix sequentially
|
|
! column by column
|
|
! 1, 2, 4, 7, 11,
|
|
! 3, 5, 8, 12,
|
|
! 6, 9, 13,
|
|
! 10 14,
|
|
! 15,
|
|
! Note that it does not matter whether it is row or column because the matrix is
|
|
! symmetric
|
|
integer j,icompactpostri
|
|
|
|
indexi(1)=icompactpostri(1,icolrow)
|
|
do j=2,icolrow
|
|
indexi(j)=indexi(j-1)+1
|
|
enddo
|
|
do j=icolrow+1,ndim
|
|
indexi(j)=icompactpostri(icolrow,j)
|
|
enddo
|
|
return
|
|
end
|
|
!
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
subroutine ivarjvartri(invars,nvars,indexvars,
|
|
& ivar,jvar,istart,jstart)
|
|
implicit none
|
|
!
|
|
!This subroutine has two functions:
|
|
!1.Given the position number in the vector that stores the upper triangle
|
|
! (including the diagonal elements) of a symmetric matrix, determine the cell position
|
|
! (ivar,jvar) indices in the actual matrix.
|
|
!2. Determine the starting cell position in the symmetric matrix formed by all
|
|
! elements of the nvars variables. The number of elements
|
|
! indexed by ivar or jvar is specified in indexvars.
|
|
|
|
integer invars,nvars,indexvars(nvars),ivar,jvar,
|
|
& istart,jstart
|
|
integer n0
|
|
|
|
jvar=int((1.0d0+dsqrt(dble(8*invars-7)))/2.0d0)
|
|
ivar=invars-(jvar*(jvar-1))/2
|
|
if(ivar.gt.jvar)then
|
|
! rounding error
|
|
jvar=jvar+1
|
|
ivar=invars-(jvar*(jvar-1))/2
|
|
endif
|
|
|
|
20 istart=0
|
|
do n0=1,ivar-1
|
|
istart=istart+indexvars(n0)
|
|
enddo
|
|
istart=istart+1
|
|
jstart=0
|
|
do n0=1,jvar-1
|
|
jstart=jstart+indexvars(n0)
|
|
enddo
|
|
jstart=jstart+1
|
|
return
|
|
end subroutine ivarjvartri
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
subroutine ivarjvaroff(invars,nvars,indexvars,
|
|
& ivar,jvar,istart,jstart)
|
|
implicit none
|
|
!
|
|
!This subroutine has two functions:
|
|
!1.Given the position number in the vector that stores the upper triangle
|
|
! (not including the diagonal elements) of a symmetric matrix, determine the cell position
|
|
! (ivar,jvar) indices in the actual matrix.
|
|
!2. Determine the starting cell position in the expanded matrix. The number of elements
|
|
! indexed by ivar or jvar is specified in indexvars.
|
|
|
|
integer invars,nvars,indexvars(nvars),ivar,jvar,
|
|
& istart,jstart
|
|
integer n0
|
|
|
|
jvar=int((3.0d0+dsqrt(dble(8*invars-7)))/2.0d0)
|
|
ivar=invars-((jvar-2)*(jvar-1))/2
|
|
if(ivar.ge.jvar)then
|
|
! rounding error
|
|
jvar=jvar+1
|
|
ivar=invars-((jvar-2)*(jvar-1))/2
|
|
endif
|
|
istart=0
|
|
do n0=1,ivar-1
|
|
istart=istart+indexvars(n0)
|
|
enddo
|
|
istart=istart+1
|
|
jstart=0
|
|
do n0=1,jvar-1
|
|
jstart=jstart+indexvars(n0)
|
|
enddo
|
|
jstart=jstart+1
|
|
return
|
|
end subroutine ivarjvaroff
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
integer function icompactpostri(irow0,jcol0)
|
|
implicit none
|
|
!
|
|
! determine the position of the cell (irow,jcol) in the vector
|
|
! that stores the upper triangle of a symmetric matrix, including
|
|
! the diagonal elements.
|
|
! irow <=jcol
|
|
|
|
integer irow0,jcol0
|
|
|
|
icompactpostri=irow0+(jcol0*(jcol0-1))/2
|
|
|
|
return
|
|
end
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
integer function icompactposoff(irow0,jcol0)
|
|
implicit none
|
|
!
|
|
! determine the position of the cell (irow,jcol) in the vector
|
|
! that stores the upper triangle of a symmetric matrix, not including
|
|
! the diagonal elements.
|
|
! irow < jcol-1
|
|
|
|
integer irow0,jcol0
|
|
|
|
icompactposoff=irow0+((jcol0-1)*(jcol0-2))/2
|
|
return
|
|
end
|
|
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
subroutine istartjstart(ivar,jvar,nvars,
|
|
& indexvars,istart,jstart)
|
|
implicit none
|
|
|
|
integer nvars,indexvars(nvars),ivar,jvar,istart,
|
|
& jstart,n0
|
|
!
|
|
istart=0
|
|
do n0=1,ivar-1
|
|
istart=istart+indexvars(n0)
|
|
enddo
|
|
istart=istart+1
|
|
jstart=0
|
|
do n0=1,jvar-1
|
|
jstart=jstart+indexvars(n0)
|
|
enddo
|
|
jstart=jstart+1
|
|
return
|
|
end subroutine istartjstart
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|
|
integer function getwhichvar(ipos,nvars,indexvars)
|
|
implicit none
|
|
!
|
|
! determine to which variable ipos belongs
|
|
integer ipos,nvars,indexvars(nvars),n0,i
|
|
!
|
|
n0=0
|
|
i=0
|
|
10 i=i+1
|
|
n0=n0+indexvars(i)
|
|
if(ipos.le.n0)then
|
|
getwhichvar=i
|
|
return
|
|
endif
|
|
goto 10
|
|
end
|
|
!&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
|