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