function prob_2_33
% 
% Written by:
% -- 
% John L. Weatherwax                2005-05-07
% 
% email: wax@alum.mit.edu
% 
% Please send comments and especially bug reports to the
% above email address.
% 
%-----
  
% Define the physical constants:
global m1 m2 L1 L2 g;
m1 = 937.5;
m2 = 312.5; 
L1 = 16; 
L2 = 16;
g = 32.0; 

options = odeset('Mass',@mass1,'MassSingular','no','Events',@events);
y0 = [ -.5; -1; 1; 2 ]; 
[ts1,ys1,te,ye,ie] = ode45(@odes1,[0 2*pi],y0,options);  
figure; plot(ts1,ys1(:,1),'-o',ts1,ys1(:,3),'-o');
if( ~isempty(ie) )
  disp( 'Nonlinear lost pallet' );
  te,ye
end

options = odeset('Mass',@mass2,'MassSingular','no','Events',@events);
y0 = [ -.5; -1; 1; 2 ]; 
[ts2,ys2,te,ye,ie] = ode45(@odes2,[0 2*pi],y0,options);  
figure; plot(ts2,ys2(:,1),'-o',ts2,ys2(:,3),'-o');
if( ~isempty(ie) )
  disp( 'Linear lost pallet' );
  te,ye
end

% Compare linear/nonlinear solutions:
figure; plot(ts1,ys1(:,1),'-go',ts1,ys1(:,3),'-go'); hold on; 
plot(ts2,ys2(:,1),'-ro',ts2,ys2(:,3),'-ro');

%===================================================================
function dydz = odes1(t,y)
global m1 m2 L1 L2 g;
dydz = [ y(2);
	 m2*L2*(y(4)^2)*sin(y(3)-y(1)) - (m1+m2)*g*sin(y(1));
	 y(4);
	 -m2*L1*(y(2)^2)*sin(y(3)-y(1)) - m2*g*sin(y(3))
	 ];
 
function A = mass1(t,y)
global m1 m2 L1 L2 g;
A = zeros(4);
A(1,1) = 1;
A(2,2) = (m1+m2)*L1;
A(2,4) = m2*L2*cos(y(3)-y(1));
A(3,3) = 1;
A(4,2) = m2*L2*cos(y(3)-y(1));
A(4,4) = m2*L2;

%===================================================================
function dydz = odes2(t,y)
global m1 m2 L1 L2 g;
dydz = [ y(2);
	 - (m1+m2)*g*y(1);
	 y(4);
	 - m2*g*(y(3))
	 ];
 
function A = mass2(t,y)
global m1 m2 L1 L2 g;
A = zeros(4);
A(1,1) = 1;
A(2,2) = (m1+m2)*L1;
A(2,4) = m2*L2;
A(3,3) = 1;
A(4,2) = m2*L1;
A(4,4) = m2*L2;


function [value,isterminal,direction] = events(t,y)

value = y(3)-pi/3;
isterminal = 1;
direction  = +1;