judge.m

规则障碍物环境下的机器人路径规划matlab源码！

function P=judge(P,d,n,O,Xlu)
t=0;
[n1,n2]=size(P);
[o1,o2]=size(O);
n3=fix(n2/2);
p=zeros(n1,n3);
for i=1:n1
    for j=2:2:n2-1
        p(i,j/2)=ymadjp2([P(i,j),P(i,j+1)],d);
    end
end
q=p;
for i=1:n1
         for j=2:2:n2-3
             p=q;
        for x=1:n1
             for y=1:n3
             coordinate=ymadjp1(p(x,y),d);%将p中各序号转换为坐标
             P(x,2*y)=coordinate(1);
             P(x,2*y+1)=coordinate(2);
             end
        end
        point0(1)=P(i,j);
        point0(2)=P(i,j+1);
        point1(1)=P(i,j+2);
        point1(2)=P(i,j+3);
       t0=ymadjp2([point0(1),point0(2)],d);
       t1=ymadjp2([point1(1),point1(2)],d);
          if t1==0
           break
          end
        for k=1:o1
          
            if   point0(1)<=O(k,1) & point0(2)<=O(k,3) %point0位于左下方，角为0到90度
                
                    if atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,4)-point0(2))/(O(k,1)-point0(1)))&...
                            atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,3)-point0(2))/(O(k,2)-point0(1)))
                        if point1(1)>=O(k,1)&point1(1)<=O(k,2)&point1(2)>=O(k,4)%point1位于上方
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
                           point=[O(k,1),O(k,4)];
                           q(i,j/2+1)=ymadjp2(point,d)-1;
                       elseif point1(1)>=O(k,2)&point1(2)>=O(k,3)%point1位于右方和右上方
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
                           point=[O(k,2),O(k,3)];
                           q(i,j/2+1)=ymadjp2(point,d)-d;
                        elseif point1(1)>O(k,1)&point1(2)<O(k,2)&point1(1)>O(k,3)&point1(2)<O(k,4)
                            q(i,j/2+1)=ymadjp2([O(k,1),O(k,3)],d)-d;
                           
                       end 
                   end
            elseif point0(1)>=O(k,1)&point0(1)<=O(k,2)&point0(2)<=O(k,3)%point0位于下方
                    
                        if point1(1)<=O(k,1)&point1(2)>=O(k,3)%point1位于左方及左上方
                           if  atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,3)-point0(2))/(O(k,1)-point0(1)))
                               for f=j/2+1:n3-1
                               q(i,f+1)=p(i,f);
                               end
                               point=[O(k,1),O(k,3)];
                               q(i,j/2+1)=ymadjp2(point,d)-1;
                           end
                         elseif point1(1)>=O(k,2)&point1(2)>=O(k,3) %point1位于右方及右上方
                           if  atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,3)-point0(2))/(O(k,2)-point0(1)))
                             for f=j/2+1:n3-1
                             q(i,f+1)=p(i,f);
                             end
                             point=[O(k,2),O(k,3)];
                             q(i,j/2+1)=ymadjp2(point,d)-d;
                           end
                           elseif point1(1)>=O(k,1)&point1(1)<=O(k,2)&point1(2)>=O(k,4) %point1位于上方
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
%                            eq1=vpa('y-point1(2)-((O(k,4)-point1(2))/(O(k,2)-point1(1)))*(x-point1(1))=0');
%                            eq2=vpa('y-point0(2)-((O(k,3)-point0(2))/(O(k,2)-point0(1)))*(x-point0(1))=0');
                           [x,y]=avail(point1(1),point1(2),O(k,2),O(k,4),point0(1),point0(2),O(k,2),O(k,3));
                            if (double(x)<0|double(y)<0)||(double(x)>10|double(y)>10)
