%planet_proj
% Program which generates a unit sphere with a texture mapped
% surface. On this is overlaid a projectile trajectory.
%
% Press p to print and q to quit.
% m creates an AVI movie of the trajectory, which incorporates rotation of
% the planet.
%
% Arrow keys change the launch latitude and longitude
% a and z change the launch azimuth
% e and r change the launch elevation
% v and c change the launch speed
%
% LAST UPDATED by Andy French May 2020

function planet_projectile
global d

%Load settings file
[filename, pathname, filterindex] = uigetfile('*.m', 'Choose settings file');
run( [pathname,filename] );

%Load planet bitmap
d.Ihighres = imread(planet); d.Ihighres = double(d.Ihighres)/255;
d.Ihighres = flipdim(d.Ihighres,1);
d.Ilowres = interp_image( d.Ihighres , 200  , 100 , 'height' );
d.I = d.Ilowres;

%Generate initial figure
d.fig = []; d.title_position = [];
[t,x,y,z,x0r,y0r,z0r] = compute_trajectory;
update_plot( t,x,y,z,x0r,y0r,z0r );
view([d.launch_long+120,d.launch_lat+40]);

%%

%Compute trajectory
function [t,x,y,z,x0r,y0r,z0r] = compute_trajectory
global d

%Determine launch conditions
[x0,y0,z0,vx0,vy0,vz0] = launch_conditions( d.launch_lat, d.launch_long,...
    d.launch_azi, d.launch_elev, d.v0, d.R, d.T );

%Determine trajectory via Verlet solver
[t,x,y,z,vx,vy,vz,v] = verlet( x0,y0,z0,vx0,vy0,vz0,d.R,d.M,d.G,d.tmax,d.dt );

%Determine launch site coordinates at time t(end)
[x0r,y0r,z0r,vx0,vy0,vz0] = launch_conditions( d.launch_lat, d.launch_long + (2*pi/d.T)*t(end),...
    d.launch_azi, d.launch_elev, d.v0, d.R, d.T );

%%

%Determine launch conditions
function [x0,y0,z0,vx0,vy0,vz0] = launch_conditions( launch_lat, launch_long,...
    launch_azi, launch_elev, v0, R, T )

%Convert input angles to radians
lat0 = launch_lat*pi/180; long0 = launch_long*pi/180;
theta = launch_elev*pi/180; a = launch_azi*pi/180;

%Determine launch x,y,z position /m
x0 = R*cos(lat0)*cos(long0); y0 = R*cos(lat0)*sin(long0);  z0 = R*sin(lat0);

%Calculate rotational speed of launch site /ms^-1
[vrotx,vroty,vrotz] = vrot( lat0, long0, R, T );
vrot_launch = sqrt( vrotx^2 + vroty^2 + vrotz^2 );

%Determine launch velocity /ms^-1
[xhat,yhat,zhat] = latlong2tangentxyz( lat0, long0 );
v = v0*( cos(theta)*cos(a)*xhat + cos(theta)*sin(a)*yhat + sin(theta)*zhat );
vx0 = v(1) + vrotx; vy0 = v(2) + vroty; vz0 = v(3)+ vrotz;

%%

%Render planet and trajectory
function update_plot( t,x,y,z, x0r,y0r,z0r )
global d

%Create figure, if not already done so
if isempty(d.fig)
    d.fig = figure('name','planet_projectile','color',[1 1 1],...
        'renderer','opengl','KeyPressFcn',@keypress,...
        'units','normalized','position',[0.3,0.3,0.5,0.5],'numbertitle','off');
else
    clf;
end

%Determine rotation of planet /radians
long_rot = (2*pi/d.T)*t(end);

%Define x,y,z coordinates of spherical planet
s = size(d.I);
lat = linspace(-pi/2,pi/2,s(1)); long = linspace(-pi,pi,s(2)) + long_rot;
[long,lat] = meshgrid(long,lat);
xp = d.R*cos(lat).*cos(long); yp = d.R*cos(lat).*sin(long); zp = d.R*sin(lat);

