pan = [ 193.274 ; -40.571 ; 224.762 ];
tilt = [ 207.774 ; -40.571 ; 185.762 ];
camera = [ 259.774 ; -40.571 ; 185.762 ];
panTh = atan2(tilt(2)-pan(2),tilt(1)-pan(1));

lfr = [ 21.441967 ; 162.43 - 80.022511 + 43 ]
rfr = [ 21.441709 ; 162.43 - 80.021546 + 43 ]

lmd1 = [ -51.24 ; 122.64 ; 67.3 ];
lmdTh = 100/180*pi;

rmd1 = [ -51.24 ; -122.64 ; 67.3 ];
rmdTh = -100/180*pi;

lbk1 = [ -148.66 ; 91.93 ; 67.3 ];
lbkTh = 115/180*pi;

rbk1 = [ -148.66 ; -91.93 ; 67.3 ];
rbkTh = -115/180*pi;

arm1 = [ 115.42 ; 81.31 ; 20 ];
armTh = 30/180*pi;

th = atan2(arm1(2),arm1(1));
d = arm1(3);
r = norm(arm1(1:2));
disp(sprintf('Arm 1 θ: %.8g°',th/pi*180));
disp(sprintf('Arm 1 d: %.8g',d));
disp(sprintf('Arm 1 α: 0'));
disp(sprintf('Arm 1 r: %.8g',r));
disp(sprintf('Arm 1 qOff: %.8g°',(armTh-th)/pi*180));
disp('')

if 0
th = atan2(pan(2),pan(1));
d = pan(3);
r = norm(pan(1:2));
disp(sprintf('Pan θ: %.8g°',th/pi*180));
disp(sprintf('Pan d: %.8g',d));
disp(sprintf('Pan α: 180°'));
disp(sprintf('Pan r: %.8g',r));
disp(sprintf('Pan qOff: %.8g°',-(panTh-th)/pi*180));
disp('')

d = pan(3)-tilt(3);
r = norm(pan(1:2)-tilt(1:2));
disp(sprintf('Tilt θ: %.8g°',0));
disp(sprintf('Tilt d: %.8g',d));
disp(sprintf('Tilt α: -90°'));
disp(sprintf('Tilt r: %.8g',r));
%disp(sprintf('Tilt qOff: %.8g°',(panTh-th)/pi*180));
disp('')

d = pan(2)-tilt(2);
r = norm(camera([1,3])-tilt([1,3]));
disp(sprintf('Camera θ: %.8g°',90));
disp(sprintf('Camera d: %.8g',d));
disp(sprintf('Camera α: %.8g°',90));
disp(sprintf('Camera r: %.8g',r));
disp(sprintf('Camera qOff: %.8g°',90));
disp('')
end

if 0
th2 = atan2(lfr1(2),lfr1(1));
r = norm(lfr1(1:2));
a = r*cos(abs(th2) + abs(lfrTh));
lfr1p=[ a*cos(lfrTh) ; a*sin(lfrTh) ; lfr1(3) ];
disp(sprintf('LFr1 θ: %.8g°',lfrTh/pi*180));
disp(sprintf('LFr1 d: %.8g',lfr1(3)));
disp(sprintf('Lfr1 α: 90°'))
disp(sprintf('Lfr1 r: %.8g',a))
disp(sprintf('Lfr1 point: '))
disp(sprintf('%.8g ',lfr1p'));
disp(sprintf('Lfr2 d: %.8g',norm(lfr2-lfr1p)))
disp('')

th2 = atan2(rfr1(2),rfr1(1));
r = norm(rfr1(1:2));
a = r*cos(abs(th2) + abs(rfrTh));
rfr1p=[ a*cos(rfrTh) ; a*sin(rfrTh) ; rfr1(3) ];
disp(sprintf('RFr1 θ: %.8g°',rfrTh/pi*180));
disp(sprintf('RFr1 d: %.8g',rfr1(3)));
disp(sprintf('Rfr1 α: 90°'))
disp(sprintf('Rfr1 r: %.8g',a))
disp(sprintf('Rfr1 point: '))
disp(sprintf('%.8g ',rfr1p'));
disp(sprintf('Rfr2 d: %.8g',norm(rfr2-rfr1p)))
disp('')
end

function backLegs(name,p1,th1)
	th2 = atan2(p1(2),p1(1));
	r = norm(p1(1:2));
	disp(sprintf('%s θ: %.8g°',name,th2/pi*180));
	disp(sprintf('%s d: %.8g',name,p1(3)));
	disp(sprintf('%s α: 180°',name))
	disp(sprintf('%s r: %.8g',name,r))
	disp(sprintf('%s qOff: %.8g°',name,-(th1-th2)/pi*180))
	disp('')
end

backLegs('LMd1',lmd1,lmdTh)
backLegs('RMd1',rmd1,rmdTh)
backLegs('LBk1',lbk1,lbkTh)
backLegs('RBk1',rbk1,rbkTh)

aul2 = [ -37 -40 0 ];
disp(sprintf('Aurora knee θ: %.8g°',atan2(aul2(2),aul2(1))/pi*180))
disp(sprintf('Aurora knee d: %.8g',0))
disp(sprintf('Aurora knee α: 180°'))
disp(sprintf('Aurora knee r: %.8g',norm(aul2(1:2))))
disp(sprintf('Aurora knee qOff: %.8g°',(pi+atan2(aul2(2),aul2(1)))/pi*180))

