solve_lambert_gooding Subroutine

public subroutine solve_lambert_gooding(r1, r2, tof, mu, long_way, multi_revs, v1, v2, status_ok)

Solve Lambert's problem using Gooding's method.

References

  1. R. H, Gooding. "A procedure for the solution of Lambert's orbital boundary-value problem" Celestial Mechanics and Dynamical Astronomy, vol. 48, no. 2, 1990, p. 145-165.
  2. A. Klumpp, "Performance Comparision of Lambert and Kepler Algorithms", JPL Interoffice Memorandum, 314.1-0426-ARK, Jan 2, 1991. Zip

Arguments

Type IntentOptional Attributes Name
real(kind=wp), intent(in), dimension(3) :: r1

first cartesian position [km]

real(kind=wp), intent(in), dimension(3) :: r2

second cartesian position [km]

real(kind=wp), intent(in) :: tof

time of flight [sec]

real(kind=wp), intent(in) :: mu

gravity parameter [km^3/s^2]

logical, intent(in) :: long_way

when true, do "long way" (>pi) transfers

integer, intent(in) :: multi_revs

maximum number of multi-rev solutions to compute

real(kind=wp), intent(out), dimension(:,:), allocatable :: v1

vector containing 3d arrays with the cartesian components of the velocities at r1

real(kind=wp), intent(out), dimension(:,:), allocatable :: v2

vector containing 3d arrays with the cartesian components of the velocities at r2

logical, intent(out) :: status_ok

true if everything is OK


Calls

proc~~solve_lambert_gooding~~CallsGraph proc~solve_lambert_gooding lambert_module::solve_lambert_gooding proc~cross vector_module::cross proc~solve_lambert_gooding->proc~cross proc~unit vector_module::unit proc~solve_lambert_gooding->proc~unit

Called by

proc~~solve_lambert_gooding~~CalledByGraph proc~solve_lambert_gooding lambert_module::solve_lambert_gooding proc~lambert_test lambert_module::lambert_test proc~lambert_test->proc~solve_lambert_gooding

