function u = mytunnel(g,i,p1,p2,p3,rh,biaozhi,temptunnel)
global UavTeam
global UavHighway
global N

% %%%%%%%%%%%%%   为了区分WAIT和Tunnel，task不一样.....已修改，从temptunnel直接引入，避免错误
% if biaozhi == 1
%     temptunnel = currenttunnel - 1;
% else
%     temptunnel = currenttunnel;
% end
% %%%%%%%%%%%%%%%

Pcur  =  UavTeam.Team(g).Uav(i).CurrentPos;
Vcur  =  UavTeam.Team(g).Uav(i).Velocity;
rm = UavTeam.Team(g).Uav(i).r;
ra = UavTeam.Team(g).Uav(i).ra;
vmax = UavTeam.Team(g).Uav(i).vmax;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
At12 = eye(2) - (p1-p2)*(p1-p2)'/(norm(p1-p2)*norm(p1-p2));
At23 = eye(2) - (p2-p3)*(p2-p3)'/(norm(p2-p3)*norm(p2-p3));

% Velocity to line
k1 = 1;
el = Pcur + 1/UavTeam.gain*Vcur - p2;
Vl = -mysat(k1*At23*el,vmax);

% Velocity away other multicopters
Vm = [0;0];
for ggg = 1:N
    for l = 1:UavTeam.AvailableNumMax
        if i~=l && UavTeam.Team(ggg).Uav(l).state ~= 2
            k2 = 1; e = 0.000001; rs = e;
            ksimil = (Pcur - UavTeam.Team(ggg).Uav(l).CurrentPos) + 1/UavTeam.gain*(Vcur - UavTeam.Team(ggg).Uav(l).Velocity);
            temp = (1+e)*norm(ksimil) - (2*rm)*mys(norm(ksimil)/(2*rm),rs);
            bil = k2*dmysigma(norm(ksimil),2*rm,ra)/temp ...
                - k2*mysigma(norm(ksimil),2*rm,ra)*((1+e)-dmys(norm(ksimil)/(2*rm),rs))/(temp^2);
            % 分子为零的危险
            if ksimil == 0
            else
                Vm = Vm - bil*(ksimil/norm(ksimil));
            end
        end
    end
end

% Velocity within the tunnel
Vt = [0;0];
k3 = 1; e = 0.000001; rs = e;
ksihi = At12*((Pcur-p2) + 1/UavTeam.gain*Vcur);
temp1  = (rh-rm)/(norm(ksihi)+e);
temp2  = (rh-rm) - norm(ksihi)*mys(temp1,rs);
ci = k3*dmysigma(rh-norm(ksihi),rm,ra)/temp2 ...
    - k3*mysigma(rh-norm(ksihi),rm,ra)*(-mys(temp1,rs)+norm(ksihi)*dmys(temp1,rs)*(rh-rm)/(norm(ksihi)+e)/(norm(ksihi)+e))/(temp2^2);
% 分子为零的危险
if ksihi == 0
else
    Vt = Vt - ci*(ksihi/norm(ksihi));
end

% Sum of all velocities
if biaozhi==0
    u = mysat(Vl+Vm+Vt,vmax);
else
    %% 挡板！
    theta1 = theta(temptunnel,g);
    ph1x1=UavHighway.group(g).Highway(temptunnel).ph1(1)+UavHighway.group(g).Highway(temptunnel).rh*sin(theta1);
    ph1x2=UavHighway.group(g).Highway(temptunnel).ph2(1)+UavHighway.group(g).Highway(temptunnel).rh*sin(theta1);
    ph1y1=UavHighway.group(g).Highway(temptunnel).ph1(2)-UavHighway.group(g).Highway(temptunnel).rh*cos(theta1);
    ph1y2=UavHighway.group(g).Highway(temptunnel).ph2(2)-UavHighway.group(g).Highway(temptunnel).rh*cos(theta1);
    ph2x1=UavHighway.group(g).Highway(temptunnel).ph1(1)-UavHighway.group(g).Highway(temptunnel).rh*sin(theta1);
    ph2x2=UavHighway.group(g).Highway(temptunnel).ph2(1)-UavHighway.group(g).Highway(temptunnel).rh*sin(theta1);
    ph2y1=UavHighway.group(g).Highway(temptunnel).ph1(2)+UavHighway.group(g).Highway(temptunnel).rh*cos(theta1);
    ph2y2=UavHighway.group(g).Highway(temptunnel).ph2(2)+UavHighway.group(g).Highway(temptunnel).rh*cos(theta1);
    p1=[(ph1x1+ph1x2)/2;(ph1y1+ph1y2)/2];
    p2=[(ph2x1+ph2x2)/2;(ph2y1+ph2y2)/2];
    %% one more time
    p1=[(p1(1)+ph1x2)/2;(p1(2)+ph1y2)/2];
    p2=[(p2(1)+ph2x2)/2;(p2(2)+ph2y2)/2];
    %% one more time
    p1=[(p1(1)+ph1x2)/2;(p1(2)+ph1y2)/2];
    p2=[(p2(1)+ph2x2)/2;(p2(2)+ph2y2)/2];
    
    At12 = eye(2) - (p1-p2)*(p1-p2)'/(norm(p1-p2)*norm(p1-p2));
    
    Vd = [0;0];
    k3 = 1; e = 0.000001; rs = e;
    ksid = At12*((Pcur-p2) + 1/UavTeam.gain*Vcur);
    temp1  = (rh-rm)/(norm(ksid)+e);
    temp2  = (rh-rm) - norm(ksid)*mys(temp1,rs);
    di = k3*dmysigma(rh-norm(ksid),rm,ra)/temp2 ...
        - k3*mysigma(rh-norm(ksid),rm,ra)*(-mys(temp1,rs)+norm(ksid)*dmys(temp1,rs)*(rh-rm)/(norm(ksid)+e)/(norm(ksid)+e))/(temp2^2);
    Vd = Vd - di*(ksid/norm(ksid));
    % 分子为零的危险
    if ksid==0
    else
        Vd = Vd - di*(ksid/norm(ksid));
    end
    u = mysat(Vl+Vm+Vt+Vd,vmax);
end