-
Notifications
You must be signed in to change notification settings - Fork 1
/
graph_expandElement.m
18 lines (17 loc) · 986 Bytes
/
graph_expandElement.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
%function [graphVector,pqOpen]=graph_expandElement(graphVector,idxNBest,idxX,idxGoal,pqOpen)
%This function expands the vertex with index @x idxX (which is a neighbor of
%the one with index @x idxNBest) and returns the updated versions of @x
%graphVector and @x pqOpen.
function [graphVector,pqOpen]=graph_expandElement(graphVector,idxNBest,idxX,idxGoal,pqOpen)
ncost=graphVector(idxNBest).neighborsCost(graphVector(idxNBest).neighbors==idxX);
if ~priority_isMember(pqOpen,idxX)
graphVector(idxX).g = graphVector(idxNBest).g + ncost;
graphVector(idxX).backpointer = idxNBest;
f_x = graphVector(idxX).g + graph_heuristic(graphVector,idxX,idxGoal);
pqOpen = priority_insert(pqOpen,idxX,f_x);
elseif graphVector(idxNBest).g+ncost<graphVector(idxX).g
graphVector(idxX).g = graphVector(idxNBest).g + ncost;
graphVector(idxX).backpointer = idxNBest;
end
%This function corresponds to lines it:expansion-start-- it:expansion-end in
%Algorithm alg:astar.