src/sim_mat.f

Fortran project SIMRES, source module src/sim_mat.f.

Source module last modified on Sun, 22 May 2005, 14:09;
HTML image of Fortran source automatically generated by for2html on Mon, 23 May 2005, 21:29.


#//////////////////////////////////////////////////////////////////////
#////
#////  R E S T R A X   4.1
#////
#////  Subroutines for simple matrix operations:
#////  
#////  
#//////////////////////////////////////////////////////////////////////


#------------------------------------------
      SUBROUTINE INVERT(n,a,na,b,nb)
#     Inverts matrix A (A is not destroyed)
#------------------------------------------
      implicit none
      integer*4 n,na,nb,nmax
      parameter(nmax=16)
      real*8 a(na,na),b(nb,nb),a1(nmax,nmax),wk(2*nmax)
      integer*4 i,j,ierr
1     format (16(1x,g10.4))      
      do 5 i=1,n
      do 5 j=1,n
5        a1(i,j)=a(i,j)                
      call KVERTD(a1,nmax,n,wk,ierr)
      if (ierr.ne.0) then
         do i=1,n
           write(*,1) (a(i,j),j=1,n)
         enddo
         stop      
      endif
      do 10 i=1,n
      do 10 j=1,n
10       b(i,j)=a1(i,j)      
      return
      end
#


#--------------------------------------------------------------------    
      SUBROUTINE STAT_INP(nd,cv,x,p)
#     accumulates covariantes matrix of vector X wit probability P      
#--------------------------------------------------------------------      
      implicit none
      integer*4 nd,n
      parameter(n=16)
      real*8 p,x(n)
      STRUCTURE /STATI/
         real*8 sum2(n,n),sum1(n),sumn,c(n,n),m(n),p
         integer*4 nc
      end structure   
      record /STATI/ cv
      integer*4 i,j
      
      cv.sumn=cv.sumn+p
      cv.nc=cv.nc+1
      do 10 i=1,nd
         cv.sum1(i)=cv.sum1(i)+x(i)*p
         do 10 j=1,nd
         cv.sum2(i,j)=cv.sum2(i,j)+x(i)*x(j)*p
10    continue
      return
      end
      
#--------------------------------------    
      SUBROUTINE STAT_CLR(nd,cv)
#     cleares covariance matrix
#--------------------------------------
      implicit none
      integer*4 nd,n
      parameter(n=16)
      STRUCTURE /STATI/
         real*8 sum2(n,n),sum1(n),sumn,c(n,n),m(n),p
         integer*4 nc
      end structure   
      record /STATI/ cv
      integer*4 i,j
            
#      write(*,*) 'STAT_CLEAR',ND,CV.NC
      cv.sumn=0
      cv.nc=0
      do 10 i=1,nd
         cv.sum1(i)=0
         do 10 j=1,nd
         cv.sum2(i,j)=0
10    continue
      return
      end         
         
      
#--------------------------------------    
      SUBROUTINE STAT_GET(nd,cv)
#     calculates covariance matrix      
#--------------------------------------      
      implicit none
      integer*4 nd,n
      parameter(n=16)
      STRUCTURE /STATI/
         real*8 sum2(n,n),sum1(n),sumn,c(n,n),m(n),p
         integer*4 nc
      end structure   
      record /STATI/ cv
      integer*4 i,j
      
      if((cv.nc.gt.0).and.(cv.sumn.gt.0)) then     
      cv.p=cv.sumn/cv.nc
      do 10 i=1,nd
         cv.m(i)=cv.sum1(i)/cv.sumn
         do 10 j=1,nd
         cv.c(i,j)=cv.sum2(i,j)/cv.sumn
10    continue
      do 20 i=1,nd
         do 20 j=1,nd
         cv.c(i,j)=cv.c(i,j)-cv.m(i)*cv.m(j)
20    continue      
      endif
      return
      end 

#
#     ------------------------------
      SUBROUTINE M3XV3(it,map,m,b,c)
#     ------------------------------
      implicit none
      integer*4 it,i,j
      logical map(3)
      real*8 m(3,3),b(3),c(3)
      do 10 j=1,3
      if (map(j)) then
         c(j)=0.
         if (it.gt.0) then
           do 20 i=1,3
20         c(j)=c(j)+m(j,i)*b(i)
         else
           do 30 i=1,3
