randn('state',0); rand('state',0);

n = 2; N = 3; T = 30; D = 0.4;

% starting and final positions
p1start = [-1,0]'; p2start = [-1,0.5]'; p3start = [-1,1]';
p1final =  [1,0]'; p2final = [1,-0.5]'; p3final = [1,-1]';

% MOVIE: Replace p1, p2, p3 with your trajectories
% ----------------------------------------------------------
% uniform speed (colliding) trajectories
p1 = repmat(p1start,1,T)+(p1final-p1start)*[0:1/(T-1):1]; 
p2 = repmat(p2start,1,T)+(p2final-p2start)*[0:1/(T-1):1]; 
p3 = repmat(p3start,1,T)+(p3final-p3start)*[0:1/(T-1):1]; 

[x,y,z] = cylinder(D/2,200);
x = x(1,:); y = y(1,:);
figure; cla; hold on; 
axis([-1.5,1.5,-1.5,1.5]); axis square; 
for t = 1:T
    cla;
    plot(p1(1,:),p1(2,:),'bd');
    plot(p2(1,:),p2(2,:),'rd');
    plot(p3(1,:),p3(2,:),'kd');
    plot(p1(1,t)+x,p1(2,t)+y,'b','linewidth',1.5);
    plot(p2(1,t)+x,p2(2,t)+y,'r','linewidth',1.5);
    plot(p3(1,t)+x,p3(2,t)+y,'k','linewidth',1.5);
    rectangle('Position',[-1,-1,2,2],'Linestyle','--');
    pause(0.5);
end
% ----------------------------------------------------------
