propagateParticle

0001 
0002 function [new_particle] = propagateParticle(particle)
0003 
0004 global struct_own_forces;
0005 global image_struct; 
0006 global roads_struct;
0007 global localAreaofInterest;
0008 global roads_struct;
0009 %take only ONE rand (own_force) attraktor
0010 friendly_pos = struct_own_forces(round(1 + (length(struct_own_forces) - 1)*rand)).pos;
0011 enemy_pos = round(particle(1:2));
0012 [gVec, cost, nodes] = ...
0013 findPathPart(roads_struct.roadPoints, roads_struct.positionRoad_no, [enemy_pos 1], [friendly_pos 1],  roads_struct.nodes, roads_struct.node1Node2Road_noCost, roads_struct.costMap);
0014 vel_dir = [cos(particle(4)) sin(particle(4))];
0015 if norm(gVec) > 0
0016     gVec = gVec./norm(gVec);
0017     resVec = gVec + vel_dir;
0018 else
0019     resVec = vel_dir;
0020 end;
0021 if norm(resVec) > 0
0022     resVec = resVec./norm(resVec);
0023 else
0024     resVec = vel_dir;
0025 end;
0026 new_point = [resVec(1)*particle(3) + enemy_pos(1); resVec(2)*particle(3) + enemy_pos(2)];  
0027 %Terrain dependency
0028 mat_coeff = round((localAreaofInterest - 1)/2); 
0029 new_point = round(new_point);
0030 if inMatrix(roads_struct.costMap, (new_point(1) - mat_coeff), (new_point(2) - mat_coeff))&&inMatrix(roads_struct.costMap, (new_point(1) + mat_coeff), (new_point(2) + mat_coeff))
0031     localCostMatrix = roads_struct.costMap((new_point(1) - mat_coeff):(new_point(1) + mat_coeff), (new_point(2) - mat_coeff):(new_point(2) + mat_coeff));
0032     min = inf; 
0033     min_idx = [];
0034     %hitta minimipunkt
0035     for j = 1:length(localCostMatrix(:,1))
0036         [min_candidate,idx] = min(localCostMatrix(:,j));
0037         if min_candidate < min
0038             min = min_candidate;
0039             min_idx = [idx, j];
0040         end;
0041     end;
0042     new_point(1) = new_point(1) - mat_coeff + min_idx(1) - 1;
0043     new_point(2) = new_point(2) - mat_coeff + min_idx(2) - 1;
0044     new_particle = [new_point(1:2)' particle(3) atan2(resVec(2), resVec(1))];
0045 else
0046     new_particle = [];
0047 end;
0048