function??[path?V]?=?FocusdDstar（map?StartPos?GoalPos）
%map——二維數組圖?0:?passable?1:?obstacle
%?startPos:?Original?Position
%?endPos:???Target?Postion


%?The?routine?is?programmed?according?to?article:
%?Anthony?Stentz.?“The?D*?Algorithm?for?Real-Time?Planning?of?Optimal
%?Traverse“?CMU-RI-TR-94-37?1994


%路徑規劃
clc;clear?all;
m=30;n=30;
for?i?=?1:m+2
????if?i?==?1
????????for?j?=?1:n+2
????????????map（ij）?=?1;
????????end
????elseif?i?==?m+2
????????for?j?=?1:n+2
????????????map（ij）?=?1;
????????end
????else
????????for?j?=?1:n+2
????????????if?（（j?==?1）||（j?==?n+2））
????????????????map（ij）?=?1;
????????????else
????????????????map（ij）?=?0;
????????????end
????????end
????end
end
%%障礙
for?j=2:10
????map（5j）=1;
end
for?j=2:15
?????map（24j）=1;
end
for?j=9:24
?????map（10j）=1;
end
for?j=20:31
????map（15j）=1;
end
for?j=5:20
???map（20j）=1;
end
for?j=18:27
????map（28j）=1;
end
for?i=2:6??
????map（i18）=1;
end
for?i=17:20
?????map（i5）=1;
end
for?i=23:25?
????map（i20）=1;
end
for?i=13:17
????map（i13）=1;
end
StartPos=[33];
GoalPos=[2922];

????clear?global?Var;
????clear?global?ori_map;
????persistent?hLine;
????if?ishandle（hLine）?
????????delete（hLine）;?end
????global?ori_map;
????global?Var;
????[mn]?=?size（map）;
????X.x=[];X.y=[];
????Var.B（mn）?=?X;?%?B（X）??:?b（X）?backpointers?
????Var.r（mn）?=X;
????Var.Tag?=?zeros（mn）;???%?Tag（X）?=?0（NEW）1（OPEN）?or?2（CLOSED）
????Var.H?=?-1?*?Inf*ones（mn）;????????%?H（X）??:?estimate?of?the?sum?of?the?arc?costs?from?X?to?G
????Var.K?=?-1?*?Inf*ones（mn）;????????%?K（X）??:?state?key?function?value?=?min（H（X）Pre（X））?By?which?the?states?on?the?OPEN?list?are?sorted
????Var.f?=?-1?*?Inf*ones（mn）;????????%?f（X）??:?state?estimate?cost?from?S?through?X?to?G
????Var.fB?=?-1?*?Inf*ones（mn）;???????%?fb（X）?:?state?bias?estimate?cost?of?X
????Var.OPEN?=?[];
????
???
????G.x?=?GoalPos（1）;?G.y?=?GoalPos（2）;
????Var.H（G.y?G.x）?=?0;
????
????S.x?=?StartPos（1）;?S.y?=?StartPos（2）;
????path?=?S;
????Var.R?=?S;
????Var.Rcur?=?S;
????Var.Dcur?=?0;
???
?
????INSERT（G?Var.H（G.y?G.x））;
????val?=?0;?
????while?~isempty（val）?&&?val（1）????????val?=?PROCESS_STATE2（map）;
????end

????if?Var.Tag（S.y?S.x）?~=?2
????????warndlg（‘There?is?no?path?existed!‘）;
????????path?=?[];
????????return;
????end
????
????cur?=?Var.B（S.y?S.x）;
????while?cur.x~=G.x?||?cur.y~=G.y
????????path?=?[path?cur];
????????cur?=?Var.B（cur.y?cur.x）;
????end
????path?=?[path?cur];
????????
????hold?on;
????hLine?=?plot（[path（:）.x]?[path（:）.y]?‘b.:‘）;

????ori_map?=?map;
????choice?=?questdlg（‘請選擇‘‘狀態變化‘?‘SetObstacle‘?‘CancelObstacle‘‘NoChange‘‘NoChange‘）;
????while?~isequal（choice?‘NoChange‘）
????????switch?choice
???????????case?‘SetObstacle‘
???????????????v?=?0;
???????????????c?=?‘k‘;
???????????case?‘CancelObstacle‘
???????????????v?=?255;
???????????????c?=?‘g‘;
????????end
????