% 北太天元實(shí)現(xiàn) A星 搜索算法, 增加了動(dòng)畫顯示openSet和closedSet的變化過程

% @地圖[input] 是一個(gè)取值為布爾值的方陣，表示網(wǎng)格化的地圖上的點(diǎn)

% 其中取值為true的點(diǎn)表示可以經(jīng)過，取值為false的點(diǎn)表示不能經(jīng)過

% @耗時(shí)[input] 是一個(gè)和地圖的大小一樣的矩陣， 耗時(shí)(i,j) 表示到達(dá)

% (i,j)點(diǎn)走單位距離需要耗費(fèi)的時(shí)間.

% @起點(diǎn)[input] 是1x2的矩陣或者1x1的矩陣，記錄出發(fā)點(diǎn)的序號(hào)，

% 如果是1x2的矩陣[m,n]，那么 起點(diǎn) 的行列號(hào)是(m,n)，

% 如果 起點(diǎn) 是一個(gè)1x1的矩陣,那么認(rèn)為它的值是起點(diǎn)的線性序號(hào) ind

% 和行列號(hào) (m,n) 的關(guān)系是

% ind = (n-1)*矩陣的行數(shù) + m,

% @終點(diǎn)[input] 是1x2的矩陣或者1x1的矩陣，記錄 終點(diǎn) 的序號(hào)，

% 如果是1x2的矩陣[m,n]，那么 終點(diǎn) 的行列號(hào)是(m,n)，

% 如果 終點(diǎn) 是一個(gè)1x1的矩陣,那么認(rèn)為它的值是 終點(diǎn) 的線性序號(hào) ind,

% 和行列號(hào)(m,n)關(guān)系是

% ind = (n-1)*矩陣的行 + m,

% @路線坐標(biāo)[output] 把路線上的點(diǎn)的線性序號(hào)轉(zhuǎn)成行列號(hào)

% @路線[output] 是一個(gè)向量，記錄著從 起點(diǎn) 到 終點(diǎn) 的路線上經(jīng)過的所有的點(diǎn)的線性序號(hào)

% %耗時(shí)_起終[output] 路線上的總耗時(shí)

%例1:

load_plugin("time");

clc

clf

clear all

close all

m = 10;

地圖 = zeros(m);

地圖(1,:) = 1;

地圖(:,end) = 1;

for i=1:m

地圖(i,i)=1;

end

耗時(shí) = ones(size(地圖));

% 耗時(shí)(3,3) = 1000;

[路線坐標(biāo), 路線, 耗時(shí)_起終] = a_星算法(logical(地圖), 耗時(shí), [1,1], [m,m])

hold on

line( [-1, m+1] , [-1,-1] )

line( [-1, m+1] , [m+1,m+1] )

line( 路線坐標(biāo)(2,:), 路線坐標(biāo)(1,:) )

text(路線坐標(biāo)(2,1), 路線坐標(biāo)(1,1), '起點(diǎn)','FontSize',24);

text( 路線坐標(biāo)(2,end),路線坐標(biāo)(1,end), '終點(diǎn)', 'FontSize',24);

for i=2:(size(路線坐標(biāo),2)-1)

text( 路線坐標(biāo)(2,i),路線坐標(biāo)(1,i), ...

['(',num2str(路線坐標(biāo)(1,i)),',',num2str(路線坐標(biāo)(2,i)),')'],'FontSize',24);

end

hold off

% 地圖

% 1 1 1 1 1

% 0 1 0 0 1

% 0 0 1 0 1

% 0 0 0 1 1

% 0 0 0 0 1

% 地圖到達(dá)每個(gè)節(jié)點(diǎn)的單位距離的耗時(shí)

% 1 1 1 1 1

% 1 1 1 1 1

% 1 1 1 1 1

% 1 1 1 1 1

% 1 1 1 1 1

/*

路線坐標(biāo) =

2x5 double

1 2 3 4 5

1 2 3 4 5

路線 =

1x5 double

1 7 13 19 25

耗時(shí)_起終 =

1x1 double

5.6569

*/

function [路線坐標(biāo), 路線, 耗時(shí)_起終] = a_星算法(地圖, 耗時(shí), 起點(diǎn), 終點(diǎn))

if ~islogical(地圖)

error('輸入變量 地圖 必須取值為布爾矩陣')

end

if ~isa(耗時(shí), 'double')

error('輸入變量 耗時(shí) 必須是double矩陣')

end

% 避免0做除數(shù) (而且如果用戶輸入的耗時(shí)是負(fù)數(shù)，也修正為eps)

耗時(shí)( 耗時(shí) < 0) = eps;

耗時(shí)單位 = min(min(耗時(shí)));

% 把耗時(shí)無量gang化

耗時(shí) = 耗時(shí) / 耗時(shí)單位;

% 默認(rèn)返回值 - 空表示尋找路線失敗

路線 = [];

路線坐標(biāo) = zeros(2,0);

耗時(shí)_起終 = inf;

地圖尺寸 = size(地圖);

地圖線性尺寸 = numel(地圖尺寸);

