3-D 算法中的三边测量返回 NaN

Question

我尝试编写this 文章中提供的步骤。

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

}

Afterz1 在z1 = Math.pow(r1,2) - Math.pow(x,2) - Math.pow(y,2) 步骤之后始终为负数。因此，取平方根为NaN。 这可能吗？也许我选择了错误的点来进行本地化。

你能告诉我我哪里弄错了吗？

Answer 1

z1 的负值表示没有真正的解决方案，例如第一个和第二个球体的交点完全落在第三个球体的内部或外部。如果您的测试值不包括至少一组存在真正解决方案的集合，z1 将始终为负数。