Trilateration in 3-D Algorithm Returns NaN

Question

I tried to code the steps provided in this article.

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 is always negative after z1 = Math.pow(r1,2) - Math.pow(x,2) - Math.pow(y,2) step. Hence, taking square root makes it NaN. Is this possible? Maybe I'm choosing the wrong points to do localization.

Could you please tell me where I'm mistaken?

Answer 1

A negative value for z1 would indicate no real solution, e.g. the intersection of the first and second sphere falls entirely inside or outside of the the third sphere. If your test values do not include at least one set for which a real solution exists, z1 will always be negative.