Source Code

    subroutine solve_lambert_gooding(r1,r2,tof,mu,long_way,multi_revs,v1,v2,status_ok)

    implicit none

    real(wp),dimension(3),intent(in)                :: r1         !! first cartesian position [km]
    real(wp),dimension(3),intent(in)                :: r2         !! second cartesian position [km]
    real(wp),intent(in)                             :: tof        !! time of flight [sec]
    real(wp),intent(in)                             :: mu         !! gravity parameter [km^3/s^2]
    logical,intent(in)                              :: long_way   !! when true, do "long way" (>pi) transfers
    integer,intent(in)                              :: multi_revs !! maximum number of multi-rev solutions to compute
    real(wp),dimension(:,:),allocatable,intent(out) :: v1         !! vector containing 3d arrays with the cartesian components of the velocities at r1
    real(wp),dimension(:,:),allocatable,intent(out) :: v2         !! vector containing 3d arrays with the cartesian components of the velocities at r2
    logical,intent(out)                             :: status_ok  !! true if everything is OK

    integer                 :: i,j,k,n,n_solutions
    real(wp)                :: num_revs,pa,ta,r1mag,r2mag,dr,r1r2
    real(wp),dimension(3,2) :: vt1,vt2
    real(wp),dimension(3)   :: r1hat,r2hat,r1xr2,rho,r1xr2_hat,etai,etaf
    real(wp),dimension(2)   :: vri,vti,vrf,vtf

    !temp arrays to hold all the solutions:
    ! they will be packed into the output arrays
    logical,dimension(2*multi_revs+1) :: solution_exists
    real(wp),dimension(3,1+2*multi_revs) :: all_vt1, all_vt2

    r1mag = norm2(r1)
    r2mag = norm2(r2)

    if ( r1mag==0.0_wp .or. r2mag==0.0_wp .or. mu<=0.0_wp .or. tof<=0.0_wp ) then
        write(*,*) 'Error in solve_lambert_gooding: invalid input'
        status_ok = .false.
        return
    end if

    !initialize:
    solution_exists = .false.
    status_ok = .true.

    dr       = r1mag - r2mag
    r1r2     = r1mag*r2mag
    r1hat    = r1/r1mag
    r2hat    = r2/r2mag
    r1xr2    = cross(r1,r2)
    if (all(r1xr2==0.0_wp)) then    !the vectors are parallel,
                                    ! so the transfer plane is undefined
        write(*,*) 'Warning: pi transfer in solve_lambert_gooding'
        r1xr2 = [0.0_wp,0.0_wp,1.0_wp]    !degenerate conic...choose the x-y plane
    end if
    r1xr2_hat = unit(r1xr2)

    !a trick to make sure argument is between [-1 and 1]:
    pa = acos(max(-1.0_wp,min(1.0_wp,dot_product(r1hat,r2hat))))

    do i=0,multi_revs

        num_revs = real(i,wp)    !number of complete revs for this case

        !transfer angle and normal vector:
        if (long_way) then ! greater than pi
            ta    =  num_revs * twopi + (twopi - pa)
            rho   = -r1xr2_hat
        else ! less than pi
            ta    = num_revs * twopi + pa
            rho   = r1xr2_hat
        end if

        etai = cross(rho,r1hat)
        etaf = cross(rho,r2hat)

        !Gooding routine:
        call vlamb(mu,r1mag,r2mag,ta,tof,n,vri,vti,vrf,vtf)

        select case (n)    !number of solutions

        case(1)

            vt1(:,1) = vri(1)*r1hat + vti(1)*etai
            vt2(:,1) = vrf(1)*r2hat + vtf(1)*etaf

        case(2)

            vt1(:,1) = vri(1)*r1hat + vti(1)*etai
            vt2(:,1) = vrf(1)*r2hat + vtf(1)*etaf

            vt1(:,2) = vri(2)*r1hat + vti(2)*etai
            vt2(:,2) = vrf(2)*r2hat + vtf(2)*etaf

        end select

        if (i==0 .and. n==1) then    !there can be only one solution
            all_vt1(:,1) = vt1(:,1)
            all_vt2(:,1) = vt2(:,1)
            solution_exists(1) = .true.
        else
            select case(n)
            case(1)
                all_vt1(:,2*i)         = vt1(:,1)
                all_vt2(:,2*i)         = vt2(:,1)
                solution_exists(2*i)   = .true.
            case(2)
                all_vt1(:,2*i)         = vt1(:,1)
                all_vt2(:,2*i)         = vt2(:,1)
                solution_exists(2*i)   = .true.
                all_vt1(:,2*i+1)       = vt1(:,2)
                all_vt2(:,2*i+1)       = vt2(:,2)
                solution_exists(2*i+1) = .true.
            end select
        end if

    end do

    !return all the solutions:
    n_solutions = count(solution_exists)

    allocate(v1(3,n_solutions))
    allocate(v2(3,n_solutions))

    k=0
    do i=1,size(solution_exists)
        if (solution_exists(i)) then
            k=k+1
            v1(:,k) = all_vt1(:,i)
            v2(:,k) = all_vt2(:,i)
        end if
    end do

    contains
