% 北太天元 實現(xiàn) A星 搜索算法
% @地圖[input] 是一個取值為布爾值的方陣，表示網(wǎng)格化的地圖上的點
%??? 其中取值為true的點表示可以經(jīng)過，取值為false的點表示不能經(jīng)過
% @耗時[input] 是一個和地圖的大小一樣的矩陣， 耗時(i,j) 表示到達
%? (i,j)點走單位距離需要耗費的時間.
% @起點[input] 是1x2的矩陣或者1x1的矩陣，記錄出發(fā)點的序號，
%????????????? 如果是1x2的矩陣[m,n]，那么 起點 的行列號是(m,n)，
%????????????? 如果 起點 是一個1x1的矩陣,那么認為它的值是起點的線性序號 ind ?
%????????????? 和行列號 (m,n) 的關(guān)系是
%???????????????? ind? = (n-1)*矩陣的行數(shù) + m,
% @終點[input] 是1x2的矩陣或者1x1的矩陣，記錄 終點 的序號，
%????????????? 如果是1x2的矩陣[m,n]，那么 終點 的行列號是(m,n)，
%????????????? 如果 終點 是一個1x1的矩陣,那么認為它的值是 終點 的線性序號 ind,
%????????????? 和行列號(m,n)關(guān)系是
%???????????????? ind = (n-1)*矩陣的行 + m,
% @路線坐標[output] 把路線上的點的線性序號轉(zhuǎn)成行列號
% @路線[output] 是一個向量，記錄著從 起點 到 終點 的路線上經(jīng)過的所有的點的線性序號
% %耗時_起終[output] 路線上的總耗時

%例1:
clc
clf
clear
close all
??? m = 5
??? 地圖 = zeros(m);
??? 地圖(1,:) = 1;
?? ??? ?地圖(:,end) = 1;
??? [路線坐標, 路線, 耗時_起終] = a_星算法(logical(地圖), ones(size(地圖)), [1,1], [m,m])
?? ??? ?
?? ??? ?hold on
?? ?? line( [-1, m+1] , [-1,-1] )
?? ?? line( [-1, m+1] , [m+1,m+1] )
?? ?? line( 路線坐標(1,:), 路線坐標(2,:) )
?? ??? ?text( 路線坐標(1,1), 路線坐標(2,1), '起點','FontSize',24);
?? ??? ?text( 路線坐標(1,end), 路線坐標(2,end), '起點', 'FontSize',24);
?? ??? ?for i=2:(size(路線坐標,2)-1)
?? ??? ??? ?text( 路線坐標(1,i), 路線坐標(2,i), ...
?? ??? ??? ? ?? ?['(',num2str(路線坐標(1,i)),',',num2str(路線坐標(2,i)),')'],'FontSize',24);
?? ??? ?end
?? ??? ?hold off
% 地圖
%???? 1?? 1? 1? 1? 1
%???? 0? 0?? 0? 0? 1
%???? 0? 0?? 0? 0? 1
%???? 0?? 0? 0? 0? 1
%???? 0?? 0? 0? 0? 1 ?

% 地圖到達每個節(jié)點的單位距離的耗時
%???? 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 ?


%輸出結(jié)果
%路線坐標 =
%? 2x8 int32
%?? 1?? 1?? 1?? 1?? 2?? 3?? 4?? 5
%?? 1?? 2?? 3?? 4?? 5?? 5?? 5?? 5
%路線 =
%? 1x8 double
%??? 1??? 6?? 11?? 16?? 22?? 23?? 24?? 25
%耗時_起終 =
%? 1x1 double
%??? 7.4142




function [路線坐標, 路線, 耗時_起終] = a_星算法(地圖, 耗時, 起點, 終點)
?????? ?
??? if ~islogical(地圖)
??????? error('輸入變量 地圖 必須取值為布爾矩陣')
??? end
??? if ~isa(耗時, 'double')
??????? error('輸入變量 耗時 必須是double矩陣')
??? end

??? % 避免0做除數(shù) (而且如果用戶輸入的耗時是負數(shù)，也修正為eps)
??? 耗時( 耗時 < 0) = eps;
?? ??? ?耗時單位 = min(min(耗時));
??? % 把耗時無量gang化
??? 耗時 = 耗時 / 耗時單位;

??? % 默認返回值 - 空表示尋找路線失敗
??? 路線 = [];
?? ??? ?路線坐標 = zeros(2,0);
?? ??? ?耗時_起終 = inf;
?? ?
??? 地圖尺寸 = size(地圖);
??? 地圖線性尺寸 = numel(地圖尺寸);

