intersection.m

Active approach - TOA Inter

function [t_1, t_2]=intersection(M)
%
% [t_1, t_2]=intersection(M)
%
% Function computs the intersection of the three circles in 3D. 
%
% M - The center's coordinates and radius of the every circle are in the
%     row of the matrix M. The center's coordinates of the first circle are
%     [0,0,0]. The size of the matrix M is 3*4. 
%
% t_1, t_2 - the coordinates of the two intersections. 
%            If the circles have only one intersection, the coordinates
%            this intersection and [i;i;i] are the output. 
%            If the circles have not the intersection or have infinitely 
%            number of the intersections, the pair [2*i;2*i;2*i],
%            [2*i;2*i;2*i] or [3*i;3*i;3*i], [3*i;3*i;3*i] is the output.   
           


%aktivny pristup
%na vstupe matica rozmerov 3*4, kde v riadku budu suradnice prijimaca v 3D
%priestore a jeho vzdialenost od vysielaca T
%program na vypocet suradnic vysielaca v 3D priestore 


x1=0;
x2=M(2,1);
x3=M(3,1);
y1=0;
y2=M(2,2);
y3=M(3,2);
z1=0;
z2=M(2,3);
z3=M(3,3);
r1=M(1,4);
r2=M(2,4);
r3=M(3,4);
s2=0.5*(r1^2-r2^2+x2^2+y2^2+z2^2);
s3=0.5*(r1^2-r3^2+x3^2+y3^2+z3^2);
s1=0.5*(r3^2-r2^2-x3^2-y3^2-z3^2+x2^2+y2^2+z2^2);
nula=10^(-10);
if (abs(x2-x3)>=nula) %x2~=x3
    if (abs(y2*x3-y3*x2)>=nula) %y2*x3~=y3*x2
        p1=(x3*s2-x2*s3)/(y2*x3-y3*x2);
        p2=(z2*x3-z3*x2)/(y2*x3-y3*x2);
        p3=s1/(x2-x3);
        p4=(y2-y3)/(x2-x3);
        p5=(z2-z3)/(x2-x3);
        k1=(p4*p2-p5)^2+p2^2+1;
        k2=2*((p3-p1*p4)*(p4*p2-p5)-(p1*p2));
        k3=(p3-p1*p4)^2+p1^2-r1^2;
        if (((k2^2)-(4*k1*k3))>=nula) %(k2^2>4*k1*k3)
            z_1=(-k2+sqrt(k2^2-4*k1*k3))/(2*k1);
            z_2=(-k2-sqrt(k2^2-4*k1*k3))/(2*k1);
            x_1=(p3-p1*p4)+(p4*p2-p5)*z_1;
            x_2=(p3-p1*p4)+(p4*p2-p5)*z_2;
            y_1=p1-p2*z_1;
            y_2=p1-p2*z_2;
            t_1=[x_1, y_1, z_1];
            t_2=[x_2, y_2, z_2];
%             r_1=[sqrt((t_1(1,1)-x1)^2+(t_1(2,1)-y1)^2+(t_1(3,1)-z1)^2);
%                  sqrt((t_1(1,1)-x2)^2+(t_1(2,1)-y2)^2+(t_1(3,1)-z2)^2);
%                  sqrt((t_1(1,1)-x3)^2+(t_1(2,1)-y3)^2+(t_1(3,1)-z3)^2)]
%             r_2=[sqrt((t_2(1,1)-x1)^2+(t_2(2,1)-y1)^2+(t_2(3,1)-z1)^2);
%                  sqrt((t_2(1,1)-x2)^2+(t_2(2,1)-y2)^2+(t_2(3,1)-z2)^2);
%                  sqrt((t_2(1,1)-x3)^2+(t_2(2,1)-y3)^2+(t_2(3,1)-z3)^2)]
        elseif (abs((k2^2)-(4*k1*k3))<nula) %(k2^2==4*k1*k3)
                z=-k2/(2*k1);
                x=(p3-p1*p4)+(p4*p2-p5)*z;
                y=p1-p2*z;
                t_1=[x, y, z];
                t_2=[i, i, i];
