3-D算法中的三邊測量返回NaN

Question

我試圖代碼中提供的步驟此文章。

public static double[] localize(final double[] P1, final double[] P2, final double[] P3, final double[] P4, final double r1, final double r2, final double r3, final double r4)
{

    Point3d p1 = new Point3d(P1);
    Point3d p2 = new Point3d(P2);
    Point3d p3 = new Point3d(P3);
    Point3d p4 = new Point3d(P4);

    Vector3d ex = new Vector3d();

    ex.sub(p2,p1);
    ex.normalize();
    Vector3d p3p1 = new Vector3d();
    p3p1.sub(p3,p1);
    double i = ex.dot(p3p1);
    Vector3d iex = new Vector3d();
    iex.scale(i,ex);
    Vector3d ey = new Vector3d(p3p1);
    ey.sub(iex);
    ey.normalize();
    Vector3d ez = new Vector3d();
    ez.cross(ex, ey);
    double d = p2.distance(p1);
    if(d - r1 < r2)
        System.out.println("d - r1 < r2");
    if(r2 < d + r1)
        System.out.println("r2 < d+r1");
    double j = ey.dot(p3p1);

    double x = (Math.pow(r1,2) - Math.pow(r2,2) + Math.pow(d,2))/(2*d);
    Vector3d exx = new Vector3d();
    exx.scale(x, ex);
    double y = ((Math.pow(r1,2) - Math.pow(r3,2) + Math.pow(i,2) + Math.pow(j,2)) / ((2*j)) - ((i/j)*x));
    Vector3d eyy = new Vector3d();
    eyy.scale(y, ey);

    double z1 = Math.pow(r1,2) - Math.pow(x,2) - Math.pow(y,2);
    z1 = Math.sqrt(z1);
    double z2 = z1*-1;

    Vector3d ezz1 = new Vector3d();
    Vector3d ezz2 = new Vector3d();
    ezz1.scale(z1, ez);
    ezz2.scale(z2, ez);
    Point3d result1 = new Point3d();
    result1.add(p1);
    result1.add(exx);
    result1.add(eyy);
    result1.add(ezz1);

    Point3d result2 = new Point3d();
    result2.add(p1);
    result2.add(exx);
    result2.add(eyy);
    result2.add(ezz2);
    if((result1.distance(p4) - r4) <= (result2.distance(p4) - r4))
        return new double[]{round(result1.x), round(result1.y), round(result1.z)};
    else
        return new double[]{round(result2.x), round(result2.y), round(result2.z)};

}

在z1之后始終為負之后z1 = Math.pow(r1,2) - Math.pow(x,2) - Math.pow(y,2)步驟。 因此，取平方根使其為NaN 。 這可能嗎？ 也許我選擇了錯誤的點進行本地化。

你能告訴我我在哪里弄錯了嗎？

Answer 1

z1的負值將表示沒有實際解，例如，第一和第二球的交集完全落在第三球的內部或外部。 如果您的測試值不包括至少一個存在真實解的集合，則z1始終為負。