Matlab 角锥中最近点的计算

图1

在上图中，有3个代理，即

i_1

、

i_2

和

i_3

。对于每个代理，我知道速度（

vux，vuy

）和位置（

x，y

）

我希望计算视野范围内最近的代理，比如代理的

x

度。（上面描述的

i_1

的视锥）

在上图中，考虑到i_1：计算结果应为i_2

我的简单方法是首先计算i_1的方向，即tan（vx/xy）。现在用

i_1

的坐标沿方向计算一条直线。然后向右旋转行

22.5

，向左旋转行

22.5

。现在以步长=~

0.01

度从左到右迭代，并计算每个方向与代理之间距离的交点。以最小距离存储代理

---

但这是非常低效的。我希望有更好的方法。

我找到了一种不同的方法；它包括以下步骤：

• 使用每个代理的速度组件计算其航向
• 识别圆锥体周围线条所形成的角度（其宽度定义为输入）
• 定义圆锥体的最大范围（如果需要“无限”圆锥体，可以指定一个相对于模拟场景“大小”大的范围）
• 通过“闭合”圆锥体来构建三角形
• 通过评估第i个代理是否在三角形内（使用

  inpolygon

  ）来评估“第i个代理在内部”情况
• 计算圆锥体（三角形）内每个代理相对于当前代理的距离
• 找到距离最小的代理

此方法已在以下代码中实现

它的实现实际上只需要几行代码（大部分代码用于打印/调试，可以删除）

代码由两个嵌套循环组成：

• 时间模拟
• 在模拟的每个时间步分析代理

代码的输出是一个struct数组，每个代理对应一个

结构文件包括：

• name=代理的名称
• ag_inside=对于每个模拟步骤：与 代理==>[0 1 0 1]代理2、3和5位于圆锥体的内侧
• 最近代理：对于每个模拟步骤，最近代理的ID
• 距离最近的距离：对于每个模拟步骤，最近代理的距离
• t:模拟时间
• pos：对于每个模拟步骤，代理的位置（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个代理是否在三角形内（使用

  inpolygon

  ）来评估“第i个代理在内部”情况
• 计算圆锥体（三角形）内每个代理相对于当前代理的距离
• 找到距离最小的代理

此方法已在以下代码中实现

它的实现实际上只需要几行代码（大部分代码用于打印/调试，可以删除）

代码由两个嵌套循环组成：

• 时间模拟
• 在模拟的每个时间步分析代理

代码的输出是一个struct数组，每个代理对应一个

结构文件包括：

• name=代理的名称
• ag_inside=对于每个模拟步骤：与 代理==>[0 1 0 1]代理2、3和5位于圆锥体的内侧
• 最近代理：对于每个模拟步骤，最近代理的ID
• 距离最近的距离：对于每个模拟步骤，最近代理的距离
• t:模拟时间
• pos：对于每个模拟步骤，代理的位置（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个代理是否在三角形内（使用

  inpolygon

  ）来评估“第i个代理在内部”情况
• 计算圆锥体（三角形）内每个代理相对于当前代理的距离
• 找到距离最小的代理

此方法已在以下代码中实现

它的实现实际上只需要几行代码（大部分代码用于打印/调试，可以删除）

代码由两个嵌套循环组成：

• 时间模拟
• 在模拟的每个时间步分析代理

代码的输出是一个struct数组，每个代理对应一个