1
$\begingroup$

I have 3 points in space A, B, and C all with (x,y,z) coordinates, therefore I know the distances between all these points. I wish to find point D(x,y,z) and I know the distances BD and CD, I do NOT know AD.

The method I have attempted to solve this using is first saying that there are two spheres known on points B and C with radius r (distance to point D). The third sphere is found by setting the law of cosines equal to the formula for distance between two vectors ((V1*V2)/(|V1||V2|)) = ((a^2+b^2-c^2)/2ab).

Now point D should be the intersection of these three spheres, but I have not been able to calculate this or find a way to. I either need help finding point D and I can give numeric points for an example, or I need to know if I need more information to solve (another point with a distance to D known).

Ok, now assuming that distance AD is known, how do I calculate point D? This is what it looks like when I graph the two spheres and the one I calculated, as you can see it intersects on point F. (D in this case)

$\endgroup$
6
  • 3
    $\begingroup$ If you don't know $AD$, what is the point of $A$? And note that the information that you've given is not enough to uniquely determine your 4th point. $\endgroup$ Oct 24, 2018 at 16:15
  • $\begingroup$ Well eventually I do need to calculate AD, but that is irrelevant for now. The problem should just boil down to the intersection of 3 spheres, which can be used to find the point right? $\endgroup$ Oct 24, 2018 at 16:18
  • $\begingroup$ I would remove $A$ from the problem. It's irrelevant here. Moreover, having two points and two distances is not enough information to determine $D$. This problem is not possible to solve uniquely. $\endgroup$ Oct 24, 2018 at 16:20
  • $\begingroup$ @JasonSmitherman: You would have three spheres, if you gave the distance $AD$. As it is (as others have pointed out), you have two spheres, whose intersection is, in general, a circle. That might help you, but it's not enough to uniquely determine the point D. The law of cosines is of no help in fixing that point. $\endgroup$
    – Brian Tung
    Oct 24, 2018 at 16:23
  • $\begingroup$ @BrianTung: What kept messing me up was that I would graph the 3rd equation I calculated and on the graph it was a completely different sphere from the other two, sure enough it intersected point D with the other 2 spheres. $\endgroup$ Oct 24, 2018 at 16:45

3 Answers 3

2
$\begingroup$

(This is not an answer to the stated question per se, but explains how to efficiently do trilateration.)

In 3D, you need four fixed points (that are not all on the same plane), and their distances to the unknown point, to exactly determine the location of the unknown point.

Three fixed points (that are not all on the same line) and their distances to the unknown point will typically give you two possibilities, symmetrically mirrored by the plane formed by the three fixed points.

Let's say the unknown point is at $\vec{p} = (x, y, z)$, the three fixed points are at $\vec{v}_1 = (x_1 , y_1 , z_1)$, $\vec{v}_2 = (x_2 , y_2 , z_2)$, and $\vec{v}_3 = (x_3 , y_3 , z_3)$, at distances $d_1$, $d_2$, and $d_3$ from the unknown point, respectively. Solving the system of equations $$\left\lbrace\begin{aligned} \left\lVert \vec{p} - \vec{p}_1 \right\rVert &= \lvert d_1 \rvert \\ \left\lVert \vec{p} - \vec{p}_2 \right\rVert &= \lvert d_2 \rvert \\ \left\lVert \vec{p} - \vec{p}_3 \right\rVert &= \lvert d_3 \rvert \\ \end{aligned}\right . \iff \left\lbrace\begin{aligned} (x - x_1)^2 + (y - y_1)^2 + (z - z_1)^2 &= d_1^2 \\ (x - x_2)^2 + (y - y_2)^2 + (z - z_2)^2 &= d_2^2 \\ (x - x_3)^2 + (y - y_3)^2 + (z - z_3)^2 &= d_3^2 \\ \end{aligned}\right.$$ is nontrivial, especially in algebraic form.

