Discover MakerZone

MATLAB and Simulink resources for Arduino, LEGO, and Raspberry Pi

Learn more

Discover what MATLAB® can do for your career.

Opportunities for recent engineering grads.

Apply Today

Thread Subject:
calculate the intersection of 3 sphere

Subject: calculate the intersection of 3 sphere

From: marcel

Date: 26 Mar, 2012 13:07:10

Message: 1 of 7

hello every baby.
I try to calculate the coordinate of two points of intersection 3-sphere, it is analytically complicated, so I asked whether it is possible to use subs in the solve function to pass parameters in the function?, and if yes how to do that?
example:
a1 = 4; b1 = 1; c1 = 2; a2 = 5, B2 = 2; c2 = 2; a3 = 5, b3 = 1; c3 = 3;
[x,y,z]=solve('(x-a1)^2+(y-b2)^2+(z-c2)=1','(x-a2)+(y-b2)+(z-c2)^2=1','(x-a3)^2+(y-b3)^2+(z-c3)^2=1')
Many Thanks
marcel

Subject: calculate the intersection of 3 sphere

From: Bruno Luong

Date: 26 Mar, 2012 13:31:10

Message: 2 of 7

"marcel " <djmoumouh87@hotmail.fr> wrote in message <jkpplu$li7$1@newscl01ah.mathworks.com>...
> hello every baby.
> I try to calculate the coordinate of two points of intersection 3-sphere [snip]...

I dig out of my stuffs a function that does just that:

function [Y1 Y2 errflag] = trilateration(xc, yc, zc, r)
%
% [Y1 Y2 errflag] = trilateration(xc, yc, zc, r)
%
% Purpose: find the two intersections of *three* spheres centered at
% (XC,YC,ZC) with radii R.

% INPUTS:
% XC, YC, ZC: coordinates of the centers, they are (3 x 1) 3D vectors
% RADII: coordinates of the centers, they are (3 x 1) array
%
% OUTPUTS
% Y1, Y2: coordinates respectively of the first and second solution
% they are two (3 x 1) 3D vectors
% The z coordinates is sorted, i.e., Y1(3)< Y2(3).
% errflag: integer 0 -> OK
% -1 -> r1+r2 < distance center#1 to center#2
% -2 -> r1+r2 ~= distance center#1 to center#2
% -3 -> Centers are colinear
% -4 -> Inconsistent radii (radii(3) is suspected)
%
% Note: The results might still be useful for errflag < 0
%
% Author: Bruno Luong
% History: 22-Sep-2010 (original)

warningflag = 'off'; % off

warning(warningflag, 'trilateration:h2neg');
warning(warningflag, 'trilateration:cneg');
warning(warningflag, 'trilateration:cpos');

errflag = 0;

% x y z in three rows
C = [xc(:) yc(:) zc(:)].'; % 3 x 3

%% Sort the distances from smallest to largest, for better stability
[r is] = sort(r,'ascend');
C = C(:,is);

a1 = 0;
% vector from center#1 to center#2
v12 = C(:,2)-C(:,1); % 3 x 1
a2 = norm(v12);
da = a2-a1;
sqrradii = r(:).^2; % 3 x 1
if (da == 0)
    error('Input points must be distincts');
end
% unit vector, pointing from 1 to 2
u12 = v12/a2;
a = 0.5*((a1+a2) + (sqrradii(1)-sqrradii(2))/da);
h2 = sqrradii(1) - (a-a1)^2; % == sqrradii(2) - (a-a2)^2

% The r1+r2 < distance center#1 to center#2
if h2 < 0
    warning('trilateration:h2neg', 'trilateration: h2 < 0')
    errflag = -1;
    h2 = 0;
end

% Radius of the circle { X : |X-C1|=r1 and |X-C2|=r2 }
h = sqrt(h2);
% Center of the circle { X : |X-C1|=r1 and |X-C2|=r2 } = { C12 + h*v: v in B}
C12 = C(:,1) + a*u12;

% two vectors perpendicular to u, i.e., u12'*B == 0
B = orthvec(u12(:)); % 3 x 2
% 3 x 3
Q=[B u12];

% Coordinates of the thirdpoint, translated to the center C12 and in the
% new Cartesian's local axis Q
xyz3 = C(:,3)-C12;
xyz3 = Q.'*xyz3;
%
x3 = xyz3(1);
y3 = xyz3(2);
%
b = sqrt(x3^2+y3^2);

