% Pend1.m
%
% Dynamic analysis of Rigid Pendulum
%
% Copyright (C) 2007 清水 信行, 曽我部 潔 All Rights Reserved.

% =================================================================================
% プログラムの使用条件
% 1. 本プログラムおよびデータを商用で，頒布してはならない．
% 2. 本プログラムおよびデータを著作者の許可なく営利目的で利用してはならない．
% 3. 本プログラムおよびデータの改変は，営利目的でない限り自由である．
% 4. 本プログラムおよびデータを使用することにより生じた損害などについては，著者，
%    プロクラム著作者，(社)日本機械学会，および(株)コロナ社は一切の責任を負わない．
% 5. 本プログラムの著作権は，本書編著者ならびにプログラム著作者に帰す．
% =================================================================================

% Setting of Parameters
L = 0.5; m = 1; g = 9.81;
x1O = -L;	y1O = 0;
x1A =  L;	y1A = 0;

% Initialization
close; % Close the figure window
a = [0 0 0]'; r = [0 0]'; % Definition of a and r vectors
A_data = []; % Initialize the Animation data
G_data = []; % Initialize the Graphics data

% Initial State of the mechanism
s0 = -pi/4;
q = [L*cos(s0) L*sin(s0) s0]';
v = [0 0 0]';

% Time Counter
Ts = 0; Te = 5; dt = 0.001;
for t = Ts:dt:Te
	% Coefficient matrix of Eq.(2.20)
	sq3 = sin(q(3)); cq3 = cos(q(3));
	A = [m 0 0 1 0
	     0 m 0 0 1
	     0 0 m*L^2/3 L*sq3 -L*cq3
	     1 0 L*sq3 0 0
	     0 1 -L*cq3 0 0];
	
	% Right hand side of Eq.(2.20)
	b = [0 -m*g 0 -L*v(3)^2*cq3 -L*v(3)^2*sq3]';
	
	% Solution of Eq.(2.20)
	x = A\b;
	
	% Time integration by the Euler method 
	a(1) = x(1); a(2) = x(2); a(3) = x(3);
	r(1) = x(4); r(2) =  x(5);
	y = [q;v]; yd = [v;a];
	ny = y+yd*dt;
	q(1) = ny(1); q(2) = ny(2); q(3) = ny(3);
	v(1) = ny(4); v(2) = ny(5); v(3) = ny(6);
	
	% Animation
	 plot([0,0],[0,0])
	axis square
	xmin = -1; xmax = 1; ymin = -1.5; ymax = 0.5;
	axis([xmin,xmax,ymin,ymax])
	hold on
	
	temp = [t];
	fprintf('t = %f\n',t)
	
	nae = 1; % Number of Animation element = 1
	xL = q(1)+(x1O*cq3-y1O*sq3); yL = q(2)+(x1O*sq3+y1O*cq3); 
	xR = q(1)+(x1A*cq3-y1A*sq3); yR = q(2)+(x1A*sq3+y1A*cq3);
	plot([xL,xR],[yL,yR],'linewidth',0.5);
	temp = [temp,[xL,yL,xR,yR]];
	
	drawnow
	hold off
	set(1,'doublebuffer','on')
	
	% Output of the Animation data 
	A_data = [A_data;temp];
	
	% Output of the Graphics data for the point of interest
	xP = q(1)+(x1A*cq3-y1A*sq3); yP = q(2)+(x1A*sq3+y1A*cq3);
	temp = [t,xP,yP];
	G_data = [G_data;temp];
end