%                            eq1=vpa('y-point1(2)-((O(k,4)-point1(2))/(O(k,1)-point1(1)))*(x-point1(1))=0');
%                            eq2=vpa('y-point0(2)-((O(k,3)-point0(2))/(O(k,1)-point0(1)))*(x-point0(1))=0');
                           [x,y]=avail(point1(1),point1(2),O(k,1),O(k,4),point0(1),point0(2),O(k,1),O(k,3));
                            end
                            q(i,j/2+1)=ymadjp2([x,y],d)+1;
                        elseif  point1(1)>O(k,1)&point1(2)<O(k,2)&point1(1)>O(k,3)&point1(2)<O(k,4)
                            q(i,j/2+1)=ymadjp2([O(k,2),O(k,3)],d)-d;
                         end
                     
            elseif point0(1)>=O(k,2)&point0(2)<=O(k,3)%point0位于右下方
               if  atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,4)-point0(2))/(O(k,2)-point0(1)))&...
                        atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,3)-point0(2))/(O(k,1)-point0(1)))
                    if point1(1)<=O(k,2)&point1(2)>=O(k,4)%point1位于上方及左上方
                      for f=j/2+1:n3-1
                      q(i,f+1)=p(i,f);
                      end
                      point=[O(k,2),O(k,4)];
                      q(i,j/2+1)=ymadjp2(point,d);
                    elseif point1(1)<=O(k,1)&point1(2)>=O(k,3)&point1(2)<=O(k,4)%point1位于左方
                         for f=j/2+1:n3-1
                         q(i,f+1)=p(i,f);
                         end
                         point=[O(k,1),O(k,3)];
                         q(i,j/2+1)=ymadjp2(point,d)-d; 
                    elseif  point1(1)>O(k,1)&point1(2)<O(k,2)&point1(1)>O(k,3)&point1(2)<O(k,4)
                        q(i,j/2+1)=ymadjp2([O(k,2),O(k,4)],d);
                    end
                end
            elseif point0(1)>=O(k,2)&point0(2)>=O(k,3)&point0(2)<=O(k,4)%point0位于右方
                  
                        if point1(1)<=O(k,2)&point1(2)>=O(k,4)%point1位于左上方和上方
                            if atan((point1(2)-point0(2))/(point1(1)-point0(1)))>atan((O(k,4)-point0(2))/(O(k,2)-point0(1)))
                            for f=j/2+1:n3-1
                            q(i,f+1)=p(i,f);
                            end
                            point=[O(k,2),O(k,4)];
                            q(i,j/2+1)=ymadjp2(point,d) ;
                            end
                   elseif point1(1)<=O(k,2)&point1(2)<=O(k,3)%point1位于左下方和下方
                      if   atan((point1(2)-point0(2))/(point1(1)-point0(1)))<atan((O(k,3)-point0(2))/(O(k,2)-point0(1)))
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
                           point=[O(k,2),O(k,3)];
                           q(i,j/2+1)=ymadjp2(point,d)+1-d;
                      end
                  elseif point1(1)<=O(k,1)&point1(2)<=O(k,4)&point1(2)>=O(k,3)%point1位于左方
                           for f=j/2+1:n3-1
                           q(i,f+1)=p(i,f);
                           end
%                            eq1=vpa('y-point1(2)-((O(k,4)-point1(2))/(O(k,1)-point1(1)))*(x-point1(1))=0');
%                            eq2=vpa('y-point0(2)-((O(k,4)-point0(2))/(O(k,2)-point0(1)))*(x-point0(1))=0');
                           [x,y]=avail(point1(1),point1(2),O(k,1),O(k,4),point0(1),point0(2),O(k,2),O(k,4));
                           if (double(x)>10|double(y)>10)||(double(x)<0|double(y)<0)
%                            eq1=vpa('y-point1(2)-((O(k,3)-point1(2))/(O(k,1)-point1(1)))*(x-point1(1))=0');
%                            eq2=vpa('y-point0(2)-((O(k,3)-point0(2))/(O(k,2)-point0(1)))*(x-point0(1))=0');
                           [x,y]=avail(point1(1),point1(2),O(k,1),O(k,3),point0(1),point0(2),O(k,2),O(k,3));
                           end
                           q(i,j/2+1)=ymadjp2([x,y],d)+d;
                        elseif  point1(1)>O(k,1)&point1(2)<O(k,2)&point1(1)>O(k,3)&point1(2)<O(k,4)
                            q(i,j/2+1)=ymadjp2([O(k,2),O(k,4)],d);
                 end