3d C# 中的三邊測量

Question

如何讓GPS算法根據三個已知點和它們的距離得到一個點？

Answer 1

它是在 Unity 中制作的，因此它使用 Vector3 和 Mathf 類，但使用每個點的 3 維數組和標准數學 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 };
}

它是 gheja 的 JS 版本的直接翻譯，所有功勞歸於他們： https://github.com/gheja/trilateration.js/blob/master/trilateration.js