if h < eps(a2)
    Y1 = C12;
    Y2 = C12;
    errflag = -2;
elseif b < eps(a2) % centers are colinear
    mindisp = Inf;
    a3 = dot(C(:,3)-C(:,1),u12);
    % Allocate memory
    Y = zeros(3,1);
    % Find the best combination of sign to minimize the dispersion
    for i=[-1 1]
        Y(1) = a1 + i*r(1);
        for j=[-1 1]
            Y(2) = a2 + j*r(2);
            for k=[-1 1]
                Y(3) = a3 + k*r(3);
                dispersion = max(Y)-min(Y);
                if dispersion < mindisp
                    Ybest = Y;
                    mindisp = dispersion;
                end
            end
        end
    end
    Y1 = C(:,1) + mean(Ybest)*u12;
    Y2 = Y1;
    errflag = -3;
else % regular trilateration
    numerator = (-sqrradii(3) + sum(xyz3.^2) + h2);
    denominator = 2*b*h;
    c = numerator / denominator;
    if abs(c) > 1
        % Increase h to compensate overflowed
        warning('trilateration:cpos', 'trilateration: abs(c) > +1')
        hscale = sqrt(abs(c));
        h = h*hscale;
        c = sign(c); % +/-1
        errflag = -4;
    end
    theta1 = atan2(y3,x3);
    theta2 = acos(c);
    
    theta = theta1 + theta2;
    Y1 = h*[cos(theta); sin(theta)]; % 2 x 1
    Y1 = C12 + B*Y1; % 3 x 1
    
    theta = theta1 - theta2;
    Y2 = h*[cos(theta); sin(theta)];
    Y2 = C12 + B*Y2;
    
    % Swap Y1,Y2 so that z coordinates increasing
    if Y1(3) > Y2(3)
        [Y1 Y2] = deal(Y2, Y1);
    end
end % cascade if

end % trilateration

%% scalar product
function res = dot(a,b)
res = a(1)*b(1) + a(2)*b(2) + a(3)*b(3);
end

%% vector norm
function res = norm(v)
res = sqrt(dot(v,v));
end

%% cross product
function c = cross(a, b)
c = [a(2)*b(3)-a(3)*b(2);
     a(3)*b(1)-a(1)*b(3);
     a(1)*b(2)-a(2)*b(1)];
end

%% find the orthogonal basis of the orthogonal space to span of the
% input vector
function B = orthvec(v)
% B = null(reshape(v,1,3));
[~, i] = min(abs(v)); % BUG corrected, was v
ei = zeros(3,1);
ei(i) = 1;
b1 = cross(v,ei);
b1 = b1/norm(b1);
b2 = cross(v,b1);
b2 = b2/norm(b2);
B = [b1 b2];
end

Subject: calculate the intersection of 3 sphere

From: marcel

Date: 26 Mar, 2012 14:39:12

Message: 3 of 7

