shoot.f90

一个基于打靶法的最优控制求解软件 求解过程中采用参数延续算法

          if (singularcontrolmode > 0) then 
             controlmode = 3 !call AlternateControl
          else
             controlmode = 2
          end if
          arcentry = 1

          !************************************************************************
          !Update dy/dz (jump) and compute switchcond/matchcond derivatives for VAR 
          if (mode == 2) then

             !*******************
             !update dy/dz (jump)
             dtaudz = 0d0
             !index of current tin in z is:  nz+(nodes-1)*xpdim + switch - 1
             dtaudz(nz+(nodes-1)*xpdim + switch - 1) = 1d0

             !compute phi2 after arc entry
             call RHS(xpdim,tin,y(1:xpdim),phinext,lambda)
             if (shootdebug > 1) write(0,*) 'Phi2 for entry',phinext

             !jump = (phi1-phi2) * dtau/dz
             deltaphi = phiprev - phinext
             do k=1,xpdim
                do j=1,n
                   jump(k,j) = deltaphi(k) * dtaudz(j)
                end do
             end do

             !update dydz in y
             do j = 1, n !et non nz hein -_-...
                y(xpdim+(j-1)*xpdim+1 : xpdim+j*xpdim) = y(xpdim+(j-1)*xpdim+1 : xpdim+j*xpdim ) + jump(1:xpdim,j)
             end do

             if (shootdebug > 1) then
                write(0,*) 'entry dtaudz',dtaudz
                write(0,*) 'entry deltaphi',deltaphi
                write(0,*) 'entry jump',jump
                write(0,*) ''
                write(0,*) 'After jump dydz'
                do j = 1,n
                   dydz(1:xpdim,j) = y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
                end do
                do j=1,xpdim
                   write(0,*) dydz(j,1:n)
                end do
             end if

             !*********************
             !matchcond derivatives
             if ((structure(arc) > 0)) then
                !ynodes(node,1:xpdim) indices in z are: nz + (node-2)*xpdim + 1 to nz + (node-1)*xpdim 
                dsdz(matchoffset:matchoffset+xpdim-1,1:n) = 0d0
                do j=1,xpdim
                   dsdz(matchoffset + j - 1, nz + (node-2)*xpdim + j) = 1d0
                end do
                !retrieve dydz after jump
                do j = 1,n
                   dydz(1:xpdim,j) = y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
                end do
