 % FIGURES

% Plot (a) for # 3
%figure(1)
%plot([1 2 2 1],[2 1 3 2],'r','LineWidth',3); hold on
%plot([0 0 4 1 2 2],[0 4 0 2 3 1],'bo')
%plot([0 1],[0 2],'gr','LineWidth',1)
%axis([0 4 0 4])
%plot([0 2],[4 3],'gr','LineWidth',1)
%plot([2 4],[1 0],'gr','LineWidth',1)
%xlabel('x')
%ylabel('y')
%title('Number 3, Figure 1.15(a)'); hold off


% Plot (b) for # 3
%figure(2)
%plot([1 2 3 1],[2 1 2 2],'r','LineWidth',3); hold on
%plot([0 0 4 1 2 3],[0 4 0 2 1 2],'bo')
%plot([0 2],[0 1],'gr','LineWidth',1)
%axis([0 4 0 4])
%plot([0 1],[4 2],'gr','LineWidth',1)
%plot([3 4],[2 0],'gr','LineWidth',1)
%xlabel('x')
%ylabel('y')
%title('Number 3, Figure 1.15(b)'); hold off

% Plot Number 4; Pose 1; x = -1.378379629593037 y = 4.806253196113937
% theta= -0.720849227905274
%figure(3)
%plot([-1.378379629593037  0.875356841150429  2.855425506737759 -1.378379629593037],[4.806253196113937 2.826184530526607 5.079921001270073 4.806253196113937],'r','LineWidth',3); hold on
%plot([0 -1.378379629593037 0.875356841150429 5 2.855425506737759 0],[0 4.806253196113937 2.826184530526607 0 5.079921001270073 6],'bo')
%plot([0 -1.378379629593037],[0 4.806253196113937],'gr','LineWidth',1)
%axis([-2 5 0 9])
%plot([5 0.875356841150429],[0 2.826184530526607],'gr','LineWidth',1)
%plot([0 2.855425506737759],[6 5.079921001270073],'gr','LineWidth',1)
%xlabel('x')
%ylabel('y')
%title('Number 4, Pose 1'); hold off

% Plot Number 4; Pose 2; x = -0.914709556358433  y = 4.915618176283484
% theta= -0.331005477905273
%figure(4)
%plot([-0.914709556358433  1.922438585422232   2.897420852743294 -0.914709556358433],[4.915618176283484 3.940635908962423 6.777784050743088 4.915618176283484],'r','LineWidth',3); hold on
%plot([0 -0.914709556358433  1.922438585422232 5  2.897420852743294 0],[0 4.915618176283484 3.940635908962423 0 6.777784050743088 6],'bo')
%plot([0 -0.914709556358433],[0 4.915618176283484],'gr','LineWidth',1)
%axis([-2 5 0 9])
%plot([5  1.922438585422232],[0 3.940635908962423],'gr','LineWidth',1)
%plot([0  2.897420852743294],[6 6.777784050743088],'gr','LineWidth',1)
%xlabel('x')
%ylabel('y')
%title('Number 4, Pose 2'); hold off

% Plot Number 4; Pose 3; x =  4.481749428451879  y =  2.216734182315399
% theta= 1.1143685817718506
%figure(5)
%plot([ 4.481749428451879  5.724477281076617   2.993977787520408  4.481749428451879],[ 2.216734182315399 4.947233675871608  6.189961528496346  2.216734182315399],'r','LineWidth',3); hold on
%plot([0  4.481749428451879  5.724477281076617 5  2.993977787520408 0],[0  2.216734182315399 4.947233675871608 0  6.189961528496346 6],'bo')
%plot([0  4.481749428451879],[0  2.216734182315399],'gr','LineWidth',1)
%axis([0 7 0 9])
%plot([5  5.724477281076617],[0 4.947233675871608],'gr','LineWidth',1)
%plot([0  2.993977787520408],[6  6.189961528496346],'gr','LineWidth',1)
%xlabel('x')
%ylabel('y')
%title('Number 4, Pose 3'); hold off

