%
% Purpose: Compare utm locations computed by planar versus spherical trig.
%
% Date: 9 June 2016
%
lat1 =   40;  %Degrees
lon1 = -126;  %Degrees.  Central merdian is -123.
brg  =  [0:5:355]';    %Degrees
dist =  10000;  %Meters;
rE = 6371009;   %Meters, average Earth radius.

[ee nn] = LL2utm(lon1, lat1);

%
% Convert to radians:
psi = brg*pi/180;
phi1 = lat1*pi/180;
lambda1 = lon1*pi/180;
delta = dist/rE;

%
% Rhumb line in UTM:
ee2 = ee + dist*sin(psi);
nn2 = nn + dist*cos(psi);

%
% Now do spherical great circle.  First convert remaining quantities to
% radians:

phi2    = asin(sin(phi1)*cos(delta) + cos(phi1)*sin(delta)*cos(psi));
lambda2 = lambda1 + atan2(sin(psi)*sin(delta)*cos(phi1), cos(delta)-sin(phi1)*sin(phi2));

[eeS nnS] = LL2utm(lambda2*180/pi, phi2*180/pi);

error = [ee2 nn2] - [eeS nnS];

% for( i=1:length(psi) )
%   fprintf(['For bearing %.4f, the [eastings, northings] great-circle error is [%.4f %.4f],'...
% 	   'and the norm is %.4f.\n'], brg(i), error(i,1), error(i,2), ...
% 	  norm(error(i,:)));
% end

%
% Rhumb line:

phi2R    = phi1 + delta*cos(psi);
deltaPhi = phi2R - phi1;
deltaPsi = log( tan( pi/4 + phi2R/2 ) / tan( pi/4 + phi1/2 ) );
q        = deltaPhi./deltaPsi;
deltaLam = delta*sin(psi)./q;
lambda2R = lambda1 + deltaLam;

[eeR nnR] = LL2utm(lambda2R*180/pi, phi2R*180/pi);

errorR = [ee2 nn2] - [eeR nnR];

% for( i=1:length(psi) )
%   fprintf(['For bearing %.4f, the [eastings, northings] rhumb-line error'...
% 	   'is [%.4f %.4f],'...
% 	   'and the norm is %.4f.\n'], ...
% 	  brg(i), errorR(i,1), errorR(i,2), norm(errorR(i)));
% end

if(~exist('position'))
  position = get(0,'screensize');
  npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
  npposition(3:4) = round([position(3)*.95 position(4)*.90]);
  position = npposition;
end

if( ~exist('utmErrorFig') )
  utmErrorFig = figure;
  set( utmErrorFig, 'Name', 'Utm Heading Error', 'DeleteFcn', 'utmErrorFig=[];');
  set(gcf,'position',position)
  fontsize = 20;
  fontweight = 'normal';
  set(gca,'fontsize',fontsize,'fontweigh',fontweight)
  set(gcf,'defaultaxesfontsize',fontsize,'defaulttextfontsize',fontsize)
end
figure(utmErrorFig);

for( i=1:length(psi) )
  errorSNorm(i,1) = norm(error(i,:));
  errorRNorm(i,1) = norm(errorR(i,:));
end

%
% Convert to percentage:
errorSNorm = errorSNorm/dist*100;
errorRNorm = errorRNorm/dist*100;

%
% Convert to Cartesian coordinates:
errorSx = errorSNorm.*sin(psi);
errorSy = errorSNorm.*cos(psi);

errorRx = errorRNorm.*sin(psi);
errorRy = errorRNorm.*cos(psi);

errorSPtr = plot(errorSx, errorSy);
hold on;
grid on;
errorRPtr = plot(errorRx, errorRy,'r');
axis equal

%
% Add polar coordinate overlay:
mag = 2;
for( angle=0:30:(360-30) )
  [x1,y1] = pol2cart(  angle / 180 * pi , [0 mag]);
  plot(x1,y1,'g')
end

for( rho=0:1:mag )
  [x1,y1] = pol2cart(  0:0.01:mag*pi , rho);
  plot(x1,y1,'g')
end

str=sprintf(['%% Position Error Versus Bearing, for %d meter displacement in UTM. ',...
	     'Origin at (lat, lon) = (%.2f, %.2f)'],dist, lat1, lon1);

title(str)
xlabel('East');
ylabel('North')
lptr=legend('Great Circle','Rhumb Line','location','best');
set(lptr,'fontsize',25);



