clear all ; clc ; close all ;
%%
% Defining the Initial position of the Agents
% -------------------------------------------------------------------------
% initial position of agent 1
p11(:,1) = 2.25 %1 ;
p12(:,1) = 3.25 %1 ;
th1(1) = 60.92+50 ;
% initial position of agent 2
p21(:,1) = 2.75 %5 ;
p22(:,1) = 5.25 %4 ;
th2(1) = 150.92 + 100 %135;
% initial position of agent 3
p31(:,1) = 4.25 %2 ;
p32(:,1) = 4.75 %6 ;
th3(1) = -29.08+70 %-45 ;
% mov = avifile('example23.avi') ;
% Perturbation in Angle
% -------------------------------------------------------------------------
pertb_ang = 120 ;
k = 1
Iter = 0
U = 0 % Path defined
n=1
% starts the loop from here
for i2 = 1:1
% angle_convert
c1 = cosd(th1(:,k)) ;
s1 = sind(th1(:,k)) ;
c2 = cosd(th2(:,k)) ;
s2 = sind(th2(:,k)) ;
c3 = cosd(th3(:,k)) ;
s3 = sind(th3(:,k)) ;
% Defining the A matrix
% -------------------------------------------------------------------------
% Variable 1 2 3 4 5 6 7 8 9
A = [ -1 1 0 0 c1 -c1 -s1 s1 -1
1 -1 0 0 -c1 c1 s1 -s1 -1
0 0 -1 1 s1 -s1 c1 -c1 -1
0 0 1 -1 -s1 s1 -c1 c1 -1
-1 1 0 0 c2 -c2 -s2 s2 -1
1 -1 0 0 -c2 c2 s2 -s2 -1
0 0 -1 1 s2 -s2 c2 -c2 -1
0 0 1 -1 -s2 s2 -c2 c2 -1
-1 1 0 0 c3 -c3 -s3 s3 -1
1 -1 0 0 -c3 c3 s3 -s3 -1
0 0 -1 1 s3 -s3 c3 -c3 -1
0 0 1 -1 -s3 s3 -c3 c3 -1
-1 0 0 0 0 0 0 0 0
0 -1 0 0 0 0 0 0 0
0 0 -1 0 0 0 0 0 0
0 0 0 -1 0 0 0 0 0
0 0 0 0 -1 0 0 0 0
0 0 0 0 0 -1 0 0 0
0 0 0 0 0 0 -1 0 0
0 0 0 0 0 0 0 -1 0
0 0 0 0 0 0 0 0 -1 ] ;
% Defining the Constraint Matrix B
% ------------------------------------------------------------------------
B = [ -p11(:,k)
p11(:,k)
-p12(:,k)
p12(:,k)
-p21(:,k)
p21(:,k)
-p22(:,k)
p22(:,k)
-p31(:,k)
p31(:,k)
-p32(:,k)
p32(:,k)
0
0
0
0
0
0
0
0
0 ] ;
lb = zeros(9,1);
% Defining The Objective Function Matrix C
% ------------------------------------------------------------------------
C = [ 0 0 0 0 0 0 0 0 1 ] ;
% Computing the Linear Program
% ------------------------------------------------------------------------
% tic
[X,lambda,exitflag,output] = linprog(C,A,B);%,[],[],lb) ;
% t=toc
% pause
x1(:,k) = X(1,:) ;
x2(:,k) = X(2,:) ;
x3(:,k) = X(3,:) ;
x4(:,k) = X(4,:) ;
x5(:,k) = X(5,:) ;
x6(:,k) = X(6,:) ;
x7(:,k) = X(7,:) ;
x8(:,k) = X(8,:) ;
x9(:,k) = X(9,:) ;
z1(:,k) = x1(:,k) - x2(:,k) ;
z2(:,k) = x3(:,k) - x4(:,k) ;
u1(:,k) = x5(:,k) - x6(:,k) ;
u2(:,k) = x7(:,k) - x8(:,k) ;
% display('The Value of Performance Radius = ')
r = x9(:,k) ;
% rectangle('Position',[z1(:,k),z2(:,k),r,r])
% axis equal
% hold on
% Square Vertex
% x_vert_u(:,k) = z1(:,k) + x9(:,k) ;
% x_vert_l(:,k) = z1(:,k) - x9(:,k) ;
%
% y_vert_u(:,k) = z2(:,k) + x9(:,k) ;
% y_vert_l(:,k) = z2(:,k) - x9(:,k) ;
% Direction of u
% -------------------------------------------------------------------------
if (u2(:,k)<0 && u1(:,k)<0)
ang_u(:,k) = 180+(atand(u2(:,k)/u1(:,k))) ;
elseif (u2(:,k)<0 && u1(:,k)>0)
ang_u(:,k) = 360 + atand(u2(:,k)/u1(:,k)) ;
elseif (u2(:,k)>0 && u1(:,k)<0)
ang_u(:,k) = 180 - abs(atand(u2(:,k)/u1(:,k))) ;
else
ang_u(:,k) = atand(u2(:,k)/u1(:,k)) ;
end
% Random Number Decleration
% -------------------------------------------------------------------------
Ran_1(:,k) = randn ;
Ran_2(:,k) = randn ;
Ran_3(:,k) = randn ;
% agent 1
% -------------------------------------------------------------------------
Pertb_ang1(:,k) = Ran_1(:,k)*pertb_ang ;
th1(:,k+1) = th1(:,k) + ang_u(:,k) %+ Pertb_ang1(:,k); % good
if (th1(:,k+1) >= 360) && (th1(:,k+1) < 720)
th1(:,k+1) = th1(:,k+1) - 360 ;
elseif (th1(:,k+1) >=720) && (th1(:,k+1) < 1080)
th1(:,k+1) = th1(:,k+1) - 720 ;
elseif (th1(:,k+1) >=1080) && (th1(:,k+1) < 1440)
th1(:,k+1) = th1(:,k+1) - 1080 ;
elseif (th1(:,k+1) >= 1440) && ( th1(:,k+1) < 1800 )
th1(:,k+1) = th1(:,k+1) -1440 ;
elseif ( th1(:,k+1) < -360 ) && ( th1(:,k+1) >= -720)
th1(:,k+1) = th1(:,k+1) + 360 ;
elseif ( th1(:,k+1) < -720 ) && (th1(:,k+1) >= -1080)
th1(:,k+1) = th1(:,k+1)+ 720 ;
else
th1(:,k+1) = th1(:,k+1) ;
end
% agent 2
% -------------------------------------------------------------------------
Pertb_ang2(:,k) = Ran_2(:,k)*pertb_ang ;
th2(:,k+1) = th2(:,k) + ang_u(:,k)%+ Pertb_ang2(:,k);
if (th2(:,k+1) >= 360) && (th2(:,k+1) < 720)
th2(:,k+1) = th2(:,k+1) - 360 ;
elseif (th2(:,k+1) >=720) && (th2(:,k+1) < 1080)
th2(:,k+1) = th2(:,k+1) - 720 ;
elseif (th2(:,k+1) >=1080) && (th2(:,k+1) < 1440)
th2(:,k+1) = th2(:,k+1) - 1080 ;
elseif (th2(:,k+1) >= 1440) && ( th2(:,k+1) < 1800 )
th2(:,k+1) = th2(:,k+1) -1440 ;
elseif ( th2(:,k+1) < -360 ) && ( th2(:,k+1) >= -720)
th2(:,k+1) = th2(:,k+1) + 360 ;
elseif ( th2(:,k+1) < -720 ) && (th2(:,k+1) >= -1080)
th2(:,k+1) = th2(:,k+1)+ 720 ;
else
th2(:,k+1) = th2(:,k+1) ;
end
% agent 3
% -------------------------------------------------------------------------
Pertb_ang3(:,k) = Ran_3(:,k)*pertb_ang ;
th3(:,k+1) = th3(:,k) + ang_u(:,k) %+ Pertb_ang3(:,k);
if (th3(:,k+1) >= 360) && (th3(:,k+1) < 720)
th3(:,k+1) = th3(:,k+1) - 360 ;
elseif (th3(:,k+1) >=720) && (th3(:,k+1) < 1080)
th3(:,k+1) = th3(:,k+1) - 720 ;
elseif (th3(:,k+1) >=1080) && (th3(:,k+1) < 1440)
th3(:,k+1) = th3(:,k+1) - 1080 ;
elseif (th3(:,k+1) >= 1440) && ( th3(:,k+1) < 1800 )
th3(:,k+1) = th3(:,k+1) -1440 ;
elseif ( th3(:,k+1) < -360 ) && ( th3(:,k+1) >= -720)
th3(:,k+1) = th3(:,k+1) + 360 ;
elseif ( th3(:,k+1) < -720 ) && (th3(:,k+1) >= -1080)
th3(:,k+1) = th3(:,k+1)+ 720 ;
else
th3(:,k+1) = th3(:,k+1) ;
end
p11(:,k+1) = p11(:,k) + u1(:,k)*c1 - u2(:,k)*s1 ;
p12(:,k+1) = p12(:,k) + u1(:,k)*s1 + u2(:,k)*c1 ;
p21(:,k+1) = p21(:,k) + u1(:,k)*c2 - u2(:,k)*s2 ;
p22(:,k+1) = p22(:,k) + u1(:,k)*s2 + u2(:,k)*c2 ;
p31(:,k+1) = p31(:,k) + u1(:,k)*c3 - u2(:,k)*s3 ;
p32(:,k+1) = p32(:,k) + u1(:,k)*s3 + u2(:,k)*c3 ;
% For LP angle validation
% -------------------------------------------------------------------------
% ang1(:,k) = atand(q12/q11);
% ang2(:,k) = atand(q22/q21);
% ang3(:,k) = atand(q32/q31);
% Total Path Covered
% -------------------------------------------------------------------------
U1(k) = sqrt(u1(:,k)*u1(:,k)+u2(:,k)*u2(:,k));
U = U + U1(k) ;
d1=U1(k)/50;
p111(1)=p11(:,k);
p112(1)=p12(:,k);
p211(1)=p21(:,k);
p212(1)=p22(:,k);
p311(1)=p31(:,k);
p312(1)=p32(:,k);
for i = 1:25
p111(i+1)=p111(i)+d1*cosd(ang_u(:,k)+th1(:,k));
p112(i+1)=p112(i)+d1*sind(ang_u(:,k)+th1(:,k));
p211(i+1)=p211(i)+d1*cosd(ang_u(:,k)+th2(:,k));
p212(i+1)=p212(i)+d1*sind(ang_u(:,k)+th2(:,k));
p311(i+1)=p311(i)+d1*cosd(ang_u(:,k)+th3(:,k));
p312(i+1)=p312(i)+d1*sind(ang_u(:,k)+th3(:,k));
% plot([p111(i) p111(i+1)],[p112(i) p112(i+1)],'ro','LineWidth',1,...
% 'MarkerEdgeColor','r',...
% 'MarkerFaceColor','g',...
% 'MarkerSize',2);
% % axis([0 10 0 10])
% axis equal
%
% xlabel('X-Axis');
%
% ylabel('Y-Axis');
%
% % axis([0,2*pi,-1,1])
% %
% % S1=sprintf('r = %.2f' ,t);
% %
% % text(3,4,S1)
% title('Trajectory of the Agents');
% hold on
%
% plot([p211(i) p211(i+1)],[p212(i) p212(i+1)],'bo','LineWidth',1,...
% 'MarkerEdgeColor','b',...
% 'MarkerFaceColor','g',...
% 'MarkerSize',2);
% hold on
%
% plot([p311(i) p311(i+1)],[p312(i) p312(i+1)],'ko','LineWidth',1,...
% 'MarkerEdgeColor','k',...
% 'MarkerFaceColor','g',...
% 'MarkerSize',2);
% hold on
%
% F=getframe;
% mov = addframe(mov,F);
% % n = n+1;
end
% S1=sprintf('r = %.2f' ,r);
%
% text(3,4,S1)
% For Plotting
% -------------------------------------------------------------------------
% trajectory_agent;
% % plot([p11(i) p11(i+1)],[p12(i) p12(i+1)],'r')
% plot([p11(i) p11(i+1)],[p12(i) p12(i+1)],'r','LineWidth',1,...
% 'MarkerEdgeColor','k',...
% 'MarkerFaceColor','g',...
% 'MarkerSize',2)
%
%
% axis([0 10 0 10])
% xlabel('X-axis')
% ylabel('Y-axis')
% % axis equal
% grid on
% drawnow
% % pause(1)
% hold on
% plot([p21(i) p21(i+1)],[p22(i) p22(i+1)],'b-.')
% % plot([p21(i) p21(i+1)],[p22(i) p22(i+1)],'bo','LineWidth',1.5,...
% % 'MarkerEdgeColor','k',...
% % 'MarkerFaceColor','g',...
% % 'MarkerSize',4)
%
%
%
%
% % axis([0 7 0 7])
% drawnow
% % pause(1)
% hold on
% plot([p31(i) p31(i+1)],[p32(i) p32(i+1)],'g')
% % plot([p31(i) p31(i+1)],[p32(i) p32(i+1)],'bo','LineWidth',1.5,...
% % 'MarkerEdgeColor','k',...
% % 'MarkerFaceColor','g',...
% % 'MarkerSize',4)
%
%
% % axis([0 7 0 7])
% drawnow
% % pause(1)
% % F(i)=getframe;
%
% clc
% Condition to Radius
% -------------------------------------------------------------------------
if ( r < 0.002)
break
else
k = k+1 ;
end
end
% max_min_avg
plotresults
%
