|
function [src_x,src_y,des_x,des_y,dis,no]=fly_control(px,py,n)<BR> <BR> tx=ones(1,n); %%¶¨Òå¶þά¿Õ¼äµÄÁ½¸ö»®·Öp1ºÍp2<BR> ty=ones(1,n);<BR> tx1=ones(1,n);<BR> ty1=ones(1,n);<BR> no=-1;<BR> for i=1:n %%p1ºÍp2×ø±ê³õʼ»¯<BR> tx(i)=Inf;<BR> ty(i)=Inf;<BR> tx1(i)=Inf;<BR> ty1(i)=Inf;<BR> end<BR> if (n==1)<BR> dis=inf; %Ö»ÓÐÒ»¸öµãʱ,¾àÀëΪÎÞÇî´ó<BR> src_x=px(1); %¼Ç¼¸ÃµãµÄ×ø±ê<BR> src_y=py(1); <BR> no=1; <BR> return; %±êÖ¾,±íÃ÷¸Ã¿Õ¼äÖÐÖ»ÓÐÒ»¸öµã<BR> elseif (n==2) %ÓÐÁ½¸öµãʱ,¾àÀëΪ¶þÕßÖ®¼äµÄ¾àÀë<BR> dis=sqrt((px(1)-px(2))^2+(py(1)-py(2))^2);<BR> src_x=px(1); %¼Ç¼ÕâÁ½¸öµãµÄ×ø±ê<BR> src_y=py(1);<BR> des_x=px(2);<BR> des_y=py(2);<BR> no=2; %±êÖ¾,±íÃ÷¸Ã¿Õ¼äÖÐÖ»ÓÐÁ½¸öµã<BR> return;<BR> else %¶à¸öµãʱµÄÌØÊâÇé¿ö<BR> m=mean(px); %ÇóÖÐλµã<BR> <BR> n1=0; %»®·ÖÁ½¸ö¿Õ¼äp1ºÍp2<BR> n2=0;<BR> for i=1:n<BR> if (px(i)<=m)<BR> n1=n1+1;<BR> tx1(n1)=px(i);<BR> ty1(n1)=py(i);<BR> else<BR> n2=n2+1;<BR> tx2(n2)=px(i);<BR> ty2(n2)=py(i);<BR> end;<BR> end; <BR> end; %end if<BR> <BR> src_x1=0;<BR> src_y1=0;<BR> des_x1=0;<BR> des_y1=0;<BR> dis1=0;<BR> no1=0;<BR> <BR> %µÝ¹éÔÚ·ÖÀí³öµÄÁ½¸ö¿Õ¼äÖÐÀûÓ÷ÖÖη¨Çó½â<BR> [src_x1,src_y1,des_x1,des_y1,dis1,no1]=fly_control(tx1,ty1,n1);<BR> [src_x2,src_y2,des_x2,des_y2,dis2,no2]=fly_control(tx2,ty2,n2);<BR> <BR> if (dis1<dis2) %ÕÒµ½Á½¸ö¿Õ¼ä½âÖеÄ×îСÕß<BR> distemp=dis1; %¾àÀë<BR> src_xtemp=src_x1; %×ø±ê<BR> src_ytemp=src_y1;<BR> des_xtemp=des_x1;<BR> des_ytemp=des_y1;<BR> else<BR> distemp=dis2; %¾àÀë<BR> src_xtemp=src_x2; %×ø±ê<BR> src_ytemp=src_y2;<BR> des_xtemp=des_x2;<BR> des_ytemp=des_y2;<BR> end<BR> <BR> %ºÏ²¢Ç°½á¹û³õʼ»¯<BR> discombine=Inf; %¾àÀë³õʼ»¯<BR> src_xcombine=Inf; %×ø±ê³õʼ»¯<BR> src_ycombine=Inf;<BR> des_xcombine=Inf;<BR> des_ycombine=Inf;<BR> <BR> %ºÏ²¢<BR> for i=1:n1<BR> if (i<n2) %×öÒ»ÏÂÅжÏ,·ÀÖ¹p2ÖеÄ×ø±êÔ½½ç<BR> k=i;<BR> else<BR> k=n2;<BR> end <BR> <BR> while_flag=1;<BR> while(abs(ty2(k)-ty1(i))>distemp)<BR> if ((ty2(k)-ty1(i))>0) %%È·¶¨É¨Ã迪ʼµã<BR> k=k-1;<BR> else<BR> k=k+1;<BR> end <BR> if ((k<1)||(k>n2)) %%ϱêÔ½½çÔòÍ˳ö<BR> while_flag=0;<BR> break;<BR> end <BR> if ((ty2(k)-ty1(i))>distemp)&&((ty1(i)-ty2(k-1))>distemp) %%·ÀֹɨÃèÖ¸Õë×÷ÎÞÇîÕñµ´<BR> while_flag=0;<BR> break;<BR> end <BR> end <BR> <BR> if (while_flag==0) %%¶Ô¸ø¶¨µÄp1(i)¶øÑÔ,p2ÖÐûÓÐÈκεãºÍËü¿ÉÄܹ¹³É×î½Ó½üµã¶Ô,½áÊø±¾´ÎÑ­»·,¿¼²ìµãp1(i+1)<BR> continue;<BR> end <BR> <BR> %¿ªÊ¼ºÏ²¢²éÕÒ<BR> while(abs(ty2(k)-ty1(i))<=distemp) %ɨÃèÖ¸ÕëÔ¼Êø<BR> distance=sqrt((tx1(i)-tx2(k))^2+(ty1(i)-ty2(k))^2); %Çó¾àÀë<BR> <BR> if (distance<discombine) %ÕÒµ½Ò»¸ö¸üСµÄ¾àÀë<BR> discombine=distance; %¾àÀë<BR> src_xcombine=tx1(i); %×ø±ê<BR> src_ycombine=ty1(i);<BR> des_xcombine=tx2(k);<BR> des_ycombine=ty2(k);<BR> end <BR> end<BR> end %%end for<BR> <BR> if (discombine<distemp)<BR> dis=discombine;<BR> src_x=src_xcombine;<BR> src_y=src_ycombine;<BR> des_x=des_xcombine;<BR> des_y=des_ycombine;<BR> <BR> else<BR> dis=distemp;<BR> src_x=src_xtemp;<BR> src_y=src_ytemp;<BR> des_x=des_xtemp;<BR> des_y=des_ytemp;<BR> end |
|