!!$                write(0,*) 'dydz after entry jump'
!!$                do j=1,xpdim
!!$                   write(0,*) dydz(j,1:n)
!!$                end do
                
                dsdz(matchoffset:matchoffset+xpdim-1,1:n) = dsdz(matchoffset:matchoffset+xpdim-1,1:n) - dydz(1:xpdim,1:n)

                if (shootdebug > 1) then
                   write(0,*) 'Entry match deriv from',matchoffset
                   do j=1,xpdim
                      write(0,*) dsdz(matchoffset+j-1,1:n)
                   end do
                end if

             end if


             !*****************************************
             !compute switchcond derivatives
             !assume cond = psi (cf dimsing reset to 1)
             !dcond/dz = dpsi/dy  (dy/dt dt/dz + dy/dz) 
             do j = 1,n
                dydz(1:xpdim,j) = y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
             end do
             do k=1,xpdim
                do j=1,n
                   aux(k,j) = phinext(k) * dtaudz(j)
                end do
             end do
             call SwitchGradient(lambda,y(1:xpdim),gradpsi)
             dsdz(singoffset,1:n) = matmul(gradpsi,aux + dydz)

             if (shootdebug > 1) then
                write(0,*) 'gradpsi',gradpsi
                write(0,*) 'dydz'
                do j = 1,n
                   dydz(1:xpdim,j) = y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
                end do
             end if
             if (shootdebug > 1) then
                write(0,*) 'Entry deriv',singoffset,':',dsdz(singoffset,1:n) 
             end if


             !reset dydz in y for VAR
             if (structure(arc) > 0) then            
                !ynodes(node,1:xpdim) indices in z are: nz + (node-2)*xpdim + 1 to nz + (node-1)*xpdim  [for i>=2]  
                dydz = 0d0
                do j=1,xpdim
                  dydz(j,nz + (node-2)*xpdim + j) = 1d0
                end do
                if (shootdebug > 1) then
                   write(0,*) 'dydz after entry '
                   do j=1,xpdim
                      write(0,*) dydz(j,1:n)
                   end do
                   write(0,*) ''
                end if
                do j=1,n
                   y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim) = dydz(1:xpdim,j)
                end do
             end if


          end if !mode == 2




       case (-3,3)  !SINGULAR ARC EXIT
 

          !***********************
          !singular exit condition
          if (dimsing == 1) then
             call SingularCondition(tin,y(1:xpdim),s(offset:offset+dimsing-1),dimsing)
             if (outmode * shootverbose > 0) write(0,*) 'Exit cond',s(offset:offset+dimsing-1)
             singoffset = offset
             offset = offset + dimsing
          end if

          !*************************
          !full matching formulation  
          if (structure(arc) > 0) then
             node = node + 1 
             !retrieve y at tin
             y(1:xpdim) = ynodes(node,1:xpdim)
             !matching conditions
             s(offset:offset+xpdim-1) = y(1:xpdim) - ymatch(1:xpdim)
             if (outmode * shootverbose > 0) write(0,*) 'Exit Match error',sqrt(sum(s(offset:offset+xpdim-1)*s(offset:offset+xpdim-1)))
             matchoffset = offset
             offset = offset + xpdim
          end if

          !**********************************
          !switch back to nonsingular control
          switch = switch + 1
          !singflag = 0
          controlmode = discshoot
          arcentry = 0

          !************************************************************************
          !Update dy/dz (jump) and compute switchcond/matchcond derivatives for VAR 
          if (mode == 2) then

             !*******************
             !update dy/dz (jump)
             dtaudz = 0d0
             !index of current tin in z is:  nz+(nodes-1)*xpdim + switch - 1
             dtaudz(nz+(nodes-1)*xpdim + switch - 1) = 1d0

             !compute phi2 after arc entry
             call RHS(xpdim,tin,y(1:xpdim),phinext,lambda)
             if (shootdebug > 1) write(0,*) 'Phi2 for exit',phinext

             !jump = (phi1-phi2) * dtau/dz
             deltaphi = phiprev - phinext

             do k=1,xpdim
                do j=1,n
                   jump(k,j) = deltaphi(k) * dtaudz(j)
                end do
             end do

             !update dydz in y
             do j = 1, n !et non nz hein -_-...
                y(xpdim+(j-1)*xpdim+1 : xpdim+j*xpdim) = y(xpdim+(j-1)*xpdim+1 : xpdim+j*xpdim ) + jump(1:xpdim,j)
             end do

 
             if (shootdebug > 1) then
                write(0,*) 'exit dtaudz',dtaudz
                write(0,*) 'exit deltaphi',deltaphi
                write(0,*) 'exit jump',jump
                write(0,*) ''
                write(0,*) 'After jump dydz'
                do j = 1,n
                   dydz(1:xpdim,j) = y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
                end do
                do j=1,xpdim
                   write(0,*) dydz(j,1:n)
                end do
             end if


             !*********************
             !matchcond derivatives
             if ((structure(arc) > 0)) then
                !ynodes(node,1:xpdim) indices in z are: nz + (node-2)*xpdim + 1 to nz + (node-1)*xpdim 
                dsdz(matchoffset:matchoffset+xpdim-1,1:n) = 0d0
                do j=1,xpdim
                   dsdz(matchoffset + j - 1, nz + (node-2)*xpdim + j) = 1d0
                end do
                !retrieve dydz after jump
                do j = 1,n
                   dydz(1:xpdim,j) = y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
                end do

