%function[phi2 , phi3 , phi4 , phi5] = analysis_position_exemple_1(phi1)
% les entr�es
AB = 0.15;
AC = 0.1;
CD = 0.15;
DF = 0.4;
AG = 0.3;
phi1= 30* pi/180;
wz01 = 50 ; % rd/s
xA = 0;
yA = 0;
xC = 0;
% Point A
w10 = [0;0;wz10]
rA = [xA;yA;0];
% Point B
xB = AB * cos(phi1);
yB = AB * sin(phi1);
rB = [xB;yB;0];
% Point C
yC = AC;
rC = [xC;yC;0];
% Point D
syms solxD solyD
eqnD1 = (solxD-xC)^2 + (solyD-yC)^2 == CD^2;
eqnD2 = '(solxD-xC)*(yB-yC) == (xB-xC)*(solyD-yC)';
solD = solve(eqnD1 , eqnD2 , solxD , solyD );
xD1 = eval(solD.solxD(1));
yD1 = eval(solD.solyD(1));
xD2 = eval(solD.solxD(2));
yD2 = eval(solD.solyD(2));
if xD1 < xC
xD = xD1;
yD = yD1;
else
xD = xD2;
yD = yD2;
end
rD = [xD;yD;0];
% les angles
phi2 = pi - atan2(yB-yD,xD-xB)
phi3 = phi2
phi4 = pi- atan2(yD-yC,xC-xD)
phi5 = phi4
% Point F
xF = DF * cos(phi2) + xD;
yF = DF * sin(phi2) + yD;
syms solxF solyF
eqnF1 = '(solxF-xD)^2 + (solyF-yD)^2 == DF^2';
eqnF2 = '(solxF-xD)*(yC-yD) == (xC-xD)*(solyF-yD)';
solF = solve(eqnF1 , eqnF2 , solxF , solyF );
xF1 = eval(solF.solxF(1));
yF1 = eval(solF.solyF(1));
xF2 = eval(solF.solxF(2));
yF2 = eval(solF.solyF(2));
if xF1 > xB
xF = xF1;
yF = yF1;
else
xF = xF2;
yF = yF2;
end
rF = [xF;yF;0]
% Point G
syms solxG solyG
eqnG1 = '(solxG-xA)^2 + (solyG-yA)^2 - AG^2';
eqnG2 = '(solxG-xA)*(yD-yA) - (xD-xA)*(solyG-yA)';
solG = solve(eqnG1 , eqnG2 , 'solxG , solyG' );
xG1 = eval(solG.solxG(1));
yG1 = eval(solG.solyG(1));
xG2 = eval(solG.solxG(2));
yG2 = eval(solG.solyG(2));
if xG1 < xA
xG = xG1;
yG = yG1;
else
xG = xG2;
yG = yG2;
end
rG = [xG;yG;0]
% Graphe
plot([xA,xB],[yA,yB],'b-o','lineWidth',2)
hold on
plot([xB,xC],[yB,yC],'g-o','lineWidth',2)
hold on
plot([xB,xF],[yB,yF],'r-o','lineWidth',2)
hold on
plot([xA,xD],[yA,yD],'y-o','lineWidth',2)
hold on
plot([xC,xD],[yC,yD],'g-o','lineWidth',2)
hold on
plot([xD,xG],[yD,yG],'k-o','lineWidth',2)
hold on
fprintf('rA = [ %g, %g, %g ] (m)\n', rA)
xlabel('x (m)'), ylabel('y (m)');
axis([-0.35 0.3 -0.3 0.4]);
text(xA,yA,'\leftarrow A = ground','HorizontalAlignment','left')
text(xC,yC,'\leftarrow C = ground','HorizontalAlignment','left')
text(xB,yB+0.005,'B')
text(xD,yD+0.005,'D')
text(xF,yF+0.005,'F')
text(xG,yG+0.005,'G')
% %Recherche des inconnus :
% syms solwz21 solwz03
% w21 = [0;0;solwz21];
% w03 = [0;0;solwz03];
% vB32 = solvB32 * [cos(phi3) ; sin(phi3); 0 ];
% w05 = [0;0;solwz05];
% w43 = [0;0;solwz43];
% vD54 = solvD54 * [cos(phi5) ; sin(phi5); 0 ];
%
% eqn1 = wz10 + solwz21 + solwz03 == 0 ;
% eqn2 = vB32 + cross(rA,w01) + cross(rB,w21) + cross(rC,w03) == 0 ;
% eqn2x = eqn2(1);
% eqn2y = eqn2(2);
% eqn3 = - solwz03 + solwz43 + solwz05 == 0 ;
% eqn4 = vD54 + cross(rA,w05) + cross(rC,-w03) + cross(rD,w43);
% eqn4x = eqn4(1);
% eqn4y = eqn4(2);
%
% solu = solve(eqn1,eqn2x,eqn2y,eqn3,eqn4x,eqn4y, solwz21,solwz03,vB32,solwz43,solwz05,solvD54);
%
% wz21 =
% wz03 =
A = [ 1 1 0 0 0 ; 0 -1 1 1 0 0 ; yB yC 0 0 cos(phi3) 0 ; xB xC 0 0 -sin(phi3) 0 ; 0 -yC yD yA 0 cos(phi5) ; 0 -xC xD xA 0 -sin(phi5)];