図1
i_1上の図では、 、 、 の 3 つのエージェントがi_2ありi_3ます。各エージェントの速度 ( v_x, v_y) と位置 ( x,y) がわかっています。
エージェントの程度など、視野の円錐内で最も近いエージェントを計算したいと思いxます。(上記の円錐視野i_1)
上の図では、i_1 を考慮すると、計算の結果は i_2 になるはずです。
私の単純なアプローチは、最初にi_1's の方向を取ることを計算することtan(vx/xy)です。i_1次に、の座標を使用して方向の線を計算し ます。22.5次に、ラインを左右に回転させ22.5ます。ここで、ステップ サイズ = ~ 度で左から右に反復し0.01、エージェントからの距離で各方向の交点を計算します。最短距離でエージェントを保管します。
しかし、これは非常に非効率的です。より良いアプローチをいただければ幸いです。
別のアプローチを見つけました。次の手順で構成されます。
inpolygon)このアプローチは、次のコードに実装されています。
その実装には実際には数行のコードしか必要ありません (コードの大部分はプロット/デバッグ用であり、削除できます)。
このコードは、2 つのネストされたループで構成されています。
コードの出力は、Agent ごとに 1 つの構造体の配列です。
構造体フィールドは次のとおりです。
コードの最後に、出力の使用例があります。
シミュレーションを図 1 に示します。
出力の例を図 2 に示します。
お役に立てれば。
% Create the "figure"
figure
% Define the maximum range of the FOR (the cone)
R=100;
% Define the angular size of the FOR (FOR_2)
FOR_2=45;
% Define half FOR (for left and right)
FOR=FOR_2/2;
FOR_R=FOR*pi/180;
% Define Agent speed complnents
% vx vy
av=[ ...
10 10
3 10
0 -10
-10 0
-3 7
];
% Define Agent initial ppositions
% x y
ag=[ ...
0 0
15 3
15 50
45 30
30 20
];
% Identify the number of Agent in the simulation
na=length(ag);
% Split variables speeed and initial positions
av_x=av(:,1);
av_y=av(:,2);
ag_x=ag(:,1);
ag_y=ag(:,2);
% Compute Agents heading based on speed components
heading=atan2(av_y,av_x);
heading_d=heading*180/pi;
% Assign generic names to the Agents
for i=1:na
ag_names{1,i}=['ag_-' num2str(i)];
end
% Define cone righr and left parts (wrt the heading)
% x y
con_m=[ ...
R*cos(heading-FOR_R) R*sin(heading-FOR_R)
];
con_mx=con_m(:,1);
con_my=con_m(:,2);
con_p=[ ...
R*cos(heading+FOR_R) R*sin(heading+FOR_R)
];
con_px=con_p(:,1);
con_py=con_p(:,2);
% Create and setup axes
axis([0 50 0 50]);
hold on
grid on
% Define a generic dt for simulation
dt=.1;
% Initialize output data struct
% Other fields are set in the following:
% ag_log fields:
% name=name of the Agent
% ag_inside=for each simulation step: logical index correspinding the
% the Agent ==> [0 1 1 0 1] Agent 2, 3 and 5 are insede the cone
% closest_ag: for each simulation step the ID of the closest Agent
% dist_closest_ag: for each simulation step the distance of the closest Agent
% t: the simulation time
% pos: for each simulation step the position (x,y) of the Agent
for i=1:na
ag_log(i).name=['ag_' num2str(i)];
end
% Time loop of the simulation
for t_loop=1:30
% Plot Agent position mark (plot in a loop to have the handle of each mark
% - just for debug purpose)
for i=1:na
ag_mark(i)=plot(ag_x(i),ag_y(i),'s','markeredgecolor','r','markerfacecolor','r');
end
% Agent analysis loop: for each Agent Vs the others
for i=1:na
% Plot Agent cone (centre, right, left)
% ag_cone(1,i)=plot([ag_x(i) R*cos(heading(i))+ag_x(i)],[ag_y(i) R*sin(heading(i))+ag_y(i)]);
ag_cone(1,i)=plot([ag_x(i) con_mx(i)+ag_x(i)],[ag_y(i) con_my(i)+ag_y(i)],'r');
ag_cone(2,i)=plot([ag_x(i) con_px(i)+ag_x(i)],[ag_y(i) con_py(i)+ag_y(i)],'r');
% The i-th Agent is inside current Agent cone if it is inside the polygong
% represented by the cone
is_inside=inpolygon(ag_x,ag_y,[ag_x(i) con_px(i)+ag_x(i) con_mx(i)+ag_x(i)], ...
[ag_y(i) con_py(i)+ag_y(i) con_my(i)+ag_y(i)]);
% Exclude current Agent
is_inside(i)=0;
% Assign list of Agents inside the curent Agent cone to the output struct
ag_log(i).ag_inside(t_loop,:)=is_inside;
% Evalaute the distances of the "inside" Agents (if any)
if(sum(is_inside) ~= 0)
% Marker of the "inside" Agent is turned into blue (for debug purpose)
set(ag_mark(is_inside),'markerfacecolor','b');
d=(((ag_x(i)-ag_x).^2+(ag_y(i)-ag_y).^2).^.5);
% Exclude the distance of the Agents outside the cone of the current Agent
% (this automatically excludes the distance of the current agent from itself
% - ref. above comment "exclude current Agent")
d(is_inside == 0)=NaN;
% Find the the "inside" Agent with the minimun distance
[min_dist,ag_idx]=min(d);
% Assign the distance and the Agent id to the output struct
ag_log(i).closest_ag(t_loop)=ag_idx;
ag_log(i).dist_closest_ag(t_loop)=min_dist;
% Marker of the closest "inside" Agent is turned into green (for debug
% purpose)
set(ag_mark(ag_idx),'markerfacecolor','g');
else
% If no Agent inside the current Agent cone, set output to "-1"
ag_log(i).closest_ag(t_loop)=-1;
ag_log(i).dist_closest_ag(t_loop)=-1;
% disp(['Noting inside ag ' num2str(i)]);
end
% Assign simulation time to the output struct
ag_log(i).t(t_loop)=dt*(t_loop-1);
% Assign current Agentt position to the output struct
ag_log(i).pos(t_loop,1)=ag_x(i);
ag_log(i).pos(t_loop,2)=ag_y(i);
% Marker of all the Agent are reset into green (for debug purpose)
set(ag_mark,'markerfacecolor','r')
end
% Update Agent position
ag_x=ag_x+av_x*dt;
ag_y=ag_y+av_y*dt;
% Pause only for debug purpose
pause(.2)
% Generate figure name and save it
% f_name=['ag_fig_' num2str(t_loop)]
% print('-djpeg50',f_name)
% Delete cones and marker form the axes before next iteration
delete(ag_cone);
delete(ag_mark);
end
% Example of usage of the output data
% For each Agent
for i=1:na
figure('numbertitle','off','name',[ag_log(i).name ' LOG'])
ag_idx=1:na;
% Exclude current Agent from plot
ag_idx(i)=[];
subplot(2,1,1)
% Plot (wrt simulatin time) the status of the other Agents (1: inside, 0:
% outside)
plot(ag_log(i).t,ag_log(i).ag_inside(:,ag_idx),'linewidth',2);
ylim([0 1.3]);
grid on
title('AG inside FOR')
legend(ag_names{ag_idx},-1);
subplot(2,1,2)
% Plot the ID of the Closest Agent (-1: nome)
plot(ag_log(i).t,ag_log(i).closest_ag,'linewidth',2);
title('Closest AG (-1 = NONE)')
legend('AG idx',-1)
grid on
% Generate figure name and save it
% f_name=['ag_log_fig_' num2str(i)]
% print('-djpeg50',f_name)
end
