f2DStewart

0001 function [result x y] = f2DStewart(theta, gamma, L2, L3, p1, p2, p3, x1, x2, y2)
0002 
0003 A2 = L3 * cos(theta) - x1;
0004 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0005 
0006 B2 = L3 * sin(theta);
0007 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0008 
0009 D = 2 * ( A2 * B3 - B2 * A3 );
0010 
0011 % test if D == 0; if yes, throw error
0012 
0013 N_term1 = p2^2 - p1^2 - A2^2 - B2^2;
0014 N_term2 = p3^2 - p1^2 - A3^2 - B3^2;
0015 
0016 N1 = ( B3 * N_term1 - B2 * N_term2 );
0017 N2 = (-A3 * N_term1 + A2 * N_term2 );
0018 
0019 x = N1 / D;
0020 y = N2 / D;
0021 
0022 result = N1^2 + N2^2 - p1^2 * D^2;
0023 
0024 end