30         c(j)=c(j)+m(i,j)*b(i)
         endif
      else
         c(j)=b(j)
      endif
10    continue      
      return
      end            

#     ------------------------------
      SUBROUTINE MXV(it,n,np,a,b,c)
#     ------------------------------
      implicit none
      integer*4 it,n,np,i,j
      real*8 a(np,np),b(np),c(np)
      do 10 j=1,n
         c(j)=0.
         if (it.gt.0) then
           do 20 i=1,n
20         c(j)=c(j)+a(j,i)*b(i)
         else
           do 30 i=1,n
30         c(j)=c(j)+a(i,j)*b(i)
         endif
10    continue      
      return
      end 
      
#     ------------------------------
      SUBROUTINE MXM(it,n,np,a,b,c)
#     ------------------------------
      implicit none
      integer*4 it,i,j,k,n,np
      real*8 a(np,np),b(np,np),c(np,np)
      do 10 j=1,n
      do 10 k=1,n     
         c(j,k)=0.
         if (it.gt.0) then
           do 20 i=1,n
20         c(j,k)=c(j,k)+a(j,i)*b(i,k)
         else
           do 30 i=1,n
30         c(j,k)=c(j,k)+a(i,j)*b(i,k)
         endif
10    continue         
      return
      end       

          
#     ------------------------------
      SUBROUTINE M3XM3(it,a,b,c)
#     ------------------------------
      implicit none
      integer*4 it,i,j,k      
      real*8 a(3,3),b(3,3),c(3,3)
      do 10 j=1,3
      do 10 k=1,3     
         c(j,k)=0.
         if (it.gt.0) then
           do 20 i=1,3
20         c(j,k)=c(j,k)+a(j,i)*b(i,k)
         else
           do 30 i=1,3
30         c(j,k)=c(j,k)+a(i,j)*b(i,k)
         endif
10    continue         
      return
      end       
        
#     ------------------------------
      SUBROUTINE V3AV3(it,a,b,c)
#     ------------------------------
      implicit none
      integer*4 it,i     
      real*8 a(3),b(3),c(3)
 
      do 10 i=1,3
10      c(i)=a(i)+it*b(i)
      return
      end 
      
#     ------------------------------
      real*8 FUNCTION ABSV3(a)
#     ------------------------------
      implicit none
      real*8 a(3),V3XV3
      ABSV3=sqrt(V3XV3(a,a))
      return
      end       
      

         
#     ------------------------------
      real*8 FUNCTION V3XV3(a,b)
#     ------------------------------
      implicit none
      integer*4 i
      real*8 a(3),b(3),z
      z=0
      do 10 i=1,3
10      z=z+a(i)*b(i)
      V3XV3=z
      return
      end  
 
#     ------------------------------
      SUBROUTINE GENROT(iax,phi,aux)
#     ------------------------------
      implicit none
      integer*4 i,j,iax
      real*8 phi,co,si,aux(3,3)
            
      si=sin(phi)
      co=sqrt(1-si**2)               
      do 20 i=1,3
      do 20 j=1,3
      if(i.eq.j) then
         if (i.eq.iax) then 
            aux(i,j)=1.
         else
            aux(i,j)=co
         endif       
      else
         if((i.eq.iax).or.(j.eq.iax)) then
           aux(i,j)=0.
         else if (i.gt.j) then
           aux(i,j)=si
         else
           aux(i,j)=-si
         endif    
      endif
20    continue
      return
      end       

         
#**************************************************************
#
      real*8 FUNCTION DETERM(b,n,a)
#     COMPUTES THE DETERMINANT OF THE MATRIX B
      implicit none
      integer*4 i,j,k,n1,n,i1,j1,k1,j2,k2
      real*8 a(n,n),b(n,n),x      
      do 55 i=1,n 
      do 55 j=1,n 
   55 a(i,j)=b(i,j) 
      n1=n-1
      DETERM=1. 
      do 1 i=1,n1 
      j1=i
      k1=i
      do 10 j2=i,n
      do 10 k2=i,n
      if(abs(a(j1,k1)).ge.abs(a(j2,k2)))go to 10
      j1=j2 
      k1=k2
10    continue
      if(abs(a(j1,k1)).gt.1.e-30)go to 11
      DETERM=0. 
      return
11    continue
      if(j1.eq.i)go to 12 
      do 5 k=i,n
      x=a(i,k)
      a(i,k)=a(j1,k)
