三维算法中的三角测量返回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)}; 

} 

z1后始终是负值后z1 = Math.pow(r1,2) - Math.pow(x,2) - Math.pow(y,2)一步。因此，采取平方根使其成为NaN。 这可能吗？也许我选择了错误的地方做本地化。

你能告诉我我错了吗？

Answer 1

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