%PGraph Graph class % % g = PGraph() create a 2D, planar, undirected graph % g = PGraph(n) create an n-d, undirected graph % % Provides support for graphs that: % - are directed % - are embedded in coordinate system % - have symmetric cost edges (A to B is same cost as B to A) % - have no loops (edges from A to A) % - have vertices are represented by integers vid % - have edges are represented by integers, eid % % Methods:: % % Constructing the graph:: % g.add_node(coord) add vertex, return vid % g.add_edge(v1, v2) add edge from v1 to v2, return eid % g.setcost(e, c) set cost for edge e % g.setdata(v, u) set user data for vertex v % g.data(v) get user data for vertex v % g.clear() remove all vertices and edges from the graph % % Information from graph:: % g.edges(v) list of edges for vertex v % g.cost(e) cost of edge e % g.neighbours(v) neighbours of vertex v % g.component(v) component id for vertex v % g.connectivity() number of edges for all vertices % % Display:: % % g.plot() set goal vertex for path planning % g.highlight_node(v) highlight vertex v % g.highlight_edge(e) highlight edge e % g.highlight_component(c) highlight all nodes in component c % g.highlight_path(p) highlight nodes and edge along path p % % g.pick(coord) vertex closest to coord % % g.char() convert graph to string % g.display() display summary of graph % % Matrix representations:: % g.adjacency() adjacency matrix % g.incidence() incidence matrix % g.degree() degree matrix % g.laplacian() Laplacian matrix % % Planning paths through the graph:: % g.Astar(s, g) shortest path from s to g % g.goal(v) set goal vertex, and plan paths % g.path(v) list of vertices from v to goal % % Graph and world points:: % g.coord(v) coordinate of vertex v % g.distance(v1, v2) distance between v1 and v2 % g.distances(coord) return sorted distances from coord to all vertices % g.closest(coord) vertex closest to coord % % Object properties (read only):: % g.n number of vertices % g.ne number of edges % g.nc number of components % % Notes:: % - Graph connectivity is maintained by a labeling algorithm and this % is updated every time an edge is added. % - Nodes and edges cannot be deleted. % - Support for edge direction is rudimentary. % Peter Corke 8/2009. % TODO: % be able to delete nodes, must update connectivity classdef PGraph < handle properties (SetAccess=private, GetAccess=private) vertexlist % vertex coordinates, columnwise, vertex number is the column number edgelist % 2xNe matrix, each column is vertex index of edge start and end edgelen % length (cost) of this edge curLabel % current label ncomponents % number of components labels % label of each vertex (1xN) labelset % set of all labels (1xNc) goaldist % distance from goal, after planning userdata % per vertex data, cell array ndims % number of coordinate dimensions, height of vertices matrix verbose measure % distance measure: 'Euclidean', 'SE2' end properties (Dependent) n % number of nodes/vertices ne % number of edges nc % number of components end methods function g = PGraph(ndims, varargin) %PGraph.PGraph Graph class constructor % % G=PGraph(D, OPTIONS) is a graph object embedded in D dimensions. % % Options:: % 'distance',M Use the distance metric M for path planning which is either % 'Euclidean' (default) or 'SE2'. % 'verbose' Specify verbose operation % % Note:: % - Number of dimensions is not limited to 2 or 3. % - The distance metric 'SE2' is the sum of the squares of the difference % in position and angle modulo 2pi. % - To use a different distance metric create a subclass of PGraph and % override the method distance_metric(). if nargin < 1 ndims = 2; % planar by default end g.ndims = ndims; opt.distance = 'Euclidean'; opt = tb_optparse(opt, varargin); g.clear(); g.verbose = opt.verbose; g.measure = opt.distance; g.userdata = {}; end function n = get.n(g) %Pgraph.n Number of vertices % % G.n is the number of vertices in the graph. % % See also PGraph.ne. n = numcols(g.vertexlist); end function ne = get.ne(g) %Pgraph.ne Number of edges % % G.ne is the number of edges in the graph. % % See also PGraph.n. ne = numcols(g.edgelist); end function ne = get.nc(g) %Pgraph.nc Number of components % % G.nc is the number of components in the graph. % % See also PGraph.component. ne = g.ncomponents; end function clear(g) %PGraph.clear Clear the graph % % G.clear() removes all vertices, edges and components. g.labelset = zeros(1, 0); g.labels = zeros(1, 0); g.edgelist = zeros(2, 0); g.edgelen = zeros(1, 0); g.vertexlist = zeros(g.ndims, 0); g.ncomponents = 0; g.curLabel = 0; end function v = add_node(g, coord, varargin) %PGraph.add_node Add a node % % V = G.add_node(X) adds a node/vertex with coordinate X (Dx1) and % returns the integer node id V. % % V = G.add_node(X, V2) as above but connected by a directed edge from vertex V % to vertex V2 with cost equal to the distance between the vertices. % % V = G.add_node(X, V2, C) as above but the added edge has cost C. % % See also PGraph.add_edge, PGraph.data, PGraph.getdata. if length(coord) ~= g.ndims error('coordinate length different to graph coordinate dimensions'); end % append the coordinate as a column in the vertex matrix g.vertexlist = [g.vertexlist coord(:)]; v = numcols(g.vertexlist); g.labels(v) = g.newlabel(); if g.verbose fprintf('add node (%d) = ', v); fprintf('%f ', coord); fprintf('\n'); end % optionally add an edge if nargin > 2 g.add_edge(v, varargin{:}); end end function u = setdata(g, v, u) %PGraph.setdata Set user data for node % % G.setdata(V, U) sets the user data of vertex V to U which can be of any % type such as number, struct, object or cell array. % % See also PGraph.data. g.userdata{v} = u; end function u = data(g, v) %PGraph.data Get user data for node % % U = G.data(V) gets the user data of vertex V which can be of any % type such as number, struct, object or cell array. % % See also PGraph.setdata. u = g.userdata{v}; end function add_edge(g, v1, v2, d) %PGraph.add_edge Add an edge % % E = G.add_edge(V1, V2) adds a directed edge from vertex id V1 to vertex id V2, and % returns the edge id E. The edge cost is the distance between the vertices. % % E = G.add_edge(V1, V2, C) as above but the edge cost is C. % cost C. % % Note:: % - Graph connectivity is maintained by a labeling algorithm and this % is updated every time an edge is added. % % See also PGraph.add_node, PGraph.edgedir. if g.verbose fprintf('add edge %d -> %d\n', v1, v2); end for vv=v2(:)' g.edgelist = [g.edgelist [v1; vv]]; if (nargin < 4) || isempty(d) d = g.distance(v1, vv); end g.edgelen = [g.edgelen d]; if g.labels(vv) ~= g.labels(v1) g.merge(g.labels(vv), g.labels(v1)); end end end function c = component(g, v) %PGraph.component Graph component % % C = G.component(V) is the id of the graph component c = []; for vv=v tf = ismember(g.labelset, g.labels(vv)); c = [c find(tf)]; end end % which edges contain v % elist = g.edges(v) function e = edges(g, v) %PGraph.edges Find edges given vertex % % E = G.edges(V) is a vector containing the id of all edges from vertex id V. % % See also PGraph.edgedir. e = [find(g.edgelist(1,:) == v) find(g.edgelist(2,:) == v)]; end function dir = edgedir(g, v1, v2) %PGraph.edgedir Find edge direction % % D = G.edgedir(V1, V2) is the direction of the edge from vertex id V1 % to vertex id V2. % % If we add an edge from vertex 3 to vertex 4 % g.add_edge(3, 4) % then % g.edgedir(3, 4) % is positive, and % g.edgedir(4, 3) % is negative. % % See also PGraph.add_node, PGraph.add_edge. n = g.edges(v1); if any(ismember( g.edgelist(2, n), v2)) dir = 1; elseif any(ismember( g.edgelist(1, n), v2)) dir = -1; else dir = 0; end end function v = vertices(g, e) %PGraph.vertices Find vertices given edge % % V = G.vertices(E) return the id of the vertices that define edge E. v = g.edgelist(:,e); end function [n,c] = neighbours(g, v) %PGraph.neighbours Neighbours of a vertex % % N = G.neighbours(V) is a vector of ids for all vertices which are % directly connected neighbours of vertex V. % % [N,C] = G.neighbours(V) as above but also returns a vector C whose elements % are the edge costs of the paths corresponding to the vertex ids in N. e = g.edges(v); n = g.edgelist(:,e); n = n(:)'; n(n==v) = []; % remove references to self if nargout > 1 c = g.cost(e); end end function [n,c] = neighbours_d(g, v) %PGraph.neighbours_d Directed neighbours of a vertex % % N = G.neighbours_d(V) is a vector of ids for all vertices which are % directly connected neighbours of vertex V. Elements are positive % if there is a link from V to the node, and negative if the link % is from the node to V. % % [N,C] = G.neighbours_d(V) as above but also returns a vector C whose elements % are the edge costs of the paths corresponding to the vertex ids in N. e = g.edges(v); n = [-g.edgelist(1,e) g.edgelist(2,e)]; n(abs(n)==v) = []; % remove references to self if nargout > 1 c = g.cost(e); end end function d = cost(g, e) %PGraph.cost Cost of edge % % C = G.cost(E) is the cost of edge id E. d = g.edgelen(e); end function d = setcost(g, e, c) %PGraph.cost Set cost of edge % % G.setcost(E, C) set cost of edge id E to C. g.edgelen(e) = c; end function p = coord(g, v) %PGraph.coord Coordinate of node % % X = G.coord(V) is the coordinate vector (Dx1) of vertex id V. p = g.vertexlist(:,v); end function c = connectivity(g) %PGraph.connectivity Graph connectivity % % C = G.connectivity() is a vector (Nx1) with the number of edges per % vertex. % % The average vertex connectivity is % mean(g.connectivity()) % % and the minimum vertex connectivity is % min(g.connectivity()) for k=1:g.n c(k) = length(g.edges(k)); end end function plot(g, varargin) %PGraph.plot Plot the graph % % G.plot(OPT) plots the graph in the current figure. Nodes % are shown as colored circles. % % Options:: % 'labels' Display vertex id (default false) % 'edges' Display edges (default true) % 'edgelabels' Display edge id (default false) % 'NodeSize',S Size of vertex circle (default 8) % 'NodeFaceColor',C Node circle color (default blue) % 'NodeEdgeColor',C Node circle edge color (default blue) % 'NodeLabelSize',S Node label text sizer (default 16) % 'NodeLabelColor',C Node label text color (default blue) % 'EdgeColor',C Edge color (default black) % 'EdgeLabelSize',S Edge label text size (default black) % 'EdgeLabelColor',C Edge label text color (default black) % 'componentcolor' Node color is a function of graph component colorlist = 'bgmyc'; % show vertices holdon = ishold; hold on % parse options opt.componentcolor = false; opt.labels = false; opt.edges = true; opt.edgelabels = false; opt.NodeSize = 8; opt.NodeFaceColor = 'b'; opt.NodeEdgeColor = 'b'; opt.NodeLabelSize = 16; opt.NodeLabelColor = 'b'; opt.EdgeColor = 'k'; opt.EdgeLabelSize = 8; opt.EdgeLabelColor = 'k'; [opt,args] = tb_optparse(opt, varargin); % set default color if none specified if ~isempty(args) mcolor = args{1}; else mcolor = 'b'; end % show the vertices as filled circles for i=1:g.n % for each node if opt.componentcolor j = mod( g.component(i)-1, length(colorlist) ) + 1; c = colorlist(j); else c = mcolor; end args = {'LineStyle', 'None', ... 'Marker', 'o', ... 'MarkerFaceColor', opt.NodeFaceColor, ... 'MarkerSize', opt.NodeSize, ... 'MarkerEdgeColor', opt.NodeEdgeColor }; if g.ndims == 3 plot3(g.vertexlist(1,i), g.vertexlist(2,i), g.vertexlist(3,i), args{:}); else plot(g.vertexlist(1,i), g.vertexlist(2,i), args{:}); end end % show edges if opt.edges for e=g.edgelist v1 = g.vertexlist(:,e(1)); v2 = g.vertexlist(:,e(2)); if g.ndims == 3 plot3([v1(1) v2(1)], [v1(2) v2(2)], [v1(3) v2(3)], ... 'Color', opt.EdgeColor); else plot([v1(1) v2(1)], [v1(2) v2(2)], ... 'Color', opt.EdgeColor); end end end % show the edge labels if opt.edgelabels for i=1:numcols(g.edgelist) e = g.edgelist(:,i); v1 = g.vertexlist(:,e(1)); v2 = g.vertexlist(:,e(2)); text('String', sprintf(' %g', g.cost(i)), ... 'Position', (v1 + v2)/2, ... 'HorizontalAlignment', 'left', ... 'VerticalAlignment', 'middle', ... 'FontUnits', 'pixels', ... 'FontSize', opt.EdgeLabelSize, ... 'Color', opt.EdgeLabelColor); end end % show the labels if opt.labels for i=1:numcols(g.vertexlist) text('String', sprintf(' %d', i), ... 'Position', g.vertexlist(:,i), ... 'HorizontalAlignment', 'left', ... 'VerticalAlignment', 'middle', ... 'FontUnits', 'pixels', ... 'FontSize', opt.NodeLabelSize, ... 'Color', opt.NodeLabelColor); end end if ~holdon hold off end end function v = pick(g) %PGraph.pick Graphically select a vertex % % V = G.pick() is the id of the vertex closest to the point clicked % by the user on a plot of the graph. % % See also PGraph.plot. [x,y] = ginput(1); v = g.closest([x; y]); end function goal(g, vg) %PGraph.goal Set goal node % % G.goal(VG) computes the cost of reaching every vertex in the graph connected % to the goal vertex VG. % % Notes:: % - Combined with G.path performs a breadth-first search for paths to the goal. % % See also PGraph.path, PGraph.Astar. % cost is total distance from goal g.goaldist = Inf*ones(1, numcols(g.vertexlist)); g.goaldist(vg) = 0; g.descend(vg); end function p = path(g, v) %PGraph.path Find path to goal node % % P = G.path(VS) is a vector of vertex ids that form a path from % the starting vertex VS to the previously specified goal. The path % includes the start and goal vertex id. % % To compute path to goal vertex 5 % g.goal(5); % then the path, starting from vertex 1 is % p1 = g.path(1); % and the path starting from vertex 2 is % p2 = g.path(2); % % Notes:: % - Pgraph.goal must have been invoked first. % - Can be used repeatedly to find paths from different starting points % to the goal specified to Pgraph.goal(). % % See also PGraph.goal, PGraph.Astar. p = [v]; while g.goaldist(v) ~= 0 v = g.next(v); p = [p v]; end end function d = distance(g, v1, v2) %PGraph.distance Distance between vertices % % D = G.distance(V1, V2) is the geometric distance between % the vertices V1 and V2. % % See also PGraph.distances. d = g.distance_metric( g.vertexlist(:,v1), g.vertexlist(:,v2)); end function [d,k] = distances(g, p) %PGraph.distances Distances from point to vertices % % D = G.distances(X) is a vector (1xN) of geometric distance from the point % X (Dx1) to every other vertex sorted into increasing order. % % [D,W] = G.distances(P) as above but also returns W (1xN) with the % corresponding vertex id. % % See also PGraph.closest. d = g.distance_metric(p(:), g.vertexlist); [d,k] = sort(d, 'ascend'); end function [c,dn] = closest(g, p) %PGraph.closest Find closest vertex % % V = G.closest(X) is the vertex geometrically closest to coordinate X. % % [V,D] = G.closest(X) as above but also returns the distance D. % % See also PGraph.distances. d = g.distance_metric(p(:), g.vertexlist); [mn,c] = min(d); if nargin > 1 dn = mn; end end function display(g) %PGraph.display Display graph % % G.display() displays a compact human readable representation of the % state of the graph including the number of vertices, edges and components. % % See also PGraph.char. loose = strcmp( get(0, 'FormatSpacing'), 'loose'); if loose disp(' '); end disp([inputname(1), ' = ']) disp( char(g) ); end % display() function s = char(g) %PGraph.char Convert graph to string % % S = G.char() is a compact human readable representation of the % state of the graph including the number of vertices, edges and components. s = ''; s = strvcat(s, sprintf(' %d dimensions', g.ndims)); s = strvcat(s, sprintf(' %d vertices', g.n)); s = strvcat(s, sprintf(' %d edges', numcols(g.edgelist))); s = strvcat(s, sprintf(' %d components', g.ncomponents)); end %% convert graphs to matrix representations function L = laplacian(g) %Pgraph.laplacian Laplacian matrix of graph % % L = G.laplacian() is the Laplacian matrix (NxN) of the graph. % % Notes:: % - L is always positive-semidefinite. % - L has at least one zero eigenvalue. % - The number of zero eigenvalues is the number of connected components % in the graph. % % See also PGraph.adjacency, PGraph.incidence, PGraph.degree. L = g.degree() - (g.adjacency() > 0); end function D = degree(g) %Pgraph.degree Degree matrix of graph % % D = G.degree() is a diagonal matrix (NxN) where element D(i,i) is the number % of edges connected to vertex id i. % % See also PGraph.adjacency, PGraph.incidence, PGraph.laplacian. D = diag( g.connectivity() ); end function A = adjacency(g) %Pgraph.adjacency Adjacency matrix of graph % % A = G.adjacency() is a matrix (NxN) where element A(i,j) is the cost % of moving from vertex i to vertex j. % % Notes:: % - Matrix is symmetric. % - Eigenvalues of A are real and are known as the spectrum of the graph. % - The element A(I,J) can be considered the number of walks of one % edge from vertex I to vertex J (either zero or one). The element (I,J) % of A^N are the number of walks of length N from vertex I to vertex J. % % See also PGraph.degree, PGraph.incidence, PGraph.laplacian. A = zeros(g.n, g.n); for i=1:g.n [n,c] = g.neighbours(i); for j=1:numel(n) A(i,n(j)) = c(j); A(n(j),i) = c(j); end end end function I = incidence(g) %Pgraph.degree Incidence matrix of graph % % IN = G.incidence() is a matrix (NxNE) where element IN(i,j) is % non-zero if vertex id i is connected to edge id j. % % See also PGraph.adjacency, PGraph.degree, PGraph.laplacian. I = zeros(g.n, numcols(g.edgelist)); for i=1:g.n for n=g.edges(i) I(i,n) = 1; end end end %% these are problematic, dont advertise them % % removing an edge may divide the graph into 2 components, this is expensive % to check and currently not implemented function delete_edge(g, e) g.edgelist(:,e) = []; % really need to check if the two halves are connected, is expensive % could use path planner end function delete_node(g, v) el = g.edges(v); el % make the column invalid, really should remove it but this % requires changing all the edgelist entries, and the vertex % numbers will change... g.vertexlist(:,v) = [NaN; NaN]; g.delete_edge(el); g.n = g.n - 1; end function highlight_node(g, verts, varargin) %PGraph.highlight_node Highlight a node % % G.highlight_node(V, OPTIONS) highlights the vertex V with a yellow marker. % If V is a list of vertices then all are highlighted. % % Options:: % 'NodeSize',S Size of vertex circle (default 12) % 'NodeFaceColor',C Node circle color (default yellow) % 'NodeEdgeColor',C Node circle edge color (default blue) % % See also PGraph.highlight_edge, PGraph.highlight_path, PGraph.highlight_component. hold on % parse options opt.NodeSize = 12; opt.NodeFaceColor = 'y'; opt.NodeEdgeColor = 'b'; [opt,args] = tb_optparse(opt, varargin); markerprops = {'LineStyle', 'None', ... 'Marker', 'o', ... 'MarkerFaceColor', opt.NodeFaceColor, ... 'MarkerSize', opt.NodeSize, ... 'MarkerEdgeColor', opt.NodeEdgeColor }; for v=verts if g.ndims == 3 plot3(g.vertexlist(1,v), g.vertexlist(2,v), g.vertexlist(3,v), ... markerprops{:}); else plot(g.vertexlist(1,v), g.vertexlist(2,v), markerprops{:}); end end end function highlight_component(g, c, varargin) %PGraph.highlight_component Highlight a graph component % % G.highlight_component(C, OPTIONS) highlights the vertices that belong to % graph component C. % % Options:: % 'NodeSize',S Size of vertex circle (default 12) % 'NodeFaceColor',C Node circle color (default yellow) % 'NodeEdgeColor',C Node circle edge color (default blue) % % See also PGraph.highlight_node, PGraph.highlight_edge, PGraph.highlight_component. nodes = find(g.labels == g.labelset(c)); for v=nodes g.highlight_node(v, varargin{:}); end end function highlight_edge(g, e, varargin) %PGraph.highlight_node Highlight a node % % G.highlight_edge(V1, V2) highlights the edge between vertices V1 and V2. % % G.highlight_edge(E) highlights the edge with id E. % % Options:: % 'EdgeColor',C Edge edge color (default black) % 'EdgeThickness',T Edge thickness (default 1.5) % % See also PGraph.highlight_node, PGraph.highlight_path, PGraph.highlight_component. % parse options opt.EdgeColor = 'k'; opt.EdgeThickness = 1.5; [opt,args] = tb_optparse(opt, varargin); hold on if (length(args) > 0) && isnumeric(args{1}) % highlight_edge(V1, V2) v1 = e; v2 = args{1}; v1 = g.vertexlist(:,v1); v2 = g.vertexlist(:,v2); else % highlight_edge(E) e = g.edgelist(:,e); v1 = g.vertexlist(:,e(1)); v2 = g.vertexlist(:,e(2)); end % create the line properties for the edges lineprops = { 'Color', opt.EdgeColor, ... 'LineWidth', opt.EdgeThickness }; if g.ndims == 3 plot3([v1(1) v2(1)], [v1(2) v2(2)], [v1(3) v2(3)], lineprops{:}); else plot([v1(1) v2(1)], [v1(2) v2(2)], lineprops{:}); end end function highlight_path(g, path) %PGraph.highlight_path Highlight path % % G.highlight_path(P, OPTIONS) highlights the path defined by vector P % which is a list of vertices comprising the path. % % Options:: % 'NodeSize',S Size of vertex circle (default 12) % 'NodeFaceColor',C Node circle color (default yellow) % 'NodeEdgeColor',C Node circle edge color (default blue) % 'EdgeColor',C Node circle edge color (default black) % % See also PGraph.highlight_node, PGraph.highlight_edge, PGraph.highlight_component. g.highlight_node(path); % highlight the edges for i=1:numel(path)-1 v1 = path(i); v2 = path(i+1); g.highlight_edge(v1, v2); end end function [path,cost] = Astar(g, vstart, vgoal) %PGraph.Astar path finding % % PATH = G.Astar(V1, V2) is the lowest cost path from vertex V1 to % vertex V2. PATH is a list of vertices starting with V1 and ending % V2. % % [PATH,C] = G.Astar(V1, V2) as above but also returns the total cost % of traversing PATH. % % Notes:: % - Uses the efficient A* search algorithm. % % References:: % - Correction to "A Formal Basis for the Heuristic Determination of Minimum Cost Paths". % Hart, P. E.; Nilsson, N. J.; Raphael, B. % SIGART Newsletter 37: 28-29, 1972. % % See also PGraph.goal, PGraph.path. % The set of vertices already evaluated. closedset = []; % The set of tentative vertices to be evaluated, initially containing the start node openset = [vstart]; came_from = []; % The map of navigated vertices. g_score(vstart) = 0; % Cost from start along best known path. h_score(vstart) = g.distance(vstart, vgoal); % Estimated total cost from start to goal through y. f_score(vstart) = g_score(vstart) + h_score(vstart); while ~isempty(openset) % current := the node in openset having the lowest f_score[] value [mn,k] = min(f_score(openset)); vcurrent = openset(k); if vcurrent == vgoal path = []; p = vgoal; while true path = [p path]; p = came_from(p); if p == 0 break; end end if nargout > 1 cost = f_score(vgoal); end return end %remove current from openset openset = setdiff(openset, vcurrent); %add current to closedset closedset = union(closedset, vcurrent); for neighbour = g.neighbours(vcurrent) if ismember(neighbour, closedset) continue; end tentative_g_score = g_score(vcurrent) + ... g.distance(vcurrent,neighbour); if ~ismember(neighbour, openset) %add neighbor to openset openset = union(openset, neighbour); h_score(neighbour) = g.distance(neighbour, vgoal); tentative_is_better = true; elseif tentative_g_score < g_score(neighbour) tentative_is_better = true; else tentative_is_better = false; end if tentative_is_better came_from(neighbour) = vcurrent; g_score(neighbour) = tentative_g_score; f_score(neighbour) = g_score(neighbour) + h_score(neighbour); end end end path = []; end end % method methods (Access='protected') % private methods % depth first function descend(g, vg) % get neighbours and their distance for nc = g.neighbours2(vg); vn = nc(1); d = nc(2); newcost = g.goaldist(vg) + d; if isinf(g.goaldist(vn)) % no cost yet assigned, give it this one g.goaldist(vn) = newcost; %fprintf('1: cost %d <- %f\n', vn, newcost); descend(g, vn); else % it already has a cost if g.goaldist(vn) <= newcost continue; else g.goaldist(vn) = newcost; %fprintf('2: cost %d <- %f\n', vn, newcost); descend(g, vn); end end end end % breadth first function descend2(g, vg) % get neighbours and their distance for vn = g.neighbours2(vg); vn = nc(1); d = nc(2); newcost = g.goaldist(vg) + d; if isinf(g.goaldist(vn)) % no cost yet assigned, give it this one g.goaldist(vn) = newcost; fprintf('1: cost %d <- %f\n', vn, newcost); descend(g, vn); elseif g.goaldist(vn) > newcost % it already has a cost g.goaldist(vn) = newcost; end end for vn = g.neighbours(vg); descend(g, vn); end end function l = newlabel(g) g.curLabel = g.curLabel + 1; l = g.curLabel; g.ncomponents = g.ncomponents + 1; g.labelset = union(g.labelset, l); end % merge label1 and label2, lowest label dominates function merge(g, l1, l2) % get the dominant and submissive labels ldom = min(l1, l2); lsub = max(l1, l2); % change all instances of submissive label to dominant one g.labels(g.labels==lsub) = ldom; % reduce the number of components g.ncomponents = g.ncomponents - 1; % and remove the submissive label from the set of all labels g.labelset = setdiff(g.labelset, lsub); end function nc = neighbours2(g, v) e = g.edges(v); n = g.edgelist(:,e); n = n(:)'; n(n==v) = []; % remove references to self c = g.cost(e); nc = [n; c]; end function d = distance_metric(g, x1, x2) % distance between coordinates x1 and x2 using the relevant metric % x2 can be multiple points represented by multiple columns switch g.measure case 'Euclidean' d = colnorm( bsxfun(@minus, x1, x2) ); case 'SE2' d = bsxfun(@minus, x1, x2); d(3,:) = angdiff(d(3,:)); d = colnorm( d ); otherwise error('unknown distance measure', g.measure); end end function vn = next(g, v) % V = G.next(VS) return the id of a node connected to node id VS % that is closer to the goal. n = g.neighbours(v); [mn,k] = min( g.goaldist(n) ); vn = n(k); end end % private methods end % classdef