MATLABの質問です。
MATLABについての質問です。
大学の研究していてMATLABでわからないところがあるのでぜひ教えて頂けたらと思っています。
よろしくお願い致します。
現在任意の多角形障害物を設置し，それらを回避する折れ線経路を全部求めるところまでできています。その後、全経路からダイクストラの方法で最短折れ線経路を求めたいのですがどのようにプログラムを持っていけばいいか分からず手詰まりしました。
ぜひご指導いただけますでしょうか。
よろしくお願いいたします。
プログラムは動きます。
close all;
clc;
init=1;
final=1000;
%図形座標取得
figure(1)
for loop=init:final
[x,y] = ginput
A=[x,y]
if 0<=x(:,:)&x(:,:)<=1&0<=y(:,:)&y(:,:)<=1
B{loop} =A
else
break
end
end
% size(B{i},1)
figure(2)
for C=init:loop-1
B{C}
fill(B{C}(:,1),B{C}(:,2),'g');hold on
axis([0,1,0,1]);
end
[x,y] = ginput
pb=[x,y]
figure(3)
for C=init:loop-1
fill(B{C}(:,1),B{C}(:,2),'g');hold on
axis([0,1,0,1]);
plot(pb(:,1),pb(:,2),'o');hold on
axis([0,1,0,1]);
end
%図形の辺
k=0;
for i=1:loop-1
for p=1:size(B{i},1)
for q=p+1:size(B{i},1)
k=k+1;
obst(k,:) = [B{i}(p,:),B{i}(q,:)];
end
end
end
Pall = pb(1, :);
Pall = [Pall; cat(1, B{:})];
Pall = [Pall; pb(2, :)];
%全経路
k=0
for i=1:size(Pall,1)
for j=i+1:size(Pall,1)
k=k+1;
path(k,:) = [Pall(i,:),Pall(j,:)];
point{k}=[i,j];
end
end
% 交差判定
H=0
for j=1:size(path,1)
cflas=1
for i=1:size(obst,1)
y1=((obst(i,1)-obst(i,3))*(path(j,2)-obst(i,2))+(obst(i,2)-obst(i,4))*(obst(i,1)-path(j,1)))
y2=((obst(i,1)-obst(i,3))*(path(j,4)-obst(i,2))+(obst(i,2)-obst(i,4))*(obst(i,1)-path(j,3)))
y3=((path(j,1)-path(j,3))*(obst(i,2)-path(j,2))+(path(j,2)-path(j,4))*(path(j,1)-obst(i,1)))
y4=((path(j,1)-path(j,3))*(obst(i,4)-path(j,2))+(path(j,2)-path(j,4))*(path(j,1)-obst(i,3)))
Y1=y1*y2
Y2=y3*y4
if Y1<0&&Y2<0
H=H+1
cflas=0
S{j}=[0,0,0,0]
S1{j}=[0,0,0,0]
end
end
if cflas==1
X=[path(j,1),path(j,3)];
Y=[path(j,2),path(j,4)];
Xa=[path(j,1),path(j,2)];
Yb=[path(j,3),path(j,4)];
S{j}=[X,Y]
S1{j}=[Xa,Yb]
plot(X,Y,'-');hold on
end
end
barricade = length(B{1,1});
M=zeros
%コストを計算
for k = 1:size(path,1)
X1=S{k}(1,1);
X2=S{k}(1,3);
Y1=S{k}(1,2);
Y2=S{k}(1,4);
cost{k}=sqrt((X2-X1)^2+(Y2-Y1)^2)
M(point{k}(1), point{k}(2) )=cost{k}
% if cost{i}==0
% cost{i}=10
% end
end
M'
g=cost{1}
for j=1:size(obst,1)+1
if(cost{j}>cost{j+1})
g= cost{j+1}
end
end