% 用起點(diǎn)初始化開放集(open set 這和數(shù)學(xué)里的開集的定義不同)

openSet = false(地圖尺寸);

openSet(起點(diǎn)) = true;

% 初始化閉合集(closed set 和數(shù)學(xué)的閉集的定義不同)

closedSet = false(地圖尺寸);

heatmap(-double(openSet)+2*double(closedSet))

title("紫色是開放集")

pause(1);

父節(jié)點(diǎn) = zeros(1, 地圖線性尺寸);

% 檢查起點(diǎn)和終點(diǎn)是否是線性序號(hào)

if( length(起點(diǎn)) ~=1 && length(起點(diǎn)) ~= 2)

disp("輸入?yún)?shù) 起點(diǎn) 應(yīng)該是 1x1 或者1x2 的矩陣");

return;

end

if( length(終點(diǎn)) ~=1 && length(終點(diǎn)) ~= 2)

disp("輸入?yún)?shù) 終點(diǎn)點(diǎn) 應(yīng)該是 1x1 或者1x2 的矩陣");

return;

end

if( length(起點(diǎn)) == 2 )

起點(diǎn) = (起點(diǎn)(2)-1)*地圖尺寸(1) + 起點(diǎn)(1);

end

if( length(終點(diǎn)) == 2 )

終點(diǎn) = (終點(diǎn)(2)-1)*地圖尺寸(1) + 終點(diǎn)(1);

end

耗時(shí)_from起點(diǎn) = inf(地圖尺寸(1),地圖尺寸(2));

耗時(shí)_from起點(diǎn)(起點(diǎn)) = 0;

%線性序號(hào) -> 行列號(hào)

[gr, gc] = ind2sub(地圖尺寸, 終點(diǎn));

估計(jì)耗時(shí)_距離終點(diǎn) = inf(地圖尺寸(1),地圖尺寸(2));

估計(jì)耗時(shí)_距離終點(diǎn)(起點(diǎn)) = compute_估計(jì)耗時(shí)(地圖尺寸, 起點(diǎn), gr, gc);

const_根號(hào)2 = sqrt(2);

% 只要 開放集 非空 就循環(huán)

while any(openSet(:) > 0)

% 估計(jì)耗時(shí)_距離終點(diǎn) 的最小值就是

% 就是 開放集 中距離 終點(diǎn) 的估計(jì)距離的最小值，因?yàn)?/p>

% 當(dāng)一個(gè)點(diǎn)被剔除出開放集的時(shí)候，相應(yīng)的 估計(jì)耗時(shí)_距離終點(diǎn) 已經(jīng)在前面修改為 inf 了。

[~, 活動(dòng)點(diǎn)] = min(估計(jì)耗時(shí)_距離終點(diǎn)(:));

% 如果 我們已經(jīng)到了終點(diǎn)

if 活動(dòng)點(diǎn) == 終點(diǎn)

% 獲取 從 起點(diǎn) 到 終點(diǎn) 的 路線

路線 = 獲取路線(父節(jié)點(diǎn), 活動(dòng)點(diǎn));

路線 = 路線(end:-1:1);

[r,c] = ind2sub(地圖尺寸, 路線);

路線坐標(biāo) = double([r;c]);

耗時(shí)_起終 = 估計(jì)耗時(shí)_距離終點(diǎn)(終點(diǎn))*耗時(shí)單位;

return

end

%線性序號(hào) -> 行列號(hào)

[rc, cc] = ind2sub(地圖尺寸, 活動(dòng)點(diǎn));

% 把 活動(dòng)點(diǎn) 從開放集 中剔除

openSet(rc, cc) = false;

% 把 活動(dòng)點(diǎn) 放入 閉合集

closedSet(rc, cc) = true;

heatmap(-double(openSet)+2*double(closedSet))

title("紅色是閉合集，紫色是開放集")

pause(1);

估計(jì)耗時(shí)_距離終點(diǎn)(rc, cc) = inf;

耗時(shí)_from起點(diǎn)_to_活動(dòng)點(diǎn) = 耗時(shí)_from起點(diǎn)(rc, cc);

% 獲得所有的鄰居點(diǎn)，這里考慮的是8個(gè)鄰居，包括上下左右 和

% 左上、右上、左下、右下8個(gè)點(diǎn)

% 第一列是行號(hào)，第二列是列號(hào)，第三列是和活動(dòng)點(diǎn)的距離

信息_鄰居點(diǎn)s = [ ...

rc + 1, cc + 1, const_根號(hào)2 ; ...

rc + 1, cc + 0, 1 ; ...

rc + 1, cc - 1, const_根號(hào)2 ; ...

rc + 0, cc - 1, 1 ; ...

rc - 1, cc - 1, const_根號(hào)2 ; ...

rc - 1, cc - 0, 1 ; ...

rc - 1, cc + 1, const_根號(hào)2 ; ...

rc - 0, cc + 1, 1 ; ...

];

%檢查鄰居點(diǎn)是否在地圖范圍之內(nèi)

有效性檢驗(yàn)_row = 信息_鄰居點(diǎn)s(:,1) >= 1 & 信息_鄰居點(diǎn)s(:,1) <= 地圖尺寸(1);

