findPathPart

 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 isParticleOnRoad = 0 (false) otherwize true 
 RS 041212 [d, pi_] = Edijkstra(C,M); (renamed function dijkstra)

0001 
0002 
0003 function [gVec, cost, nodes] = findPathPart(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 isParticleOnRoad = 0 (false) otherwize true
0016 % RS 041212 [d, pi_] = Edijkstra(C,M); (renamed function dijkstra)
0017 
0018 M = 1000;
0019 cost =0; 
0020 nodes = [enemy_pos(1:2); nodes];
0021 [roadNo, pxNo1] = findRoadPix(enemy_pos(1),enemy_pos(2),positionRoad_no);
0022 isParticleOnRoad = roadNo;
0023 if roadNo == 0
0024     [roadNo,cost_this1, pxNo1] = findClosestRoad(roadPoints, positionRoad_no, enemy_pos, costMap, friendly_pos);
0025 else
0026    cost_this1 = 1;
0027 end;
0028 if (friendly_pos(1) == pxNo1(1))&&(friendly_pos(2) == pxNo1(2))
0029   cost = cost_this1;
0030   % This road is not inserted:(
0031   gVec = friendly_pos(1:2) - enemy_pos(1:2);
0032   return;
0033 else    
0034   nodes = [nodes; pxNo1(1) pxNo1(2)];
0035   [positionRoad_no, node1Node2Road_noCost] = insertRoad(positionRoad_no, node1Node2Road_noCost, roadNo, pxNo1);
0036   positionRoad_no = positionRoad_no; node1Node2Road_noCost = node1Node2Road_noCost;
0037   [roadNo, pxNo2] = findRoadPix(enemy_pos(1),enemy_pos(2),positionRoad_no);
0038   if roadNo == 0
0039     [roadNo,cost_this2, pxNo2] = findClosestRoad(roadPoints, positionRoad_no, friendly_pos, costMap, enemy_pos);
0040   else
0041       cost_this2 = 1;
0042   end;
0043   nodes = [nodes; pxNo2(1) pxNo2(2)];
0044   [positionRoad_no, node1Node2Road_noCost] = insertRoad(positionRoad_no, node1Node2Road_noCost, roadNo, pxNo2);
0045   positionRoad_no = positionRoad_no; node1Node2Road_noCost = node1Node2Road_noCost;
0046   nodes = [nodes; friendly_pos(1:2)];
0047   %Add costs to reach roads
0048   node1Node2Road_noCost = [node1Node2Road_noCost; enemy_pos(1) enemy_pos(2) pxNo1(1) pxNo1(2) (max(node1Node2Road_noCost(:,5))+1) cost_this1];
0049   node1Node2Road_noCost = [node1Node2Road_noCost; friendly_pos(1) friendly_pos(2) pxNo2(1) pxNo2(2) (max(node1Node2Road_noCost(:,5))+1) cost_this2];
0050   C = getCostMatrix(node1Node2Road_noCost, nodes);
0051   %make cost matrix
0052   [d, pi_] = Edijkstra(C,M);
0053   cost = d(length(d));
0054   path = [];
0055   path_idx =  length(pi_); 
0056   Paths_idxs = [];
0057   pi_ = pi_;
0058 end;
0059 
0060 % Particles are moving in road direction
0061 
0062 third_node_idx = [];
0063 t = length(pi_) + 1;
0064 second_node_idx = pi_(length(pi_));
0065 if (t > 3)&(second_node_idx > 0)
0066     while t > 0
0067         if pi_(second_node_idx) == 1 
0068             break;
0069         else
0070             third_node_idx = second_node_idx;
0071             second_node_idx = pi_(second_node_idx);
0072         end;
0073             t = t - 1;
0074      end;
0075  else
0076   gVec = friendly_pos(1:2) - enemy_pos(1:2);   
0077   return;
0078  end;       
0079  if isempty(third_node_idx)||(isParticleOnRoad ~= 0)
0080     gVec = nodes(second_node_idx,:)- enemy_pos(1:2);
0081  else
0082     gVec = nodes(third_node_idx,:)- enemy_pos(1:2);
0083  end;