%Plot sphere and texture-map planet bitmap onto it
surf(xp,yp,zp,d.I);
axis vis3d; shading interp; hold on; set(gca,'fontsize',d.fsize);
xlabel('x','fontsize',d.fsize); ylabel('y','fontsize',d.fsize); zlabel('z','fontsize',d.fsize);
axis off;
zoom(1.2);

%Lines of longitude
long = 0 : 10*pi/180 : 2*pi - 2*pi/10;
for n=1:length(long)
    [xc,yc,zc] = sphere_circle( 0, long(n) + long_rot, d.R, pi/2, 300);
    plot3(xc,yc,zc,'w-');
end

%Lines of latitude
lat = 0 : 10*pi/180 : pi/2;
for n=1:length(lat)
    [xc,yc,zc] = sphere_circle( pi/2, 0, d.R, lat(n), 300);
    plot3(xc,yc,zc,'w-');
    [xc,yc,zc] = sphere_circle( -pi/2, 0, d.R, lat(n), 300);
    plot3(xc,yc,zc,'w-');
end

%Plot trajectory
plot3( x,y,z ,'r-' );
plot3( x(1),y(1),z(1) ,'g*' ); plot3( x(end),y(end),z(end) ,'r*' );
plot3( x0r,y0r,z0r ,'y*' );

%Title
d.title = title( ['t=',num2str( t(end)/3600,3),' hours, ',...
    'T=',num2str(d.T/3600,3),' hours, ',...
    'lat_0=',num2str(d.launch_lat),...
    '^o, long_0=',num2str(d.launch_long),...
    '^o, azi_0=',num2str(d.launch_azi),...
    '^o, elev_0=',num2str(d.launch_elev),...
    '^o, v_0=',num2str(d.v0),'m/s',...
    ],'fontsize',d.fsize );
if ~isempty( d.title_position )
    set(d.title,'position',d.title_position )
end
d.title_position = get( d.title, 'position' );

%%

%Position, time, velocity solver using verlet method
function [t,x,y,z,vx,vy,vz,v] = verlet( x0,y0,z0,vx0,vy0,vz0,R,M,G,tmax,dt )

%Initialize outputs
t = 0;
x = x0; y = y0; z = z0; r = sqrt( x0^2 + y0^2 + z0^2 );
vx = vx0; vy = vy0; vz = vz0; v = sqrt( vx0^2 + vy0^2 + vz0^2 );

%Verlet iteration
n=1;
while ( r>=R ) && ( t(n) < tmax )
    
    %Compute acceleration
    ax = -G*M*x(n)/(r^3); ay = -G*M*y(n)/(r^3); az = -G*M*z(n)/(r^3);
    
    %Update positions
    x(n+1) = x(n) + vx(n)*dt + 0.5*ax*(dt^2);
    y(n+1) = y(n) + vy(n)*dt + 0.5*ay*(dt^2);
    z(n+1) = z(n) + vz(n)*dt + 0.5*az*(dt^2);
    
    %Update accelerations
    r = sqrt( x(n+1)^2 + y(n+1)^2 + z(n+1)^2 );
    aax = -G*M*x(n+1)/(r^3); aay = -G*M*y(n+1)/(r^3); aaz = -G*M*z(n+1)/(r^3);
    
    %Update velocities
    vx(n+1) = vx(n) + 0.5*( ax + aax )*dt;
    vy(n+1) = vy(n) + 0.5*( ay + aay )*dt;
    vz(n+1) = vz(n) + 0.5*( az + aaz )*dt;
    v(n+1) = sqrt( vx(n+1)^2 + vy(n+1)^2 + vz(n+1)^2 );
    
    %Update time and counter n
    t(n+1) = t(n) + dt;
    n = n+1;
end

