%% Circle to circle intersection
%% More details at https://lucidar.me/en/mathematics/how-to-calculate-the-intersection-points-of-two-circles
close all;
clear all;
clc;

%% Parameters of the circles
P1=[3;2]; r1=3.4;
P2=[-1;4]; r2=2.2;

%% Draw circles
drawCircle = @(C,R,color) plot ( C(1) + R*cos([0:0.01:2*pi]) , C(2) + R*sin([0:0.01:2*pi]), color );

figure;
hold on;
grid on;
axis square equal;
drawCircle(P1, r1, 'r');
drawCircle(P2, r2, 'g');


%% Calculate d
d=sqrt( (P2(1)-P1(1))*(P2(1)-P1(1)) + (P2(2)-P1(2))*(P2(2)-P1(2)) )


%% Calculate a and b
a = (r1*r1 - r2*r2 + d*d) / (2*d)
b = (r2*r2 - r1*r1 + d*d) / (2*d)

%% Calculate h
h=sqrt(r1*r1 - a*a)

%% Coordinates of P5
P5 = [ P1(1) + (a/d)*(P2(1)-P1(1)) , P1(2) + (a/d)*(P2(2)-P1(2)) ]

%% Intersection points
P3 = [ P5(1) - h*(P2(2)-P1(2))/d ; P5(2) + h*(P2(1)-P1(1))/d] 
P4 = [ P5(1) + h*(P2(2)-P1(2))/d ; P5(2) - h*(P2(1)-P1(1))/d] 
plot (P3(1), P3(2), 'o');
plot (P4(1), P4(2), 'o');