!!$                write(0,*) 'dydz after exit jump'
!!$                do j=1,xpdim
!!$                   write(0,*) dydz(j,1:n)
!!$                end do

                dsdz(matchoffset:matchoffset+xpdim-1,1:n) = dsdz(matchoffset:matchoffset+xpdim-1,1:n) - dydz(1:xpdim,1:n)
                if (shootdebug > 1) then
                   write(0,*) 'Exit match deriv from',matchoffset
                   do j=1,xpdim
                      write(0,*) dsdz(matchoffset+j-1,1:n)
                   end do
                end if
             end if


             !**************************************
             !switching condition derivative for VAR
             !assume cond = psi (cf dimsing reset to 1)
             !dcond/dz = dpsi/dy  (dy/dt dt/dz + dy/dz) 
             if (dimsing == 1) then
                do j = 1,n
                   dydz(1:xpdim,j) = y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
                end do
                do k=1,xpdim
                   do j=1,n
                      aux(k,j) = phinext(k) * dtaudz(j)
                   end do
                end do
                call SwitchGradient(lambda,y(1:xpdim),gradpsi)
                dsdz(singoffset,1:n) = matmul(gradpsi,aux + dydz)
                if (shootdebug > 1) then
                   !write(0,*) 'gradpsi',gradpsi
                   write(0,*) 'Exit deriv',singoffset,':',dsdz(singoffset,1:n) 
                end if
             end if !dimsing == 1


             !reset dydz in y for VAR
             if (structure(arc) > 0) then            
                !ynodes(node,1:xpdim) indices in z are: nz + (node-2)*xpdim + 1 to nz + (node-1)*xpdim 
                dydz = 0d0
                do j=1,xpdim
                  dydz(j,nz + (node-2)*xpdim + j) = 1d0
                end do
                if (shootdebug > 1) then
                   write(0,*) 'dydz after exit'
                   do j=1,xpdim
                      write(0,*) dydz(j,1:n)
                   end do
                   write(0,*) ''
                end if
                do j=1,n
                   y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim) = dydz(1:xpdim,j)
                end do
             end if
          end if !mode == 2


!!$       case (-4)   !TOUCH POINT 
!!$
!!$          x(1:4) = y(1:4) / yscal(1:4)
!!$          !cond S = S'= 0
!!$          s(offset) =  (sqrt(8d0) - sqrt(x(1)**2 + x(2)**2)) *1d2
!!$          s(offset+1) = (x(1)*x(3) + x(2)*x(4)) *1d2
!!$          offset = offset + 2
!!$          if (outmode > 0 .and. shootverbose > 0) write(0,*) 'Touch at arc',arc,s(offset-2:offset-1)
!!$          
!!$          !jump sur p1,p2 a la goret: z=[tf,p1,p2,p3,p4,t1,t2,pi1,pi2]
!!$          y(ns+1) = y(ns+1) + pjump(arc-1) * x(1) / sqrt(x(1)**2 + x(2)**2) * yscal(ns+1)
!!$          y(ns+2) = y(ns+2) + pjump(arc-1) * x(2) / sqrt(x(1)**2 + x(2)**2) * yscal(ns+2)
!!$          if (outmode > 0 .and. shootverbose > 0) write(0,*) 'Pi at arc',arc,pjump(arc-1)



