function theta_y2 = geoy2SL(SL)
syms theta_y2
p0_y2 = [0;0];
p1_y2 = [19.1816*10^(-3);-68.7660*10^(-3)];
p2_y2 = [29.4079*10^(-3);-227.950*10^(-3)];
p3_y2 = [-245.761*10^(-3);-114.832*10^(-3)];
p4_y2 = [214.428*10^(-3);-108.923*10^(-3)];
p5_y2 = [806.212*10^(-3);-106.555*10^(-3)];
r_a_y2=sqrt(sum((p1_y2-p0_y2).^2));
r_b_y2=sqrt(sum((p2_y2-p1_y2).^2));
r_c_y2=sqrt(sum((p3_y2-p2_y2).^2));
r_d_y2=sqrt(sum((p3_y2-p0_y2).^2));
r_e_y2=sqrt(sum((p4_y2-p2_y2).^2));
r_f_y2=sqrt(sum(p5_y2.^2));
r_g0_y2=sqrt(sum((p5_y2-p4_y2).^2));
theta_a0y2=asin((p1_y2(2)-p0_y2(2))/r_a_y2);
theta_ay2=theta_a0y2+theta_y2;
theta_c0y2=atan((p3_y2(2)-p2_y2(2))/(p3_y2(1)-p2_y2(1)));
theta_d0y2=atan2((p3_y2(2)-p0_y2(2)),(p3_y2(1)-p0_y2(1)));
theta_e0y2=atan2((p4_y2(2)-p2_y2(2)),(p4_y2(1)-p2_y2(1)));
theta_f0y2=asin((p5_y2(2)-p0_y2(2))/r_f_y2);
theta_fy2=theta_f0y2+theta_y2;
theta_2y2=-(theta_d0y2-theta_a0y2)+theta_y2;
r_y2=r_d_y2-r_a_y2*cos(theta_2y2);
s_y2=r_a_y2*sin(theta_2y2);
t_y2=sqrt(r_a_y2^2+r_d_y2^2-2*r_a_y2*r_d_y2*cos(theta_2y2));
delta_y2=acos((r_b_y2^2+r_c_y2^2-t_y2.^2)/(2*r_b_y2*r_c_y2));
p_y2=r_b_y2-r_c_y2*cos(delta_y2);
q_y2=r_c_y2*sin(delta_y2);
theta_3y2=atan2((q_y2.*r_y2-p_y2.*s_y2),(p_y2.*r_y2+q_y2.*s_y2));
theta_4y2=delta_y2+theta_3y2;
theta_by2=theta_d0y2+theta_3y2;
theta_cy2=theta_d0y2+theta_4y2;
theta_cey2=pi-(abs(theta_e0y2)+abs(theta_c0y2));
theta_ey2=pi-(theta_cey2-theta_cy2);
va_y2=r_a_y2*[cos(theta_ay2);sin(theta_ay2)];
vb_y2=r_b_y2*[cos(theta_by2);sin(theta_by2)];
vc_y2=r_c_y2*[cos(theta_cy2);sin(theta_cy2)];
vd_y2=r_d_y2*[cos(theta_d0y2);sin(theta_d0y2)];
ve_y2=r_e_y2*[cos(theta_ey2);sin(theta_ey2)];
vf_y2=r_f_y2*[cos(theta_fy2);sin(theta_fy2)];
vg_y2=vd_y2+vc_y2+ve_y2-vf_y2;
r_g_y2=sqrt(sum(vg_y2.^2));
theta_gy2=atan2(vg_y2(2),vg_y2(1));
sl_y2=r_g_y2-r_g0_y2;
eqn=SL==sl_y2;
theta_y2=vpasolve(eqn,theta_y2);
end
0 Comments
Sign in to comment.