%                 r_1=[sqrt((t_1(1,1)-x1)^2+(t_1(2,1)-y1)^2+(t_1(3,1)-z1)^2);
%                      sqrt((t_1(1,1)-x2)^2+(t_1(2,1)-y2)^2+(t_1(3,1)-z2)^2);
%                      sqrt((t_1(1,1)-x3)^2+(t_1(2,1)-y3)^2+(t_1(3,1)-z3)^2)]
        else t_1=[2*i, 2*i, 2*i];
             t_2=[2*i, 2*i, 2*i];
             %write('nema riesenie-komplexne cisla')
        end
    else %(y2*x3==y3*x2) 
        if  (abs(z2*x3-z3*x2)>=nula) %(z2*x3~=z3*x2)
            p1=(x3*s2-x2*s3)/(z2*x3-z3*x2);
            p2=(s1/(x2-x3))-(((z2-z3)*p1)/(x2-x3));
            p3=-(y2-y3)/(x2-x3);
            if ((((2*p3*p2)^2)-(4*(1+p3^2)*(p2^2+p1^2-r1^2)))>=nula) %(((2*p3*p2)^2)>(4*(1+p3^2)*(p2^2+p1^2-r1^2)))
                y_1=(-2*p2*p3+sqrt((2*p3*p2)^2-4*(1+p3^2)*(p2^2+p1^2-r1^2)))/(2*(1+p3^2));
                y_2=(-2*p2*p3-sqrt((2*p3*p2)^2-4*(1+p3^2)*(p2^2+p1^2-r1^2)))/(2*(1+p3^2));
                x_1=p3*y_1+p2;
                x_2=p3*y_2+p2;
                z_1=p1;
                z_2=p1;
                t_1=[x_1,y_1,z_1];
                t_2=[x_2,y_2,z_2];
%                 r_1=[sqrt((t_1(1,1)-x1)^2+(t_1(2,1)-y1)^2+(t_1(3,1)-z1)^2);
%                      sqrt((t_1(1,1)-x2)^2+(t_1(2,1)-y2)^2+(t_1(3,1)-z2)^2);
%                      sqrt((t_1(1,1)-x3)^2+(t_1(2,1)-y3)^2+(t_1(3,1)-z3)^2)]
%                 r_2=[sqrt((t_2(1,1)-x1)^2+(t_2(2,1)-y1)^2+(t_2(3,1)-z1)^2);
%                      sqrt((t_2(1,1)-x2)^2+(t_2(2,1)-y2)^2+(t_2(3,1)-z2)^2);
%                      sqrt((t_2(1,1)-x3)^2+(t_2(2,1)-y3)^2+(t_2(3,1)-z3)^2)]
            elseif (abs(((2*p3*p2)^2)-(4*(1+p3^2)*(p2^2+p1^2-r1^2)))<nula) %(((2*p3*p2)^2)==(4*(1+p3^2)*(p2^2+p1^2-r1^2)))
                y=(-2*p2*p3)/(2*(1+p3^2));
                x=p3*y+p2;
                z=p1;
                t_1=[x,y,z];
                t_2=[i,i,i];
%                 r_1=[sqrt((t_1(1,1)-x1)^2+(t_1(2,1)-y1)^2+(t_1(3,1)-z1)^2);
%                      sqrt((t_1(1,1)-x2)^2+(t_1(2,1)-y2)^2+(t_1(3,1)-z2)^2);
%                      sqrt((t_1(1,1)-x3)^2+(t_1(2,1)-y3)^2+(t_1(3,1)-z3)^2)]
            else t_1=[2*i, 2*i, 2*i];
                 t_2=[2*i, 2*i, 2*i];
                 %write('nema riesenie-komplexne cisla')
            end
        else %(y2*x3==y3*x2)&&(z2*x3==z3*x2)
            if (abs(y2*z3-y3*z2)>=nula) %(y2*z3~=y3*z2)
                p1=(z3*s2-z2*s3)/(y2*z3-y3*z2);
                p2=(s1/(x2-x3))-((y2-y3)*p1)/(x2-x3);
                p3=-(z2-z3)/(x2-x3);
                if (((2*p3*p2)^2-4*(1+p3^2)*(p2^2+p1^2-r1^2))>=nula)
                    z_1=(-2*p2*p3+sqrt((2*p3*p2)^2-4*(1+p3^2)*(p2^2+p1^2-r1^2)))/(2*(1+p3^2));
                    z_2=(-2*p2*p3-sqrt((2*p3*p2)^2-4*(1+p3^2)*(p2^2+p1^2-r1^2)))/(2*(1+p3^2));
                    x_1=p3*z_1+p2;
                    x_2=p3*z_2+p2;
                    y_1=p1;
                    y_2=p1;
                    t_1=[x_1,y_1,z_1];
                    t_2=[x_2,y_2,z_2];
%                     r_1=[sqrt((t_1(1,1)-x1)^2+(t_1(2,1)-y1)^2+(t_1(3,1)-z1)^2);
%                          sqrt((t_1(1,1)-x2)^2+(t_1(2,1)-y2)^2+(t_1(3,1)-z2)^2);
%                          sqrt((t_1(1,1)-x3)^2+(t_1(2,1)-y3)^2+(t_1(3,1)-z3)^2)]
%                     r_2=[sqrt((t_2(1,1)-x1)^2+(t_2(2,1)-y1)^2+(t_2(3,1)-z1)^2);
%                          sqrt((t_2(1,1)-x2)^2+(t_2(2,1)-y2)^2+(t_2(3,1)-z2)^2);
%                          sqrt((t_2(1,1)-x3)^2+(t_2(2,1)-y3)^2+(t_2(3,1)-z3)^2)]
                elseif (abs((2*p3*p2)^2-4*(1+p3^2)*(p2^2+p1^2-r1^2))<nula)
                    z=(-2*p2*p3)/(2*(1+p3^2));
                    x=p3*z+p2;
                    y=p1;
                    t_1=[x,y,z];
                    t_2=[i,i,i];