%Projectile has crash landed. Don't update position.
if ( r<R )
    x(n) = x(n); y(n) = y(n); z(n) = z(n);
    vx(n) = NaN; vy(n) = NaN; vz(n) = NaN; v(n) = NaN;
end

%%

%vrot
% Determines x,y,z velocity vectors of a point on the surface of a planet,
% assuming it is rotating clockwise with period T seconds about the z axis
function [vrotx,vroty,vrotz] = vrot( lat, long, R, T )
vrotx = -R*(2*pi/T)*cos(lat).*cos(long);
vroty = R*(2*pi/T)*cos(lat).*sin(long);
vrotz = 0;

%%

%latlong2tangentxyz
% Determines x,y,z unit vectors in local tangent plane to sphere surface.
% z is a radial, y points to 'North' and x completes a right handed set.
function [xhat,yhat,zhat] = latlong2tangentxyz( lat, long )
xhat = [ -sin(long) ; cos(long) ; 0 ];
yhat = [ -cos(long)*sin(lat) ; -sin(long)*sin(lat) ; cos(lat) ];
zhat = [cos(long)*cos(lat) ; sin(long)*cos(lat) ; sin(lat) ];

%%

%spherecircle
% Defines Cartesian coordinates of a circle on the surface of a sphere.
% alpha is opening angle of the circle from the centre of the sphere. R is
% the radius of the sphere. N is the number of circle points.
function [x,y,z] = sphere_circle( lat, long, R, alpha, N)

%Define circle polar angle
g = linspace(0,2*pi,N);

%Define unit vectors of tangent plane
[xhat,yhat,zhat] = latlong2tangentxyz( lat, long );

%Define circle coordinates as a vector equation
xhat = repmat(xhat,[1,N]); yhat = repmat(yhat,[1,N]); zhat = repmat(zhat,[1,N]);
r = R*sin(alpha)*( [cos(g) ; cos(g) ; cos(g) ].*xhat +...
    [sin(g) ; sin(g) ; sin(g) ].*yhat ) + R*cos(alpha)*zhat;

%Determine outputs
x = r(1,:); y = r(2,:); z = r(3,:);

%%

%Make video of trajectory
function make_video_of_trajectory(t,x,y,z)
global d

%Start stopwatch
t0 = tic;

%Determine current scene information
cp = get(gca,'CameraPosition'); ct = get(gca,'CameraTarget'); cu = get(gca,'CameraUpVector');

%Open video file and set frame rate. Video file will have name of first
%indexed image.
V = VideoWriter('trajectory.avi');
V.FrameRate = 30; open(V); frame = 1;
disp(' ');
disp(' Generating video frames ... ')

%Step through trajectory and generate frames
for n=1:length(t)
    
    %Determine launch site coordinates at time t(n)
    rotation_in_deg =(180/pi)*(2*pi/d.T)*t(n);
    [x0r,y0r,zr0,vx0,vy0,vz0] = launch_conditions( d.launch_lat,...
        d.launch_long + rotation_in_deg,d.launch_azi, d.launch_elev,...
        d.v0, d.R, d.T );
    
    %Update plot
    update_plot( t(1:n),x(1:n),y(1:n),z(1:n),x0r,y0r,zr0 );
    
    %Restore scene view
    set( gca,'CameraPosition',cp,'CameraTarget',ct,'CameraUpVector',cu );
    
    %Add video frame
    print( d.fig, 'trajectory.png','-r300','-dpng' );
    I = imread( 'trajectory.png' );
    writeVideo( V,uint8(I) );
    disp([' Frame ',leadzero(4,frame),' of ',num2str(length(t)),' added to video ...'] );
    frame = frame + 1;
end
disp([' Video construction complete in ',num2str(toc(t0)/60),' minutes.']);
disp(' ');

%%

