568 lines
14 KiB
FortranFixed
568 lines
14 KiB
FortranFixed
C This file contains all the subroutines needed by the nonlinear solver broydn
|
|
C code modified based on on-line version in Numerical Recipes Website, Dec 28, 2004
|
|
|
|
SUBROUTINE broydn(x0min,x,x0max,STPMX,n,
|
|
& fveccopy,funcv,TOLF,ierr)
|
|
implicit none
|
|
INTEGER n,NP,MAXITS,ierr
|
|
double precision x(n),EPS,TOLF,TOLMIN,TOLX,STPMX
|
|
double precision x0min(n),x0max(n),fveccopy(n)
|
|
LOGICAL check
|
|
! PARAMETER (NP=1000,MAXITS=250,EPS=1.0d-7,TOLF=1.0d-4,
|
|
! & TOLMIN=1.d-6,TOLX=EPS)
|
|
PARAMETER(NP=1000,MAXITS=250,EPS=1.0d-7,TOLX=EPS)
|
|
CU USES fdjac,funcv,lnsrch,qrdcmp,qrupdt,rsolv
|
|
INTEGER i,its,j,k
|
|
double precision den,f,fold,stpmax,sum,temp,test,c(NP),
|
|
& d(NP),fvcold(NP),g(NP),p(NP),qt(NP,NP),r(NP,NP),
|
|
& s(NP),t(NP),w(NP),xold(NP),fvec(NP)
|
|
LOGICAL restrt,sing,skip
|
|
EXTERNAL funcv
|
|
TOLMIN=TOLF*0.01d0
|
|
call funcv(n,x,fvec,f)
|
|
test=0.0d0
|
|
do 11 i=1,n
|
|
if(dabs(fvec(i)).gt.test)test=dabs(fvec(i))
|
|
11 continue
|
|
if(test.lt..01d0*TOLF)then
|
|
ierr=0
|
|
! check=.false.
|
|
return
|
|
endif
|
|
sum=0.0d0
|
|
do 12 i=1,n
|
|
sum=sum+x(i)*x(i)
|
|
12 continue
|
|
stpmax=STPMX*dmax1(dsqrt(sum),dble(n))
|
|
restrt=.true.
|
|
do 42 its=1,MAXITS
|
|
if(restrt)then
|
|
do i=1,n
|
|
if(x(i).lt.x0min(i).or.x(i).gt.x0max(i))then
|
|
ierr=1
|
|
return
|
|
endif
|
|
enddo
|
|
call fdjac(n,x,fvec,NP,r,funcv)
|
|
call qrdcmp(r,n,NP,c,d,sing)
|
|
! if(sing) pause 'singular Jacobian in broydn'
|
|
if(sing)then
|
|
ierr=2
|
|
return
|
|
end if
|
|
do 14 i=1,n
|
|
do 13 j=1,n
|
|
qt(i,j)=0.0d0
|
|
13 continue
|
|
qt(i,i)=1.0d0
|
|
14 continue
|
|
do 18 k=1,n-1
|
|
if(c(k).ne.0.0d0)then
|
|
do 17 j=1,n
|
|
sum=0.0d0
|
|
do 15 i=k,n
|
|
sum=sum+r(i,k)*qt(i,j)
|
|
15 continue
|
|
sum=sum/c(k)
|
|
do 16 i=k,n
|
|
qt(i,j)=qt(i,j)-sum*r(i,k)
|
|
16 continue
|
|
17 continue
|
|
endif
|
|
18 continue
|
|
do 21 i=1,n
|
|
r(i,i)=d(i)
|
|
do 19 j=1,i-1
|
|
r(i,j)=0.0d0
|
|
19 continue
|
|
21 continue
|
|
else
|
|
do 22 i=1,n
|
|
s(i)=x(i)-xold(i)
|
|
22 continue
|
|
do 24 i=1,n
|
|
sum=0.0d0
|
|
do 23 j=i,n
|
|
sum=sum+r(i,j)*s(j)
|
|
23 continue
|
|
t(i)=sum
|
|
24 continue
|
|
skip=.true.
|
|
do 26 i=1,n
|
|
sum=0.0d0
|
|
do 25 j=1,n
|
|
sum=sum+qt(j,i)*t(j)
|
|
25 continue
|
|
w(i)=fvec(i)-fvcold(i)-sum
|
|
if(dabs(w(i)).ge.EPS*(dabs(fvec(i))+
|
|
& dabs(fvcold(i))))then
|
|
skip=.false.
|
|
else
|
|
w(i)=0.0d0
|
|
endif
|
|
26 continue
|
|
if(.not.skip)then
|
|
do 28 i=1,n
|
|
sum=0.0d0
|
|
do 27 j=1,n
|
|
sum=sum+qt(i,j)*w(j)
|
|
27 continue
|
|
t(i)=sum
|
|
28 continue
|
|
den=0.0d0
|
|
do 29 i=1,n
|
|
den=den+s(i)*s(i)
|
|
29 continue
|
|
do 31 i=1,n
|
|
s(i)=s(i)/den
|
|
31 continue
|
|
call qrupdt(r,qt,n,NP,t,s)
|
|
do 32 i=1,n
|
|
if(r(i,i).eq.0.0d0) then
|
|
write(*,*) 'r singular in broydn'
|
|
end if
|
|
d(i)=r(i,i)
|
|
32 continue
|
|
endif
|
|
endif
|
|
do 34 i=1,n
|
|
sum=0.0d0
|
|
do 33 j=1,n
|
|
sum=sum+qt(i,j)*fvec(j)
|
|
33 continue
|
|
p(i)=-sum
|
|
34 continue
|
|
do 36 i=n,1,-1
|
|
sum=0.0d0
|
|
do 35 j=1,i
|
|
sum=sum-r(j,i)*p(j)
|
|
35 continue
|
|
g(i)=sum
|
|
36 continue
|
|
do 37 i=1,n
|
|
xold(i)=x(i)
|
|
fvcold(i)=fvec(i)
|
|
37 continue
|
|
fold=f
|
|
call rsolv(r,n,NP,d,p)
|
|
|
|
! Gu modification starts
|
|
do 100 i=1,n
|
|
if(xold(i).lt.x0min(i).or.xold(i).gt.x0max(i))then
|
|
ierr=1
|
|
return
|
|
endif
|
|
100 continue
|
|
! Gu modification ends
|
|
call lnsrch(n,xold,fold,g,p,x,f,
|
|
& stpmax,check,funcv,fvec)
|
|
test=0.0d0
|
|
do 38 i=1,n
|
|
if(dabs(fvec(i)).gt.test)test=dabs(fvec(i))
|
|
fveccopy(i)=fvec(i)
|
|
38 continue
|
|
if(test.lt.TOLF)then
|
|
ierr=0
|
|
! check=.false.
|
|
return
|
|
endif
|
|
if(check)then
|
|
if(restrt)then
|
|
ierr=3
|
|
return
|
|
else
|
|
test=0.0d0
|
|
den=dmax1(f,.5d0*dble(n))
|
|
do 39 i=1,n
|
|
temp=dabs(g(i))*dmax1(dabs(x(i)),1.0d0)/den
|
|
if(temp.gt.test)test=temp
|
|
39 continue
|
|
if(test.lt.TOLMIN)then
|
|
ierr=4
|
|
return
|
|
else
|
|
restrt=.true.
|
|
endif
|
|
endif
|
|
else
|
|
restrt=.false.
|
|
test=0.0d0
|
|
do 41 i=1,n
|
|
temp=(dabs(x(i)-xold(i)))/dmax1(dabs(x(i)),1.0d0)
|
|
if(temp.gt.test)test=temp
|
|
41 continue
|
|
if(test.lt.TOLX)then
|
|
ierr=4
|
|
! check=.true.
|
|
return
|
|
endif
|
|
endif
|
|
42 continue
|
|
ierr=5
|
|
return
|
|
END
|
|
|
|
SUBROUTINE fdjac(n,x,fvec,np,df,funcv)
|
|
implicit none
|
|
INTEGER n,np
|
|
double precision df(np,np),fvec(n),x(n),EPS
|
|
PARAMETER (EPS=1.0d-4)
|
|
CU USES funcv
|
|
INTEGER i,j,k
|
|
double precision h,temp,f(n),fsqsum
|
|
external funcv
|
|
do 12 j=1,n
|
|
temp=x(j)
|
|
h=EPS*dabs(temp)
|
|
if(h.eq.0.0d0)h=EPS
|
|
x(j)=temp+h
|
|
h=x(j)-temp
|
|
call funcv(n,x,f,fsqsum)
|
|
x(j)=temp
|
|
do 11 i=1,n
|
|
df(i,j)=(f(i)-fvec(i))/h
|
|
11 continue
|
|
12 continue
|
|
return
|
|
END
|
|
c
|
|
SUBROUTINE lnsrch(n,xold,fold,g,p,x,f,
|
|
& stpmax,check,funcv,fvec)
|
|
implicit none
|
|
INTEGER n
|
|
LOGICAL check
|
|
DOUBLE PRECISION f,fold,stpmax,g(n),p(n),x(n),
|
|
*xold(n),ALF,TOLX,fvec(n)
|
|
PARAMETER (ALF=1.d-4,TOLX=1.d-7)
|
|
EXTERNAL funcv
|
|
CU USES funcv
|
|
INTEGER i
|
|
DOUBLE PRECISION a,alam,alam2,alamin,b,disc,
|
|
*f2,rhs1,rhs2,slope,sum,temp,test,tmplam
|
|
check=.false.
|
|
sum=0.0d0
|
|
do 11 i=1,n
|
|
sum=sum+p(i)*p(i)
|
|
11 continue
|
|
sum=dsqrt(sum)
|
|
if(sum.gt.stpmax)then
|
|
do 12 i=1,n
|
|
p(i)=p(i)*stpmax/sum
|
|
12 continue
|
|
endif
|
|
slope=0.0d0
|
|
do 13 i=1,n
|
|
slope=slope+g(i)*p(i)
|
|
13 continue
|
|
! if(slope.ge.0.0d0)pause 'roundoff problem in lnsrch'
|
|
test=0.0d0
|
|
do 14 i=1,n
|
|
temp=dabs(p(i))/dmax1(dabs(xold(i)),1.0d0)
|
|
if(temp.gt.test)test=temp
|
|
14 continue
|
|
alamin=TOLX/test
|
|
alam=1.0d0
|
|
1 continue
|
|
do 15 i=1,n
|
|
x(i)=xold(i)+alam*p(i)
|
|
15 continue
|
|
call funcv(n,x,fvec,f)
|
|
if(alam.lt.alamin)then
|
|
do 16 i=1,n
|
|
x(i)=xold(i)
|
|
16 continue
|
|
check=.true.
|
|
return
|
|
else if(f.le.fold+ALF*alam*slope)then
|
|
return
|
|
else
|
|
if(alam.eq.1.0d0)then
|
|
tmplam=-slope/(2.0d0*(f-fold-slope))
|
|
else
|
|
rhs1=f-fold-alam*slope
|
|
rhs2=f2-fold-alam2*slope
|
|
a=(rhs1/alam**2-rhs2/alam2**2)/(alam-alam2)
|
|
b=(-alam2*rhs1/alam**2+alam*rhs2/alam2**2)/
|
|
& (alam-alam2)
|
|
if(a.eq.0.0d0)then
|
|
tmplam=-slope/(2.0d0*b)
|
|
else
|
|
disc=b*b-3.0d0*a*slope
|
|
if(disc.lt.0.0d0) then
|
|
tmplam=0.5d0*alam
|
|
else if(b.le.0.0d0)then
|
|
tmplam=(-b+dsqrt(disc))/(3.0d0*a)
|
|
else
|
|
tmplam=-slope/(b+dsqrt(disc))
|
|
endif
|
|
endif
|
|
if(tmplam.gt..5d0*alam)tmplam=.5d0*alam
|
|
endif
|
|
endif
|
|
alam2=alam
|
|
f2=f
|
|
alam=dmax1(tmplam,.1d0*alam)
|
|
goto 1
|
|
END
|
|
c
|
|
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 scale,sigma,sum,tau
|
|
sing=.false.
|
|
do 17 k=1,n-1
|
|
scale=0.0d0
|
|
do 11 i=k,n
|
|
scale=dmax1(scale,dabs(a(i,k)))
|
|
11 continue
|
|
if(scale.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)/scale
|
|
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)=-scale*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 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
|