% this function is zero for values of theta that are valid solutions
% for the forward Stewart Platform problem
% Inputs: theta (the angle between the platform and the x-axis), and the length of struts 1, 2 and 3
function out = f(theta, p1, p2, p3)                                                         
                                                                                
L1 = 3;             % the length of side one of the platform connecting struts 2 and 3                                               
L2 = 3 * sqrt(2);   % the length of the side of the platform connecting struts 1 and 3
L3 = 3;             % the length of the side of the platform connecting struts 1 and 2 
gam = pi / 4;       % the angle between sides L2 and L3                                                                              
                    % the strut p1 is assumed to be at coordinates (0, 0)
x1 = 5;             % the x coordinate where p2 connects to the x axis                                                                         
                    % p2 is assumed to be anchored to the x axis
x2 = 0;             % the x coordinate of p3's anchor                                                                         
y2 = 6;             % the y coordinate of p3's anchor                                                                                                                                     
                                                                                
A2 = L3*cos(theta)-x1;                                                          
B2 = L3*sin(theta);                                                             
A3 = L2*[cos(theta)*cos(gam) - sin(theta)*sin(gam)] - x2;                       
B3 = L2*[cos(theta)*sin(gam) - sin(theta)*cos(gam)] - y2;                       
D = 2.*(A2.*B3-B2.*A3);                                                         
N1 = B3.*(p2.^2-p1.^2 - A2.^2 - B2.^2) - B2.*(p3.^2 - p1.^2 -A3.^2 - B3.^2);    
N2 = -A3 .*(p2.^2-p1.^2 - A2.^2 - B2.^2) + A2.*(p3.^2 - p1.^2 -A3.^2 - B3.^2);  
                                                                                
out = N1.^2+N2.^2-p1.^2.*D.^2;