Colony

function [mov] = visualizeRobots(f,n,Xs,Ys,Thetas,Speeds,idx,color,mov,winsize)

% Scales arrows to length

arrowScale = 1.5;

clf;

ax = [min(min(Xs))-1, max(max(Xs))+1, min(min(Ys))-1, max(max(Ys))+1];

if ax(1) > -10

    ax(1) = -10;

end

if ax(2) < 10

    ax(2) = 10;

end

if ax(3) > -10

    ax(3) = -10;

end

if ax(4) < 10

    ax(4) = 10;

end

axis(ax);

%axis([-10 10 -10 10])

axis manual

set( gca,'nextplot','add' );

for j = 1:n

    h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx) + Speeds(j, idx-1) * arrowScale * cos(Thetas(j,idx)),Ys(j,idx) + Speeds(j, idx-1) * arrowScale * sin(Thetas(j,idx)),'color',color(j,:),'facecolor',color(j,:),'edgecolor',color(j,:) );

   	%h = plot_arrow( Xs(j,idx),Ys(j,idx),Xs(j,idx) + cos(Thetas(j,idx)),Ys(j,idx) + sin(Thetas(j,idx)),'color',color(j,:),'facecolor',color(j,:),'edgecolor',color(j,:) );

    set( h,'linewidth',3 );

	d = plot( Xs(j,idx),Ys(j,idx), '.k');

	set( d,'MarkerSize',5 );

end

hold off;

if idx < 3 %s o we don't see giant arrows

    clf

end

%mov(:,idx) = getframe(f,winsize);

pause(.001);

end