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

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

% Setting of Parameters
L = 0.5; m = 1;
L1 = L;  m1 = m;
L2 = L;  m2 = m;
g = 9.81;
x1O = -L1;	y1O = 0;  x1P =  L1; y1P = 0;
x2P = -L2;	y2P = 0;  x2A =  L2; y2A = 0;

% Initialization
close; % Close the figure window
a = [0 0 0 0 0 0]'; r = [0 0 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
s10 = -pi/4; s20 = -pi/2;
q = [L1*cos(s10) L1*sin(s10) s10 2*L1*cos(s10)+L2*cos(s20) 2*L1*sin(s10)+L2*sin(s20) s20]';
v = [0 0 0 0 0 0]';

% Time counter
Ts = 0; Te = 5; dt = 0.001;

for t = Ts:dt:Te
	% Coefficient matrix of Eq.(2.11)
	sq3 = sin(q(3)); cq3 = cos(q(3));
	sq6 = sin(q(6)); cq6 = cos(q(6));
	
	M = [m1 0 0 0 0 0
	    0 m1 0 0 0 0
	    0 0 m1*L1^2/3 0 0 0
	    0 0 0 m2 0 0
	    0 0 0 0 m2 0
	    0 0 0 0 0 m2*L2^2/3];
	Fq = [1 0 L1*sq3 0 0 0
	     0 1 -L1*cq3 0 0 0
	     -1 0 L1*sq3 1 0 L2*sq6
	     0 -1 -L1*cq3 0 1 -L2*cq6];
	O = [0 0 0 0; 0 0 0 0; 0 0 0 0; 0 0 0 0];
	A = [M,Fq';Fq,O];
	
	% Right hand side of Eq.(2.11)
	QA = [0 -m1*g 0 0 -m2*g 0]';
	gam = -[L1*v(3)^2*cq3
	      L1*v(3)^2*sq3
	      L1*v(3)^2*cq3+L2*v(6)^2*cq6
	      L1*v(3)^2*sq3+L2*v(6)^2*sq6];
	b = [QA;gam];
	
	% Solution of Eq.(2.11)
	x = A\b;
	
	% Time integration by the Euler method
	for j = 1:6 a(j) = x(j); end
	for j = 1:4 r(j) = x(6+j); end
	y = [q;v]; yd = [v;a];
	ny = y+yd*dt;
	for j = 1:6 q(j) = ny(j); end
	for j = 1:6 v(j) = ny(6+j); end
	
	% Animation
	plot([0,0],[0,0])
	axis square
	xmin = -2; xmax = 2; ymin = -3; ymax = 1;
	axis([xmin,xmax,ymin,ymax])
	hold on
	
	temp = [t];
	fprintf('t = %f\n',t)
	
	nae = 1; % Number of Animation elements = 1
	xL = q(1)+(x1O*cq3-y1O*sq3); yL = q(2)+(x1O*sq3+y1O*cq3);
	xR = q(1)+(x1P*cq3-y1P*sq3); yR = q(2)+(x1P*sq3+y1P*cq3);
	plot([xL,xR],[yL,yR],'linewidth',0.5);
	temp = [temp,[xL,yL,xR,yR]];
	
	nae = 2; % Number of Animation elements = 2
	xL = q(1)+(x1P*cq3-y1P*sq3); yL = q(2)+(x1P*sq3+y1P*cq3);
	xR = q(4)+(x2A*cq6-y2A*sq6); yR = q(5)+(x2A*sq6+y2A*cq6);
	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(4)+(x2A*cq6-y2A*sq6); yP = q(5)+(x2A*sq6+y2A*cq6);
	temp = [t,xP,yP];
	G_data = [G_data;temp];
end
