Trilateration C# How to get back into “normal” coordinates?

问题

I am trying to calculate my location with help of 3 stations (p1,p2,p3) with radius (r1,r2,r3). Stations are in 3D space.

Example 1:

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

Result should be: P=(2,3,0)

Example 2:

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

Result should be: P=(2,3,0)

My code:

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());
}

So my question is what does K1 and K2 means. I am guessing it has to do something with going back to "normal" coordinates but I do not understand the real concept of this two equations. And how am I suppost to get P (the result) from that K1 and K2?

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

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

来源：https://stackoverflow.com/questions/50317591/trilateration-c-sharp-how-to-get-back-into-normal-coordinates

标签

math

geometry