refresh.m

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

function P=refresh(P,d) 
[n1,n2]=size(P);
n3=fix(n2/2);
p=zeros(n1,n3);
for i=1:n1
    for j=2:2:n2
        p(i,j/2)=ymadjp2([P(i,j),P(i,j+1)],d);
    end
end
q=p;
for i=1:n1
    for j=1:n3
        for k=j+1:n3
            if q(i,k)==0
                break
            end
            if q(i,j)==q(i,k)
                for h=k:n3
                q(i,h-1)=q(i,h);
                end
            end
        end
    end
end
p=q;
 for i=1:n1
    for j=1:n3
         coordinate=ymadjp1(p(i,j),d);%将p中各序号转换为坐标
         P(i,2*j)=coordinate(1);
         P(i,2*j+1)=coordinate(2);
    end
end