Instead, change to a coordinate system where $(x_1 , y_1 , z_1)$ is at origin, $(x_2 , y_2 , z_2)$ is at $(h , 0 , 0)$, and $(x_3 , y_3 , z_3)$ is at $(i, j, 0)$. The unit vectors $\hat{e}_1 = ( X_1 , Y_1 , Z_1 )$, $\hat{e}_2 = ( X_2 , Y_2 , Z_2 )$, and $\hat{e}_3 = (X_3 , Y_3 , Z_3 )$ are $$\left\lbrace\begin{aligned} \vec{e}_1 &= \vec{v}_2 - \vec{v}_1 \\ \hat{e}_1 &= \frac{\vec{e}_1}{\left\lVert\vec{e}_1\right\rVert} \\ \vec{e}_2 &= \vec{v}_3 - \vec{v}_1 - \hat{e}_1 \left ( \hat{e}_1 \cdot \left ( \vec{v}_3 - \vec{v}_1 \right ) \right ) \\ \hat{e}_2 &= \frac{\vec{e}_2}{\left\lVert\vec{e}_2\right\rVert} \\ \hat{e}_3 &= \hat{e}_1 \times \hat{e}_2 \\ \end{aligned}\right.$$ and the fixed point coordinates are $$\left\lbrace\begin{aligned} h &= \left\lVert \vec{v}_2 - \vec{v}_1 \right\rVert = \sqrt{ (x_2 - x_1)^2 + (y_2 - y_1)^2 + (z_2 - z_1)^2 } \\ i &= \hat{e}_1 \cdot \left(\vec{v}_3 - \vec{v}_1\right) = X_1 ( x_3 - x_1 ) + Y_1 ( y_3 - y_1 ) + Z_1 ( Z_3 - Z_1 ) \\ j &= \hat{e}_2 \cdot \left(\vec{v}_3 - \vec{v}_1\right) = X_2 ( x_3 - x_1 ) + Y_2 ( y_2 - y_1 ) + Z_2 ( Z_3 - Z_2 ) \\ \end{aligned}\right.$$ The distances stay the same, but the system of equations is much simpler. For clarity, I'll use $(u, v, w)$ in these new coordinates instead of $(x, y, z)$: $$\left\lbrace\begin{aligned} u^2 + v^2 + w^2 &= d_1^2 \\ (u - h)^2 + v^2 + w^2 &= d_2^2 \\ (u - i)^2 + (v - j)^2 + w^2 &= d_3^2 \end{aligned}\right.$$ which is easily solved: $$\left\lbrace\begin{aligned} u &= \frac{d_1^2 - d_2^2 + h^2}{2 h} \\ v &= \frac{d_1^2 - d_3^2 + i^2 + j^2 - 2 i u}{2 j} \\ w &= \pm \sqrt{d_1^2 - u^2 - v^2} \\ \end{aligned}\right.$$ In the original coordinate system, $$\vec{p} = \vec{v}_1 + u \hat{e}_1 + v \hat{e}_2 + w \hat{e}_3 \quad \iff \quad \left\lbrace\begin{aligned} x &= x_1 + u X_1 + v X_2 + w X_3 \\ y &= y_1 + u Y_1 + v Y_2 + w Y_3 \\ z &= z_1 + u Z_1 + v Z_2 + w Z_3 \\ \end{aligned}\right.$$ noting that if $w$ is not a real, then there is no solution; if $w \approx 0$, there is one solution; and otherwise there are two solutions, one with positive $w$, and the other with negative $w$.

If you know the distances to four fixed points, you only really need the fourth point (not coplanar with the three other fixed points) to distinguish which case it is. If the distances contain noise, it might make sense to calculate the result using each unique triplet ($123$, $124$, $134$, and $234$), and return their mean.

In pseudocode, for trilateration, you should precalculate the values that only depend on the fixed points:

Let  ex1 = x2 - x1
Let  ey2 = y2 - y1
Let  ez2 = z2 - z1
Let  h = sqrt( ex1*ex1 + ey1*ey1 + ez1*ez1 )
If h <= epsilon:
    Error: First and second point are too close.