% Plot Number 4; Pose 4; x =  4.571830214733669 y =  2.024443017805437
% theta = 2.115909099578857
%figure(6)
%plot([ 4.571830214733669  3.016286647854695   0.451080182179576  4.571830214733669],[ 2.024443017805437 4.589649483480557   3.034105916601583  2.024443017805437],'r','LineWidth',3); hold on
%plot([0  4.571830214733669  3.016286647854695 5  0.451080182179576 0],[0  2.024443017805437 4.589649483480557 0   3.034105916601583 6],'bo')
%plot([0  4.571830214733669],[0  2.024443017805437],'gr','LineWidth',1)
%axis([0 7 0 9])
%plot([5  3.016286647854695],[0 4.589649483480557],'gr','LineWidth',1)
%plot([0  0.451080182179576],[6   3.034105916601583],'gr','LineWidth',1)
%xlabel('x')
%ylabel('y')
%title('Number 4, Pose 4'); hold off

%%% PLOT NUMBER 5; POSE 1; x=-4.228670427627346  y= 2.668044798407097
%theta= -0.707080078125000
%figure(7)
%plot([ -4.228670427627346  -1.947884595328101   9.653186424545268e-004  -4.228670427627346],[ 2.668044798407097 0.719194884436542  2.999980716735787  2.668044798407097],'r','LineWidth',3); hold on
%plot([0  -4.228670427627346  -1.947884595328101 5  9.653186424545268e-004 0],[0  2.668044798407097 0.719194884436542 0   2.999980716735787 6],'bo')
%plot([0  -4.228670427627346],[0  2.668044798407097],'gr','LineWidth',1)
%axis([-5 7 0 6])
%plot([5  -1.947884595328101],[0 0.719194884436542],'gr','LineWidth',1)
%plot([0  9.653186424545268e-004],[6   2.999980716735787],'gr','LineWidth',1)
%xlabel('x')
%ylabel('y')
%title('Number 5, Pose 1, theta= -0.707080078125000'); hold off

%%% PLOT NUMBER 5; POSE 2; x=-4.861993772407707  y= 1.166677306799984
%theta= -0.277099609375000
x= -4.861993772407707;
y= 1.166677306799984;
epsi= -1.976434969822150;
omega= 0.345976145096769;
beta=-1.155733808118934;
delta=3.231534947682326;
figure(8)
plot([ x  epsi   beta  x],[ y omega  delta  y],'r','LineWidth',3); hold on
plot([0  x  epsi 5  beta 0],[0 y omega 0 delta 6],'bo')
plot([0  x],[0  y],'gr','LineWidth',1)
axis([-5 7 0 6])
plot([5  epsi],[0 omega],'gr','LineWidth',1)
plot([0  beta],[6 delta],'gr','LineWidth',1)
xlabel('x')
ylabel('y')
title('Number 5, Pose 2, theta= -0.277099609375000'); hold off


%%% PLOT NUMBER 5; POSE 3; x=-4.948028617891736  y= 0.718785456097449
%theta= -4.394531250000000e-004
x= -4.948028617891736;
y= 0.718785456097449;
epsi= -1.948028907570305;
omega=  0.717467096764883;
beta=-1.946710548237737;
delta=3.717466807086314;
figure(9)
plot([ x  epsi   beta  x],[ y omega  delta  y],'r','LineWidth',3); hold on
plot([0  x  epsi 5  beta 0],[0 y omega 0 delta 6],'bo')
plot([0  x],[0  y],'gr','LineWidth',1)
axis([-5 7 0 6])
plot([5  epsi],[0 omega],'gr','LineWidth',1)
plot([0  beta],[6 delta],'gr','LineWidth',1)
xlabel('x')
ylabel('y')
title('Number 5, Pose 3, theta= -4.394531250000000e-004'); hold off



