function [ intersections ] = Trilateration( centers, radii )
%TRILATERATION intersection of 3 spheres
%   centers is 3x3, where each 3x1 column represents one center
%   radii is 3x1, where each 1x1 element represents the radius of the
%   corresponding sphere
 
p1 = centers(:,1);
p2 = centers(:,2);
p3 = centers(:,3);
r1 = radii(1);
r2 = radii(2);
r3 = radii(3);

 p21 = p2-p1;
 p31 = p3-p1;
 c = cross(p21,p31);
 c2 = sum(c.^2);
 u1 = cross(((sum(p21.^2)+r1^2-r2^2)*p31 - (sum(p31.^2)+r1^2-r3^2)*p21)/2,c)/c2;
 v = sqrt(r1^2-sum(u1.^2))*c/sqrt(c2);
 i1 = p1+u1+v;
 i2 = p1+u1-v;
 
 intersections=[i1 i2];

end

