cart2kepler.m

It s verry good for you

function [a, e, i, omega, Omega, theta] = cart2kepler(varargin)
%CART2KEPLER             Transform Cartesian coordinates to Kepler elements
%
%   [a, e, i, omega, Omega, theta] = CART2KEPLER(x, y, z, xdot, ydot, zdot,
%   mu),  or equivalently, CART2KEPLER(statevec, mu), transforms the given
%   Cartesian coordinates to the corresponding Keplerian elements. Here,
%   [statevec] is a matrix containing all of the Cartesian coordinates. 

%   Author: Rody P.S. Oldenhuis
%   Delft University of Technology
%   E-mail: [e-mail]
%   Last edited 27/Feb/2009

    % default errortrap
    error(nargchk(1, 7, nargin));
    
    % global variable (default main body is the Sun)
    global muS
    
    % parse input
    if (nargin == 1)
        statevec = varargin{1};        
              mu = muS;
    elseif (nargin == 2)
        statevec = varargin{1};        
              mu = varargin{2};    
    elseif (nargin >= 6)
        statevec = [varargin{1}, varargin{2}, varargin{3}, ...
                    varargin{4}, varargin{5}, varargin{6}];
              mu = muS;
    elseif (nargin == 7)
        statevec = [varargin{1}, varargin{2}, varargin{3}, ...
                    varargin{4}, varargin{5}, varargin{6}];
              mu = varargin{7};    
    end
    rvec = statevec(:, 1:3, :);
    Vvec = statevec(:, 4:6, :);
    
    % errortrap
    if (size(statevec, 2) ~= 6)
        error('cart2kepler:statevector_incomplete', ....
              'State vector should have 6 columns.');
    end
    if isempty(mu)                          % the one mistake I made one billion times
        clear global muS
        error('cart2kepler:empty_GM', ...
             ['The standard gravitational parameter of the main body is empty.\n', ...
              'Either provide it explicitly as the last argument, ',...
              'or reload the Solar system model.'])
    end

    % compute constants required for transformation
    r = sqrt(sum(rvec.^2, 2));   % radius 
    V = sqrt(sum(Vvec.^2, 2));   % speed
    H = cross(rvec, Vvec, 2);    % angular momentum
    
    % and transform                                % normal vector (to [0,0,1] and H)
    N = [-H(:, 2, :), H(:, 1, :), zeros(size(H(:, 1, :)))];

    % a, e, i
     rrr = [r, r, r];                              % for vectorization
       a = 1 ./ (2./r - V.^2/mu);                  % from vis-viva
    evec = cross(Vvec, H, 2)/mu - rvec./rrr;       % eccentricity vector
       e = sqrt(sum(evec.^2, 2));                  % eccentricity
       i = acos( H(:, 3, :) ./ sqrt(sum(H.^2, 2)));% (unambiguous) inclination
       
    % Omega   
     Nxy = sqrt(sum(N.^2, 2));                     % magnitude of normal vector
      nz = (Nxy ~= 0);                             % prevention of division-by-zero
   Omega = zeros(size(Nxy));                       % initialize Omegas
   omega = zeros(size(Nxy));
    if any(nz)                                     % Omega is atan2(Hx/H, Hy/H)
        Omega(nz) = atan2(N(nz, 2, :)./Nxy(nz), N(nz, 1, :)./Nxy(nz));
    end
   Omega(~nz) = 0;                                 % or zero in case of zero-inclination
     Nxy = [Nxy, Nxy, Nxy];                        % for vectorization
     eee = [e, e, e];                              % for vectorization     
   
    % omega, theta  
     if any(nz)
         pm1 = sign( dot(  cross(N./Nxy, evec, 2), H, 2 ) );   % sign of the acos for omega
         omega(nz) = pm1 .* acos( dot(evec./eee, N./Nxy, 2) ); % omega is acos(e種) (in units)
     end
   omega(~nz) = 0;                                 % or zero in case of zero-inclination  
   pm2   = sign( dot(  cross(evec, rvec, 2), H, 2 ) );         % sign of the acos for theta
   theta = pm2 .* acos( dot(rvec./rrr, evec./eee, 2) );        % theta is acos(r積) (in units)
    
end