cir_cir_intersection.m

toa代码仿真

function [ex,ey]=cir_cir_intersection(bx,by,d)

        
        if  abs(diff(by)/(diff(bx)+eps)) > 1e-2
            a_c=-(bx(1)-bx(2))/(by(1)-by(2)+eps);
            b_c=((bx(1)^2-bx(2)^2)+(by(1)^2-by(2)^2)-(d(1)^2-d(2)^2))/(2*(by(1)-by(2))+eps);
        
            c_c=1+a_c^2;
            d_c=-2*bx(1)+2*a_c*(b_c-by(1));
            e_c=bx(1)^2+(b_c-by(1))^2-d(1)^2;
        
            ex=[ (-d_c+sqrt(d_c^2-4*c_c*e_c))/(2*c_c)  (-d_c-sqrt(d_c^2-4*c_c*e_c))/(2*c_c)];
            ey=a_c*ex+b_c;
        else
            bc=-(by(1)-by(2))/(bx(1)-bx(2)+eps);
            ac=((bx(1)^2-bx(2)^2)+(by(1)^2-by(2)^2)-(d(1)^2-d(2)^2))/(2*(bx(1)-bx(2))+eps);
            
            a=1+bc^2;
            b=-(2*bx(1)*bc+2*by(1)-2*ac*bc);
            c=ac^2-2*bx(1)*ac+bx(1)^2+by(1)^2-d(1)^2;
            
            ey=[(-b+sqrt(b^2-4*a*c))/(2*a) (-b-sqrt(b^2-4*a*c))/(2*a)];
            ex=ac+ey*bc;
        end