-
Notifications
You must be signed in to change notification settings - Fork 1
/
a_star_plot.m
64 lines (55 loc) · 1.3 KB
/
a_star_plot.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
function a_star_plot(map, costs, route)
% A_STAR_PLOT plots the output of A_STAR.
% MAP is the MAP input to A_STAR
% COSTS is the COST input to A_STAR
% ROUTE is the output of A_STAR
% Alex Ranaldi
if ~islogical(map)
error('map must be logical')
end
mapSize = size(map);
hf = figure( ...
'Color', 'white' ...
);
ha = axes( ...
'parent', hf, ...
'ydir','reverse',...
'xdir','normal',...
'xlim', [1 mapSize(1)+1], ...
'ylim', [1 mapSize(2)+1], ...
'xtick', 1:mapSize(1)+1, ...
'ytick', 1:mapSize(2)+1 ...
);
grid on;
% normalize costs
minCost = min(costs(:));
maxCost = max(costs(:));
mmCost = maxCost - minCost;
if mmCost == 0
mmCost = 1;
end
costs = (costs(:) - minCost) / mmCost;
colorIndex = fix(costs*100)+1;
colors = jet(101);
for k = 1 : numel(map)
[r, c] = ind2sub(mapSize, k);
if map(k)
patch( ...
'parent', ha, ...
'xdata', [c c c+1 c+1], ...
'ydata', [r r+1 r+1 r], ...
'facecolor', colors(colorIndex(k),:) ...
);
end
end
[r, c] = ind2sub(mapSize, route);
for k = 1 : length(route)
line('parent', ha, ...
'xdata', c + 0.5, ...
'ydata', r + 0.5, ...
'linewidth', 2, ...
'color', 'r', ...
'linestyle', '-.' ...
);
end