%Update scene when a key is pressed
function keypress_update
[t,x,y,z,x0r,y0r,z0r] = compute_trajectory;
cp = get(gca,'CameraPosition'); ct = get(gca,'CameraTarget'); cu = get(gca,'CameraUpVector');
update_plot( t,x,y,z,x0r,y0r,z0r );
set( gca,'CameraPosition',cp,'CameraTarget',ct,'CameraUpVector',cu );

%%

%Function which executes when a key is pressed
function keypress(fig,evnt)
global d

if strcmp(get(fig,'currentkey'),'p')==1
    print(gcf,'planet_projectile.png','-dpng','-r300');
    
elseif strcmp(get(fig,'currentkey'),'h')==1
    d.I = d.Ihighres;
    keypress_update
elseif strcmp(get(fig,'currentkey'),'l')==1
    d.I = d.Ilowres;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'a')==1
    d.launch_azi = d.launch_azi + d.delevazi;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'z')==1
    d.launch_azi = d.launch_azi - d.delevazi;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'e')==1
    d.launch_elev = d.launch_elev + d.delevazi;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'r')==1
    d.launch_elev = d.launch_elev - d.delevazi;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'v')==1
    d.v0 = d.v0 + d.dv0;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'c')==1
    d.v0 = d.v0 - d.dv0;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'uparrow')==1
    d.launch_lat = d.launch_lat + d.dlatlong;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'downarrow')==1
    d.launch_lat = d.launch_lat - d.dlatlong;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'rightarrow')==1
    d.launch_long = d.launch_long + d.dlatlong;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'leftarrow')==1
    d.launch_long = d.launch_long - d.dlatlong;
    keypress_update
    
elseif strcmp(get(fig,'currentkey'),'t')==1
    units = get( d.title, 'units' );
    set( d.title, 'units', 'normal', 'position',[0.5,0.95,1] );
    set( d.title,'units', units );
    d.title_position = get( d.title, 'position' );
    
elseif strcmp(get(fig,'currentkey'),'m')==1
    [t,x,y,z,x0r,y0r,z0r] = compute_trajectory;
    make_video_of_trajectory( t,x,y,z );
    
elseif strcmp(get(fig,'currentkey'),'q')==1
    close(gcf); return
end

%%

%Leading zero function
function s=leadzero(Nz,z)
fmt=num2str(Nz,'%%0%dg');
s=sprintf(fmt,z);

%%

% Bitmap image interpolation function.
function II = interp_image( I , II_width , II_height , fit_by )

%Get dimensions of I
[heightI,widthI,num_cols]  = size(I);

%Define index vectors for height and width of 'initial_image'
w = 1:widthI;
h = 1:heightI;

%Define index vectors for height and width of output image
if strcmp(fit_by,'height')
    W = linspace(1,widthI,II_height);
    H = linspace(1,heightI,II_height );
elseif strcmp(fit_by,'width')
    W = linspace(1,widthI,II_width);
    H = linspace(1,heightI,II_width );
else
    W = linspace(1,widthI,II_width);
    H = linspace(1,heightI,II_height );
end

%Meshgrid these
[h,w] = meshgrid(h,w);
[H,W] = meshgrid(H,W);

%If I is a grayscale image, then replicate for R,G,B components
if num_cols ==1
    II = interp2(h,w,double(I.'),H,W).';
else
    %Create R,G,B matricies from image matrix I.
    R = ones(heightI,widthI);
    G = ones(heightI,widthI);
    B = ones(heightI,widthI);
    R(:,:) = I(:,:,1);
    G(:,:) = I(:,:,2);
    B(:,:) = I(:,:,3);
    
    %Interpolate these colour matricies
    R = interp2(h,w,R.',H,W);
    G = interp2(h,w,G.',H,W);
    B = interp2(h,w,B.',H,W);
    
    %Initialise output array
    dim = size(R.');
    II = zeros(dim(1),dim(2),3);
    II(:,:,1) = R.';
    II(:,:,2) = G.';
    II(:,:,3) = B.';
end

%End of code