clear;

%% 设置位置矢量和速度矢量

R=[8228 %km

    389

    6888]; 

V=[-0.7 %km/s

    6.6

    -0.6]; 

 [a, e, i, OMEGA, omega, gama] = getOrbEles(R, V);

%% 根据R和V矢量计算轨道六根数，长度单位为km，时间单位为s

function [a, e, i, OMEGA, omega, gama] = getOrbEles(R, V)    

    a=-1;

    e=-1;

    i=-1;

    OMEGA=-1;

    omega=-1;

    gama=-1;

    Rdim = size(R);

    Vdim = size(V);

    if Rdim(1)==1 && Rdim(2)==3

        R = R';

        disp("换了R的维度");

    elseif Rdim(1)==3 && Rdim(2)==1

        %正确格式，是想要的

        disp("R格式正确");

    else

       disp("R格式不正确，请输入3×1或1×3矢量");

        return;

    end

      if Vdim(1)==1 && Vdim(2)==3

        V = V';

        disp("换了V的维度");

    elseif Vdim(1)==3 && Vdim(2)==1

        %正确格式，是想要的

        disp("V格式正确");

    else

        disp("V格式不正确，请输入3×1或1×3矢量");

        return;

      end

    MIU = 3.986E5; %km2/s2

    %% 计算6根数

    Rsc = sqrt(R'*R);

    Vsc = sqrt(V'*V);

    E = Vsc*Vsc/2 - MIU/Rsc;

    % a

    a = -MIU/2/E;

    % e

    evec = 1/MIU * ((Vsc*Vsc - MIU/Rsc)*R - dot(R,V)*V);

    e = sqrt(evec'*evec);

    % i

    h = cross(R,V);

    I =[1

        0

        0];

    K = [0

        0

        1];

    cosi = dot(K,h)/(sqrt(K'*K)*sqrt(h'*h));

    i = acos(cosi)*180/pi();

    % OMEGA

    n=cross(K, h);

    cosOMEGA = dot(I, n)/(sqrt( I'*I)*sqrt(n'*n));

    OMEGA = acos(cosOMEGA)*180/pi();

    nj = n(2);

    ni = n(1);

    if abs(nj)<1e-6

        if ni >= 0

            OMEGA = 0;

        else

            OMEGA = 180;

        end

    elseif nj < 0

        OMEGA = 360 - OMEGA;

    end

    % omega

    cosomega = dot(n,evec)/(sqrt(n'*n)*sqrt(evec'*evec));

    omega = acos(cosomega)*180/pi();

    ek = evec(3);

    if ek <= 0

        omega = 360-omega;

    end

    % gama

    cosgama = dot(evec, R)/(sqrt(evec'*evec)*sqrt(R'*R));

    gama = acos(cosgama)*180/pi();

    if dot(R,V) <0

        gama = 360-gama;

    elseif abs(dot(R,V)) <1e-6

        Ra = a*(1+e);

        Rp = a*(1-e);

        if abs(Rsc - Ra) < 1e-6

            gama = 180;

        elseif abs(Rsc - Rp)<1e-6

            gama = 0;

        end

    end    

end