5     a(j1,k)=-x
12    if(k1.eq.i)go to 13 
      do 6 j=1,n
      x=a(j,i)
      a(j,i)=a(j,k1)
6     a(j,k1)=-x
13    i1=i+1
      do 30 j=i1,n 
      if(a(j,i).eq.0.)go to 30
      x=a(j,i)/a(i,i) 
      do 7 k=i,n
7     a(j,k)=a(j,k)-x*a(i,k)
30    continue
1     DETERM=DETERM*a(i,i)
      DETERM=DETERM*a(n,n)
      return
      end 
#                  
#
#
        SUBROUTINE DIAG(a,ada,b)
#***********************************************************************
#   diagonalizes real*4 matrix A(4,4), B(4,4) is corresponding rotation matrix
#***********************************************************************
        implicit none
        integer*4 i,j,k,l,n,nd,kk,ki,kj,jr,kl,ndk,ndi,ndj,jtes,ndn,ii
        integer*4 ij,ji,jj,jk,ik,ites
        real*4 a(16),ada(16),b(16),armax(16),jrmax(16)
        real*4 e,y,x,t,ty,tsq,c,s,csq,amax,aii,ajj,aij
        data n,nd,e/4,4,1.e-24/
        ndn=nd*n
        do 1 k=1,ndn
        ada(k)=a(k)
        b(k)=0.
    1        continue
        do 2 k=1,n
        kk=k*(nd+1)-nd
        armax(k)=0.
        b(kk)=1.
        do 3 l=k,n
        if(l-k)4,3,4
    4        kl=k+nd*(l-1)
        y=abs(ada(kl))
        if(armax(k)-y)5,3,3
    5        armax(k)=y
        jrmax(k)=l
    3        continue
    2        continue
   11        amax=0.
        do 6 k=1,n
        y=abs(armax(k))
        if(amax-y)7,6,6
    7        amax=y
        i=k
    6        continue
        j=jrmax(i)
        if(e-amax)8,9,9
    8        ndi=nd*(i-1)
        ndj=nd*(j-1)
        ii=i+ndi
        jj=j+ndj
        ij=i+ndj
        ji=j+ndi
        aii=ada(ii)
        ajj=ada(jj)
        aij=ada(ij)
        y=2.*aij
        x=aii-ajj
        t=sign(1.e0,x)*y/(abs(x)+sqrt(x**2+y**2))
        tsq=t**2
        c=1./sqrt(abs(1.+tsq))
        ty=t*y
        s=t*c
        csq=c**2
        ada(ii)=csq*(aii+ty+ajj*tsq)
        ada(jj)=csq*(ajj-ty+aii*tsq)
        ada(ij)=0.
        ada(ji)=0.
        do 10 k=1,n
        jtes=(k-i)*(k-j)
        ndk=nd*(k-1)
        ki=k+ndi
        kj=k+ndj
        if(jtes)13,12,13
   13        jk=j+ndk
        ik=i+ndk
        ada(ki)=c*ada(ik)+s*ada(jk)
        ada(kj)=-s*ada(ik)+c*ada(jk)
        ada(jk)=ada(kj)
        ada(ik)=ada(ki)
   12        x=b(ki)
        b(ki)=c*x+s*b(kj)
        b(kj)=-s*x+c*b(kj)
   10        continue
        armax(i)=0.
        do 14 k=1,n
        if(k-i)15,14,15
   15        ik=i+nd*(k-1)
        y=abs(ada(ik))
        if(armax(i)-y)16,14,14
   16        armax(i)=y
        jrmax(i)=k
   14        continue
        armax(j)=0.
        do 17 k=1,n
        if(k-j)18,17,18
   18        jk=j+nd*(k-1)
        y=abs(ada(jk))
        if(armax(j)-y)19,17,17
   19        armax(j)=y
        jrmax(j)=k
   17        continue
        do 20 k=1,n
        ites=(k-i)*(k-j)
        ki=k+ndi
        kj=k+ndj
        if(ites)21,20,21
   21        x=abs(ada(ki))
        y=abs(ada(kj))
        jr=j
        if(x-y)22,22,23
   23        y=x
        jr=i
   22        if(armax(k)-y)24,20,20
   24        armax(k)=y
        jrmax(k)=jr

   20        continue
        goto 11
9        continue
        return
        end

