% RotBar.m
%
% Kinematic analysis of Rotating L-shape Bar
%
% Copyright (C) 2007 清水 信行, 曽我部 潔 All Rights Reserved.

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

% Setting of Parameters
x1A = -1; y1A = 0;
x1B = 1;  y1B = 0;
x1C = 1;  y1C = 1;

% Initialization
close;
A_data = []; % Initialize the Animation data
G_data = []; % Initialize the Graphics data

% Initial state of the mechanism
q = [0.7071 0.7071 7*pi/4]';

% Time counter
Ts = 0; Te = 10; dt = 0.025

for t = Ts:dt:Te
	% Displacement analysis
	iteration = 0; iconv = 1;
	while (iteration <= 20) & (iconv == 1)
		sq3 = sin(q(3)); cq3 = cos(q(3));
		Fq = [ 1 0 -(x1A*sq3+y1A*cq3)
		      0 1  (x1B*cq3-y1B*sq3)
		      0 0   1 ];
		F = [ q(1)+(x1A*cq3-y1A*sq3)
		q(2)+(x1B*sq3+y1B*cq3)
		q(3)-(-pi/4+0.5*t^2) ];
		dq = -Fq\F;
		q = q+dq;
		iconv = 0;
		for i = 1:3
			if(abs(dq(i)>0.001)) iconv = 1; end
		end
		iteration = iteration+1;
	end
	
	% Velocity analysis
	sq3 = sin(q(3)); cq3 = cos(q(3));
	Fq = [ 1 0 -(x1A*sq3+y1A*cq3)
	      0 1  (x1B*cq3-y1B*sq3)
	      0 0   1 ];
	v = [ 0 0 t ]';
	qd = Fq\v;
	
	% Acceleration analysis
	gam = [(x1A*cq3-y1A*sq3)*qd(3)^2
	       (x1B*sq3+y1B*cq3)*qd(3)^2
	       1 ];
	qdd = Fq\gam;
	
	% Animation
	plot([0,0],[0,0])
	axis square
	xmin = -2.5; xmax = 2.5; ymin = -2.5; ymax = 2.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)+(x1A*cq3-y1A*sq3); yL = q(2)+(x1A*sq3+y1A*cq3);
	xR = q(1)+(x1B*cq3-y1B*sq3); yR = q(2)+(x1B*sq3+y1B*cq3);
	plot([xL,xR],[yL,yR],'linewidth',0.5);
	temp = [temp,[xL,yL,xR,yR]];
	
	nae = 2; % Number of Animation element = 2
	xL = q(1)+(x1B*cq3-y1B*sq3); yL = q(2)+(x1B*sq3+y1B*cq3);
	xR = q(1)+(x1C*cq3-y1C*sq3); yR = q(2)+(x1C*sq3+y1C*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 Graphiacs data for the point of interest
	xP = q(1)+(x1C*cq3-y1C*sq3); yP = q(2)+(x1C*sq3+y1C*cq3);
	temp = [t,xP,yP];
	G_data = [G_data;temp];
end
