求解轨道力学二体意义下的Lambert方程（兰伯特方程）的Fortran程序

!*****************************************************************
    subroutine solve_lambert(r1,r2,tt,GM,lw,N,nBranch,v1,v2)
    implicit real(8)(A-H,O-Z)
    dimension r1(3),r2(3),v1(3),v2(3),tmp3(3),wih(3),r1p(3),r2p(3)
    external x2tof,fnorm
    !This routine implements a new algorithm that solves Lambert‘s problem. The
    !algorithm has two major characteristics that makes it favorable to other
    !existing ones.
    !
    !   1) It describes the generic orbit solution of the boundary condition
    !   problem through the variable X=log(1+cos(alpha/2)). By doing so the
    !   graphs of the time of flight become defined in the entire real axis and
    !   resembles a straight line. Convergence is granted within few iterations
    !   for all the possible geometries (except, of course, when the transfer
    !   angle is zero). When multiple revolutions are considered the variable is
    !   X=tan(cos(alpha/2)*pi/2).
    !
    !   2) Once the orbit has been determined in the plane, this routine
    !   evaluates the velocity vectors at the two points in a way that is not
    !   singular for the transfer angle approaching to pi (Lagrange coefficient
    !   based methods are numerically not well suited for this purpose).
    !
    !   As a result Lambert‘s problem is solved (with multiple revolutions
    !   being accounted for) with the same computational effort for all
    !   possible geometries. The case of near 180 transfers is also solved
    !   efficiently.
    !
    !   We note here that even when the transfer angle is exactly equal to pi
    !   the algorithm does solve the problem in the plane (it finds X), but it
    !   is not able to evaluate the plane in which the orbit lies. A solution
    !   to this would be to provide the direction of the plane containing the
    !   transfer orbit from outside. This has not been implemented in this
    !   routine since such a direction would depend on which application the
    !   transfer is going to be used in.
    !
    !Usage: [v1,v2,a,p,theta,iter]=lambertI(r1,r2,t,GM,lw,N,nBranch)
    !
    !Inputs:
    !           r1=Position vector at departure (column,km)
    !           r2=Position vector at arrival (column, same units as r1,km)
    !           t=Transfer time (scalar,s)
    !           GM=gravitational parameter (scalar, units have to be
    !           consistent with r1,t units,km^3/s^2)
    !           lw=1 if long way is chosen
    !           nBranch=‘1‘ if the left nBranch is selected in a problem where N
    !           is not 0 (multirevolution)
    !    天体运行是由分支.所以nBranch一般选择0
    !           N=number of revolutions
    !
    !      说明:当N~=0时,旋转方向不光用lw来控制还要先用nBranch来控制.
    !Outputs:
    !           v1=Velocity at departure        (consistent units)(km/s)
    !           v2=Velocity at arrival              (km/s)

    !           iter=number of iteration made by the newton solver (usually 6)
    !
    !    当需要时可以加上.
    !补充说明:
    !                [v1,v2,a,p,theta,iter]=lambertI(r1,r2,t,GM,lw,N,nBranch)



    !nBranch=1!here 1 is represent left
    !Preliminary control on the function call
    pi = 3.141592653589793D0
    
    if (tt<=0) then
        v1=1/0D0
        v2=1/0D0
        return
    end if

    tol=1D-11  !Increasing the tolerance does not bring any advantage as the
    !precision is usually greater anyway (due to the rectification of the tof
    !graph) except near particular cases such as parabolas in which cases a
    !lower precision allow for usual convergence.

    !Non dimensional units
    R=sqrt(r1(1)**2+r1(2)**2+r1(3)**2)
    V=sqrt(GM/R)
    T=R/V

    !working with non-dimensional radii and time-of-flight
    r1p=r1/R
    r2p=r2/R
    t=tt/T

    !Evaluation of the relevant geometry parameters in non dimensional units
    r2mod=sqrt(r2p(1)**2+r2p(2)**2+r2p(3)**2)
    theta=acos((r1p(1)*r2p(1)+r1p(2)*r2p(2)+r1p(3)*r2p(3))/r2mod)
    !close to pi and the acos function could return complex numbers

    !计算夹角,并确定是大弧还是小弧.
    if (lw>=1) theta=2*pi-theta

    c=sqrt(1D0+r2mod**2-2D0*r2mod*cos(theta)) !non dimensional chord
    s=(1D0+r2mod+c)/2D0                      !non dimensional semi-perimeter
    am=s/2D0                               !minimum energy ellipse semi major axis
    wlambda=sqrt(r2mod)*cos(theta/2D0)/s    !lambda parameter defined in BATTIN‘s book

    !We start finding the log(x+1) value of the solution conic:
    !!NO MULTI REV --> (1 SOL)
    if (N==0) then
        winn1=-0.5233D0    !first guess point
        winn2=0.5233D0     !second guess point
        x1=log(1D0+winn1)
        x2=log(1D0+winn2)
        y1=log(x2tof(winn1*1D0,s,c,lw,N))-log(t)
        y2=log(x2tof(winn2*1D0,s,c,lw,N))-log(t)

        !Newton iterations
        err=1
        i=0
        do while ((err>tol) .and. (y1/=y2))
            i=i+1
            xnew=(x1*y2-y1*x2)/(y2-y1)
            ynew=log(x2tof(exp(xnew)-1,s,c,lw,N))-log(t)
            x1=x2
            y1=y2
            x2=xnew
            y2=ynew
            err=abs(x1-xnew)
        end do
        iter=i
        x=exp(xnew)-1

        !!MULTI REV --> (2 SOL) SEPARATING RIGHT AND LEFT BRANCH
    else
        if (nBranch==1) then
            winn1=-0.5234D0
            winn2=-0.2234D0
        else
            winn1=0.2D0
            winn2=0.5234D0
        end if
        x1=tan(winn1*pi/2)
        x2=tan(winn2*pi/2)
        y1=x2tof(winn1,s,c,lw,N)-t

        y2=x2tof(winn2,s,c,lw,N)-t
        err=1
        i=0

        !Newton Iteration
        do while ((err>tol) .and. (i<90) .and. (y1/=y2))
            i=i+1
            xnew=(x1*y2-y1*x2)/(y2-y1)
            ynew=x2tof(atan(xnew)*2/pi,s,c,lw,N)-t
            x1=x2
            y1=y2
            x2=xnew
            y2=ynew
            err=abs(x1-xnew)
        end do
        x=atan(xnew)*2/pi
        iter=i
    end if

    !The solution has been evaluated in terms of log(x+1) or tan(x*pi/2), we
    !now need the conic. As for transfer angles near to pi the lagrange
    !coefficient technique goes singular (dg approaches a zero/zero that is
    !numerically bad) we here use a different technique for those cases. When
    !the transfer angle is exactly equal to pi, then the wih unit vector is not
    !determined. The remaining equations, though, are still valid.


    a=am/(1-x**2)                       !solution semimajor axis
    !calcolo psi
    if (x<1D0) then !ellisse
        beta=2D0*asin(sqrt((s-c)/2D0/a))
        if (lw>=1) beta=-beta

        alfa=2D0*acos(x)
        psi=(alfa-beta)/2D0
        eta2=2*a*sin(psi)**2/s
        eta=sqrt(eta2)
    else !iperbole
        beta=2*asinh(sqrt((c-s)/2/a))
        if (lw>=1) beta=-beta
        
        alfa=2*acosh(x)
        psi=(alfa-beta)/2
        eta2=-2*a*sinh(psi)**2/s
        eta=sqrt(eta2)
    end if
    p=r2mod/am/eta2*sin(theta/2)**2     !parameter of the solution
    sigma1=1/eta/sqrt(am)*(2*wlambda*am-(wlambda+x*eta))
    call cross(r1p,r2p,tmp3)
    wih=tmp3/fnorm(tmp3,3)
    if (lw>=1) wih=-wih

    vr1 = sigma1
    vt1 = sqrt(p)
    call cross(wih,r1p,tmp3)
    v1  = vr1 * r1p   +   vt1 * tmp3

    vt2=vt1/r2mod
    vr2=-vr1+(vt1-vt2)/tan(theta/2)
    call cross(wih,r2p/r2mod,tmp3)
    v2=vr2*r2p/r2mod+vt2*tmp3

    v1=v1*V
    v2=v2*V
    if (err>tol) then
        v1=(/100D0,100D0,100D0/)
        v2=(/100D0,100D0,100D0/)
    end if
    end subroutine
