Трилатерация C # Как вернуться в «нормальные» координаты?

Question

Я пытаюсь вычислить свое местоположение с помощью 3 станций (p1, p2, p3) с радиусом (r1, r2, r3). Станции в трехмерном пространстве.

• Пример 1:

P1 = (2,2,0), P2 = (3,3,0), P3 = (1,4,0) r1 = 1 r2 = 1 r3 = 1.4142

Результат должен быть: P = (2,3,0)

• Пример 2:

P1 = (2,1,0), P2 = (4,3,0), P3 = (4,4,1) r1 = 2, r2 = 2, r3 = 2.449

Результат должен быть: P = (2,3,0)

Мой код:

private void button1_Click(object sender, EventArgs e)
{
    double x1, x2, x3, y1, y2, y3, z1, z2, z3;

    double.TryParse(textBox1.Text, out x1);
    double.TryParse(textBox2.Text, out x2);
    double.TryParse(textBox3.Text, out x3);
    double.TryParse(textBox4.Text, out y1);
    double.TryParse(textBox5.Text, out y2);
    double.TryParse(textBox6.Text, out y3);
    double.TryParse(textBox7.Text, out z1);
    double.TryParse(textBox8.Text, out z2);
    double.TryParse(textBox9.Text, out z3);


    Vector3D p1 = new Vector3D(x1, y1, z1);
    Vector3D p2 = new Vector3D(x2, y2, z2);
    Vector3D p3 = new Vector3D(x3, y3, z3);

    Vector3D p_1 = new Vector3D();
    Vector3D p_2 = new Vector3D();
    Vector3D p_3 = new Vector3D();

    Vector3D xn = new Vector3D();
    Vector3D yn = new Vector3D();
    Vector3D zn = new Vector3D();
    double r1_pow, r2_pow, r3_pow;

    double.TryParse(textBox10.Text, out r1_pow);
    double.TryParse(textBox11.Text, out r2_pow);
    double.TryParse(textBox12.Text, out r3_pow);


    //r1_pow = p1.Length;
    // r2_pow = p2.Length;
    //r3_pow = p3.Length;
    double X, Y, Z1, Z2, d, i, j;
    p_1 = p2 - p1;
    p_2 = p3 - p1;
    xn = Vector3D.Divide(p_1, p_1.Length);
    i = Vector3D.DotProduct(xn, p_2);
    d = p_1.Length;
    yn = Vector3D.Divide((p_2 - (i * xn)), (p_2 - (i * xn)).Length);
    j = Vector3D.DotProduct(yn, p_2);
    zn = Vector3D.CrossProduct(xn, yn);
    X = ((r1_pow * r1_pow) - (r2_pow * r2_pow) + (d * d)) / (2 * d);
    Y = (((r1_pow * r1_pow) - (r3_pow * r3_pow) + (i * i) + (j * j)) / (2 * j)) - ((i / j) * X);
    double X2 = X * X;
    double Y2 = Y * Y;
    Z1 = Math.Sqrt((r1_pow * r1_pow) - Math.Round(X2, 1) - Math.Round(Y2, 1));
    Z2 = -Math.Sqrt((r1_pow * r1_pow) - Math.Round(X2, 1) - Math.Round(Y2, 1));



    double a, B;
    Vector3D k2 = new Vector3D();
    Vector3D k1 = new Vector3D();
    k1 = p1 + ((X * xn) + (Y * yn) + (Z1 * zn));
    //  k1.Normalize();
    k2 = p1 + (X * xn) + (Y * yn) - (Z2 * zn);

    MessageBox.Show(k1.ToString());
}

Так что мой вопрос в том, что означают К1 и К2. Я предполагаю, что это связано с возвращением к «нормальным» координатам, но я не понимаю реальной концепции этих двух уравнений. И как мне предположить получить P (результат) от этих K1 и K2?

K1 = P1 + X * Xn + Y * Yn + z1 * Zn

K2 = P1 + X * Xn + Y * Yn - z2 * Zn