!!$          case (-5,5)   !ACTIVE CONSTRAINT ENTRY
!!$          
!!$
!!$             controlmode = 4
!!$             dimact = 2
!!$             call ConstraintCondition(tin,y(1:xpdim),s(offset:offset+dimact-1),dimact,1)
!!$             if (outmode*shootverbose > 0) write(0,*) 'Active conditions at entry',s(offset:offset+dimact-1)
!!$             offset = offset + dimact
!!$
!!$             !jump pb scaling aparemment, a checker
!!$             y(ns+3) = y(ns+3) - pijump/yscal(ns+3)
!!$             ymatch(ns+3) = ymatch(ns+3) - pijump/yscal(ns+3)
!!$             
!!$
!!$             !*************************
!!$             !full matching formulation  
!!$             if (structure(arc) > 0) then
!!$                node = node + 1 
!!$                !retrieve y at tin
!!$                y(1:xpdim) = ynodes(node,1:xpdim)
!!$                !matching conditions
!!$                s(offset:offset+xpdim-1) = y(1:xpdim) - ymatch(1:xpdim)
!!$                if (outmode * shootverbose > 0) write(0,*) 'Entry Match',s(offset:offset+xpdim-1)
!!$                matchoffset = offset
!!$                offset = offset + xpdim
!!$             end if
!!$
!!$
!!$
!!$
!!$          case (-6,6)  !ACTIVE CONSTRAINT EXIT
!!$
!!$             controlmode = 0
!!$             dimact = 1
!!$             call ConstraintCondition(tin,y(1:xpdim),s(offset:offset+dimact-1),dimact,2)
!!$             if (outmode*shootverbose > 0) write(0,*) 'Active conditions at exit',s(offset:offset+dimact-1)
!!$             offset = offset + dimact
!!$
!!$
!!$             !*************************
!!$             !full matching formulation  
!!$             if (structure(arc) > 0) then
!!$                node = node + 1 
!!$                !retrieve y at tin
!!$                y(1:xpdim) = ynodes(node,1:xpdim)
!!$                !matching conditions
!!$                s(offset:offset+xpdim-1) = y(1:xpdim) - ymatch(1:xpdim)
!!$                if (outmode * shootverbose > 0) write(0,*) 'Exit Match',s(offset:offset+xpdim-1)
!!$                matchoffset = offset
!!$                offset = offset + xpdim
!!$             end if

       case default
          write(0,*) 'ERROR: IVP >>> Unknown structure for initial time...',structure(arc),'at',arc
          stop
       end select





       !*********************
       !INTEGRATE CURRENT ARC

       if (outmode * shootdebug > 0) write(0,*) 'Integrate between',tin,tout,'phase',phase,'control',controlmode
       if (tin < tout) then
          call Integrate(dim,lambda,tin,tout,y(1:dim),mode)
       else
          if (outmode > 0 .or. shootdebug > 0) write(0,*) 'Ghost arc...',tin,tout
       end if




       !***************************
       !POST INTEGRATION OPERATIONS
       select case (structure(arc+1))
       case (-7,-6,-5,-4,0,5,6)
          !FINAL TIME OR SIMPLE INTERIOR NODE
          !save match for interior node
          if (arc < nbtimes - 1) then
             ymatch(1:xpdim) = y(1:xpdim)
          end if


       case (-2,2)
          !SINGULAR ARC ENTRY

          !save phi1 before arc entry for VAR
          if (mode == 2) then
             call RHS(xpdim,tin,y(1:xpdim),phiprev,lambda)
          end if

          !save match for entry
          if (structure(arc+1) > 0) then
             ymatch(1:xpdim) = y(1:xpdim)
          end if

       case (-3,3)
          !SINGULAR ARC EXIT

          !save phi1 before arc exit for VAR
          if (mode == 2) then
             call RHS(xpdim,tin,y(1:xpdim),phiprev,lambda)
          end if
 
          !save match for exit
          if (structure(arc+1) > 0) then
             ymatch(1:xpdim) = y(1:xpdim)
          end if


       case default
          write(0,*) 'ERROR: IVP >>> Unknown structure for final time...',structure(arc),'at',arc
          stop
       end select

    end do


    !************************************
    !COMPUTE FINAL CONDITIONS (CF and CT)
    call FinalConditions(y(1:xpdim),s(1:ncf),dsdy)


    !compute dsdz for VAR
    if (mode == 2) then

       !retrieve dy/dz matrix 
       do j = 1,n
          dydz(1:xpdim,j) = y(xpdim + (j-1)*xpdim + 1 : xpdim + j*xpdim)
       end do


       dsdz(1:ncf,1:n) = matmul(dsdy,dydz)
       !note: derivatives after ncf already filled during integration

       if (shootdebug > 1) then
          write(0,*) 'final dydz'
          do j=1,xpdim
             write(0,*) dydz(j,1:n)
          end do
          write(0,*) ''
       end if

    end if