c     error analysis of a leaf-spring suspension   nov. 2000 ew
c     updates:
c     24 feb. 2001: partial derivatives of nonlinearity included
c
      program leafpart
c
c     partielle ableitungen von frequenz, nichtlin. und masse nach
c     den parametern xu,yu,wu, xo,yo,wo, fl,ax,fk, rot
c
c     eingabedaten:
c     ng: anzahl federglieder. ng=64 gibt sehr genaue resultate.
c     xu,yu,wu: lage und richtung der festen federklemme (cm und grad)
c     xo,yo,wo: dasselbe fuer die bewegliche klemme
c     fl: federlaenge. step: schrittweite zu nf (cm)
c     ax: winkel der empfindlichen achse gegen die vertikale (grad)
c          beschreibt beim feedbackseismometer den einfluss von neigung)
c     fekru: federkruemmung vor dem einspannen in grad
c
c     es wird angenommen, dass die masse punktfoermig ist und sich in
c     5 cm abstand vom gelenk befindet.
c
c     zusaetzliche parameter:
c     acc: genauigkeitsforderung bei der iteration (cm)
c     xinc: inkrement beim berechnen der partiellen ableitungen (cm)
c     hstep: winkelinkrement bei der berechnung der pot. energie (rad)
c     maxit: abbruch der iteration nach maxit aufrufen von blf
c
c     achtung: es kommen drei verschiedene normierungen des drehmoments
c     vor. routine blf rechnet mit dimensionslosem drehmoment. bei der
c     inversion nach den startparametern ist dieses durch 5 dividiert.
c     das physikalische drehmoment in kp*cm ergibt sich durch multi-
c     plikation mit faktor*2*pi/180. derselbe faktor ist bei der
c     kraft anzuwenden.
c     analog kommt auch die kraft in zwei verschiedenen normierungen vor.
c
      implicit double precision (a-h,o-z)
      dimension gg(3),hh(3),fqd(11),tqd(11),gmd(11)
      character*2 txt(10)
      character*80 line
      data txt /'  ','xu','yu','wu','xo','yo','wo','fl','ax','fk'/
      common acc,xinc,ax,pi4,halb,ngg,gl,ramin,ramax,fl,itera,hstep,
     -nix,bu,nf,maxit,fakt,dfak,fekru,torad,dnull,d5
      dnull=0.d0
      d5=5.d0
      halb=5.d-1
      pi4=datan2(halb,halb)
      torad=pi4/45.d0
      t5=torad*d5
      hstep=1.d-2
      wstep=hstep/t5
      acc=1.d-11
      xinc=1.d-4
      maxit=333
      nix=0
      open(7,file='leaf.in')
      open(8,file='part.out')
      ng=64

      write(6,15) 
      write(8,15) 
   15 format('Leaf-spring seismometer:'/
     &'this program computes partial derivatives of the squared eigenfre
     &quency,'/'of its nonlinearity, and of the seismic mass with respec
     &t to the geometry.'/'Partial derivatives are per centimeter or 0.
     &2 rad.'/)
      write(6,*) '    Files:  leaf.in ==> leafpart ==> part.out'
      write(8,*) '    Files:  leaf.in ==> leafpart ==> part.out'
      write(6,*) '    File part.out contains two diagrams (ASCII art).'


   10 read(7,'(a)',err=50,end=50) line
      read(line,'(i2)') iopt
      if(iopt.eq.9) goto 10
      if(iopt.eq.0) then
        write(6,150)
  150   format(' Option iopt=0 or blank in col.1 - stop')
        stop
      endif
       read (line,1,err=50,end=50) xu,yu,wu,xo,yo,wo,fl,ax,fekru,
     & breite,dicke,emodul,xki,yki,di
    1 format(2x,15f5.3)


c   10 read (7,1,err=50) iopt,xu,yu,wu,xo,yo,wo,fl,ax,fekru,breite,dicke,
c     & emodul,xki,yki,di
c    1 format(i1,1x,15f5.3)
      write(6,4)     xu,yu,wu,xo,yo,wo,fl,ax,fekru,breite,dicke,emodul,
     & xki,yki,di
      write(8,4)     xu,yu,wu,xo,yo,wo,fl,ax,fekru,breite,dicke,emodul,
     & xki,yki,di
    4 format(/' spring geometry (input data):'//
     &'       xu      yu      wu      xo      yo      wo      fl      ax
     &'/ 1x,8f8.3//
     &'       fk      wi      th              em     xki     yki      di
     &'/ 1x,3f8.3,f16.3,3f8.3//)
      goto 70
   50 write(6,60)
   60 format(' input error. the file leaf.in must contain the following'
     &/' 15 parameters in one line in format (15f5.3):'/)
      write(6,40)
      stop
   70 write(6,40)
      write(8,40)
   40 format(' xu, yu, wu: coordinates and angle of one end of the sprin
     &g'/' xo, yo, wo: same for the other end of the spring. coordinates
     & in'/'             cm from the hinge. angles in degrees ccw from t
     &he'/'             x axis. spring may be rotated around the hinge.'
     & /' fl, ax, fk: length of the spring, angle of the axis of sensiti
     &vity'/'             against the vertical, initial curvature of the 
     & spring'/'             (end-to-end, without load) in degrees.
     &'/' wi, th, em: width, thickness, elast. modulus of the spring
     &'/' xki,yki,di: estimated spring force and moment (start parameter
     &s)'/)

      write(6,41)
      write(8,41)
   41 format(' This program should be run with the EXACT parameters'
     &/' of the suspension such as determined with LEAFSPRING'
     &/' It does not automatically adjust the parameters for a long free
     & period.')
 
      fakt=emodul*breite*dicke**3/24.d0
      dfak=fakt*2.*torad
      xk=xki/dfak
      yk=yki/dfak
      d=di/dfak/d5
      ax=ax*torad
      fk=fk*torad
      xuv=xu
      yuv=yu
      do 101 k=1,10
      if(k.eq.2) xuv=xuv+hstep
      if(k.eq.3) yuv=yuv+hstep
      if(k.eq.4) wu=wu+wstep
      if(k.eq.5) xo=xo+hstep
      if(k.eq.6) yo=yo+hstep
      if(k.eq.7) wo=wo+wstep
      if(k.eq.8) fl=fl+hstep
      if(k.eq.9) ax=ax+wstep*torad
      if(k.eq.10) fk=fk+wstep*torad
c     if(k.eq.11) call rot(xuv,yuv,wu,wstep*torad)
      nix=0
      gl=fl/ng
      ngg=ng-1
      fekru=fk/ng
      r=dsqrt(xo*xo+yo*yo)
      call freq(xuv,yuv,wu,xo,yo,wo,fq,tq,gm,gg,hh,ter,xk,yk,d,r)
      if(k.eq.1) then
      fqref=fq 
      tqref=tq
      gmref=gm 
      endif
      if(k.eq.2) xuv=xuv-hstep
      if(k.eq.3) yuv=yuv-hstep
      if(k.eq.4) wu=wu-wstep
      if(k.eq.5) xo=xo-hstep
      if(k.eq.6) yo=yo-hstep
      if(k.eq.7) wo=wo-wstep
      if(k.eq.8) fl=fl-hstep
      if(k.eq.9) ax=ax-wstep*torad
      if(k.eq.10) fk=fk-wstep*torad
c     if(k.eq.11) call rot(xuv,yuv,wu,-wstep/torad)
      fqd(k)=(fq-fqref)/hstep
      gmd(k)=(gm-gmref)/hstep
      tqd(k)=(tq-tqref)/hstep
      if(k.eq.1) write(6,12)
      if(k.eq.1) write(8,12)
   12 format( /' case       f^2      nlin      mass   der_f^2  der_nlin
     &  der_mass   param.')
      write(6,6) k-1,fq,tq,gm,fqd(k),tqd(k),gmd(k),txt(k)
      write(8,6) k-1,fq,tq,gm,fqd(k),tqd(k),gmd(k),txt(k)
    6 format(i5,3f10.4,3f10.3,5x,a2)
  101 continue
      call display(fqd,tqd,'nlin')
      call display(fqd,gmd,'mass')
      end

      subroutine display(fqd,tqd,text)
      implicit double precision (a-h,o-z)
      dimension fqd(11),tqd(11),kf(11),kt(11)
      character zeile*73,blank*1,stern*1,text*4
      data blank,stern/' ','*'/
      fmax=0.d0
      tmax=0.d0
      do 1 j=2,10
      fmax=max(fmax,dabs(fqd(j)))
    1 tmax=max(tmax,dabs(tqd(j)))
      write(8,6) fmax,text,tmax
    6 format(///
     &'diagram: horizontal: freq^2, max=',f7.3,'   vertical: ',a4,', max
     &=',f7.3//'wrt: 1-xu, 2-yu, 3-wu, 4-xo, 5-yo, 6-wo, 7-fl, 8-ax, 9-f
     &k'//)
      do 2 j=1,10
      kf(j)=37.5d0+35d0*(fqd(j)/fmax)
    2 kt(j)=19.5d0-17d0*(tqd(j)/tmax)
      do 3 nz=1,37
      do 4 nc=1,73
    4 zeile(nc:nc)=blank
      if(nz.eq.1.or.nz.eq.37) then
        do 7 nc=1,73,2
    7   zeile(nc:nc)=stern
      else
        zeile(1:1)=stern
        zeile(73:73)=stern
      endif
      do 5 j=1,10
      if(kt(j).eq.nz) then
        ncol=kf(j)
        if(zeile(ncol:ncol).ne.blank) then
          zeile(ncol:ncol)=stern
          else
          zeile(ncol:ncol)=char(j+47)
          if(j.eq.11) zeile(ncol:ncol)='t'
        endif
      endif
    5 continue
      if(zeile(37:37).eq.blank) zeile(37:37)='|'
      if(nz.eq.19) then
        do 8 nc=2,72
    8   if(zeile(nc:nc).eq.blank) zeile(nc:nc)='-'
      endif
    3 write(8,'(1x,a)') zeile
      return
      end

      subroutine rot(x,y,w,rw)
      implicit double precision (a-h,o-z)
      common acc,xinc,ax,pi4,halb,ngg,gl,ramin,ramax,fl,itera,hstep,
     -nix,bu,nf,maxit,fakt,dfak,fekru,torad,dnull,d5
      xalt=x
      yalt=y
      c=dcos(rw)
      s=dsin(rw)
      x=c*xalt-s*yalt
      y=s*xalt+c*yalt
      w=w+rw/torad
      return
      end

      subroutine freq(xu,yu,wu,xo,yo,wo,fm,fdif,gm,gg,hh,ter,xk,yk,d,r)
      implicit double precision (a-h,o-z)
      dimension e(5),fkr(4),fko(3),fq(3),gg(3),hh(3)
      common acc,xinc,ax,pi4,halb,ngg,gl,ramin,ramax,fl,itera,hstep,
     -nix,bu,nf,maxit,fakt,dfak,fekru,torad,dnull,d5
c     achtung... winkeleinheit fuer b ist 10 grad
c     achtung... wahres drehmoment ist  5.*d *dfak
      b=wo/10.d0
      bu=wu/10.d0
      rm=5.d0
      if(nix.gt.0) goto 20
   20 zpiq = 8.*pi4
      zpiq = zpiq*zpiq
      ter=dnull
c  berechnen des elastischen potentials in 5 benachbarten lagen
      do 30 l=1,5
      h = hstep*(3-l)
      call kreis(xo,yo,b,h,xz,yz,bz,r)
      call such(xk,yk,d,xz-xu,yz-yu,bz,e(l))
      e(l)=e(l)*fakt
      ter=ter+itera/1000.d0
      if(nix.eq.0) goto 40
      if(l.ne.3) goto 30
      hh(1)=ramin
      hh(2)=ramax
      gg(1)=xk*dfak
      gg(2)=yk*dfak
      gg(3)=d5*d*dfak
   30 continue
c  drehmoment und seismische masse
      dreh=(e(4)-e(2))/(2.*hstep)
      rmg=dreh/dcos(ax)
      hh(3)=rmg/d5
      gm=rmg/rm
c  hinzunahme des schwerepotentials
      do 71 l=1,5
   71 e(l)=e(l)+rmg*dsin(ax+hstep*(3-l))
      fm=e(3)
      do 31 l=1,5
   31 e(l)=e(l)-fm
c  drehmoment als ableitung des potentials nach dem drehwinkel
      do 70 l=1,4
   70 fkr(l) =(e(l+1)-e(l))/hstep
c  rueckstellmoment als ableitung des drehmoments
      do 80 l=1,3
      fko(l) =(fkr(l+1)-fkr(l))/hstep
c  quadrat der eigenfrequenz
   80 fq(l) = fko(l)/gm*981./rm/d5/zpiq
      fm=fq(2)
      fdif=0.5d0*(fq(3)-fq(1))/(hstep*d5)
      ter=(fq(3)-fq(2)*2.+fq(1))/(hstep*d5)**2
      return
   40 fm=dnull
      fdif=dnull
      do 50 j=1,3
      gg(j)=dnull
   50 hh(j)=dnull
      return
      end

      subroutine kreis(xo,yo,ba,defl,x,y,b,r)
      implicit double precision (a-h,o-z)
      common acc,xinc,ax,pi4,halb,ngg,gl,ramin,ramax,fl,itera,hstep,
     -nix,bu,nf,maxit,fakt,dfak,fekru,torad,dnull,d5
      del = datan2(yo,xo) +defl
      x=r*dcos(del)
      y=r*dsin(del)
      b = ba+defl/torad/1.d1
      return
      end

      subroutine such(xk,yk,d,xz,yz,bz,e)
      implicit double precision (a-h,o-z)
      dimension a(3,3),dif(3),xkor(3)
      common acc,xinc,ax,pi4,halb,ngg,gl,ramin,ramax,fl,itera,hstep,
     -nix,bu,nf,maxit,fakt,dfak,fekru,torad,dnull,d5
      ab(a1,a2,a3)=a1*a1+a2*a2+a3*a3
      abq=acc*acc
      q=xinc
      itera=0
      fak=1.d00
    7 if(nix.eq.0) call blf(xk,yk,d,x,y,b,e)
    6 dif(1)=xz-x
      dif(2)=yz-y
      dif(3)=bz-b
      abneu=ab(dif(1),dif(2),dif(3))
      if(nix.gt.0) goto 5
    2 call blf(xk+q,yk,d,a(1,1),a(2,1),a(3,1),e)
      call blf(xk,yk+q,d,a(1,2),a(2,2),a(3,2),e)
      call blf(xk,yk,d+q,a(1,3),a(2,3),a(3,3),e)
      a(1,1)=(a(1,1)-x)/q
      a(2,1)=(a(2,1)-y)/q
      a(3,1)=(a(3,1)-b)/q
      a(1,2)=(a(1,2)-x)/q
      a(2,2)=(a(2,2)-y)/q
      a(3,2)=(a(3,2)-b)/q
      a(1,3)=(a(1,3)-x)/q
      a(2,3)=(a(2,3)-y)/q
      a(3,3)=(a(3,3)-b)/q
    5 xkalt=xk
      ykalt=yk
      dalt=d
      abalt=abneu
      xalt=x
      yalt=y
      balt=b
    3 call matin(a,dif,abneu*fak,xkor)
      xk=xkalt+xkor(1)
      yk=ykalt+xkor(2)
      d= dalt+xkor(3)
      nix=1
      call blf(xk,yk,d,x,y,b,e)
      dif(1) = xz-x
      dif(2) = yz-y
      dif(3) = bz-b
      abneu=ab(dif(1),dif(2),dif(3))
      if(itera.lt.maxit) goto 11
      nix=0
      return
   11 if(abneu.le.abq) return
      if(abneu.lt.abalt) goto 2
      if(itera.gt.2) goto 14
      nix=0
      goto 7
   14 fak=fak*32.d00
      dif(1)=xz-xalt
      dif(2)=yz-yalt
      dif(3)=bz-balt
      goto 3
      end

      subroutine matin(a,dif,wq,xkor)
      implicit double precision (a-h,o-z)
      dimension a(3,3),dif(3),xkor(3),b(3,3),c(3)
      dnull=0.d0
      do 1 j=1,3
      c(j)=dnull
      do 1 k=1,3
      c(j)=c(j)+a(k,j)*dif(k)
      b(j,k)=dnull
      if(j.eq.k) b(j,k)=wq
      do 1 i=1,3
    1 b(j,k)=b(j,k)+a(i,j)*a(i,k)
      call gaus3(b,c,xkor)
      return
      end

      subroutine gaus3(aik,rs,f)
      implicit double precision (a-h,o-z)
      dimension aik(3,3),rs(3),f(3),h(4),imax(3)
      dnull=0.d0
      do 1401 j=1,3
      aikmax=dnull
      do 1402 k=1,3
      h(k)=aik(j,k)
      if(abs(h(k)).le.aikmax) go to 1402
      aikmax=abs(h(k))
      index=k
 1402 continue
      h(4)=rs(j)
      do 1403 k=1,3
      q=aik(k,index)/h(index)
      do 1404 l=1,3
 1404 aik(k,l)=aik(k,l)-q*h(l)
 1403 rs(k)=rs(k)-q*h(4)
      do 1405 k=1,3
 1405 aik(j,k)=h(k)
      rs(j)=h(4)
 1401 imax(j)=index
      do 1406 j=1,3
      index=imax(j)
 1406 f(index)=rs(j)/aik(j,index)
      return
      end

      subroutine blf(xkk,ykk,dk,xx,yy,bb,ee)
      implicit double precision (a-h,o-z)
      common acc,xinc,ax,pi4,halb,ngg,gl,ramin,ramax,fl,itera,hstep,
     -nix,bu,nf,maxit,fakt,dfak,fekru,torad,dnull,d5
      itera=itera+1
      xk = xkk
      yk = ykk
      d = dk*d5
      w=torad*gl
      e=dnull
      b=10.d0*bu*w/gl
      x=halb*dcos(b)*gl
      y=halb*dsin(b)*gl
      rmin=1.d12
      rmax=dnull
      do 1 j=1,ngg
      db=w*(yk*x-xk*y-d)
      b=b+db+fekru
      bq=db*db
      e=e+bq
      rmin=dmin1(rmin,bq)
      rmax=dmax1(rmax,bq)
      x=x+dcos(b)*gl
    1 y=y+dsin(b)*gl
      db=w*(yk*x-xk*y-d)
      b=b+db+fekru
      bq=db*db
      e=e+bq
      rmin=dmin1(rmin,bq)
      rmax=dmax1(rmax,bq)
      xx=x+halb*dcos(b)*gl
      yy=y+halb*dsin(b)*gl
      bb = b/w*gl/10.d0
      ee = e/gl
      ramin=gl/dsqrt(rmin)
      ramax=gl/dsqrt(rmax)
      return
      end