% HW 5 for ME 256 in 2009
% We use the geodesic approach to compute the minimum-distance path in a
% plane with obstacles. For this, we define a potential function of a rough
% terrain with obstacles modeled using the generalized Gaussian function.
% Read the code to creat Z(X,Y) surface on which you need to create a
% geodeisc between two given points.
clc;
clear all;

[X,Y]=meshgrid(0:0.05:6,0:0.05:6);
% The value of n decides how sharp the obstacle is (like the unit circle
% problem of HW 3
% "sigmasqrd" decides the spread or the size of the obstacle
% # in (X - #) and $ in (Y - $) decide the location of the obstacle
n = 8;
sigmasqrd = 0.01;
Z = exp(-0.1*((X-2).^n+(((Y-2)).^n))./sigmasqrd);
n = 2;
sigmasqrd = 0.2;
Z = Z + 2.0*exp(-((X-2.5).^n+((Y-4).^2))./sigmasqrd);
figure(1)
clf
surf(X,Y,Z)
hold on
plot3(2,0,0,'g*','MarkerSize',10,'LineWidth',2);
plot3(3,5,0,'r*','MarkerSize',10,'LineWidth',2);
xlabel('x-axis');
ylabel('y-axis');
zlabel('Height');
figure(2)
clf
contour(X,Y,Z)
hold on
% Starting point
plot(2.5,0,'go','MarkerSize',10,'LineWidth',2);
% Destination point
plot(3,5,'ro','MarkerSize',10,'LineWidth',2);
grid

