R1x_star = 20.96*cos(113*pi/180);
R1y_star = 20.96*sin(113*pi/180);

R2x_star = 23.61*cos(96*pi/180);
R2y_star = 23.61*sin(96*pi/180);

R3x_star = 24.0*cos(81*pi/180);
R3y_star = 24.*sin(81*pi/180);

alp2 = 5.7306;
alp3 = -1.1639;

d1x = R3x_star*cos(alp2)-R3y_star*sin(alp2)-R2x_star*cos(alp3)+R2y_star*sin(alp3);
d1y = R3y_star*cos(alp2)+R3x_star*sin(alp2)-R2y_star*cos(alp3)-R2x_star*sin(alp3);

qd1 = atan2(d1y,d1x);

d2x = R1x_star*cos(alp3)-R1y_star*sin(alp3)-R3x_star;
d2y = R1y_star*cos(alp3)+R1x_star*sin(alp3)-R3y_star;

qd2 = atan2(d2y,d2x);

d3x = R2x_star -R1x_star*cos(alp2)+R1y_star*sin(alp2);
d3y = R2y_star -R1y_star*cos(alp2)-R1x_star*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
psi2a = qd1-qd2-acos( (d3^2-d1^2-d2^2)/(2*d1*d2));
psi2b = qd1-qd2+acos( (d3^2-d1^2-d2^2)/(2*d1*d2));

%checking for trivial solution
if psi2b==alp2
      psi2 = psi2a
   else
      psi2 = psi2b
end


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

coeff=zeros(4,4);
rhs = zeros(4,1);
dyad_right = zeros(4,1);
W_star=zeros(2);
Z_star=zeros(2);


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

rhs = [R1x_star; R1y_star; R2x_star; R2y_star];

dyad_right = coeff\rhs;

Wx_star = dyad_right(1);
Wy_star = dyad_right(2);
qw_star = atan2(Wy_star,Wx_star);
W_star =[sqrt(Wx_star^2+Wy_star^2); qw_star*180/pi];

Zx_star = dyad_right(3);
Zy_star = dyad_right(4);
qz_star = atan2(Zy_star,Zx_star);
Z_star= [sqrt(Zx_star^2+Zy_star^2);qz_star*180/pi];



   

