angle_ok.m

来自「基于声纳信息的粒子滤波算法」· M 代码 · 共 27 行

function tt=angle_ok(x1,y1,x2,y2,Q)
   pi=3.1415926;
   if(Q<0)
       Q=Q+360*10;
   end
   angle=mod(Q,360);
     if((tan((angle./180)*3.14)>=0)&&(x2>x1)&&(y2>=y1)&&(angle>0)&&(angle<=90))
         tt=1;   
      else
          if(tan((angle./180)*3.14)<=0&&x2<x1&&y2>=y1&&angle>90&&angle<=180)
              tt=1;        
           else    
               if(tan((angle./180)*3.14)>=0&&x2<x1&&y2<=y1&&angle>180&&angle<=270)
                 tt=1;            
              else
                  if(tan((angle./180)*3.14)<=0&&x2>x1&&y2<=y1&&angle>270&&angle<=360)
                     tt=1;
                 else
                     tt=0;
                 end
               end
           end
       end