function [a,norm_e,i,Omega,omega,f] = calculateOrbitPara(r,v)
%CALCULATEORBITPARA 根据位置和速度矢量计算轨道根数
% 输入：
%   r   卫星在地心赤道惯性坐标系中的位置矢量，3*1列向量，单位：km
%   v   卫星在地心赤道惯性坐标系中的速度矢量，3*1列向量，单位：km/s
% 输出：
%   a       半长轴，单位：km
%   norm_e	偏心率，0 < norm_e <1
%   i       轨道倾角，0 <= i <= pi，单位：rad
%   Omega	升交点赤经，-pi < Omega <= pi，单位：rad
%   omega	近地点角距，-pi < Omega <= pi，单位：rad
%   f       真近点角，-pi < Omega <= pi，单位：rad
% 
% 参考：https://wenku.baidu.com/view/59a2d1b981eb6294dd88d0d233d4b14e84243e4b.html

mu = 3.9860044e5;   % 地心引力常数，单位：km^3/s^2
x = [1;0;0];    % 地心赤道惯性坐标系中指向春分点的坐标轴单位矢量
y = [0;1;0];
z = [0;0;1];    % 地心赤道惯性坐标系中指向北极的坐标轴单位矢量

h = cross(r,v);    % 卫星相对于地心的动量矩
norm_h = norm(h);
n = cross(z, h);   %升交点地心单位矢量
norm_n = norm(n);

e = ((norm(v)^2-mu/norm(r))*r - (r'*v)*v)/mu;   % 矢量方向为从焦点指向近拱点
norm_e = norm(e);    % 偏心率（离心率）

% E = norm(v)^2/2 - mu/norm(r);   % 机械能
% a = -mu/E/2;       % 机械能直接决定了轨道的半长轴
a = norm_h^2 / mu / (1-norm_e^2);
i = acos(z'*h /norm_h);      % 轨道倾角

if y'*n >= 0
    Omega = acos(x'*n / norm_n);     % 升交点赤经
else
    Omega = -acos(x'*n / norm_n);
end

if z'*e >= 0
    omega = acos(e'*n / norm_n / norm_e);  % 近地点幅角
else
    omega = -acos(e'*n / norm_n / norm_e);
end

norm_r= norm(r);
if r'*v >= 0
    f = acos(r'*e / norm_r / norm_e);  % 真近点角
else
    f = -acos(r'*e / norm_r / norm_e);
end

end