voro.m

%%%%% 与えられた半径内のボロノイ領域を求める関数 %%%%%
% 引数のリスト
    % pos:エージェント群の位置[x,y]
    % R:ボロノイ領域を求める領域の半径
    % i:ボロノイ領域を求めるエージェントのインデックス
    % Q:フィールドの大きさ[x,y]
    % dx:微小要素の大きさ（x方向）
    % dy:微小要素の大きさ（y方向）

% 出力するもの
    % v:ボロノイ領域（配列内の1の要素がボロノイ領域を表す）
    % x_l:vの左下の要素のインデックス（x）
    % y_u:vの左下の要素のインデックス（y）
    % q_max:ボロノイ領域内の各点からエージェントiまでの最大距離
    
function [v,x_l,y_u,q_max] = voro(pos,R,i,Q,dx,dy)
    % エージェントiの座標を保存
    X=pos(i,1);                 %x座標
    Y=pos(i,2);                 %y座標
    
    % エージェントiから半径R内にいるエージェントを求める
    dis=(pos(:,1)-X).^2+(pos(:,2)-Y).^2;            %エージェントiと他のエージェントとの距離を測る
    D=pos((dis<R^2)&(dis~=0),:);                    %半径R内にいるエージェントを求める

    % 半径R内にエージェントがいない場合，リターンする
    if isempty(D)==1                            
        v=0;                                        %ボロノイ領域なし
        x_l=1;
        y_u=1;
        q_max=0;
        return
    end

    % エージェントiと他のエージェントとの垂直二等分線を求める
    a=-(D(:,1)-X)./(D(:,2)-Y);                              %傾き
    b=(D(:,2).^2-Y^2+D(:,1).^2-X^2)./(2*(D(:,2)-Y));        %切片
    c=(Y-a*X-b)>0;                                          %エージェントiがその線の上or下どちらにいるか

    % フィールドQの中でどこを調べるかを決める（エージェントiの位置を中心とした一辺2Rの正方形）
    x_l=floor((X-R)/dx);            %検査領域の左端
    if x_l<1
        x_l=1;                      %Qからはみ出た場合はQの左端から
    end
    
    x_r=ceil((X+R)/dx);             %検査領域の右端
    if x_r>(Q(1)/dx)
        x_r=floor(Q(1)/dx);         %Qからはみ出た場合はQの右端から
    end
    
    y_u=floor((Y-R)/dy);            %検査領域の下端
    if y_u<1
        y_u=1;                      %Qからはみ出た場合はQの下端から
    end
    
    y_o=ceil((Y+R)/dy);             %検査領域の上端
    if y_o>(Q(2)/dy)
        y_o=floor(Q(2)/dy);         %Qからはみ出た場合はQの上端から
    end
    
    y_m=ceil(Y/dy);                 %検査領域の中央（y方向）

    % ボロノイ領域を求める（準備）
    v=zeros((y_o-y_u+1),(x_r-x_l+1));       %ボロノイ領域（検査領域に対応する大きさの配列）
    x=((x_l:x_r)-1)*dx+dx/2;                %検査領域におけるx方向のインデックスを座標へ変換
    A=a*x;                                  %上で求めた垂直二等分線の傾きとx座標の積
    B=b*ones(1,length(x));                  %上で求めた垂直二等分線の切片
    C=c*ones(1,length(x));                  %上で求めた垂直二等分線とエージェントiの位置関係
    q_max=0;                                %エージェントiからの最大距離


    % ボロノイ領域を求める（エージェントiの位置から上方向に調べていく）
    j_y=y_m;                                %調べる場所のy方向のインデックス
    v_x=ones(1,length(x));                  %その場所がボロノイ領域なら1を要素にもつ行ベクトル
    
        % 検査領域すべてを調べるか，v_xの要素がすべて0になる（それより上はボロノイ領域でないため）まで調べる
        while (j_y<=y_o)&&(max(v_x)~=0)
            y=(j_y-1)*dy+dy/2;              %y方向のインデックスを座標へ変換
            d=(y-A-B)>0;                    %調べている点と垂直二等分線の位置関係
            e=sqrt((x-X).^2+(y-Y).^2);      %エージェントiから各点までの距離
            v_x=(~any(C-d))&(e<R);          %垂直二等分線との位置関係が同じでかつ半径R内に入っていれば，その点はボロノイ領域
            v((j_y-y_u+1),:)=v_x;           %ボロノイ領域の情報を更新
            if max(e.*v_x)>q_max
                q_max=max(e.*v_x);          %上で求めた距離e中で最大距離q_maxよりも大きいものがあればq_maxを更新
            end
            j_y=j_y+1;                      %インデックスを1つ更新して1つ上を調べる
        end

    % ボロノイ領域を求める（エージェントiの位置から下方向に調べていく）
    j_y=y_m-1;                              %調べる場所のy方向のインデックス
    v_x=ones(1,length(x));                  %その場所がボロノイ領域なら1を要素にもつ行ベクトル
    
        % 検査領域すべてを調べるか，v_xの要素がすべて0になる（それより下はボロノイ領域でないため）まで調べる
        while (j_y>=y_u)&&(max(v_x)~=0)
            y=(j_y-1)*dy+dy/2;              %y方向のインデックスを座標へ変換
            d=(y-A-B)>0;                    %調べている点と垂直二等分線の位置関係
            e=sqrt((x-X).^2+(y-Y).^2);      %エージェントiから各点までの距離
            v_x=(~any(C-d))&(e<R);          %垂直二等分線との位置関係が同じでかつ半径R内に入っていれば，その点はボロノイ領域
            v((j_y-y_u+1),:)=v_x;           %ボロノイ領域の情報を更新
            if max(e.*v_x)>q_max
                q_max=max(e.*v_x);          %上で求めた距離e中で最大距離q_maxよりも大きいものがあればq_maxを更新
            end
            j_y=j_y-1;                      %インデックスを1つ更新して1つ下を調べる
        end

end