%%% PLOT NUMBER 5; POSE 4; x=-0.812039505950754  y= 4.933811656557006
%theta= 0.454638671875000
x= -0.812039505950754;
y= 4.933811656557006;
epsi= 1.883219772943368;
omega= 6.251224811013547;
beta=0.565806618486828;
delta=8.946484089907671;
figure(10)
plot([ x  epsi   beta  x],[ y omega  delta  y],'r','LineWidth',3); hold on
plot([0  x  epsi 5  beta 0],[0 y omega 0 delta 6],'bo')
plot([0  x],[0  y],'gr','LineWidth',1)
axis([-4 7 0 9])
plot([5  epsi],[0 omega],'gr','LineWidth',1)
plot([0  beta],[6 delta],'gr','LineWidth',1)
xlabel('x')
ylabel('y')
title('Number 5, Pose 4, theta= 0.454638671875000'); hold off


%%% PLOT NUMBER 5; POSE 5; x=2.327517156226411  y= 4.425318637799768
%theta= 0.978076171875000
x= 2.327517156226411;
y= 4.425318637799768;
epsi= 4.003374903760125;
omega= 6.913591293686021;
beta=1.515102247873873;
delta=8.589449041219737;
figure(11)
plot([ x  epsi   beta  x],[ y omega  delta  y],'r','LineWidth',3); hold on
plot([0  x  epsi 5  beta 0],[0 y omega 0 delta 6],'bo')
plot([0  x],[0  y],'gr','LineWidth',1)
axis([-4 7 0 9])
plot([5  epsi],[0 omega],'gr','LineWidth',1)
plot([0  beta],[6 delta],'gr','LineWidth',1)
xlabel('x')
ylabel('y')
title('Number 5, Pose 5, theta= 0.978076171875000'); hold off


%%% PLOT NUMBER 5; POSE 6; x=3.228581883272749  y= 3.817912776142017
%theta= 2.512237548828125
x= 3.228581883272749;
y= 3.817912776142017;
epsi=  0.803360052543708;
omega= 5.583783403270537;
beta= -0.962510574584813;
delta=3.158561572541497;
figure(12)
plot([ x  epsi   beta  x],[ y omega  delta  y],'r','LineWidth',3); hold on
plot([0  x  epsi 5  beta 0],[0 y omega 0 delta 6],'bo')
plot([0  x],[0  y],'gr','LineWidth',1)
axis([-3 7 0 6])
plot([5  epsi],[0 omega],'gr','LineWidth',1)
plot([0  beta],[6 delta],'gr','LineWidth',1)
xlabel('x')
ylabel('y')
title('Number 5, Pose 6, theta= 2.512237548828125'); hold off

%%% PLOT NUMBER 6; POSE 1; x=0.913417464955391  y= 4.915789103172178
%theta= 1.070947265625000
x= 0.913417464955391;
y= 4.915789103172178;
epsi=  2.351296680530086;
omega= 7.548753850641900;
beta= -0.281668066939637;
delta=8.986633066216596;
figure(13)
plot([ x  epsi   beta  x],[ y omega  delta  y],'r','LineWidth',3); hold on
plot([0  x  epsi 5  beta 0],[0 y omega 0 delta 6],'bo')
plot([0  x],[0  y],'gr','LineWidth',1)
axis([-3 7 0 9])
plot([5  epsi],[0 omega],'gr','LineWidth',1)
plot([0  beta],[6 delta],'gr','LineWidth',1)
xlabel('x')
ylabel('y')
title('Number 6, Pose 1, theta= 1.070947265625000'); hold off


%%% PLOT NUMBER 6; POSE 2; x=2.322521610125502  y= 4.427890157243639
%theta= 2.544287109375000
x= 2.322521610125502;
y= 4.427890157243639;
epsi=  -0.158040453604527;
omega=  6.115139945503639;
beta= -1.845290241864529;
delta=3.634577881773610;
figure(14)
plot([ x  epsi   beta  x],[ y omega  delta  y],'r','LineWidth',3); hold on
plot([0  x  epsi 5  beta 0],[0 y omega 0 delta 6],'bo')
plot([0  x],[0  y],'gr','LineWidth',1)
axis([-3 7 0 9])
plot([5  epsi],[0 omega],'gr','LineWidth',1)
plot([0  beta],[6 delta],'gr','LineWidth',1)
xlabel('x')
ylabel('y')
title('Number 6, Pose 2, theta= 2.544287109375000'); hold off