% Delta IV 3-stage rocket launch to orbit.  Rocket is launched from the 
% equator of a rotating earth and inherits the launch point velocity and
% rotation rate.  The 1st and 2nd stages are powered, with the 1st stage
% having a programmed thrust angle.

clear
G=6.7e-11;                   % gravitational constant, m3/kg/s2
mEarth = 5.9742e24;          % mass of the earth, kg 
rEarth = 6.378e6;            % radius of the earth, r
EarthRate = 2*pi/(24*60*60); % earth rotation rate, rad./s


% Delta IV first stage 
Thrust1 = 3212000; % first stage thrust, n
Length1 = 70;    % first stage total length, m
Mass1 = 250000;  % first stage initial mass, kg  
Fuel1 = 150000;  % first stage initial fuel mass, kg
BurnTime1 = 260; % first stage burn time

% Delta IV second stage 
Thrust2 = 310000;   % second stage thrust, n 
Length2 = 50;       % second stage total length, m
Mass2 = 50000;      % second stage initial mass, kg
Fuel2 = 25000;      % second stage initial fuel mass, kg
BurnTime2 = 850;    % second stage burn time

% initial rocket state
xR(1) = rEarth;                 % position
yR(1) = 0;                      
vxR(1) = 0;                     % velocity
vyR(1) = rEarth * EarthRate;    % rocket inhertits launch site y velocity
alpha(1) = 0;                   % initial orientation
valpha(1) = EarthRate;          % rocket inherits earth's rotation rate

t(1) = 0;                 
dt=1;
n = 8000;

%program 1st stage thrust angle theta
theta=zeros(1,n);  % program thrust angle to steer into orbit
for i= 50:100      % determined purely by trial and error 
   theta(i) = -0.0018*pi/180;        
end

for i=1:n
   t(i+1) = t(i) + dt;         
   
   xR(i+1) = xR(i) + vxR(i)*dt;    % update rocket position, orientation
   yR(i+1) = yR(i) + vyR(i)*dt;  
   alpha(i+1) = alpha(i) + valpha(i)*dt;
   
   R = sqrt(xR(i)^2+yR(i)^2);  % calculate acceleration due to gravity  
   A = -G*mEarth/R^2;           

   % set rocket parameters according to current stage of fight
   if (t(i) <= BurnTime1)    % first stage ?  
       Thrust = Thrust1;     % yes  
       Length = Length1;
       Mass = Mass1 - t(i)/BurnTime1*Fuel1; % adjust mass for fuel  
   elseif (t(i) <= BurnTime1 + BurnTime2)    % second stage ?   
       Thrust = Thrust2;                     % yes 
       Length = Length2; 
       Mass = Mass2 - (t(i)- BurnTime1)/BurnTime2*Fuel2; % adjust mass      
   else
       Thrust = 0;    % flyout stage
   end
  
   Thrustx = Thrust*cos(alpha(i)+theta(i));  % resolve thrust into x and 
   Thrusty = Thrust*sin(alpha(i)+theta(i));  % y conmponents
 
   Torque = -Thrust*sin(theta(i))*Length/2;  % calculate torque
   I = Mass*Length^2/12;     % and rocket moment of inertia
       
   % update rocket velocity and orientation rate
   vxR(i+1)= vxR(i) + (A*xR(i)/R + Thrustx/Mass)*dt;
   vyR(i+1)= vyR(i) + (A*yR(i)/R + Thrusty/Mass)*dt;
   valpha(i+1) = valpha(i) + Torque/I*dt;    
end

figure(1)
plot(xR, yR);    % plot rocket trajectory
for j=1:361     % draw a circle for the earth
   xE(j)= rEarth*cos(j*pi/180);
   yE(j)= rEarth*sin(j*pi/180);
end
hold on
plot(xE,yE)                       % draw earth
axis equal
title('Rocket launch to orbit')
xlabel('x')
ylabel('y')
title('Launch to orbit')
hold off   



