Localizing a point using distances to three other points in 3-D

Question

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)

Answer 1

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

1. e_x = (P2 - P1) / ‖P2 - P1‖
2. i = e_x(P3 - P1)
3. e_y = (P3 - P1 - i · e_x) / ‖P3 - P1 - i · e_x‖
4. d = ‖P2 - P1‖
5. j = e_y(P3 - P1)
6. x = (r₁² - r₂² + d²) / 2d
7. y = (r₁² - r₃² + i² + j²) / 2j - ix / j
8. z = ±sqrt(r₁² - x² - y²)