Computing nearest point in a cone of angle

Question

Figure 1

In the above figure, there are 3 agents namely i_1, i_2 and i_3. For each agent, I know the velocity (v_x, v_y) and the position (x,y).

I wish to compute the nearest agent within the cone of vision say x degrees of an agent. (Cone of vision depicted above for i_1)

In above figure, considering the i_1: the computation should result in i_2.

My naive approach is to first calculate i_1's direction taking tan(vx/xy). Now compute a line in direction with i_1's coordinates. Then rotating the line 22.5 right and 22.5 left. Now iterate from left to right with step size=~0.01 degrees and compute the intersection at each direction with distance from agent. Store the agent with least distance.

But this is highly inefficient. I would appreciate better approaches.

Answer 1

I've found a different approach; it consists of the following steps:

• compute the heading of each Agent using its speed components
• identify the angles made by the lines defnig the cone (its width is defined as input)
• define a maximum range for the cone (in case you need an "infinite" cone, you can specify a range way big with respect to the "size" of the simulation scenario)
• build a triangle by "closing" the cone
• assess the "i-th Agent is inside" condition by evaluating if the i-th Agent is inside the triangle (using inpolygon)
• evaluate the distance of each Agent inside the cone (triangle) with respect to the current Agent
• find the Agent with the minimum distance

This approach has been implemented in the following code.

Its implementation acltually takes few line of code (the most part of the code is for plotting / debug purpose and can be removed)

The code consists of two nested loop:

• time simulation
• analysis of the Agents at each time step of the simulation

The output of the code is an array of struct, one for each Agent.

The struct filds are:

• 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

At the end of the code there is an example of usage of the output.

The simulation is shown in Figure 1.

The example of the output is presented in Figure 2.

Hope this helps.

% 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