function [p1,p2,m1,m2] = twolink_ani(theta1,theta2,p1,p2,m1,m2)



L1 = 1;
L2 = 1;
L1m = .6;
L2m = 1;

x1 = [0 L1]*cos(theta1);
y1 = [0 L1]*sin(theta1);
x1m = [L1m]*cos(theta1);
y1m = [L1m]*sin(theta1);

x2 = x1(2)+[0 L2]*cos(theta1 + theta2);
y2 = y1(2)+[0 L2]*sin(theta1 + theta2);
x2m = x1(2)+[L2m]*cos(theta1 + theta2);
y2m = y1(2)+[L2m]*sin(theta1 + theta2);

if exist('m2','var') && ~isempty(m2)
    set(p1,'XData',x1,'YData',y1);
    set(p2,'XData',x2,'YData',y2);
    set(m1,'XData',x1m,'YData',y1m);
    set(m2,'XData',x2m,'YData',y2m);
else
    p1 = plot(x1,y1,'b-','LineWidth',2); hold on
    p2 = plot(x2,y2,'r-','LineWidth',2);
    m1 =plot(x1m,y1m,'ko','MarkerSize',10,'MarkerFaceColor','b');
    m2 = plot(x2m,y2m,'ko','MarkerSize',10,'MarkerFaceColor','r');
end

axis image
axis((L1+L2)*[-1 1 -1 1]*1.05)