End If
Let  ex1 = ex1 / h
Let  ey1 = ey1 / h
Let  ez1 = ez1 / h  

Let  i = ex1*(x2 - x1) + ey1*(y2 - y1) + ez1*(z2 - z1)

Let  ex2 = x3 - x1 - i*ex1
Let  ey2 = y3 - y1 - i*ey1
Let  ez2 = z3 - z1 - i*ez1
Let  t = sqrt(ex2*ex2 + ey2*ey2 + ez2*ez2)
If t <= epsilon:
    Error: the three fixed points are too close to being on the same line.
End If
Let  ex2 = ex2 / t
Let  ey2 = ey2 / t
Let  ez2 = ez2 / t

Let  j = ex2*(x3 - x1) + ey2*(y3 - y1) + ez2*(z3 - z1)
If j <= epsilon and j >= -epsilon:
    Error: the three fixed points are too close to being on the same line.
End If

Let  ex3 = ey1*ez2 - ez1*ey2
Let  ey3 = ez1*ex2 - ex1*ez2
Let  ez3 = ex1*ey2 - ey2*ex1

where epsilon is the largest positive number that should be treated as zero, and represents the expected precision in coordinates and distances, for example 0.001.

The function that finds the coordinates for the unknown point is then

# Fixed points are at (x1,y1,z1), (x2,y2,z2), (x3,y3,z3)
# Function takes the distances to fixed points d1, d2, d3
# Function expects unit vectors (ex1,ey1,ez1), (ex2,ey2,ez2), (ex3,ey3,ez3)
#   to be precalculated, with fixed points at (0,0,0), (h,0,0), (i,j,0)
#   in that coordinate system, without changing the distances

Function Trilaterate(d1, d2, d3):
    Let  u = (d1*d1 - d2*d2 + h*h) / (2*h)
    Let  v = (d1*d1 - d3*d3 + i*(i - 2*u) + j*j) / (2*j)
    Let  ww = d1*d1 - u*u - v*v
    If ww < -epsilon:
        Return no solutions
    Else
    If ww < epsilon:
        Return a single solution:
            x = x1 + u*ex1 + v*ex2
            y = y1 + u*ey1 + v*ey2
            z = z1 + u*ez1 + v*ez2
    Else:
        w = sqrt(ww)
        Return two solutions:
            x = x1 + u*ex1 + v*ex2 + w*ex3
            y = y1 + u*ey1 + v*ey2 + w*ey3
            z = z1 + u*ez1 + v*ez2 + w*ez3
        And
            x = x1 + u*ex1 + v*ex2 - w*ex3
            y = y1 + u*ey1 + v*ey2 - w*ey3
            z = z1 + u*ez1 + v*ez2 - w*ez3
    End If
End Function

This is simple enough to be done on 8-bit microcontrollers, if necessary.

$\endgroup$
5
  • $\begingroup$ thank you, that helps greatly. I will have to change that code to java to work for me. $\endgroup$ Oct 25, 2018 at 17:17
  • $\begingroup$ this post is extremely helpful. There's just a slight correction that needs to be made in the pseudo code where precalculations are happening. On the 2nd and 3rd line, the values ey2 and ez2 should be changed to ey1 and ez1 respectively. $\endgroup$
    – SomeGuy
    May 3, 2020 at 4:22
  • $\begingroup$ also, I think the line Let ez3 = ex1*ey2 - ey2*ex1 should probably be Let ez3 = ex1*ey2 - ex2*ey1 $\endgroup$
    – SomeGuy
    May 4, 2020 at 4:17
  • $\begingroup$ Also, the line that reads Let i = ex1*(x2 - x1) + ey1*(y2 - y1) + ez1*(z2 - z1) should be Let i = ex1*(x3 - x1) + ey1*(y3 - y1) + ez1*(z3 - z1) $\endgroup$
    – SomeGuy
    May 5, 2020 at 2:56
  • $\begingroup$ I used Nominal's pseudocode and fixes that I found to go and make a trilateration example code in C++ that people can copy, paste, and adopt to whatever their purposes are: github.com/amizan8653/trilaterationExample/blob/master/test.cpp $\endgroup$
    – SomeGuy
    May 5, 2020 at 3:09
