root / branches / 16299_s10 / matlab / visualizeRobots.m @ 1812
History | View | Annotate | Download (878 Bytes)
1 |
function [mov] = visualizeRobots(f,n,Xs,Ys,Thetas,Speeds,idx,color,mov,winsize) |
---|---|
2 |
|
3 |
% Scales arrows to length |
4 |
arrowScale = 1.5; |
5 |
|
6 |
clf; |
7 |
ax = [min(min(Xs))-1, max(max(Xs))+1, min(min(Ys))-1, max(max(Ys))+1]; |
8 |
|
9 |
if ax(1) > -10 |
10 |
ax(1) = -10; |
11 |
end |
12 |
if ax(2) < 10 |
13 |
ax(2) = 10; |
14 |
end |
15 |
if ax(3) > -10 |
16 |
ax(3) = -10; |
17 |
end |
18 |
if ax(4) < 10 |
19 |
ax(4) = 10; |
20 |
end |
21 |
|
22 |
axis(ax); |
23 |
|
24 |
%axis([-10 10 -10 10]) |
25 |
|
26 |
axis manual |
27 |
set( gca,'nextplot','add' ); |
28 |
for j = 1:n |
29 |
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)),... |
30 |
'color',color(j,:),'facecolor',color(j,:),'edgecolor',color(j,:) ); |
31 |
set( h,'linewidth',3 ); |
32 |
d = plot( Xs(j,idx),Ys(j,idx), '.k'); |
33 |
set( d,'MarkerSize',5 ); |
34 |
end |
35 |
hold off; |
36 |
|
37 |
if idx < 3 %s o we don't see giant arrows |
38 |
clf |
39 |
end |
40 |
mov(:,idx) = getframe(f,winsize); |
41 |
|
42 |
end |