#-----------------------------------------------------------------    
      SUBROUTINE LINFIT(x,y,n,dx,dy,dz,nd,ampl,back,chisq)
#     Linear fit of the function (X,Y,N) to the data (DX,DY,DZ,ND)
# in REAL*4 !!
#----------------------------------------------------------------- 
      implicit none
      integer*4 i,k,n,nd,kk
      real*4 ampl,back,chisq
      real*4 x(n),y(n),dx(nd),dy(nd),dz(nd)
      real*4 c1,c2,c3,c4,c5,c6,z,yy,w,ddx,zmin
      
      c1=0
      c2=0
      c3=0
      c4=0
      c5=0
      c6=0
      kk=0
      zmin=0
      do i=1,n 
         if(abs(y(i)).gt.zmin) zmin=abs(y(i))
      enddo
      zmin=abs(zmin/10.)   
      do i=1,nd
        ddx=x(2)-x(1)
        z=(dx(i)-x(1))/ddx
        if(z.ge.0) then
           k=int(z)+1
        else
           k=int(z)
        endif      
        if((k.gt.0).and.(k.lt.n)) then      
           kk=kk+1
           yy=y(k)+(y(k+1)-y(k))*(z+1-k)   ! linear interpolation  
#           SIG2=DZ(I)**2
#           IF(SIG2.EQ.0) SIG2=1.
#           W=1/SIG2

            if (abs(yy).le.zmin) then 
               w=sqrt(zmin) 
            else
               w=sqrt(abs(yy))    ! weighted by SQRT(Y) (for RESTRAX only)
            endif 
           c1=c1+dy(i)*w
           c2=c2+yy*w
           c3=c3+w
           c4=c4+dy(i)*yy*w
           c5=c5+yy*yy*w
           c6=c6+dy(i)*dy(i)*w
        endif            
      end do
      if(kk.gt.0) then
         if((c2*c2-c3*c5).eq.0) then
            ampl=0
            back=0
            chisq=c6
         else         
            ampl= (c1*c2-c3*c4)/(c2*c2-c3*c5)
            back=(c4*c2-c5*c1)/(c2*c2-c3*c5)
            chisq=(ampl**2)*c5+(back**2)*c3+c6+2*ampl*back*c2-2*ampl*c4-
     1         2*back*c1
            chisq=chisq/kk/c3
         endif   
      else
         write(*,*)  'Cannot fit data ! '
         ampl=0.
         back=0.
      endif
      return
      end
                  
#------------------------------------------------           
      real*4 FUNCTION LINTERP4(x,y,n,z)
# linear interpolation on equidistant data      
# in REAL*4 !!
#------------------------------------------------      
      implicit none
      
      integer*4 i0,n
      real*4 z0,x(n),y(n),z,dx
      
      if (z.le.x(1)) then
         LINTERP4=y(1)
         return
      else if (z.ge.x(n)) then         
         LINTERP4=y(n)
         return
      else
         dx=(x(n)-x(1))/(n-1)
         z0=(z-x(1))/dx
         i0=int(z0)+1
         LINTERP4=y(i0)+(y(i0+1)-y(i0))*(z0-i0+1)         
         return
      endif        
      end 

#------------------------------------------------           
      real*8 FUNCTION LINTERP8(x,y,n,z)
# linear interpolation on equidistant data      
# in REAL*8!!
#------------------------------------------------      
      implicit none
      
      integer*4 i0,n
      real*8 z0,x(n),y(n),z,dx
      
      if (z.le.x(1)) then
         LINTERP8=y(1)
         return
      else if (z.ge.x(n)) then         
         LINTERP8=y(n)
         return
      else
         dx=(x(n)-x(1))/(n-1)
         z0=(z-x(1))/dx
         i0=int(z0)+1
         LINTERP8=y(i0)+(y(i0+1)-y(i0))*(z0-i0+1)         
         return
      endif        
      end 

#------------------------------------------------           
      real*8 FUNCTION LINTERP(y,n,x0,dx,z)
# linear interpolation on equidistant data      
# x=X0+i*DX, i=1..N
# extrapolation=0
#------------------------------------------------      
      implicit none
      
      real*8 y(n),x0,dx,z
      integer*4 n
      real*8 z0
      integer*4 i0
      
