%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% //-- Program to calculate the length of the legs --\\                 %%    
%%                                                                       %%
%% // Designed by: Marco Eckert 21/07/2011                               %% 
%%                                                                       %%
%% // Version 1.23                                                       %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% centre of the ball on the top of each leg for the leg direction calculation

ballcentre_1 = TCP {p} + ed_1;
ballcentre_2 = TCP {p} + ed_2;
ballcentre_3 = TCP {p} + ed_3;

% Calculation for the 1.actuator:

sphere_1 = [ 1 -( 2*ballcentre_1(2,1)) +( ballcentre_1(1,1)^2 + ballcentre_1(2,1)^2 +...
            railline_1(3,1)^2 - 2*railline_1(3,1)*ballcentre_1(3,1) + ballcentre_1(3,1)^2 - l(1,1)^2)];

Q_1 = roots(sphere_1);

P_11 = railline_1 + [0; Q_1(1,1); 0];

P_12 = railline_1 + [0; Q_1(2,1); 0];

if (P_11(2,1) > ballcentre_1(2,1))
    leg_1 = -P_12 + ballcentre_1;
else
    leg_1 = -P_11 + ballcentre_1;
end

% Calculation for the 2.actuator:

sphere_2 = [ 1 -( 2*ballcentre_2(2,1)) +( ballcentre_2(1,1)^2 + ballcentre_2(2,1)^2 +...
            railline_2(3,1)^2 - 2*railline_2(3,1)*ballcentre_2(3,1) + ballcentre_2(3,1)^2 - l(2,1)^2)];

Q_2 = roots(sphere_2);

P_21 = railline_2 + [0; Q_2(1,1); 0];

P_22 = railline_2 + [0; Q_2(2,1); 0];

if (P_21(2,1) > ballcentre_2(2,1))
    leg_2 = -P_22 + ballcentre_2;
else
    leg_2 = -P_21 + ballcentre_2;
end 

% Calculation for the 3.actuator:

sphere_3 = [ 1 -( 2*ballcentre_1(2,1)) +( ballcentre_3(1,1)^2 + ballcentre_3(2,1)^2 +...
            railline_3(3,1)^2 - 2*railline_3(3,1)*ballcentre_3(3,1) + ballcentre_3(3,1)^2 - l(3,1)^2)];

Q_3 = roots(sphere_3);

P_31 = railline_3 + [0; Q_3(1,1); 0];

P_32 = railline_3 + [0; Q_3(2,1); 0];


if (P_31(2,1) > ballcentre_3(2,1))
    leg_3 = -P_32 + ballcentre_3;
else
    leg_3 = -P_31 + ballcentre_3;
end