有效性檢驗(yàn)_col = 信息_鄰居點(diǎn)s(:,2) >= 1 & 信息_鄰居點(diǎn)s(:,2) <= 地圖尺寸(2);

信息_鄰居點(diǎn)s = 信息_鄰居點(diǎn)s(有效性檢驗(yàn)_row & 有效性檢驗(yàn)_col, :);

% 行列號(hào) -> 線性序號(hào)

鄰居點(diǎn) = sub2ind(地圖尺寸, 信息_鄰居點(diǎn)s(:,1), 信息_鄰居點(diǎn)s(:,2) );

% 只保留在地圖中，且不在 閉合集 中的 鄰居點(diǎn)

線性序號(hào)_地 = 地圖(鄰居點(diǎn)) & ~closedSet(鄰居點(diǎn));

鄰居點(diǎn) = 鄰居點(diǎn)(線性序號(hào)_地);

% 活動(dòng)點(diǎn) 到 這些篩選出的 鄰居點(diǎn) 的距離

距離 = 信息_鄰居點(diǎn)s(線性序號(hào)_地, 3);

% 把每一個(gè)篩選出的 鄰居點(diǎn) 放入 開放集

openSet(鄰居點(diǎn)) = true;

heatmap(-double(openSet)+2*double(closedSet))

title("紅色是閉合集，紫色是開放集")

pause(2);

% 暫定_GSCORE 是從 起點(diǎn) 經(jīng)過 活動(dòng)點(diǎn) 這個(gè)點(diǎn) 到 鄰居點(diǎn)的 耗時(shí)

暫定_耗時(shí)_起鄰 = 耗時(shí)_from起點(diǎn)_to_活動(dòng)點(diǎn) + 耗時(shí)(鄰居點(diǎn)) .* 距離;

% IXBETTER 指示 經(jīng)過 活動(dòng)點(diǎn) 是否 找到了更短的耗時(shí)路徑

ixBetter = 暫定_耗時(shí)_起鄰 < 耗時(shí)_from起點(diǎn)(鄰居點(diǎn));

bestNeighbors = 鄰居點(diǎn)(ixBetter);

% 對(duì)于 找到了更短路徑的 鄰居點(diǎn)，修改它的父節(jié)點(diǎn)為 活動(dòng)點(diǎn)

% 修改它的 耗時(shí)_from起點(diǎn) 為 經(jīng)由 活動(dòng)點(diǎn) 得到的 暫定_耗時(shí)_起鄰

父節(jié)點(diǎn)(bestNeighbors) = 活動(dòng)點(diǎn);

耗時(shí)_from起點(diǎn)(bestNeighbors) = 暫定_耗時(shí)_起鄰(ixBetter);

估計(jì)耗時(shí)_距離終點(diǎn)(bestNeighbors) = 耗時(shí)_from起點(diǎn)(bestNeighbors) ...

+ compute_估計(jì)耗時(shí)(地圖尺寸, bestNeighbors, gr, gc);

end % while

end

% 返回路徑, 用的方法就是從活動(dòng)點(diǎn) 活動(dòng)點(diǎn) 利用父節(jié)點(diǎn)一個(gè)一個(gè)向前回溯

function p = 獲取路線(父節(jié)點(diǎn), 活動(dòng)點(diǎn))

序號(hào)_非零點(diǎn) = find(父節(jié)點(diǎn));

p = nan(1, length(序號(hào)_非零點(diǎn)));

p(1) = 活動(dòng)點(diǎn);

next = 1;

while any(活動(dòng)點(diǎn) == 序號(hào)_非零點(diǎn))

活動(dòng)點(diǎn) = 父節(jié)點(diǎn)(活動(dòng)點(diǎn));

next = next + 1;

p(next) = 活動(dòng)點(diǎn);

end

p(isnan(p)) = [];

end

% @地圖尺寸[input] 是一個(gè)1x2的矩陣，給出地圖矩陣的行數(shù)和列數(shù)

% @出發(fā)點(diǎn)們 [input] 是一個(gè)1xk 的矩陣, 是k個(gè)出發(fā)點(diǎn)的線性序號(hào)

% @終點(diǎn)行號(hào) [input] 是終點(diǎn)的 行號(hào)

% @終點(diǎn)列號(hào) [input] 是終點(diǎn)的 列好

% @耗時(shí)_估計(jì) [output] 是估算從 出發(fā)點(diǎn)們 到 終點(diǎn)的 估計(jì)耗時(shí) (不是精確耗時(shí))

function 耗時(shí)_估計(jì) = compute_估計(jì)耗時(shí)(地圖尺寸, 出發(fā)點(diǎn)們, 終點(diǎn)行號(hào), 終點(diǎn)列號(hào))

[行號(hào)_出s,列號(hào)_出s] = ind2sub(地圖尺寸, 出發(fā)點(diǎn)們);

耗時(shí)_估計(jì) = sqrt((行號(hào)_出s - 終點(diǎn)行號(hào)).^2 + (列號(hào)_出s - 終點(diǎn)列號(hào)).^2);

end