%
%  Syntax:  draw(X,Phi)
%
%  Inputs:  X is a scalar specifying the cart's position in [m].  
%           Phi is a scalar specifying the ball's angular deviation in [rad].
%
%  Result:  The cart/ball system is drawn in the main window.  The center of
%           the cart is at (X,0) and the ball is drawn on top of the cart,
%           positioned according to its angular deviation from the top of the
%           rails, as measured from the rails' center of curvature.
%
%           Petur Snaeland, 25.04.1995.
%           Revised Hordur Kvaran, 03.12.97
function draw(X,Phi)

%
%  Access handles to the Patch objects that represent the cart and ball:
%

hCart = findobj('Tag','objCart');
hBall = findobj('Tag','objBall');

%
%  Define the width and height of the cart, the ball radius and the radius
%  of curvature for the rails:
%
W = 0.35; H = 0.05; R = 0.055; Rcurvature = 0.50;
numCircle = 20;
%
%  Define the coordinates for the four corners of the cart and for the
%  nodes that make up the curved top surface of the cart.  NOTE! HORIZONTAL
%  COORDINATES ARE REFERRED TO HERE AS X AND VERTICAL COORDINATES AS Y, ALTHOUGH
%  THE CART/BALL SYSTEM USES Y FOR THE HORIZONTAL DISTANCE TRAVELLED BY THE CART.
%
CartNodeX = zeros(1,17);
CartNodeY = zeros(1,17);
CartNodeX(1:3) = [X-W/2-.02 X-W/2-.02 X-W/2];
CartNodeY(1:3) = [    0       2*H       H  ];
for i = 4:14,
   CartNodeX(i) = X - (W/2)*cos((i-4)*pi/10);
   CartNodeY(i) = H + (H/2)*sin((i-4)*pi/10);
end;
CartNodeX(15:17) = [X+W/2 X+W/2+.02 X+W/2+.02];
CartNodeY(15:17) = [  H      2*H        0    ];
%
%  Initialize all ball nodes to the ball's center.  Then move 20 nodes 
%  so that they lie evenly spaced on the ball's perimeter.  Use the first
%  and last nodes as endpoints for the mark.  
%
BallNodeX = ones(1,numCircle+2) * ( X + (Rcurvature + W/2)*tan(Phi));
BallNodeY = ones(1,numCircle+2) * ( H + R - Rcurvature*(1-cos(Phi)) + (H/2)*cos(Phi));
Roll = Rcurvature*tan(Phi)/(2*pi*R)*2*pi;   % X distance traveled due to roll.  
for i = 0:numCircle,
   BallNodeX(i+2) = BallNodeX(i+2) + R*sin(Roll+i*2*pi/numCircle);
   BallNodeY(i+2) = BallNodeY(i+2) + R*cos(Roll+i*2*pi/numCircle);
end

%
%  Assign new coordinates to the cart/ball patch objects, and redraw:
%  Note that precisely this method seems to be the fastest way to update
%  the display.  This is used in Matlab's Simulink animation demos.
%
set(hCart,'Xdata',CartNodeX,'Ydata',CartNodeY,'UserData',X);
set(hBall,'Xdata',BallNodeX,'Ydata',BallNodeY,'UserData',Phi);

drawnow;