R1x = 3.42*cos(50*pi/180);
R1y = 3.42*sin(50*pi/180);

R2x = 3.27*cos(70*pi/180);
R2y = 3.27*sin(70*pi/180);

R3x = 4.25*cos(47*pi/180);
R3y = 4.25*sin(47*pi/180);

alp2 = 28*pi/180;
alp3 = 13*pi/180;

d1x = R3x*cos(alp2)-R3y*sin(alp2)-R2x*cos(alp3)+R2y*sin(alp3);
d1y = R3y*cos(alp2)+R3x*sin(alp2)-R2y*cos(alp3)-R2x*sin(alp3);

qd1 = atan2(d1y,d1x);

d2x = R1x*cos(alp3)-R1y*sin(alp3)-R3x;
d2y = R1y*cos(alp3)+R1x*sin(alp3)-R3y;

qd2 = atan2(d2y,d2x);

d3x = R2x -R1x*cos(alp2)+R1y*sin(alp2);
d3y = R2y -R1y*cos(alp2)-R1x*sin(alp2);

qd3 = atan2(d3y,d3x);

d1 = sqrt(d1x^2+d1y^2);
d2 = sqrt(d2x^2+d2y^2);
d3 = sqrt(d3x^2+d3y^2);

%one of these--the trivial solution--will be the same as alp2
bet2a = qd1-qd2-acos( (d3^2-d1^2-d2^2)/(2*d1*d2));
bet2b = qd1-qd2+acos( (d3^2-d1^2-d2^2)/(2*d1*d2));

%checking for trivial solution
if bet2a==alp2
      beta2 = bet2b
   else
      beta2 = bet2a
end


beta3 = atan2(-d1*sin(qd1)-d2*sin(qd2+beta2),-d1*cos(qd1)-d2*cos(qd2+beta2))-qd3;

coeff=zeros(4,4);
rhs = zeros(4,1);
dyad_left = zeros(4,1);
W=zeros(2);
Z=zeros(2);


coeff=[1 0 1 0;
   0 1 0 1;
   cos(beta2) -sin(beta2) cos(alp2) -sin(alp2);
   sin(beta2) cos(beta2) sin(alp2) cos(alp2);]

rhs = [R1x; R1y; R2x; R2y];

dyad_left = coeff\rhs;

Wx = dyad_left(1);
Wy = dyad_left(2);
qw = atan2(Wy,Wx);
W =[sqrt(Wx^2+Wy^2); qw*180/pi];

Zx = dyad_left(3);
Zy = dyad_left(4);
qz = atan2(Zy,Zx);
Z = [sqrt(Zx^2+Zy^2);qz*180/pi];



   