#      write(*,*)  '1D: ',X0,Z,X0+(N-1)*DX 
      if (z.le.x0.or.z.ge.x0+(n-1)*dx) then
         LINTERP=0.d0
         return
      else
         z0=(z-x0)/dx
         i0=int(z0)+1
#       write(*,*) '1D: ', Z0,I0,Y(I0)+(Y(I0+1)-Y(I0))*(Z0-I0+1)
         LINTERP=y(i0)+(y(i0+1)-y(i0))*(z0-i0+1)         
         return
      endif        
      end 

#-----------------------------------------------------           
      real*8 FUNCTION LINTERP2D(y,ni,nj,nd,zi,zj,di,dj,z)
# linear interpolation on equidistant data in 2D 
# x=ZI+i*DI, i=1..NI
# y=ZJ+j*DJ, j=1..NJ
#-----------------------------------------------------      
      implicit none
      
      integer*4 ni,nj,nd
      real*8 y(nd,nj),zi,zj,di,dj,z(2)            
      real*8 jj,yy1,yy2
      integer*4 j0
      real*8 LINTERP
      
#      write(*,*)  '2D: ',ZJ,Z(2),ZJ+(NJ-1)*DJ 

      if (z(2).le.zj.or.z(2).ge.zj+(nj-1)*dj) then
         LINTERP2D=0.d0
         return
      else
         jj=(z(2)-zj)/dj
         j0=int(jj)+1
         yy1=LINTERP(y(1,j0),ni,zi,di,z(1))
         yy2=LINTERP(y(1,j0+1),ni,zi,di,z(1))        
         LINTERP2D=yy1+(yy2-yy1)*(jj-j0+1)         
#       write(*,*) '2D: ', JJ,J0,YY1,YY2
         return
      endif        

      end 
      
#------------------------------------------------           
      real*4 FUNCTION QINTERP4(x,y,n,z)
# quadratic interpolation (X monotonous)      
# in REAL*4 !!
#------------------------------------------------      
      implicit none
     
      integer*4 i0,n,i
      real*4 x(n),y(n),z,a(3,3),b(3),c(3)
      
      if (z.le.x(1)) then
         QINTERP4=y(1)
         return
      else if (z.ge.x(n)) then         
         QINTERP4=y(n)
         return
      else
        i0=1
        do while(x(i0).lt.z)
          i0=i0+1
        enddo
        if(i0.gt.n-1) i0=n-1
        if(i0.lt.2) i0=2
        if((x(i0).eq.x(i0-1)).or.(x(i0).eq.x(i0+1))) then
           if(x(i0-1).eq.x(i0+1)) then
              QINTERP4=y(i0)
           else
              c(1)=(y(i0+1)-y(i0-1))/(x(i0+1)-x(i0-1))
              QINTERP4=y(i0-1)+c(1)*(z-x(i0-1))
           endif
           return
        endif      
        do i=1,3
          a(i,1)=x(i+i0-2)**2
          a(i,2)=x(i+i0-2)
          a(i,3)=1
          b(i)=y(i+i0-2)
        enddo
        call GAUSS(a,b,c,3)        
        QINTERP4=c(1)*z**2+c(2)*z+c(3)
      endif                
      end
      
      
      
      
#------------------------------------------------      
      SUBROUTINE GAUSS(a,b,c,n)
#// solve linear equations by Gauss elimination  
# in REAL*4 !!
#------------------------------------------------    
      implicit none
      integer*4 max
      parameter(max=16)
      integer*4 n,i,j,k
      real*4 a(n,n),c(n),b(n),aux(max,max+1),v(max+1),m,sum
      
      do i=1,n
      do j=1,n
        aux(i,j)=a(i,j)
      enddo
      enddo
      
      do i=1,n
        aux(i,n+1)=b(i)
      enddo
      
      do k=1,n-1
        i=k
        do while(aux(i,k).eq.0)
          i=i+1
          if (i.gt.n) goto 10
        enddo  
        do j=1,n+1
          v(j)=aux(i,j)
          aux(i,j)=aux(k,j)
          aux(k,j)=v(j)
        enddo
        do i=k+1,n
          m=aux(i,k)/aux(k,k)
          do j=k,n+1
            aux(i,j)=aux(i,j)-m*aux(k,j)
          enddo
        enddo
      enddo
      if (aux(n,n).eq.0) goto 10
      c(n)=aux(n,n+1)/aux(n,n)
      do k=1,n-1
        sum=0
        do j=n-k+1,n
          sum=sum+aux(n-k,j)*c(j)
        enddo  
        c(n-k)=(aux(n-k,n+1)-sum)/aux(n-k,n-k) 
      enddo
      return              
