0001 0002 0003 function [positionRoad_no, node1Node2Road_noCost] = insertRoad(positionRoad_no, node1Node2Road_noCost, roadNo, pxNo) 0004 firstRoad = [] ; secondRoad = [] ; 0005 [maxNo, maxNo_idx] = max(positionRoad_no(:,4)); 0006 idx_row = find(positionRoad_no(:,4) == roadNo); 0007 idx_row_rest = find(positionRoad_no(:,4) ~= roadNo); 0008 newPositionRoad_no = [positionRoad_no(idx_row_rest, :)]; 0009 [roadNoCost, pxNoCost] = findRoadPix(pxNo(1),pxNo(2),positionRoad_no); 0010 if length(pxNoCost) < 3 0011 firstRoadCost = 1; 0012 else 0013 firstRoadCost = pxNoCost(3); 0014 end; 0015 idx_row_cost = find(node1Node2Road_noCost(:,5) == roadNo); 0016 secondRoadCost = node1Node2Road_noCost(idx_row_cost,6) - firstRoadCost; 0017 node1 = node1Node2Road_noCost(idx_row_cost, 1:2); 0018 node2 = node1Node2Road_noCost(idx_row_cost, 3:4); 0019 node1Node2Road_noCost = node1Node2Road_noCost(find(node1Node2Road_noCost(:,5) ~= roadNo), :); 0020 firstRoad = []; 0021 secondRoad = []; 0022 changeToOtherRoad = 0; 0023 for i = 1:length(idx_row) 0024 if (positionRoad_no(idx_row(i),1) == pxNo(1))&&(positionRoad_no(idx_row(i),2) == pxNo(2) ) 0025 changeToOtherRoad = 1; 0026 end; 0027 if (changeToOtherRoad == 0) 0028 firstRoad = [firstRoad; positionRoad_no(idx_row(i),1:3) roadNo]; 0029 else 0030 if (length(firstRoad) > 0) 0031 secondRoad = [secondRoad; positionRoad_no(idx_row(i),1:3) maxNo+1]; 0032 else 0033 secondRoad = [secondRoad; positionRoad_no(idx_row(i),1:3) roadNo]; 0034 end; 0035 end; 0036 end; 0037 addRoad = [firstRoad ; secondRoad]; 0038 positionRoad_no = [newPositionRoad_no; addRoad]; 0039 [maxNo, maxNo_idx] = max(positionRoad_no(:,4)); 0040 0041 if length(firstRoad) > 0 0042 node1Node2Road_noCost = [node1Node2Road_noCost; node1 [pxNo(1) pxNo(2)] firstRoad(1,4) firstRoadCost]; 0043 end; 0044 if length(secondRoad) > 0 0045 node1Node2Road_noCost = [node1Node2Road_noCost; [pxNo(1) pxNo(2)] node2 secondRoad(1,4) secondRoadCost]; 0046 end; 0047 0048 positionRoad_no = positionRoad_no; node1Node2Road_noCost = node1Node2Road_noCost; 0049