3/9/19 3:10 PM C:\Users\Prince\Desktop\robot...\cspace.

m 1 of 1

%cspace
clc
clear
close all

x = [4 6 5 6 2 3.5 2 ];%x cordinates of vertices of obstacle

y = [0 -2 2 4 4 2 -2];%y cordinates of vertices of obstacle
figure(1);
hold on
axis([-8,8,-8,8]);
obstacle = polyshape(x,y);
plot(obstacle); %plotting the obstacle
[link1x,link1y,link2x,link2y] = robot(30,120);
plot(link1x,link1y,'Linewidth',4);
plot(link2x,link2y,'Linewidth',4);
%plotting the cspace
figure(2);
axis([0,360,0,360]);
hold on
scatter(30,120,'filled');
theta_1 = 0:2:360;
theta_2 = 0:2:360;
hold on
for i = 1:size(theta_1,2)
for j = 1:size(theta_2,2)
[link1x,link1y,link2x,link2y] = robot(theta_1(i),theta_2(j)); %return the
coordinates of the two links
[in1,on1]= inpolygon(link1x,link1y,x,y);
[in2,on2]= inpolygon(link2x,link2y,x,y);
a = numel(link1x(in1));
b = numel(link2x(in2));
if a|b
plot(theta_2(j),theta_1(i), '.','Color','r')
end
end
end