I'm trying to solve the following problem:
Given the absolute positions of four points in 3D space, and the distances from these four points to a fifth point, find the position of the fifth point. Or in other words:
(X0-Xn)^2 + (Y0-Yn)^2 + (Z0-Zn)^2 = Dn^2 for n={1,2,3,4}; find X0,Y0,Z0.
For the immediate purpose, we can assume that the distances are consistent - we can detect and reject the opposite case separately.
I reduced this to a system-of-linear-equations problem in the following form:
X0(X1-Xn)+Y0(Y1-Yn)+Z0(Z1-Zn)=Cn for n={2,3,4}, where Cn=((Dn^2+Xn^2+Yn^2+Zn^2)-(D1^2+X1^2+Y1^2+Z1^2))/2.
After much work with pen and paper, I thought I had a solution implemented in Python:
def Resolve(s1, s2, s3, s4, d1, d2, d3, d4):
c1 = (d1**2 + s1.coords[0]**2 + s1.coords[1]**2 + s1.coords[2]**2)
c2 = (d2**2 + s2.coords[0]**2 + s2.coords[1]**2 + s2.coords[2]**2 - c1) / 2
c3 = (d3**2 + s3.coords[0]**2 + s3.coords[1]**2 + s3.coords[2]**2 - c1) / 2
c4 = (d4**2 + s4.coords[0]**2 + s4.coords[1]**2 + s4.coords[2]**2 - c1) / 2
[dx12,dy12,dz12] = s1.coords - s2.coords
[dx13,dy13,dz13] = s1.coords - s3.coords
[dx14,dy14,dz14] = s1.coords - s4.coords
z = ((c2 * dx13 - c3 * dx12)*(dx12 * dy14 - dy12 * dx14) + (c2 * dx14 - c4 * dx12)*(dx12 * dy13 - dy12 * dx13)) / ((dx12 * dz13 - dz12 * dx13)*(dx12 * dy14 - dy12 * dx14) + (dz12 * dx14 - dx12 * dz14))
y = (c3 * dx12 - c2 * dx13 - z * (dx12 * dz13 - dz12 * dx13)) / (dx12 * dy13 - dy12 * dx13)
x = (c2 - y * dy12 - z * dz12) / dx12
return array([x,y,z])
However, when run with test data, it produces obviously incorrect results. Where have I gone wrong?