replace.m

来自「规则障碍物环境下的机器人路径规划matlab源码！」· M 代码 · 共 32 行

function person=replace(BesT,d,m,n,MM,O,Xlu,pt) 
person=zeros(1,9*n+10);
if pt(1)==1
  person(1:2)=pt(1:2);
        person(n+1)=round(2+(MM-1-2)*rand);
        coordinate=ymadjp1(person(n+1),d);
        x0=coordinate(1);
        y0=coordinate(2);
    if (1<=x0&&x0<=4 &&14<=y0&&y0<=18)||(8<=x0&&x0<=13 && 8<=y0&&y0<=13)|| (14<=x0&&x0<=17 &&2<=y0&&y0<=5)
        person(n+1)=round(2+(54-2)*rand);
    end
    person(n+2)=d*d;
 elseif pt(2)==d*d
        person(1)=1;
        person(2)=round(2+(MM-1-2)*rand);
        coordinate=ymadjp1(person(2),d);
        x0=coordinate(1);
        y0=coordinate(2);
    if (1<=x0&&x0<=4 &&14<=y0&&y0<=18)||(8<=x0&&x0<=13 && 8<=y0&&y0<=13)|| (14<=x0&&x0<=17 &&2<=y0&&y0<=5)
        person(2)=round(2+(54-2)*rand);
    end
    person(n+1:n+2)=pt(1:2);
else
            person(1)=1;
            person(2:3)=pt(1:2);
            person(n+2)=d*d;
        
end
per=xutubiao(person,d);
per=judge(per,d,n,O,Xlu);
person=biaotuxu(per,d);