% specify start and end position 
% gomi@cslab  21/July/2000

if ~exist('Fig1'); Fig1=figure; end;
figure(Fig1)
set(gca,'PlotBoxAspectRatio',[1,1,1])
set(gca,'ylim',[-0.4,0.6]);  set(gca,'xlim',[-0.4,0.6]);
%set(gca,'ylim',[0,0.8]);  set(gca,'xlim',[-0.4,0.4]); 

disp('Specify a start hand pos in Fig1')
stendpos=ginput(1);
TrialStartX = stendpos(1,1)
TrialStartY = stendpos(1,2)

hold on
plot(TrialStartX,TrialStartY,'r+')

disp('Specify a end hand pos in Fig1')
stendpos=ginput(1);
TrialEndX = stendpos(1,1)
TrialEndY = stendpos(1,2)

plot(TrialEndX,TrialEndY,'ro')

% arm figures
xx = [TrialStartX,TrialEndX];
yy = [TrialStartY,TrialEndY];

Kappa = ((xx.^2 + yy.^2 + L1^2 + L2^2).^2)...
		- 2.0*((xx.^2 + yy.^2).^2 + L1^4 + L2^4);
Kappa = sqrt(Kappa);
th_a(1,:) = atan2(yy, xx) - atan2(Kappa, xx.^2 + yy.^2 + L1^2 - L2^2);
th_a(2,:) = atan2(Kappa, xx.^2 + yy.^2 - L1^2 - L2^2);

el_x = L1*cos(th_a(1,:)); 
el_y = L1*sin(th_a(1,:)); 
ha_x = xx;
ha_y = yy;

if exist('arm1')
  delete(arm1); 
  delete(arm2);
end
arm1=plot([0,el_x(1),ha_x(1)],[0,el_y(1),ha_y(1)],'LineWidth', ...
	      2,'color','k');
arm2=plot([0,el_x(2),ha_x(2)],[0,el_y(2),ha_y(2)],'LineWidth', ...
	      2,'color','k');

pause(0.5)