1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41
| clear
N=10;%Nombre de lignes et de colonnes
U1=[2 3];U2=[8 6];%Coordonnées des unités
X_obs=[5 5 5 7 7 7 7 5];%Pour le tracé de l'obstacle
Y_obs=[4 5 5 5 5 4 4 4];%Pour le tracé de l'obstacle
pos_obs=[46 47];%Numéros des cases ocupées par l'de l'obstacle
%Matrice obstacle
Mobs=zeros(10,10);
Mobs(N-fix(pos_obs(1)/10),pos_obs(1)-fix(pos_obs(1)/10)*10)=1;
Mobs(N-fix(pos_obs(2)/10),pos_obs(2)-fix(pos_obs(2)/10)*10)=1;
%Equation de la droite entre unités
A=(U2(2)-U1(2))/(U2(1)-U1(1));
B=-1;
C=-A*U1(1)+U1(2);
D=(sqrt(A^2+B^2));
%Calcul distances et affectation des cases coupées
for j=0:N-1
for k=0:N-1
d=(A*(k+0.5)+B*(j+0.5)+C)/D;
if abs(d) < 0.5
M(N-j,k+1)=1;
else
if abs(d) > 0.707
M(N-j,k+1)=0;
else
M(N-j,k+1)=1;
end
end
end
end
%Tracé
plot([U1(1) U2(1)],[U1(2) U2(2)])
hold on
h=line(X_obs,Y_obs);
set(h,'Color','r','LineWidth',3)
scatter(U1(1),U1(2),300,[0 0 0])
scatter(U2(1),U2(2),300,[0 0 1])
grid
axis([0 10 0 10])
%Test de présence d'obstacle
[ligne,colonne] =find(M-2*Mobs==-1) |
Partager