计算角锥的最近点

Question

图1

在上图中，有3个代理，即i_1，i_2和i_3。对于每个代理，我知道速度（v_x, v_y）和位置（x,y）。

我希望在视锥内计算最近的代理，并说明代理的x度。 （上面描述的i_1）

的视锥

在上图中，考虑到i_1：计算应该导致i_2。

我天真的方法是首先计算i_1的{​​{1}}方向。现在使用tan(vx/xy)坐标计算方向线。然后向右旋转i_1行，向左旋转22.5。现在从左到右迭代，步长=〜22.5度，并计算每个方向的交点与距离代理的距离。以最小距离存储代理。

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

Answer 1

我发现了一种不同的方法;它包括以下步骤：

• 使用其速度组件计算每个代理的标题
• 识别由线条定义的角度（该宽度定义为输入）
• 定义锥体的最大范围（如果您需要“无限”锥体，您可以指定相对于模拟场景的“大小”的范围大）
• 通过“关闭”圆锥
来构建三角形
• 通过评估第i个Agent是否在三角形内（使用inpolygon）
来评估“第i个Agent在里面”状态
• 评估锥体（三角形）内每个代理相对于当前代理
的距离
• 找到距离最小的座席

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

它的实现只需几行代码（代码的大部分内容用于绘图/调试目的，可以删除）

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

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

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

struct filds是：

• name =代理商的名称
• ag_inside =对于每个模拟步骤：对应的逻辑索引          代理==＆gt; [0 1 1 0 1]代理2,3和5是锥形
• nearest_ag：对于每个模拟步骤，最近的代理的ID
• dist_closest_ag：对于每个模拟步骤，最近的代理
的距离
• 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