C# 中的 3d 三边测量答案

【问题标题】：3d Trilateration in C#C# 中的 3d 三边测量
【发布时间】：2022-10-05 22:45:14
【问题描述】：

如何使 GPS 算法根据三个已知点及其距离得到一个点？

【解决方案1】：

它是在 Unity 中制作的，因此它使用 Vector3 和 Mathf 类，但使用每个点的 3 大小数组和标准 Math 类可以轻松删除这些依赖项。

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

【讨论】：