10    write(*,*)  'Matrix not invertible'
      do i=1,n
        c(i)=0
      enddo   
      pause
      return
  
      end            
      
#--------------------------------------------      
      real*8 FUNCTION ROT3(i,j,k,alfa)
#--------------------------------------------            
      implicit none
      
      integer*4 i,j,k
      real*8 alfa
      
      if((i.eq.j).or.(i.eq.k)) then
        if(j.eq.k) then
          ROT3=1
        else
          ROT3=0
        endif
      else if (j.eq.k) then
        ROT3=cos(alfa)
      else
        if(j.lt.k) then
           ROT3=-sin(alfa)
        else
           ROT3=sin(alfa)
        endif
      endif
      end                 
      

#--------------------------------------------      
      SUBROUTINE MK_ROT3(i,alfa,rt)
#--------------------------------------------            
      implicit none
      
      integer*4 i,j,k      
      real*8 alfa,rt(3,3),ROT3
      
      do j=1,3
      do k=1,3
         rt(j,k)=ROT3(i,j,k,alfa)
      enddo
      enddo
      end   
      
      

#------------------------------------------
      SUBROUTINE JACOBI(aa,a,n,np,d,v,nrot)
#  Diagonalize matrix AA, 
# returns the diagonalized matrix, V 
# and the transformation matrix, A
#  V=A^T*AA*A
#  (Numerical Recipes)
#------------------------------------------
      implicit none
      integer*4 nmax,n,np,nrot
      parameter(nmax=100)
      real*8 a(np,np),d(np),v(np,np),b(nmax),z(nmax),aa(np,np)
      integer*4 iq,ip,i,j
      real*8 sm,g,s,c,t,tau,h,tresh,theta
      
      do 12 ip=1,n
         do 11 iq=1,n
            v(ip,iq)=0
            a(ip,iq)=aa(ip,iq)
11       continue 
         v(ip,ip)=1 
12    continue          
      do 13 ip=1,n
        b(ip)=a(ip,ip)
        d(ip)=b(ip)
        z(ip)=0
13    continue
      nrot=0
      do 24 i=1,50
        sm=0.
        do 15 ip=1,n-1
          do 14 iq=ip+1,n
            sm=sm+abs(a(ip,iq))
14        continue                  
15      continue 
        if(sm.eq.0) goto 99
        if(i.lt.4) then
           tresh=0.2*sm/n**2
        else
           tresh=0.
        endif
        do 22 ip=1,n-1
          do 21 iq=ip+1,n
            g=100.*abs(a(ip,iq))
            if((i.gt.4).and.(abs(d(ip))+g.eq.abs(d(ip)))
     1       .and.(abs(d(iq))+g.eq.abs(d(iq)))) then
               a(ip,iq)=0
            else if (abs(a(ip,iq)).gt.tresh) then
               h=d(iq)-d(ip)
               if (abs(h)+g.eq.abs(h)) then
                  t=a(ip,iq)/h
               else
                  theta=0.5*h/a(ip,iq)
                  t=1./(abs(theta)+sqrt(1.+theta**2))
                  if(theta.lt.0.) t=-t
               endif
               c=1./sqrt(1+t**2)
               s=c*t
               tau=s/(1.+c)
               h=t*a(ip,iq)
               z(ip)=z(ip)-h
               z(iq)=z(iq)+h
               d(ip)=d(ip)-h
               d(iq)=d(iq)+h
               a(ip,iq)=0.
               do 16 j=1,ip-1
                  g=a(j,ip)
                  h=a(j,iq)
                  a(j,ip)=g-s*(h+g*tau)
                  a(j,iq)=h+s*(g-h*tau)
16             continue
               do 17 j=ip+1,iq-1
                  g=a(ip,j)
                  h=a(j,iq)
                  a(ip,j)=g-s*(h+g*tau)
                  a(j,iq)=h+s*(g-h*tau)
17             continue
               do 18 j=iq+1,n
                  g=a(ip,j)
                  h=a(iq,j)
                  a(ip,j)=g-s*(h+g*tau)
                  a(iq,j)=h+s*(g-h*tau)