"Bruno Luong" <b.luong@fogale.findmycountry> wrote in message <jkpr2u$qrb$1@newscl01ah.mathworks.com>...
> "marcel " <djmoumouh87@hotmail.fr> wrote in message <jkpplu$li7$1@newscl01ah.mathworks.com>...
> > hello every baby.
> > I try to calculate the coordinate of two points of intersection 3-sphere [snip]...
>
> I dig out of my stuffs a function that does just that:
>
> function [Y1 Y2 errflag] = trilateration(xc, yc, zc, r)
> %
> % [Y1 Y2 errflag] = trilateration(xc, yc, zc, r)
> %
> % Purpose: find the two intersections of *three* spheres centered at
> % (XC,YC,ZC) with radii R.
>
> % INPUTS:
> % XC, YC, ZC: coordinates of the centers, they are (3 x 1) 3D vectors
> % RADII: coordinates of the centers, they are (3 x 1) array
> %
> % OUTPUTS
> % Y1, Y2: coordinates respectively of the first and second solution
> % they are two (3 x 1) 3D vectors
> % The z coordinates is sorted, i.e., Y1(3)< Y2(3).
> % errflag: integer 0 -> OK
> % -1 -> r1+r2 < distance center#1 to center#2
> % -2 -> r1+r2 ~= distance center#1 to center#2
> % -3 -> Centers are colinear
> % -4 -> Inconsistent radii (radii(3) is suspected)
> %
> % Note: The results might still be useful for errflag < 0
> %
> % Author: Bruno Luong
> % History: 22-Sep-2010 (original)
>
> warningflag = 'off'; % off
>
> warning(warningflag, 'trilateration:h2neg');
> warning(warningflag, 'trilateration:cneg');
> warning(warningflag, 'trilateration:cpos');
>
> errflag = 0;
>
> % x y z in three rows
> C = [xc(:) yc(:) zc(:)].'; % 3 x 3
>
> %% Sort the distances from smallest to largest, for better stability
> [r is] = sort(r,'ascend');
> C = C(:,is);
>
> a1 = 0;
> % vector from center#1 to center#2
> v12 = C(:,2)-C(:,1); % 3 x 1
> a2 = norm(v12);
> da = a2-a1;
> sqrradii = r(:).^2; % 3 x 1
> if (da == 0)
> error('Input points must be distincts');
> end
> % unit vector, pointing from 1 to 2
> u12 = v12/a2;
> a = 0.5*((a1+a2) + (sqrradii(1)-sqrradii(2))/da);
> h2 = sqrradii(1) - (a-a1)^2; % == sqrradii(2) - (a-a2)^2
>
> % The r1+r2 < distance center#1 to center#2
> if h2 < 0
> warning('trilateration:h2neg', 'trilateration: h2 < 0')
> errflag = -1;
> h2 = 0;
> end
>
> % Radius of the circle { X : |X-C1|=r1 and |X-C2|=r2 }
> h = sqrt(h2);
> % Center of the circle { X : |X-C1|=r1 and |X-C2|=r2 } = { C12 + h*v: v in B}
> C12 = C(:,1) + a*u12;
>
> % two vectors perpendicular to u, i.e., u12'*B == 0
> B = orthvec(u12(:)); % 3 x 2
> % 3 x 3
> Q=[B u12];
>
> % Coordinates of the thirdpoint, translated to the center C12 and in the
> % new Cartesian's local axis Q
> xyz3 = C(:,3)-C12;
> xyz3 = Q.'*xyz3;
> %
> x3 = xyz3(1);
> y3 = xyz3(2);
> %
> b = sqrt(x3^2+y3^2);
>
> if h < eps(a2)
> Y1 = C12;
> Y2 = C12;
> errflag = -2;
> elseif b < eps(a2) % centers are colinear
> mindisp = Inf;
> a3 = dot(C(:,3)-C(:,1),u12);
> % Allocate memory
> Y = zeros(3,1);
> % Find the best combination of sign to minimize the dispersion
> for i=[-1 1]
> Y(1) = a1 + i*r(1);
> for j=[-1 1]
> Y(2) = a2 + j*r(2);
> for k=[-1 1]
> Y(3) = a3 + k*r(3);
> dispersion = max(Y)-min(Y);
> if dispersion < mindisp
> Ybest = Y;
> mindisp = dispersion;
> end
> end
> end
> end
> Y1 = C(:,1) + mean(Ybest)*u12;
> Y2 = Y1;
> errflag = -3;
> else % regular trilateration
> numerator = (-sqrradii(3) + sum(xyz3.^2) + h2);
> denominator = 2*b*h;
> c = numerator / denominator;
> if abs(c) > 1
> % Increase h to compensate overflowed
> warning('trilateration:cpos', 'trilateration: abs(c) > +1')
> hscale = sqrt(abs(c));
> h = h*hscale;
> c = sign(c); % +/-1
> errflag = -4;
> end
> theta1 = atan2(y3,x3);
> theta2 = acos(c);
>
> theta = theta1 + theta2;
> Y1 = h*[cos(theta); sin(theta)]; % 2 x 1
> Y1 = C12 + B*Y1; % 3 x 1
>
> theta = theta1 - theta2;
> Y2 = h*[cos(theta); sin(theta)];
> Y2 = C12 + B*Y2;
>
> % Swap Y1,Y2 so that z coordinates increasing
> if Y1(3) > Y2(3)
> [Y1 Y2] = deal(Y2, Y1);
> end
> end % cascade if
>
> end % trilateration
>
> %% scalar product
> function res = dot(a,b)
> res = a(1)*b(1) + a(2)*b(2) + a(3)*b(3);
> end
>
> %% vector norm
> function res = norm(v)
> res = sqrt(dot(v,v));
> end
>
> %% cross product
> function c = cross(a, b)
> c = [a(2)*b(3)-a(3)*b(2);
> a(3)*b(1)-a(1)*b(3);
> a(1)*b(2)-a(2)*b(1)];
> end
>
> %% find the orthogonal basis of the orthogonal space to span of the
> % input vector
> function B = orthvec(v)
> % B = null(reshape(v,1,3));
> [~, i] = min(abs(v)); % BUG corrected, was v
> ei = zeros(3,1);
> ei(i) = 1;
> b1 = cross(v,ei);
> b1 = b1/norm(b1);
> b2 = cross(v,b1);
> b2 = b2/norm(b2);
> B = [b1 b2];
> end
hello bruno
thank you for this function very interesting but unfortunately I need to calculate the Cartesian coordinate of the intersection points, because I reuse them in my program
marcel.

