findShortestPath

 copy and remove road but store into oldRoads and oldCosts 
 make new road by changing road pix into node
 cost to node 1 could be part of road pix 
 C(n1->newNode) = px(3)
 C(newNode-> n2) = px(3)
 Three cases
 1) Find closest road and then deixtras
 2) On road already
 3) It is closer to us by using only grasfield
 Good indication is taking same road, and strictly is when C(dejxtras) < C(Astar_search) 
 RS 041212 [d, pi_] = Edijkstra(C,M); (renamed function dijkstra)

0001 
0002 
0003 function [cost, nodes, positionRoad_no, node1Node2Road_noCost] = findShortestPath(roadPoints, positionRoad_no, enemy_pos, friendly_pos,  nodes, node1Node2Road_noCost, costMap) 
0004  
0005 % copy and remove road but store into oldRoads and oldCosts
0006 % make new road by changing road pix into node
0007 % cost to node 1 could be part of road pix
0008 % C(n1->newNode) = px(3)
0009 % C(newNode-> n2) = px(3)
0010 % Three cases
0011 % 1) Find closest road and then deixtras
0012 % 2) On road already
0013 % 3) It is closer to us by using only grasfield
0014 % Good indication is taking same road, and strictly is when C(dejxtras) < C(Astar_search)
0015 % RS 041212 [d, pi_] = Edijkstra(C,M); (renamed function dijkstra)
0016 M = 1000;
0017 cost =0; 
0018 nodes = [enemy_pos(1:2); nodes];
0019 
0020 [roadNo,cost_this1, pxNo1] = findClosestRoad(roadPoints, positionRoad_no, enemy_pos, costMap, friendly_pos);
0021 %disp('closest road'), pxNo1
0022 %friendly_pos
0023 %pxNo1
0024 if (friendly_pos(1) == pxNo1(1))&&(friendly_pos(2) == pxNo1(2))
0025   cost = cost_this1;
0026   nodes =[nodes; friendly_pos(1:2)];
0027   positionRoad_no = positionRoad_no; 
0028   node1Node2Road_noCost = node1Node2Road_noCost;
0029   % This road is not inserted:(
0030   return;
0031 else    
0032   nodes = [nodes; pxNo1(1) pxNo1(2)];
0033   [positionRoad_no, node1Node2Road_noCost] = insertRoad(positionRoad_no, node1Node2Road_noCost, roadNo, pxNo1);
0034   positionRoad_no = positionRoad_no; node1Node2Road_noCost = node1Node2Road_noCost;
0035   [roadNo,cost_this2, pxNo2] = findClosestRoad(roadPoints, positionRoad_no, friendly_pos, costMap, enemy_pos);
0036   nodes = [nodes; pxNo2(1) pxNo2(2)];
0037   [positionRoad_no, node1Node2Road_noCost] = insertRoad(positionRoad_no, node1Node2Road_noCost, roadNo, pxNo2);
0038   positionRoad_no = positionRoad_no; node1Node2Road_noCost = node1Node2Road_noCost;
0039   nodes = [nodes; friendly_pos(1:2)];
0040   %Add costs to reach roads
0041   node1Node2Road_noCost = [node1Node2Road_noCost; enemy_pos(1) enemy_pos(2) pxNo1(1) pxNo1(2) (max(node1Node2Road_noCost(:,5))+1) cost_this1];
0042   node1Node2Road_noCost = [node1Node2Road_noCost; friendly_pos(1) friendly_pos(2) pxNo2(1) pxNo2(2) (max(node1Node2Road_noCost(:,5))+1) cost_this2];
0043   C = getCostMatrix(node1Node2Road_noCost, nodes);
0044   %make cost matrix
0045   [d, pi_] = Edijkstra(C,M);
0046   d = d; pi_ = pi_; k = 0;
0047   cost = d(length(d));
0048 end;