3d Trilateration in C#

Question

How to make the GPS algorithm to get a point based on three known points and their distances?

Answer 1

It is made in Unity, so it uses the Vector3 and Mathf classes, but it would be easy to remove those dependencies using a 3-sized array for each point and the standart Math class.

static float sqr(float a)
{
    return a * a;
}
static float norm(Vector3 a)
{
    return Mathf.Sqrt(sqr(a.x) + sqr(a.y) + sqr(a.z));
}
static float dot(Vector3 a, Vector3 b)
{
    return a.x * b.x + a.y * b.y + a.z * b.z;
}
static Vector3 vector_cross(Vector3 a, Vector3 b)
{
    return new Vector3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}

public static Vector3[] Trilaterate(Vector3 p1, float r1, Vector3 p2, float r2, Vector3 p3, float r3)
{
    Vector3 ex = (p2 - p1) / norm(p2 - p1);
    float i = dot(ex, (p3 - p1));
    Vector3 a = ((p3 - p1) - (ex * i));
    Vector3 ey = (a / norm(a));
    Vector3 ez = vector_cross(ex, ey);
    float d = norm(p2 - p1);
    float j = dot(ey, p3 - p1);

    float x = (sqr(r1) - sqr(r2) + sqr(d)) / (2 * d);
    float y = (sqr(r1) - sqr(r3) + sqr(i) + sqr(j)) / (2 * j) - (i / j) * x;

    float b = sqr(r1) - sqr(x) - sqr(y);

    // floating point math flaw in IEEE 754 standard
    // see https://github.com/gheja/trilateration.js/issues/2
    if (Mathf.Abs(b) < 0.0000000001)
    {
        b = 0;
    }

    float z = Mathf.Sqrt(b);

    // no solution found
    if (float.IsNaN(z))
    {
        return new Vector3[] { Vector3.zero };
    }

    Vector3 aa = p1 + ((ex * x) + (ey * y));
    Vector3 p4a = (aa + (ez * z));
    Vector3 p4b = (aa - (ez * z));

    return new Vector3[] { p4a, p4b };
}

It is a direct translation of the JS version from gheja, all credits to them: https://github.com/gheja/trilateration.js/blob/master/trilateration.js