%% Create geoTrajectory and Look Up Pose
% Create a geoTrajectory with starting LLA at [15 15 0] and ending LLA at [75 75 100]. Set the flight time to ten hours. Sample the trajectory every 1000 seconds.

startLLA = [15 15 0];
endLLA = [75 75 100];
timeOfTravel = [0 3600*10];
sampleRate  = 0.001;

trajectory = geoTrajectory([startLLA;endLLA],timeOfTravel,'SampleRate',sampleRate);

% Output the LLA waypoints of the trajectory.

positionsLLA = startLLA;
while ~isDone(trajectory)
    positionsLLA = [positionsLLA;trajectory()];  
end
positionsLLA

% Look up the Cartesian waypoints of the trajectory in the ECEF frame by using the lookupPose function.

sampleTimes = 0:1000:3600*10;
n = length(sampleTimes);
positionsCart = lookupPose(trajectory,sampleTimes,'ECEF');

% Visualize the results in the ECEF frame.

figure()
km = 1000;
plot3(positionsCart(1,1)/km,positionsCart(1,2)/km,positionsCart(1,3)/km, 'b*');
hold on;
plot3(positionsCart(end,1)/km,positionsCart(end,2)/km,positionsCart(end,3)/km, 'bo');
plot3(positionsCart(:,1)/km,positionsCart(:,2)/km,positionsCart(:,3)/km,'b');

plot3([0 positionsCart(1,1)]/km,[0 positionsCart(1,2)]/km,[0 positionsCart(1,3)]/km,'k:');
plot3([0 positionsCart(end,1)]/km,[0 positionsCart(end,2)]/km,[0 positionsCart(end,3)]/km,'k:');
xlabel('x (km)'); ylabel('y (km)'); zlabel('z (km)');
legend('Start position','End position', 'Trajectory')

% Copyright 2020 The MathWorks, Inc.