18             continue
               do 19 j=1,n
                  g=v(j,ip)
                  h=v(j,iq)
                  v(j,ip)=g-s*(h+g*tau)
                  v(j,iq)=h+s*(g-h*tau)
19             continue
               nrot=nrot+1
            endif
21        continue
22      continue                
        do 23 ip=1,n
           b(ip)=b(ip)+z(ip)
           d(ip)=b(ip)
           z(ip)=0.
23      continue 
24    continue
      pause  '50 ITERATION SHOULD NEVER HAPPEN'
      return
99    continue

      return      
      end            
      
              
       
#------------------------------------------------
      SUBROUTINE KVERTD(v,lv,n,w,ierr)
# invert matrix, from http://www.netlib.org/napack
# rewritten from KVERT to real*8 by J.S.
# added IRES .. result indicator
#------------------------------------------------
#      ________________________________________________________
#     |                                                        |
#     |     INVERT A GENERAL MATRIX WITH COMPLETE PIVOTING     |
#     |                                                        |
#     |    INPUT:                                              |
#     |         V     --ARRAY CONTAINING MATRIX                |
#     |         LV    --LEADING (ROW) DIMENSION OF ARRAY V     |
#     |         N     --DIMENSION OF MATRIX STORED IN ARRAY V  |
#     |         W     --WORK ARRAY WITH AT LEAST 2N ELEMENTS   |
#     |    OUTPUT:                                             |
#     |         V     --INVERSE                                |
#     |    BUILTIN FUNCTIONS: ABS                              |
#     |________________________________________________________|
#
      implicit none
      integer*4 lv,n,h,i,j,k,l,m,o,p,q,ierr
      real*8 v(lv,1),w(1),s,t
      
      if ( n .eq. 1 ) goto 120
      o = n + 1
      l = 0
      m = 1
10    if ( l .eq. n ) goto 90
      k = l
      l = m
      m = m + 1
#     ---------------------------------------
#     |*** FIND PIVOT AND START ROW SWAP ***|
#     ---------------------------------------
      p = l
      q = l
      s = abs(v(l,l))
      do 20 h = l,n
           do 20 i = l,n
                t = abs(v(i,h))
                if ( t .le. s ) goto 20
                p = i
                q = h
                s = t
20    continue
      w(n+l) = p
      w(o-l) = q
      do 30 i = 1,n
           t = v(i,l)
           v(i,l) = v(i,q)
30         v(i,q) = t
      s = v(p,l)
      v(p,l) = v(l,l)
      if ( s .eq. 0. ) goto 130
#     -----------------------------
#     |*** COMPUTE MULTIPLIERS ***|
#     -----------------------------
      v(l,l) = -1.
      s = 1./s
      do 40 i = 1,n
40         v(i,l) = -s*v(i,l)
      j = l
50    j = j + 1
      if ( j .gt. n ) j = 1
      if ( j .eq. l ) goto 10
      t = v(p,j)
      v(p,j) = v(l,j)
      v(l,j) = t
      if ( t .eq. 0. ) goto 50
#     ------------------------------
#     |*** ELIMINATE BY COLUMNS ***|
#     ------------------------------
      if ( k .eq. 0 ) goto 70
      do 60 i = 1,k
60         v(i,j) = v(i,j) + t*v(i,l)
70    v(l,j) = s*t
      if ( m .gt. n ) goto 50
      do 80 i = m,n
80         v(i,j) = v(i,j) + t*v(i,l)
      goto 50
#     -----------------------
#     |*** PIVOT COLUMNS ***|
#     -----------------------
90    l = w(k+n)
      do 100 i = 1,n
           t = v(i,l)
           v(i,l) = v(i,k)
100        v(i,k) = t
      k = k - 1
      if ( k .gt. 0 ) goto 90
#     --------------------
#     |*** PIVOT ROWS ***|
#     --------------------
      do 110 j = 1,n
           do 110 i = 2,n
                p = w(i)
                h = o - i
                t = v(p,j)
                v(p,j) = v(h,j)
                v(h,j) = t
# OK
110   ierr=0 
      return
      
# OK, scalar only 
120   if ( v(1,1) .eq. 0. ) goto 130
      v(1,1) = 1./v(1,1)
      ierr=0
      return
# No inverse      
130   write(6,*)  'MATRIX HAS NO INVERSE'
      ierr=1
      end