pathList

-----------------------RS040217-----------------------------------
 Returns ...

0001 
0002 
0003 function [roadNo, pxNo, path] = findClosestRoad(positionRoad_no, point, costMap, crossingPoints, markedNods)
0004 
0005 %-----------------------RS040217-----------------------------------
0006 % Returns ...
0007 
0008 
0009 %-------------------------------------------------------------------
0010 roadNo  = 0; pxNo = []; noChildren = 0;path = [];
0011 [newNode, roadNo, pxNo, noChildren] = getFieldCandiadte(positionRoad_no, [point 0], costMap);
0012 nodeList = [newNode 0];
0013 idx = 1;
0014 pathList = [newNode(1) newNode(2)];
0015 [sx1, sy1] = size(nodeList);
0016 while (roadNo == 0)||(sx1 < 1)
0017      [val, idx] = min(nodeList(:,3));
0018      nodeToExpand = nodeList(idx, :);
0019      [sx1, sy1] = size(nodeList);
0020      if sx1 > 0
0021       for i = 1:sx1
0022           if (i ~= idx)&&(noChildren == 1)
0023             nodeList(i,4) = 1;
0024           end;
0025       end;
0026      end;
0027      nodeList = newNodeList;    
0028      [sx1, sy1] = size(nodeList);
0029      [newNode, roadNo, pxNo, noChildren, markedNods] = getFieldCandiadte(positionRoad_no, nodeToExpand, costMap, markedNods);
0030      if (noChildren == 1)
0031          nodeList(idx,3) = inf;
0032      else
0033         nodeList = [nodeList; newNode];
0034      end;
0035      [sx2, sy2] = size(pathList);
0036      [sx3, sy3] = size(newNode);
0037      if sx3 > 0
0038         pathList = [pathList -1.*ones(sx2,2); pathList(idx,:) newNode(1) newNode(2)];
0039         nodeList = [nodeList; newNode];
0040      end;     
0041  end; 
0042  
0043  if (roadNo == 0)
0044      disp('Road not found')
0045      pause
0046  else
0047      v = [pathList(idx,:) newNode(1) newNode(2)];
0048      for i = 1:(length(v)-1)
0049          if v(i) > -1 
0050              path = [path; v(i) v(i+1)];
0051          end;
0052      end;
0053  end;
0054              
0055      
0056      
0057 
0058 
0059