Sampling and collision detection: determine whether a point is inside a convex polygon
Consider a simple workspace
clear all
close all
fig_1=figure();
h1=figure();
%define the polygon counterclock wise
Poly=[2 5; %vertex
4.5 4.8 %vertex 2
3.5 6; %vertex 3
2 5]; %repeating vertex 1
xlim([1 14])
ylim([0 8])
hold on
%Now lets draw the polygon (to draw a polygon we draw its line segments
m_p=length(Poly)-1; %getting the number of segments
for i=1:m_p
plot(Poly(i,1), Poly(i,2),'k');
hold on
for t=0:0.01:1
x_segment=(1-t)*Poly(i,1)+t*Poly(i+1,1);
y_segment=(1-t)*Poly(i,2)+t*Poly(i+1,2);
plot(x_segment,y_segment,'.k');
end
end
box on
The procedure to determine if a point is inside a polygon or not:
q=[3 5.5];
for i=1:m_p
inward_normal(i,:)=[-(Poly(i+1,2)-Poly(i,2)) Poly(i+1,1)-Poly(i,1)];
%vv(i,:)=[Poly(i+1,1)-Poly(i,1) Poly(i+1,2)-Poly(i,2)];
vertex_to_point(i,:)=[q(1,1)-Poly(i,1) q(1,2)-Poly(i,2)];
%Check the sign of the dot product and the vertex_to_point vector
if dot(inward_normal(i,:),vertex_to_point(i,:))<0
inside_detector_flag(i)=-1;
else
inside_detector_flag(i)=1;
end
end
if min(inside_detector_flag)<0 %we will plot the point in red if it is inside the polygon
plot(q(1,1),q(1,2),'go')
else %we will plot the point in green if it is inside the polygon
plot(q(1,1),q(1,2),'ro')
end
inside_detector_flag
Draw the workspace
clear all
fig2=figure();
h1=figure();
number_of_Obst=2; %define the number of obstacles
O={}; %this tells matlab that O is a cell; O is used to store the obstacles
%define the obstacle polygon counterclock wise
O(1)={[2 5; %vertex 1
4.5 4.8 %vertex 2
3.5 6; %vertex 3
2 5]}; %repeating vertex 1
O(2)={[8 4.5; %vertex 1
11 3.5; %vertex 2
12.5 4.3; %vertex 3
10 6; %vertex 4
8 4.5]}; %repeating vertex 1
xlim([1 14])
ylim([0 8])
hold on
%Now lets draw the polygon (to draw a polygon we draw its line segments
for j=1:number_of_Obst
m(j)=length(O{j}(:,:))-1; %getting the number of segments
for i=1:m(j)
for t=0:0.01:1
x_segment=(1-t)*O{j}(i,1)+t*O{j}(i+1,1);
y_segment=(1-t)*O{j}(i,2)+t*O{j}(i+1,2);
plot(x_segment,y_segment,'.k');
end
end
end
box on
Generate 100 samples uniformly distributed over the workspace of [1 14]x[0 8]
n_samples=100;
x = 1 + (14-(1))*rand(100,1); % x coordiante
y = 0 + (8-(0))*rand(100,1); % y coordiante
plot(x,y,'o')
Now lets cross off the samples that are inside the polygon
clear inside_detector_flag;
for k=1:n_samples
q=[x(k) y(k)];
for j=1:number_of_Obst
for i=1:m(j)
inward_normal(i,:)=[-(O{j}(i+1,2)-O{j}(i,2)) O{j}(i+1,1)-O{j}(i,1)];
vertex_to_point(i,:)=[q(1,1)-O{j}(i,1) q(1,2)-O{j}(i,2)];
%Check the sign of the dot product and the vertex_to_point vector
if dot(inward_normal(i,:),vertex_to_point(i,:))<0
inside_detector_flag(i)=-1;
else
inside_detector_flag(i)=1;
end
end
if min(inside_detector_flag)>0 %we will plot the point in red if it is inside the polygon
plot(q(1,1),q(1,2),'rx')
end
%think about why you sbould clear the following variables
clear vertex_to_point;
clear inside_detector_flag;
clear inward_normal
end
end
