1

Assume that we have 4 points in 3-D (P1, P2, P3, P4). If the coordinates of those points are given with their euclidian distances to a fifth point P5 (r1, r2, r3, r4), how to calculate the coordinates of P5?

In this post, answer of Don Reba is perfect for 2-D. But how do I extend it to 3-D?

Here is my code for 2D:

    static void localize(double[] P1, double[] P2, double[] P3, double r1, double r2, double r3)
    {
        double[] ex = normalize(difference(P2, P1));
        double i = dotProduct(ex, difference(P3, P1));
        double[] ey = normalize(difference(difference(P3, P1), scalarProduct(i, ex)));
        double d = magnitude(difference(P2, P1));
        double j = dotProduct(ey, difference(P3, P1));
        double x = ((r1*r1) - (r2*r2) + (d*d)) / (2*d);
        double y = (((r1*r1) - (r3*r3) + (i*i) + (j*j)) / (2*j)) - ((i*x) / j);
        System.out.println(x + " " + y);

    }

I want to overload the function with the signature

static void localize(double[] P1, double[] P2, double[] P3, double[] P4, double r1, double r2, double r3, double r4)
Community
  • 1
  • 1
padawan
  • 1,295
  • 1
  • 17
  • 45

2 Answers2

2

The Wikipedia trilateriation article describes the answer. The calculation steps are:

  1. ex = (P2 - P1) / ‖P2 - P1‖
  2. i = ex(P3 - P1)
  3. ey = (P3 - P1 - i · ex) / ‖P3 - P1 - i · ex‖
  4. d = ‖P2 - P1‖
  5. j = ey(P3 - P1)
  6. x = (r12 - r22 + d2) / 2d
  7. y = (r12 - r32 + i2 + j2) / 2j - ix / j
  8. z = ±sqrt(r12 - x2 - y2)
Don Reba
  • 13,814
  • 3
  • 48
  • 61
  • This is perfect. But there is something not right. Shouldn't I need 4 coordinates to calculate a fifth? Only three points are used here. – padawan May 04 '14 at 09:29
  • 1
    A point and a distance define a sphere in space. Intersection of 2 spheres is a circle. Intersection of 3 spheres is 2 points. You could use the fourth to choose one of the two points. – Don Reba May 04 '14 at 21:53
  • Could you please check my code and tell me if it is true? I don't get the coordinates I need. I may have confused some operations (dot product vs. scalar product) – padawan May 08 '14 at 10:44
0

You need to solve system of four equations (i=1..4, Di is distance to ith point)

(X-Xi)^2+(Y-Yi)^2+(Z-Zi)^2=Di^2

It is possible to solve system of three equations and use fourth to choose proper solution (from two).

This is how GPS works (where time delays are for distances).

In GPS receivers optimization methods are frequently used, especially when many satellites are available and algebraic solution may be instable.

MBo
  • 77,366
  • 5
  • 53
  • 86
  • This is true. But I need to write a program to do this. So, "solve this equation" does not work :/ – padawan May 01 '14 at 19:06