%------------------------------------------------------------------------
%                       SWARM SIMULATION PROGRAM 
%                        DESIGNED BY Jun Lu
%                   Intelligent and Complex Systems Lab
%               HuaZhong University of Science and Technology
%------------------------------------------------------------------------

%This program works on the line of sight testing
%判斷LOS
function G=TC(obs_cent_x,obs_cent_y,obs_rad,x)%輸入障礙物的圓點（obs_cent_x,obs_cent_y），半徑obs_rad，x為機器人的初始化坐標
xc=obs_cent_x;
yc=obs_cent_y;
Rob_num=length(x);%機器人的個數
G=zeros(Rob_num);%zeros:產生全為0的矩陣 ，產生機器人個數的全0矩陣
dl=sqrt((x(1,1)-xc)^2 + (x(1,2)-yc)^2);

for i=1:Rob_num
    for j=1:Rob_num
        xs=x(i,1);%第1行，第1列    x為機器人的初始化坐標，  n*2:n行：表示機器人的個數； 共2列，第一列為x軸坐標，第二列為y軸坐標
        ys=x(i,2);      %coordinates of starting points 起點坐標
        xe=x(j,1);
        ye=x(j,2);      %coordinates of ending points 終點坐標
        A=ys-ye;
        B=xe-xs;
        C=ye*xs-xe*ys;  %A,B,C are coefficients of line passing through (xs,ys) and (xe,ye)

    
            if ((i~=j) & (xe>=xs)) % here only test those points which have bigger x value;只考慮每個機器人前面的機器人
                if xs==xe   %并排
                    Dist_C=abs(xc-xs);%  障礙物圓點到起點與終點連線的距離
                    cross_x=xs;    %障礙物到連線的垂線的垂點
                    cross_y=yc;
                elseif ys==ye
                    Dist_C=abs(yc-ys);
                    cross_x=xc;
                    cross_y=ys;
                else
                    Dist_C=abs(A*xc+B*yc+ye*xs-xe*ys)/sqrt(A^2+B^2);
                    cross_x=-(A*B*yc-B*B*xc+A*C)/(A^2+B^2);
                    cross_y=-(A*B*xc-A*A*yc+B*C)/(A^2+B^2);
                end  
                if Dist_C>=obs_rad  % the distance check makes sure no cross point with the circle;如果障礙物圓點到起點與終點連線的距離大于障礙物半徑，則相連
                    G(i,j)=1;
                    G(j,i)=1;
                else %調用check_on_line函數，判斷垂點是在起點和終點的連線上，還是在連線的延長線上
                    sign_on_line=check_on_line(xs,ys,xe,ye,cross_x,cross_y) ;% check if the point is inside (xs,ys) and (xe,ye)
                    if (sign_on_line==0)
                        G(i,j)=1;
                        G(j,i)=1;
                    end
                end
            end
        end
    end

if(dl<obs_rad)%如果leader在障礙物內
    for i=1:Rob_num
        G(1,i)=0;
        G(i,1)=0;
    end
end