Subject: calculate the intersection of 3 sphere

From: Bruno Luong

Date: 26 Mar, 2012 14:45:12

Message: 4 of 7

"marcel " <djmoumouh87@hotmail.fr> wrote in message
> thank you for this function very interesting but unfortunately I need to calculate the Cartesian coordinate of the intersection points,

I'm not sure about your concern, my function works with Cartesian coordinates.

Bruno

Subject: calculate the intersection of 3 sphere

From: marcel

Date: 26 Mar, 2012 14:58:11

Message: 5 of 7

hello bruno.
yes it's true, your fonction work with Cartesian coordinates, i'm sorry ,i just watch that.
think you for all.
marcel.

Subject: calculate the intersection of 3 sphere

From: Roger Stafford

Date: 26 Mar, 2012 16:18:11

Message: 6 of 7

"marcel " <djmoumouh87@hotmail.fr> wrote in message <jkpplu$li7$1@newscl01ah.mathworks.com>...
> I try to calculate the coordinate of two points of intersection 3-sphere, it is analytically complicated, so I asked whether it is possible to use subs in the solve function to pass parameters in the function? .....
- - - - - - - - -
  Yes, a single analytic expression would be quite complicated. However, a sequence of mathematical operations can accomplish the same task with much less complexity. I dug the following up which I wrote from an old CSSM thread which you might be interested in. Even though this is matlab code, the operations expressed by it can be considered to express mathematically the analytic solution to your problem, though not in a single expression.

  The thread is located at:

http://www.mathworks.com/matlabcentral/newsreader/view_thread/239659
Date: Thu, 29 Jan 2009 00:19:02 +0000 (UTC)

The pertinent text is as follows:

  "Given three spheres, you want to find their two points of intersection (if they actually intersect.) As you can see, it gives an answer without any iteration. My claim is that the only time these calculations become subject to inaccuracy is when the problem is inherently ill-conditioned, as for example when the centers are nearly colinear, or when the two points of intersection are very close together.

  Let p1, p2, and p3 be three-element vectors giving the x,y,z coordinates of the spheres' three centers, and r1, r2, and r3 their respective radii. Then do this:

 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;

The i1 and i2 will be vectors for the two points of intersection. If there is any possibility of having radii such that no solution is possible (which can happen very easily) you should test that the first square root used in the calculation of v is real. An impossible problem will make it imaginary."

Roger Stafford

Subject: calculate the intersection of 3 sphere

From: Todd

Date: 23 Aug, 2012 20:32:07

Message: 7 of 7

Hey Roger,

Thank you for the help you've provided in this thread so far. It has been very helpful to me.

In your last sentence, "If there is any possibility of having radii such that no solution is possible (which can happen very easily) you should test that the first square root used in the calculation of v is real. An impossible problem will make it imaginary."

What would be your suggestion as to how to deal with such a scenario? As you mention, it does crop up rather often and it's a huge part of my project.

Any help would be appreciated.
Best,
Yash

> The pertinent text is as follows:
>
> "Given three spheres, you want to find their two points of intersection (if they actually intersect.) As you can see, it gives an answer without any iteration. My claim is that the only time these calculations become subject to inaccuracy is when the problem is inherently ill-conditioned, as for example when the centers are nearly colinear, or when the two points of intersection are very close together.
>
> Let p1, p2, and p3 be three-element vectors giving the x,y,z coordinates of the spheres' three centers, and r1, r2, and r3 their respective radii. Then do this:
>
> 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;
>
> The i1 and i2 will be vectors for the two points of intersection. If there is any possibility of having radii such that no solution is possible (which can happen very easily) you should test that the first square root used in the calculation of v is real. An impossible problem will make it imaginary."
>
> Roger Stafford

Tags for this Thread

What are tags?

A tag is like a keyword or category label associated with each thread. Tags make it easier for you to find threads of interest.

Anyone can tag a thread. Tags are public and visible to everyone.

Contact us