!*****************************************************************
    real(8) function x2tof(x,s,c,lw,N)
    implicit real(8)(A-H,O-Z)
    external tofabn
    !Subfunction that evaluates the time of flight as a function of x
    am=s/2D0
    a=am/(1D0-x**2)
    if (x<1D0) then
        beta=2D0*asin(sqrt((s-c)/2D0/a))
        if (lw>=1) beta=-beta
        alfa=2*acos(x)
    else   !!IPERBOLE
        alfa=2*acosh(x)
        beta=2*asinh(sqrt((s-c)/(-2D0*a)))
        if (lw>=1) beta=-beta
    end if
    x2tof=tofabn(a,alfa,beta,N)
    end function
!*****************************************************************
    real(8) function tofabn(sigma,alfa,beta,N)
    implicit real(8)(A-H,O-Z)
    !subfunction that evaluates the time of flight via Lagrange expression
    pi = 3.141592653589793D0
    if (sigma>0D0) then
        tofabn=sigma*sqrt(sigma)*((alfa-sin(alfa))-(beta-sin(beta))+N*2D0*pi)
    else
        tofabn=-sigma*sqrt(-sigma)*((sinh(alfa)-alfa)-(sinh(beta)-beta))
    end if
    end function
!*****************************************************************
    subroutine cross(A,B,C)
    implicit none
    real(8) :: A(3),B(3),C(3)
!----------------------------------------------------------------
!   **计算矢量A(三维)与B的叉乘,C为返回的矢量**
!----------------------------------------------------------------
    C(1)=A(2)*B(3)-A(3)*B(2)
    C(2)=A(3)*B(1)-A(1)*B(3)
    C(3)=A(1)*B(2)-A(2)*B(1)
    end subroutine
!*****************************************************************
    real(8) function fnorm(X,N)
    implicit none
    integer(4) :: N
    real(8) :: X(N)
!----------------------------------------------------------------
!   求数组X的二范数
!----------------------------------------------------------------
    fnorm = sqrt(dot_product(X(1:N),X(1:N)))
    return
    end function
!*****************************************************************