function drawpendulum(tstep, length, radius, theta)
%
% DrawPendulum animates the response of an inverted pendulum.
%
% DrawPendulum(tstep, length, radius, theta) displays the
% animated response of a pendulum where:
%   tstep is the (scalar) time step taken between displays
%   in seconds,
%   length is the (scalar) length of the pendulum in meters,
%   radius is the (scalar) radius of the mass at the end of
%       the pendulum in meters, and
%   theta is the (vector) of pendulum angles taken from
%       vertical (and down) in radians.
%

theta   = theta - pi/2; % rotate theta = 0 from matlab's horizontal
                        % to our vertical (and down)
                        
xmass = length*cos(theta); % coordinates of mass
ymass = length*sin(theta); 

mangles = -pi:pi/30:pi; % angles for drawing circle representing mass
mx      = radius*cos(mangles); % x-coordinates of perimeter of mass
my      = radius*sin(mangles); % y-coordinates of perimeter of mass

% open figure in which to put pendulum animation
figure('name', 'Pendulum Animation', 'color', 'white'); 

% draw line to represent rod of pendulum
hline = line([0, xmass(1)], [0, ymass(1)], 'LineWidth', 3, 'Color', 'b');

% set axis size, make axis square, and hold intitial figure
axis((length+radius)*[-1, 1, -1, 1]); axis square; 
hold on;

% draw circle to represent mass of pendulum
hmass = patch(xmass(1) + mx, ymass(1) + my, 'b');

% loop over and display pendulum for given angles
for it = 2:max(size(theta)),
    pause(tstep); % wait tstep seconds
    % set new coordinates of line/rod and mass/circle
    set(hline, 'Xdata', [0, xmass(it)], 'Ydata', [0, ymass(it)]);
    set(hmass, 'Xdata', xmass(it) + mx, 'Ydata', ymass(it) + my);
    drawnow; % redraw shapes with new coordinates
end;
hold off;