??? % 用起點初始化開放集(open set 這和數(shù)學里的開集的定義不同)
??? openSet = false(地圖尺寸);
??? openSet(起點) = true;

??? % 初始化閉合集(closed set 和數(shù)學的閉集的定義不同)
??? closedSet = false(地圖尺寸);
?? ?
??? 父節(jié)點 = zeros(1, 地圖線性尺寸);

?? ??? ?% 檢查起點和終點是否是線性序號
?? ??? ?if( length(起點) ~=1 && length(起點) ~= 2)
?? ??? ??? ?disp("輸入?yún)?shù) 起點 應(yīng)該是 1x1 或者1x2 的矩陣");
?? ??? ??? ?return;
?? ??? ?end
?? ??? ?if( length(終點) ~=1 && length(終點) ~= 2)
?? ??? ??? ?disp("輸入?yún)?shù)? 終點點 應(yīng)該是 1x1 或者1x2 的矩陣");
?? ??? ??? ?return;
?? ??? ?end
?? ??? ?if( length(起點) == 2 )
?? ??? ??? ?起點 = (起點(2)-1)*地圖尺寸(1) + 起點(1);
?? ??? ?end
?? ??? ?if( length(終點) == 2 )
?? ??? ??? ?終點 = (終點(2)-1)*地圖尺寸(1) + 終點(1);
?? ??? ?end

??? 耗時_from起點 = inf(地圖尺寸(1),地圖尺寸(2));
??? 耗時_from起點(起點) = 0;

?? ?
?? ??? ?%線性序號 ->? 行列號
??? [gr, gc] = ind2sub(地圖尺寸, 終點);
?? ?
??? 估計耗時_距離終點 = inf(地圖尺寸(1),地圖尺寸(2));
??? 估計耗時_距離終點(起點) = compute_估計耗時(地圖尺寸, 起點, gr, gc);
??? ?
??? const_根號2 = sqrt(2);

??? % 只要 開放集 非空 就循環(huán) ?
??? while any(openSet(:) > 0)

??????? % 估計耗時_距離終點 的最小值就是
?? ??? ??? ??? ?% 就是 開放集? 中距離 終點 的估計距離的最小值，因為
?? ??? ??? ??? ?% 當一個點被剔除出開放集的時候，相應(yīng)的 估計耗時_距離終點 已經(jīng)在前面修改為 inf 了。
??????? [~, 活動點] = min(估計耗時_距離終點(:));

??????? % 如果 我們已經(jīng)到了終點
??????? if 活動點 == 終點
??????????? % 獲取 從 起點 到 終點 的 路線
??????????? 路線 = 獲取路線(父節(jié)點, 活動點);
?? ??? ??? ??? ??? ??? ?路線 = 路線(end:-1:1);
?? ??? ??? ??? ??? ??? ?[r,c] = ind2sub(地圖尺寸, 路線);
?? ??? ??? ??? ??? ??? ?路線坐標 = double([r;c]);
?? ??? ??? ??? ??? ??? ?耗時_起終 = 估計耗時_距離終點(終點)*耗時單位;?? ?
??????????? return
??????? end

??????? %線性序號 -> 行列號
??????? [rc, cc] = ind2sub(地圖尺寸, 活動點);
?????? ?
??????? % 把 活動點 從開放集 中剔除
??????? openSet(rc, cc) = false;
??????? % 把 活動點 放入 閉合集
??????? closedSet(rc, cc) = true;
?????? ?
??????? 估計耗時_距離終點(rc, cc) = inf;
??????? 耗時_from起點_to_活動點 = 耗時_from起點(rc, cc);
?????? ?
??????? % 獲得所有的鄰居點，這里考慮的是8個鄰居，包括上下左右 和
?? ??? ??? ??? ?% 左上、右上、左下、右下8個點
??????? % 第一列是行號，第二列是列號，第三列是和活動點的距離
??????? 信息_鄰居點s = [ ...
??????????? rc + 1, cc + 1, const_根號2 ; ...
??????????? rc + 1, cc + 0, 1 ; ...
??????????? rc + 1, cc - 1, const_根號2 ; ...
??????????? rc + 0, cc - 1, 1 ; ...
??????????? rc - 1, cc - 1, const_根號2 ; ...
??????????? rc - 1, cc - 0, 1 ; ...
??????????? rc - 1, cc + 1, const_根號2 ; ...
??????????? rc - 0, cc + 1, 1 ; ...
??????? ];

