breadthFirst

crossingPoints = contains all crossing points
n = no of levels in breadth-first search
markedRoad = pixles that are visited
point = [x y depth]  
reachedCrossing = has value Yes if crossing is candidate

0001 
0002 function [reachedCrossing, markedRoad, nodes, node1Node2Road_noCost, positionRoad_no] = breadthFirst(roadPoints, point, crossingPoints, markedRoad, node1Node2Road_noCost, positionRoad_no, nodes, crossing)
0003 
0004 %crossingPoints = contains all crossing points
0005 %n = no of levels in breadth-first search
0006 %markedRoad = pixles that are visited
0007 %point = [x y depth]
0008 %reachedCrossing = has value Yes if crossing is candidate
0009 %
0010 reachedCrossing = 0;
0011 candidateList = [];
0012 crossP = []; 
0013 unexpanded = [point];
0014 reachedCrossing == 0;
0015 cost_old_point = 1;
0016 [sx0,sy0,sz0] = size(positionRoad_no); 
0017 if  sx0 >= 1
0018     rj = positionRoad_no(sx0,4) + 1;
0019 else
0020     rj = 1;
0021 end;
0022 positionRoad_no = [positionRoad_no; crossing 0 rj];
0023 [sx,sy] = size(unexpanded);
0024 while (sx > 0)&&(reachedCrossing == 0)
0025      p = unexpanded(1,:);
0026      positionRoad_no = [positionRoad_no; p(1) p(2) p(3) rj];
0027      crossP = nextCrossing(roadPoints, [p(1) p(2)], crossingPoints, markedRoad);
0028      [xc, yc] = size(crossP);
0029   if xc > 0
0030         reachedCrossing = 1;
0031          [sx,sy] = size(unexpanded);
0032          positionRoad_no = [positionRoad_no; p(1) p(2) p(3) rj; crossP(1) crossP(2) p(3)+1 rj ];
0033          node1Node2Road_noCost = [node1Node2Road_noCost; crossing(1,1) crossing(1,2) crossP rj p(3)+1];
0034          rj = rj + 1;
0035          return;
0036          if sx >= 2
0037             unexpanded = [ unexpanded(2:sx,:); candidateList];
0038         else 
0039             unexpanded = candidateList;
0040         end; 
0041         if sx < 1
0042          rj = rj + 1; 
0043          return;
0044         end;
0045    else
0046         reachedCrossing = 0;
0047    end; 
0048    [candidateList, markedRoad] = getCandidates(roadPoints, p, markedRoad);   
0049     [sx,sy] = size(unexpanded);
0050     if sx >= 2
0051         unexpanded = [ unexpanded(2:sx,:); candidateList];
0052     else 
0053         unexpanded = candidateList;
0054     end;       
0055    [sx,sy] = size(unexpanded);
0056    cost_old_point = roadPoints(p(1),p(2)) ;
0057 end;
0058 
0059 if (reachedCrossing == 0) 
0060  endnode = [p(1) p(2)];
0061  nodes = [nodes; p(1) p(2)];
0062   n = roadPoints(p(1),p(2))*p(3);
0063   node1Node2Road_noCost = [node1Node2Road_noCost; [crossing(1,1) crossing(1,2)]  endnode rj n+1];
0064 else
0065  nodes = nodes;
0066 end;
0067 markedRoad = markedRoad;
0068 positionRoad_no =  positionRoad_no;
0069 node1Node2Road_noCost =  node1Node2Road_noCost; 
0070 
0071 
0072 
0073 
0074  
0075 
0076        
0077