## finding interception of 3 spheres to calculate the coordinates of the 2 points

### Pietro Di Modica (view profile)

on 10 Aug 2018
Latest activity Edited by David Goodmanson

on 15 Aug 2018

### David Goodmanson (view profile)

Hi all, My issue is that I have the radii and the coordinate of the centre of three spheres and I want to find the 2 point intercept of the spheres. The application is that I have 3 string potentiometers data of a point on a rigid body and I want to deduce the actual [x,y,z] coordinates. I have come up with a function that does that for a simple set of data but when I enter the samples acquired from the measurements it does not work, intercept 3 figures higher. The following is the program flow, units are [m].
SPcnfg1 = [-9.920,11.163,61.056];
SPcnfg2 = [-2.000,.414,63.156];
SPcnfg3 = [-1.237,.333,61.156];
IniLength = [8.225,10.021,9.967];
SP10 = .46623487;
SP20 = .92222617;
SP30 = .85754269;
% Coordintes of the three spheres centres, centred on the string pots wire
% exit point
C10 = SPcnfg1;
C20 = SPcnfg2;
C30 = SPcnfg3;
% Calculating the real radii from the SP extension signal
R1 = IniLength(1) + SP10;
R2 = IniLength(2) + SP20;
R3 = IniLength(3) + SP30;
% Constant for calculations
a21 = C20(1) - C10(1);
a31 = C30(1) - C10(1);
b21 = C20(2) - C10(2);
b31 = C30(2) - C10(2);
c21 = C20(3) - C10(3);
c31 = C30(3) - C10(3);
d212 = C20(1)^2 - C10(1)^2 + C20(2)^2 - C10(2)^2 + C20(3)^2 - C10(3)^2;
d312 = C30(1)^2 - C10(1)^2 + C30(2)^2 - C10(2)^2 + C30(3)^2 - C10(3)^2;
R212 = R2.^2 - R1.^2;
R312 = R3.^2 - R1.^2;
%Coeefficient of the 2 reduced linear equations
A = (b31*R212 - b21*R312 + d312*b21 - d212*b31)/(2*(a31*b21 - a21*b31)) - C10(1);
B = (c21*b31 - c31*b21) / (a31*b21 - a21*b31);
C = (a31*R212 - a21*R312 + d312*a21 - d212*a31)/(2*(a21*b31 - a31*b21)) - C10(2);
D = (c21*a31 - c31*a21) / (a21*b31 - a31*b21);
% Coefficients of the single variable quadratic equation
E = B.^2 + D.^2 + 1;
F = 2*(A.*B + C.*D - C10(3));
G = A.^2 + C.^2 + C10(3).^2 - R1.^2;
zp = (-F + sqrt(F.^2-4*E*G)) / 2*E;
xp = A + C10(1) + zp * B;
yp = C + C10(2) + zp * D;
S1p = [xp yp zp];
zn = (-F - sqrt(F.^2-4*E*G)) / 2*E;
xn = A + C10(1) + zn * B;
yn = C + C10(2) + zn * D;
S1n = [xn yn zn];
figure; plot3(xp, yp, zp, '*r', xn, yn, zn, '*k');
xlabel ('x'); ylabel ('y'); zlabel ('z');
Any help is welcome and any suggestion to make it more elegant is more than welcome. The final goal is to send a bunch of data to the function and calculate the positions for every sample triplet.

R2018a

### David Goodmanson (view profile)

on 11 Aug 2018
Edited by David Goodmanson

### David Goodmanson (view profile)

on 15 Aug 2018

MODIFIED
Hi Pietro, I don't know whether the issue is with coming up with the correct positions for the centers of the spheres and the correct radii, OR having good center positions and radii but not coming up with the correct two points. If it's the latter, I thought you might be interested in some code for comparison.
The following assumes positions p1,p2 and p3, all of which are 3x1 column vectors, and radii R1,R2,R3. The method is generally similar to yours. If the geometry is not physically realizable you get a complex answer for the points, but the check for R1,R2,R3 still seems to come out pretty well. (Note the transpose .' in the check, so there is no complex conjugation).
q2 = p2-p1;
q3 = p3-p1;
q23 = [q2 q3];
q4 = null(q23'); % q4 is orthogonal to q23 space and normalized to 1
D =(-1/2)*[R2^2-q2'*q2-R1^2; R3^2-q3'*q3-R1^2];
x23 = pinv(q23')*D;
c = sqrt(R1^2-x23'*x23); % reverse the sign of c for the second point
x = p1 + x23 + c*q4
% check for R1,R2,R3
sqrt((x-p1).'*(x-p1))
sqrt((x-p2).'*(x-p2))
sqrt((x-p3).'*(x-p3))

Pietro Di Modica

### Pietro Di Modica (view profile)

on 13 Aug 2018
Hi David, Thanks a lot for your help, much appreciated. It looks like it works. I will check with all the other values of the test data and, fingers crossed, it will work just fine. I will have to document this and explain what is the logic behind it for traceability purpose, could you please expand a bit on the passages? I think I get the idea but I am missing a couple of passages, like the null calculation and the addition of the c element.
Regards, Pietro.
David Goodmanson

### David Goodmanson (view profile)

on 15 Aug 2018
Hi Pietro, Asking for more details helped because I had to work on an explanation, which led to the idea that the code could be simplified. I modified the answer, which is two lines shorter.
Subtracting p1 off all the coordinates puts R1 centered at the origin and the other two spheres centered at q2 and q3. Then of course p1 is added back in at the end.
q2 and q3 are the basis for a 2d subspace. A third dimension, perpendicular to the subspace, is needed to fully define x. cross(q2,q3) would do it. However, 'null' provides a perpendicular vector q4 in the same direction as the cross product but which is conveniently a unit vector.
With R1 centered at the origin then
x'*x = R1^2.
where the inner product x' *x is the dot product. (If v is a matrix with a set of column vectors, then v ' * x is a set of dot products).
For sphere 2,
(x-q2)'*(x-q2) = R2^2.
Multiplying this out and rearranging gives
q2'*x = (-1/2)*(R2^2-q2'*q2-R1^2),
same for q3 and by stacking the two equations you can arrive at
q23'*x = D
where q23 is 3x2, x is 3x1, D is 2x1.
In the equation above, x has three components and D only has two, but it is possible to find a solution for x that lies strictly within the 2d q2,q3 subspace. The pseudoinverse 'pinv' provides the answer, and x23 lies within the subspace.
Point x can be defined as
x = x23 + c*q4.
for unknown c. To find c, since q4 is normalized (and perpendicular to the subspace),
x'*x = x23'*x23 + c^2 = R1^2
Then solve for c, which since it comes in as the square can be of either sign.

on 10 Aug 2018
Edited by Rik

### Rik (view profile)

on 13 Aug 2018

I don't have the symbolic toolbox, so I can't test any of this, but it should at least give you an idea of how to start.
xyzr=[0 0 1;1 0 1;2 0 1];%each row is the x,y,z,radius of a sphere
syms x y z
conds=cell(3,1);
for k=1:3
cond{k} = (x-xyzr(k,1))^2 + (y-xyzr(k,2))^2 + (z-xyzr(k,3))^2 == xyzr(k,4);
end
conds = [conds{:}];
sol = solve(conds, [x y z]);

Pietro Di Modica

### Pietro Di Modica (view profile)

on 13 Aug 2018
I also do not have the symbolic math toolbox so I cannot use this answer. I must say very concise! although I think there is an end missing somewhere...
Rik

### Rik (view profile)

on 13 Aug 2018
You are right, I fixed it in case there is someone with a similar problem that does have the toolbox.