0
$\begingroup$

Knowing $|BD|$ and $|CD|$ is only enough to know that $D$ is on the intersection of two spheres, one around $B$ and one around $C$. Assuming they intersect at all, the intersection is likely a circle and $D$ could be anywhere on that circle. $A$ is not giving you anything. Even if you get $|AD|$ you will still have an ambiguity between two points unless two of the spheres are tangent.

$\endgroup$
9
  • $\begingroup$ So this does make sense to me, I realize for GPS to work they need 3 satellites and that will give you longitude and latitude. What I don't understand here is why point A is irrelevant, it alone gives the equation for the 3rd circle. $\endgroup$ Oct 24, 2018 at 16:29
  • $\begingroup$ Because you don't have information of the distance from $A$ to $D$. I could move $A$ anywhere and you still have the same information about $D$, just the distances to $B$ and $C$. For GPS they can assume you are on the surface of the earth, which gives a third sphere. You actually need three satellites then because you need to determine time as well. You need four if you are going to find a point in space without assuming it is on the surface of the earth. $\endgroup$ Oct 24, 2018 at 16:33
  • $\begingroup$ Ok, so in order to solve I need to know distance AD. I solved this in 2D very easily using the same formula (3 known points, with 2 known distances). Therefore, it does make sense that you would need another point if you add a dimension. Now that I have the 3 spheres, how do I calculate the intersection? $\endgroup$ Oct 24, 2018 at 16:38
  • $\begingroup$ The same way as in $2D$. You write three equations that reflect the distances from $A,B,C$, like $(x-x_A)^2+(y-y_A)^2+(z-z_A)^2=AD^2$. You solve them simultaneously. $\endgroup$ Oct 24, 2018 at 16:56
  • $\begingroup$ So solving like this will give me two different circular equations, right? When I did it before it only gave me one. $\endgroup$ Oct 24, 2018 at 17:05
0
$\begingroup$

Suppose the known distances are $$d(B,C)=d(A,C)=d(C,D)=1$$ and $$d(A,B)=d(B,D)=\sqrt{2}$$ For concreteness, we can place $A,B,C$ ae $$C=(0,0,0),\;\;B=(1,0,0),\;\;A=(0,1,0)$$ Then if $D$ is any point on the circle in the $yz$-plane, centered at the origin, with radius $1$, all of the distance specifications are satisfied.

But since $D$ can be any point on that circle, it follows $D$ is not uniquely determined.

Note that as $D$ traverses the circle, $d(A,D)$ varies from a minimum of $0$ (when $D=A$), to a maximum of $2$ (when $D=(0,-1,0))$, so $d(A,D)$ is also not uniquely determined.

If $d(A,D)$ is also given, say $d(A,D)=t$, where $0\le t\le 2$, then $D=(x,y,z)$ is determined by the system $$ \begin{cases} x=0\\[4pt] y^2+z^2=1\\[4pt] x^2+(y-1)^2+z^2=t^2\\ \end{cases} $$ which yields $$D=\left(0,\,1-{\small{\frac{t^2}{2}}},\,\pm{\small{\frac{t}{2}}}\sqrt{4-t^2}\right)$$ hence,

  • If $t=0$, we get $D=(0,1,0)$.$\\[4pt]$
  • If $t=2$, we get $D=(0,-1,0)$.$\\[4pt]$
  • If $0 < t < 2$, there are two choices for $D$, as specified above.
$\endgroup$
1
  • $\begingroup$ That clarifies it more @quasi, the intersection of 2 spheres is a circle. When I was solving this before I would set all the equations for the spheres equal to each other and EVERY one gave me the same equation no matter what orientation, which I found interesting and infuriating. $\endgroup$ Oct 24, 2018 at 16:43

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.