!*****************************************************************************************

    !*************************************************************************************
        subroutine vlamb(gm,r1,r2,th,tdelt,n,vri,vti,vrf,vtf)

        !!  Gooding support routine
        !!  Note: this contains the modification from [2]

        implicit none

        real(wp),intent(in) :: gm
        real(wp),intent(in) :: r1
        real(wp),intent(in) :: r2
        real(wp),intent(in) :: th
        real(wp),intent(in) :: tdelt
        integer,intent(out) :: n
        real(wp),dimension(2),intent(out) :: vri
        real(wp),dimension(2),intent(out) :: vti
        real(wp),dimension(2),intent(out) :: vrf
        real(wp),dimension(2),intent(out) :: vtf

        integer :: m,i
        real(wp) :: thr2,r1r2th,csq,c,s,gms,qsqfm1,q,rho,sig,t,x1,x2,x,unused,&
                    qzminx,qzplx,zplqx,vt2,vr1,vt1,vr2

        !the following yields m = 0 when th = 2 pi exactly
        ! neither this nor the original code works for th < 0.0
        thr2 = th
        m = 0
        do while (thr2 > twopi)
            thr2 = thr2 - twopi
            m = m + 1
        end do
        thr2   = thr2 / 2.0_wp

        !note: dr and r1r2 are computed in the calling routine

        r1r2th = 4.0_wp*r1r2*sin(thr2)**2
        csq    = dr*dr + r1r2th
        c      = sqrt(csq)
        s      = (r1 + r2 + c)/2.0_wp
        gms    = sqrt(gm*s/2.0_wp)
        qsqfm1 = c/s
        q      = sqrt(r1r2)*cos(thr2)/s

        if (c/=0.0_wp) then
            rho = dr/c
            sig = r1r2th/csq
        else
            rho = 0.0_wp
            sig = 1.0_wp
        end if

        t = 4.0_wp*gms*tdelt/s**2

        call xlamb(m,q,qsqfm1,t,n,x1,x2)

        !proceed for single solution, or a pair

        do i=1,n

            if (i==1) then
                x = x1
            else
                x = x2
            end if

            call tlamb(m,q,qsqfm1,x,-1,unused,qzminx,qzplx,zplqx)

            vt2 = gms*zplqx*sqrt(sig)
            vr1 = gms*(qzminx - qzplx*rho)/r1
            vt1 = vt2/r1
            vr2 = -gms*(qzminx + qzplx*rho)/r2
            vt2 = vt2/r2

            vri(i) = vr1
            vti(i) = vt1
            vrf(i) = vr2
            vtf(i) = vt2

        end do

        end subroutine vlamb
    !*************************************************************************************

    !*************************************************************************************
        subroutine tlamb(m,q,qsqfm1,x,n,t,dt,d2t,d3t)

        !!  Gooding support routine

          implicit none

          real(wp),intent(in)  :: q
          real(wp),intent(in)  :: qsqfm1
          real(wp),intent(in)  :: x
          integer,intent(in)   :: n
          real(wp),intent(out) :: t
          real(wp),intent(out) :: dt
          real(wp),intent(out) :: d2t
          real(wp),intent(out) :: d3t

          integer   :: m,i
          real(wp)  :: qsq,xsq,u,y,z,&
                        qx,a,b,aa,bb,g,f,fg1,term,fg1sq,twoi1,&
                        told,qz,qz2,u0i,u1i,u2i,u3i,tq,tqsum,&
                        ttmold,p,tterm,tqterm
          logical   :: lm1, l1, l2, l3

          real(wp), parameter :: sw  = 0.4_wp

          lm1 = n==-1
          l1 = n>=1
          l2 = n>=2
          l3 = n==3
          qsq = q*q
          xsq = x*x
          u = (1.0_wp - x)*(1.0_wp + x)
          if (.not.lm1) then
            !(needed if series, and otherwise useful when z = 0)
            dt = 0.0_wp
            d2t = 0.0_wp
            d3t = 0.0_wp
          end if
          if (lm1 .or. m>0 .or. x<0.0_wp .or. abs(u)>sw) then
            !direct computation (not series)
            y = sqrt(abs(u))
            z = sqrt(qsqfm1 + qsq*xsq)
            qx = q*x
            if (qx<=0.0_wp) then
              a = z - qx
              b = q*z - x
            end if
            if (qx<0.0_wp .and. lm1) then
              aa = qsqfm1/a
              bb = qsqfm1*(qsq*u - xsq)/b
            end if
            if (qx==0.0_wp.and.lm1 .or. qx>0.0_wp) then
              aa = z + qx
              bb = q*z + x
            end if
            if (qx>0.0_wp) then
              a = qsqfm1/aa
              b = qsqfm1*(qsq*u - xsq)/bb
            end if
            if (.not.lm1) then
              if (qx*u>=0.0_wp) then
                g = x*z + q*u
               else
                g = (xsq - qsq*u)/(x*z - q*u)
              end if
              f = a*y
              if (x<=1.0_wp) then
                t = m*pi + atan2(f, g)
               else
                if (f>sw) then
                  t = log(f + g)
                 else
                  fg1 = f/(g + 1.0_wp)
                  term = 2.0_wp*fg1
                  fg1sq = fg1*fg1
                  t = term
                  twoi1 = 1.0_wp
                  do
                      twoi1 = twoi1 + 2.0_wp
                      term = term*fg1sq
                      told = t
                      t = t + term/twoi1
                      if (t/=told) cycle
                      exit
                  end do    !(continue looping for inverse tanh)
                end if
              end if
              t = 2.0_wp*(t/y + b)/u
              if (l1 .and. z/=0.0_wp) then
                qz = q/z
                qz2 = qz*qz
                qz = qz*qz2
                dt = (3.0_wp*x*t - 4.0_wp*(a + qx*qsqfm1)/z)/u
                if (l2) d2t = (3.0_wp*t + 5.0_wp*x*dt + 4.0_wp*qz*qsqfm1)/u
                if (l3) d3t = (8.0_wp*dt + 7.0_wp*x*d2t - 12.0_wp*qz*qz2*x*qsqfm1)/u
              end if
             else
              dt = b
              d2t = bb
              d3t = aa
            end if
           else
            !compute by series
            u0i = 1.0_wp
            if (l1) u1i = 1.0_wp
            if (l2) u2i = 1.0_wp
            if (l3) u3i = 1.0_wp
            term = 4.0_wp
            tq = q*qsqfm1
            i = 0
            if (q<0.5_wp) tqsum = 1.0_wp - q*qsq
            if (q>=0.5_wp) tqsum = (1.0_wp/(1.0_wp + q) + q)*qsqfm1
            ttmold = term/3.0_wp
            t = ttmold*tqsum

            do
                i = i + 1
                p = i
                u0i = u0i*u
                if (l1 .and. i>1) u1i = u1i*u
                if (l2 .and. i>2) u2i = u2i*u
                if (l3 .and. i>3) u3i = u3i*u
                term = term*(p - 0.5_wp)/p
                tq = tq*qsq
                tqsum = tqsum + tq
                told = t
                tterm = term/(2.0_wp*p + 3.0_wp)
                tqterm = tterm*tqsum
                t = t - u0i*((1.5_wp*p + 0.25_wp)*tqterm/(p*p - 0.25_wp)-ttmold*tq)
                ttmold = tterm
                tqterm = tqterm*p
                if (l1) dt = dt + tqterm*u1i
                if (l2) d2t = d2t + tqterm*u2i*(p - 1.0_wp)
                if (l3) d3t = d3t + tqterm*u3i*(p - 1.0_wp)*(p - 2.0_wp)
                if (i<n .or. t/=told) cycle
                exit
            end do

            if (l3) d3t = 8.0_wp*x*(1.5_wp*d2t - xsq*d3t)
            if (l2) d2t = 2.0_wp*(2.0_wp*xsq*d2t - dt)
            if (l1) dt = -2.0_wp*x*dt
            t = t/xsq
          end if

        end subroutine tlamb
    !*************************************************************************************

    !*************************************************************************************
        pure function d8rt(x)

        !!  8th root function, used by xlamb

        implicit none

        real(wp) :: d8rt
        real(wp),intent(in) :: x

        d8rt = sqrt(sqrt(sqrt(x)))

        end function d8rt
    !*************************************************************************************

    !*************************************************************************************
        subroutine xlamb(m,q,qsqfm1,tin,n,x,xpl)

        !!  Gooding support routine

        implicit none

        integer,intent(in)   :: m
        real(wp),intent(in)  :: q
        real(wp),intent(in)  :: qsqfm1
        real(wp),intent(in)  :: tin
        integer,intent(out)  :: n
        real(wp),intent(out) :: x
        real(wp),intent(out) :: xpl

        integer  :: i,ij
        real(wp) :: thr2,t0,dt,d2t,d3t,tdiff,w,xm,tmin,&
                    xmold,xtest,tdiffm,d2t2,t,tdiff0

        real(wp),parameter :: tol = 3.0e-7_wp
        real(wp),parameter :: c0  = 1.7_wp
        real(wp),parameter :: c1  = 0.5_wp
        real(wp),parameter :: c2  = 0.03_wp
        real(wp),parameter :: c3  = 0.15_wp
        real(wp),parameter :: c41 = 1.0_wp
        real(wp),parameter :: c42 = 0.24_wp

        thr2 = atan2(qsqfm1, 2.0_wp*q)/pi

        if (m==0) then

            !single-rev starter from t (at x = 0) & bilinear (usually)

            n = 1
            call tlamb(m,q,qsqfm1,0.0_wp,0,t0,dt,d2t,d3t)
            tdiff = tin - t0
            if (tdiff<=0.0_wp) then
                x = t0*tdiff/(-4.0_wp*tin)
                !(-4 is the value of dt, for x = 0)
            else
                x = -tdiff/(tdiff + 4.0_wp)
                w = x + c0*sqrt(2.0_wp*(1.0_wp - thr2))
                if (w<0.0_wp) x = x - sqrt(d8rt(-w))*(x + sqrt(tdiff/(tdiff + 1.5_wp*t0)))
                w = 4.0_wp/(4.0_wp + tdiff)
                x = x*(1.0_wp + x*(c1*w - c2*x*sqrt(w)))
            end if

        else

            !with multirevs, first get t(min) as basis for starter

            xm = 1.0_wp/(1.5_wp*(m + 0.5_wp)*pi)
            if (thr2<0.5_wp) xm = d8rt(2.0_wp*thr2)*xm
            if (thr2>0.5_wp) xm = (2.0_wp - d8rt(2.0_wp - 2.0_wp*thr2))*xm
            !(starter for tmin)

            do i=1,12
                call tlamb(m,q,qsqfm1,xm,3,tmin,dt,d2t,d3t)
                if (d2t==0.0_wp) exit
                xmold = xm
                xm = xm - dt*d2t/(d2t*d2t - dt*d3t/2.0_wp)
                xtest = abs(xmold/xm - 1.0_wp)
                if (xtest<=tol) exit
            end do

            if (i>12) then
                !(break off & exit if tmin not located - should never happen)
                !now proceed from t(min) to full starter
                n = -1
                return
            end if

            tdiffm = tin - tmin
            if (tdiffm<0.0_wp) then

                n = 0
                return
                !(exit if no solution with this m)

            else if (tdiffm==0.0_wp) then

                x = xm
                n = 1
                return
                !(exit if unique solution already from x(tmin))

            else

                n = 3
                if (d2t==0.0_wp) d2t = 6.0_wp*m*pi
                x = sqrt(tdiffm/(d2t/2.0_wp + tdiffm/(1.0_wp - xm)**2))
                w = xm + x
                w = w*4.0_wp/(4.0_wp + tdiffm) + (1.0_wp - w)**2
                x = x*(1.0_wp - (1.0_wp + m + c41*(thr2 - 0.5_wp))/&
                    (1.0_wp + c3*m)*x*(c1*w + c2*x*sqrt(w))) + xm
                d2t2 = d2t/2.0_wp
                if (x>=1.0_wp) then
                    n = 1
                    goto 3
                end if
                !(no finite solution with x > xm)

            end if

        end if

    !(now have a starter, so proceed by halley)

    5    continue

            do i=1,3
                call tlamb(m,q,qsqfm1,x,2,t,dt,d2t,d3t)
                t = tin - t
                if (dt/=0.0_wp) x = x + t*dt/(dt*dt + t*d2t/2.0_wp)
            end do
            if (n/=3) return
            !(exit if only one solution, normally when m = 0)

            n = 2
            xpl = x
            !(second multi-rev starter)
        3   call tlamb(m,q,qsqfm1,0.0_wp,0,t0,dt,d2t,d3t)
            tdiff0 = t0 - tmin
            tdiff = tin - t0
            if (tdiff<=0) then
                x = xm - sqrt(tdiffm/(d2t2 - tdiffm*(d2t2/tdiff0 - 1.0_wp/xm**2)))
            else
                x = -tdiff/(tdiff + 4.0_wp)
                ij = 200
                w = x + c0*sqrt(2.0_wp*(1.0_wp - thr2))
                if (w<0.0_wp) x = x - sqrt(d8rt(-w))*(x + sqrt(tdiff/(tdiff+1.5_wp*t0)))
                w = 4.0_wp/(4.0_wp + tdiff)
                x = x*(1.0_wp + (1.0_wp + m + c42*(thr2 - 0.5_wp))/&
                    (1.0_wp + c3*m)*x*(c1*w - c2*x*sqrt(w)))
                if (x<=-1.0_wp) then
                    n = n - 1
                    !(no finite solution with x < xm)
                    if (n==1) x = xpl
                end if
            end if

        goto 5

        end subroutine xlamb
    !*************************************************************************************

    end subroutine solve_lambert_gooding