%                     r_1=[sqrt((t_1(1,1)-x1)^2+(t_1(2,1)-y1)^2+(t_1(3,1)-z1)^2);
%                          sqrt((t_1(1,1)-x2)^2+(t_1(2,1)-y2)^2+(t_1(3,1)-z2)^2);
%                          sqrt((t_1(1,1)-x3)^2+(t_1(2,1)-y3)^2+(t_1(3,1)-z3)^2)]
                else t_1=[2*i, 2*i, 2*i];
                     t_2=[2*i, 2*i, 2*i];
                     %write('nema riesenie-komplexne cisla')  
                end
            else %(z2*x3==z3*x2)&&(y2*z3==y3*z2)&&(y2*x3==y3*x2)
                if ((abs(x3*s2-x2*s3)>=nula) || (abs(y3*s2-y2*s3)>=nula) || (abs(z3*s2-z2*s3)>=nula))
                    t_1=[2*i, 2*i, 2*i];
                    t_2=[2*i, 2*i, 2*i];
                    %write('nema riesenie 0~=1')
                else %((x3*s2)==(x2*s3)...)
                    if (abs(x2)<nula)
                        if (abs(r1^2-r2^2)>=nula)
                            t_1=[2*i, 2*i, 2*i];
                            t_2=[2*i, 2*i, 2*i];
                            %write('nema riesenie - prvy a druhy prijimac su rovnake, ale vzdial. ciela ine')
                        else t_1=[3*i, 3*i, 3*i];
                             t_2=[3*i, 3*i, 3*i];
                             %write ('nekonecne vela rieseni - na urcenie ciela potrebujem dalsi prijimac')
                        end
                    else %(x2~=0)
                        if (abs(x3)<nula)
                            if (abs(r1^2-r3^2)>=nula)
                                t_1=[2*i, 2*i, 2*i];
                                t_2=[2*i, 2*i, 2*i];
                                %write('nema riesenie - prvy a treti prijimac su rovnake, ale vzdialenosti ciela ine')
                            else t_1=[3*i, 3*i, 3*i];
                                 t_2=[3*i, 3*i, 3*i];
                                 %write ('nekonecne vela rieseni - na urcenie ciela potrebujem dalsi prijimac')
                            end
                        else %(x2~=0)&&(x3~=0)
                            if (abs(y2)<nula)
                                if (abs(y3)<nula)
                                    if ((abs(z2-z3)<nula) && (abs(z2)>=nula) && (abs(z3)>=nula))
                                        x=s1/(x2-x3);
                                        z=s2/z2-(s1*x2)/((x2-x3)*z2);
                                        if ((r1^2-x^2-z^2)>=nula)
                                            y_1=sqrt(r1^2-x^2-z^2);
                                            y_2=-sqrt(r1^2-x^2-z^2);
                                            t_1=[x,y_1,z];
                                            t_2=[x,y_2,z];
%                                             r_1=[sqrt((t_1(1,1)-x1)^2+(t_1(2,1)-y1)^2+(t_1(3,1)-z1)^2);
%                                                  sqrt((t_1(1,1)-x2)^2+(t_1(2,1)-y2)^2+(t_1(3,1)-z2)^2);
%                                                  sqrt((t_1(1,1)-x3)^2+(t_1(2,1)-y3)^2+(t_1(3,1)-z3)^2)]
%                                             r_2=[sqrt((t_2(1,1)-x1)^2+(t_2(2,1)-y1)^2+(t_2(3,1)-z1)^2);
%                                                  sqrt((t_2(1,1)-x2)^2+(t_2(2,1)-y2)^2+(t_2(3,1)-z2)^2);
%                                                  sqrt((t_2(1,1)-x3)^2+(t_2(2,1)-y3)^2+(t_2(3,1)-z3)^2)]
                                        elseif (abs(r1^2-x^2-z^2)<nula)
                                            t_1=[x,0,z];
                                            t_2=[i,i,i];
%                                             r_1=[sqrt((t_1(1,1)-x1)^2+(t_1(2,1)-y1)^2+(t_1(3,1)-z1)^2);
%                                                  sqrt((t_1(1,1)-x2)^2+(t_1(2,1)-y2)^2+(t_1(3,1)-z2)^2);
%                                                  sqrt((t_1(1,1)-x3)^2+(t_1(2,1)-y3)^2+(t_1(3,1)-z3)^2)]
                                        else t_1=[2*i, 2*i, 2*i];
                                             t_2=[2*i, 2*i, 2*i];
                                             %write('nema riesenie - komplexne cisla')
                                        end
                                    elseif (abs(z2)<nula) %z2==0
                                            if (abs(z3)>=nula) %z3~=0