1M=[15,-1,-2;-1,14,1;-2,1,16];
2q=[-1;2;3];
3q2=[q(2);q(3)];
4M2=[M(2,2),M(2,3);M(3,2),M(3,3)];
5M1_=[M(2,1);M(3,1)];
6mu=0.1;
7NormD2=M(1,2)*M(1,2)+M(1,3)*M(1,3);
8NormD=sqrt(NormD2);
9e=mu*NormD/M(1,1);
10d=abs(q(1))/NormD;
11p=e*d;
12OD=[-(M(1,2)*q(1))/NormD2;-((-M(1,2)*M(1,2)*q(1))/NormD2+q(1))/M(1,3)];
13D_Dir=[1;-M(1,2)/M(1,3)];
14
15[Vaux,D] = eig(M2);
16d1=D(1,1);
17d2=D(2,2);
18Vaux2=Vaux';
19V=[V(2,1),V(2,2);V(1,1),V(1,2)];
20d1=D(2,2);
21d2=D(1,1);
22OD2=V*OD;
23D_Dir2=V*D_Dir;
24phi=atan(OD2(2)/OD2(1));
25if(OD2(1)<0)
26    phi=phi+acos(-1);
27end
28cosphi=cos(phi);
29sinphi=sin(phi);
30
31q2b=V*q2;
32M1_b=V*M1_;
33a1=-M1_b(1)/mu;
34a2=-M1_b(2)/mu;
35AA=-e*q2b(2)*cosphi;
36BB=e*q2b(1)*sinphi;
37CC=(d1-d2)*p+e*cosphi*q2b(1)-e*sinphi*q2b(2);
38DD=q2b(1)-p*a1;
39EE=-q2b(2)+p*a2;
40
41
42P4=AA-EE;
43P3=-2*CC+2*DD;
44P2=4*BB-2*AA;
45P1=2*CC+2*DD;
46P0=AA+EE;
47
48poly = [P4 P3 P2 P1 P0];
49racines=roots(poly);
50
51r1=racines(4);
52theta=2*atan(r1);
53r=p/(1+e*cos(theta-phi));
54RTb=[r*cos(theta);r*sin(theta)];
55alpha1=(-q2b(2)+a2*r)/RTb(2)-d2;
56alpha2=(-q2b(1)+a1*r)/RTb(1)-d1;
57alpha=alpha1;
58
59zero1=(d1-d2)*r*cos(theta)*sin(theta)+q2b(1)*sin(theta)-q2b(2)*cos(theta)-r*(a1*sin(theta)-a2*cos(theta));
60zero2=(d1-d2)*RTb(1)*RTb(2)+q2b(1)*RTb(2)-q2b(2)*RTb(1)-(a1*RTb(2)-a2*RTb(1))*sqrt(RTb(1)*RTb(1)+RTb(2)*RTb(2));
61RT=V'*RTb
62RESIDUb=(r/mu)*M1_b+([alpha+d1,0;0,alpha+d2])*RTb+q2b
63RESIDU=(r/mu)*M1_+(M2+[alpha,0;0,alpha])*RT+q2
64ONELIPSE2=norm(RTb)/sqrt((D_Dir2(2)*RTb(1)-D_Dir2(1)*RTb(2)-OD2(1)*D_Dir2(2)+D_Dir2(1)*OD2(2))^2/(D_Dir2(1)*D_Dir2(1)+D_Dir2(2)*D_Dir2(2)))-e
65ONELIPSE=(M(1,1)*M(1,1)/(mu*mu))*r*r-(q(1)+M(1,2)*RT(1)+M(1,3)*RT(2))^2
66s1=M(1,1)*norm(RT)/mu
67s2=-q(1)-M(1,2)*RT(1)-M(1,3)*RT(2)
68R=[norm(RT)/mu;RT]
69M*R+q