??????? %檢查鄰居點是否在地圖范圍之內(nèi)
??????? 有效性檢驗_row = 信息_鄰居點s(:,1) >= 1 & 信息_鄰居點s(:,1) <= 地圖尺寸(1);
??????? 有效性檢驗_col = 信息_鄰居點s(:,2) >= 1 & 信息_鄰居點s(:,2) <= 地圖尺寸(2);
??????? 信息_鄰居點s = 信息_鄰居點s(有效性檢驗_row & 有效性檢驗_col, :);
??????? % 行列號 -> 線性序號
??????? 鄰居點 = sub2ind(地圖尺寸, 信息_鄰居點s(:,1),? 信息_鄰居點s(:,2) );
??????? % 只保留在地圖中，且不在 閉合集 中的 鄰居點
??????? 線性序號_地 = 地圖(鄰居點) & ~closedSet(鄰居點);
??????? 鄰居點 = 鄰居點(線性序號_地);
??????? % 活動點 到 這些篩選出的 鄰居點 的距離
??????? 距離 = 信息_鄰居點s(線性序號_地, 3)

??????? % 把每一個篩選出的 鄰居點 放入 開放集
??????? openSet(鄰居點) = true;

??????? % 暫定_GSCORE 是從 起點 經(jīng)過 活動點 這個點 到 鄰居點的 耗時
??????? 暫定_耗時_起鄰 = 耗時_from起點_to_活動點 + 耗時(鄰居點) .* 距離
?????? ?
??????? % IXBETTER 指示 經(jīng)過 活動點 是否 找到了更短的耗時路徑
??????? ixBetter = 暫定_耗時_起鄰 < 耗時_from起點(鄰居點)
??????? bestNeighbors = 鄰居點(ixBetter)?????? ?

??????? % 對于 找到了更短路徑的 鄰居點，修改它的父節(jié)點為 活動點
?? ??? ??? ??? ?% 修改它的 耗時_from起點 為 經(jīng)由 活動點 得到的 暫定_耗時_起鄰
??????? 父節(jié)點(bestNeighbors) = 活動點;
??????? 耗時_from起點(bestNeighbors) = 暫定_耗時_起鄰(ixBetter);
??????? 估計耗時_距離終點(bestNeighbors) = 耗時_from起點(bestNeighbors)? ...
?? ??? ??? ??? ??? ??? ??? ??? ?+ compute_估計耗時(地圖尺寸, bestNeighbors, gr, gc);

??? end % while
end

% 返回路徑, 用的方法就是從活動點 活動點 利用父節(jié)點一個一個向前回溯
function p = 獲取路線(父節(jié)點, 活動點)
??? 序號_非零點 = find(父節(jié)點);
??? p = nan(1, length(序號_非零點));
??? p(1) = 活動點;
??? next = 1;
??? while any(活動點 == 序號_非零點)
??????? 活動點 = 父節(jié)點(活動點);
??????? next = next + 1;
??????? p(next) = 活動點;
??? end
??? p(isnan(p)) = [];
end

% @地圖尺寸[input] 是一個1x2的矩陣，給出地圖矩陣的行數(shù)和列數(shù)
% @出發(fā)點們 [input]? 是一個1xk 的矩陣, 是k個出發(fā)點的線性序號
% @終點行號 [input] 是終點的 行號
% @終點列號 [input] 是終點的 列好
% @耗時_估計 [output] 是估算從 出發(fā)點們 到 終點的 估計耗時 (不是精確耗時)
function 耗時_估計 = compute_估計耗時(地圖尺寸, 出發(fā)點們, 終點行號, 終點列號)
??? [行號_出s,列號_出s] = ind2sub(地圖尺寸, 出發(fā)點們);
??? 耗時_估計 = sqrt((行號_出s - 終點行號).^2 + (列號_出s - 終點列號).^2);
end


%關(guān)于A星算法 可以參考 http://theory.stanford.edu/~amitp/GameProgramming/

/*

我們試圖解決的問題是把一個游戲?qū)ο髲钠瘘c帶到目標。尋路解決了從起點到目標找到一條好路的問題——避開障礙，避開敵人，并將成本（燃料、時間、距離、設(shè)備、金錢等）降至最低。運動解決了走一條路并沿著它前進的問題。你可以只在其中一條上付出努力。在一個極端，一個復(fù)雜的探路器與一個瑣碎的移動算法相結(jié)合，可以在物體開始移動時找到一條路徑，而物體會沿著這條路徑移動，而不會注意到其他一切。在另一個極端，僅限運動的系統(tǒng)不會向前看以找到路徑（相反，初始的“路徑”將是一條直線），而是一步一個腳印，考慮到每個點的局部環(huán)境。通過使用尋路和移動算法可以獲得最佳結(jié)果。

*/