turdicus rex

function out = Lambert(rbar1,rbar2,TOF,tm,mu)
    r1 = norm(rbar1);
    r2 = norm(rbar2);
    TOFn = 0;
    delV = acos(dot(rbar1,rbar2)/(r1*r2));
    A = tm*sqrt(r1*r2*(1+cos(delV)));

    if A == 0
        out = [0,0];
    else
        z = 0;
        C = 1/2;
        S = 1/6;
        zUp = 4*pi^2;
        zLow =-4*pi;

        while (abs(TOFn - TOF) > 1*10^(-6))
            yn = r1+r2+A*(z*S-1)/sqrt(C);
            xn = sqrt(yn/C);
            TOFn = (xn^3*S+A*sqrt(yn))/sqrt(mu);
            if (TOFn <= TOF)
                zLow = z;
            else
                zUp = z;
            end
            z = (zUp+zLow)/2;
            if (z > 1*10^(-6))
                C = (1-cos(sqrt(z)))/z;
                S =(sqrt(z)-sin(sqrt(z)))/sqrt(z^3);
            elseif (z < -1*10^(-6))
                C = (1-cosh(sqrt(-z)))/z;
                S = (sinh(sqrt(-z))-sqrt(-z))/sqrt((-z)^3);
            else
                C = 1/2;
                S = 1/6;
            end
        end
        f = 1-yn/r1;
        gdot = 1 - yn/r2;
        g = A*sqrt(yn/mu);
        v1 = (rbar2 - f*rbar1)/g;
        v2 = (gdot*rbar2 - rbar1)/g;
        out = [v1',v2'];
    end
end