计算锥形角中的最近点
Computing nearest point in a cone of angle
图一
上图中，有i_1、i_2、i_33个agent。对于每个代理，我知道速度 (v_x, v_y) 和位置 (x,y)。
我希望计算视锥内最近的代理，比如代理的 x 度。 （上图 i_1 的视锥）
在上图中，考虑i_1：计算结果应该是i_2。
我天真的做法是先计算i_1的方向tan(vx/xy)。现在用 i_1 的坐标计算一条方向线。然后向右旋转 22.5 线，向左旋转 22.5 线。现在从左到右迭代步长=~0.01 度，并计算每个方向上与代理的距离的交点。以最小距离存储代理。
但这是非常低效的。我将不胜感激更好的方法。
我找到了不同的方法；它包括以下步骤：
- 使用其速度分量计算每个代理的航向
- 确定由定义圆锥体的线所成的角度（其宽度定义为输入）
- 定义锥体的最大范围（如果你需要一个"infinite"锥体，你可以指定一个相对于模拟场景的"size"更大的范围）
- 用"closing"圆锥
构建一个三角形
- 通过评估第 i 个智能体是否在三角形内（使用
inpolygon）来评估 "i-th Agent is inside" 条件
- 计算圆锥体（三角形）内每个 Agent 相对于当前 Agent 的距离
- 找到最小距离的代理
此方法已在以下代码中实现。
它的实现实际上只需要几行代码（大部分代码用于绘图/调试目的，可以删除）
代码由两个嵌套循环组成：
- 时间模拟
- 模拟每个时间步的代理分析
代码的输出是一个结构数组，每个代理一个。
结构字段是：
- name=代理的名称
- ag_inside=对于每个模拟步骤：逻辑索引对应
Agent ==> [0 1 1 0 1] Agent 2、3 和 5 在圆锥体中
- closest_ag：对于每个模拟步骤，最近的 Agent
的 ID
- dist_closest_ag：对于每个模拟步骤最近的Agent
的距离
- t：模拟时间
- pos：对于每个模拟步骤，Agent
的位置 (x,y)
代码末尾有一个输出用法示例。
仿真如图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
图一
上图中，有i_1、i_2、i_33个agent。对于每个代理，我知道速度 (v_x, v_y) 和位置 (x,y)。
我希望计算视锥内最近的代理，比如代理的 x 度。 （上图 i_1 的视锥）
在上图中，考虑i_1：计算结果应该是i_2。
我天真的做法是先计算i_1的方向tan(vx/xy)。现在用 i_1 的坐标计算一条方向线。然后向右旋转 22.5 线，向左旋转 22.5 线。现在从左到右迭代步长=~0.01 度，并计算每个方向上与代理的距离的交点。以最小距离存储代理。
但这是非常低效的。我将不胜感激更好的方法。
我找到了不同的方法；它包括以下步骤：
- 使用其速度分量计算每个代理的航向
- 确定由定义圆锥体的线所成的角度（其宽度定义为输入）
- 定义锥体的最大范围（如果你需要一个"infinite"锥体，你可以指定一个相对于模拟场景的"size"更大的范围）
- 用"closing"圆锥 构建一个三角形
- 通过评估第 i 个智能体是否在三角形内（使用
inpolygon）来评估 "i-th Agent is inside" 条件 - 计算圆锥体（三角形）内每个 Agent 相对于当前 Agent 的距离
- 找到最小距离的代理
此方法已在以下代码中实现。
它的实现实际上只需要几行代码（大部分代码用于绘图/调试目的，可以删除）
代码由两个嵌套循环组成：
- 时间模拟
- 模拟每个时间步的代理分析
代码的输出是一个结构数组，每个代理一个。
结构字段是：
- name=代理的名称
- ag_inside=对于每个模拟步骤：逻辑索引对应 Agent ==> [0 1 1 0 1] Agent 2、3 和 5 在圆锥体中
- closest_ag：对于每个模拟步骤，最近的 Agent 的 ID
- dist_closest_ag：对于每个模拟步骤最近的Agent 的距离
- t：模拟时间
- pos：对于每个模拟步骤，Agent 的位置 (x,y)
代码末尾有一个输出用法示例。
仿真如图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