MATLAB: Finding interception of 3 spheres to calculate the coordinates of the 2 points

cartesian coordinatedata analysisinterceptionMATLABspheres

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.

Best Answer

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))
Related Question