clg



nlocations = 30;



%% setup all the possible locations

locations = rand(nlocations,2) * 100;



%% generate the connections between locations

conns = cell(nlocations,1);

for i = 1:nlocations-1

  otherlocs = i+1:nlocations;



  dists = sqrt((locations(i,1) - locations(otherlocs, 1)).^2 + ...

	       (locations(i,2) - locations(otherlocs, 2)).^2)';

  [dists, idx] = sort(dists);



  cnt = 0;

  for j = length(conns{i})+1:min(3, length(dists))

    cnt = cnt + 1;

    conns{i}(j) = otherlocs(idx(cnt));

    conns{otherlocs(idx(cnt))}(end+1) = i;

  endfor

endfor



%% clean up all the connections to make sure that all connections are

%% bi-directional

for i = 1:length(conns)

  for j = 1:length(conns{i})

    conns{conns{i}(j)}(end+1) = j;

  endfor

endfor

for i = 1:length(conns)

  conns{i} = unique(conns{i});

endfor



%% Uncomment this to see it fail to find a path because one does not

%% exist.

% for i = 1:length(conns)

%  conns{i} = conns{i}(conns{i} ~= length(conns));

% endfor

% conns{end} = [];



plotcmds = {};

for i = 1:nlocations

  text(locations(i,1) + 3, locations(i,2), num2str(i));

  for j = 1:length(conns{i})

    if (i < conns{i}(j))

      %% only plot everything once

      thisplot = locations([i conns{i}(j)], :);

      plotcmds = {plotcmds{:} thisplot(:,1) thisplot(:,2) "*-b"};

    endif

  endfor

endfor

plot(plotcmds{:});



newax = [min(locations(:,1)) - 1, max(locations(:,1)) + 1, ...

	 min(locations(:,2)) - 1, max(locations(:,2)) + 1];



axis(newax);



function [act, est, compl] = mydist(thispath, locations, connections, goal)



  if (thispath(end) == goal)

    compl = 1;

  elseif (length(connections{thispath(end)}) == 1)

    compl = -1;

  elseif (length(unique(thispath)) ~= length(thispath))

    %% check to make sure that there are no path loops

    compl = -1;

  else

    compl = 0;

  endif



  %% determine the actual distance

  act = 0;

  for i = 1:length(thispath)-1

    act = act + sqrt(sum((locations(thispath(i),:) - locations(thispath(i+1))).^2));

  endfor



  %% determine the estimated distance of the rest of the path

  est = sqrt(sum((locations(thispath(end),:) - locations(goal,:)).^2));

endfunction



function [conns] = myconns(thispath, locations, connections, goal)

  conns = connections{thispath(end)};

  %% don't return down the path we just came from

  conns = conns(conns ~= thispath(end));

endfunction



%% Solve the actual astar graph

finalpath = astar(@mydist, @myconns, {1}, locations, conns, nlocations)



%% Overlay the final path on the graph

hold on

thisplot = [];

for i = 1:length(finalpath)

  thisplot(end+1,:) = locations(finalpath(i), :);

endfor

plot(thisplot(:,1), thisplot(:,2), "*-r");