3D Triangle with 2 Points, All Lengths, and Z of the 3rd Point Given

288 Views Asked by At

I have a 3D triangle ABC with known Lengths AB, BC, and AC and known points A(x,y,z), B(x,y,z) and C(z). I need to be able to find the x and y coordinates of C using a non-iterative method.

I've been going through another post and was following along until the aa bb and c.z calulations in the answer. Unfortunately, I don't have enough reputation to comment on that post...

3d geometry: triangle 2 points known, find 3rd point

Any help is greatly appreciated!

2

There are 2 best solutions below

5
On BEST ANSWER

The point $C$ lies somewhere on the intersection of a pair of spheres with centers $A$ and $B$. Since we also know that $C$ lies somewhere on the plane $z=z_C$, this reduces to finding the intersection of a pair of circles.

Let $r_A=|AC|$ and $r_B=|BC|$. The equations of the two spheres are $$(x-x_A)^2+(y-y_A)^2+(z-z_A)^2=r_A^2 \\ (x-x_B)^2+(y-y_B)^2+(z-z_B)^2=r_B^2.$$ If $|z_C-z_A|\gt r_A$ or $|z_C-z_B|\gt r_B$, then there’s no solution. Otherwise substitute $z=z_C$ into the above equations to get the equations of two circles: $$(x-x_A)^2+(y-y_A)^2=r_A^2-(z_C-z_A)^2 \\ (x-x_B)^2+(y-y_B)^2=r_B^2-(z_C-z_B)^2.$$ If you subtract one of these equations from the other you get a linear equation in $x$ and $y$, which you can solve for one or the other and then back-substitute into either circle equation to get a solution. The most common case will be two solutions, but if the vertices are lined up just right you might get one or an infinite number of them (the two circles coincide).

Alternatively, a direct calculation with vectors produces the points of intersection of the two circles. Let $r_A'^2=r_A^2-(z_C-z_A)^2$ and $r_B'^2=r_B^2-(z_C-z_B)^2$ be the radii of the two circles, and $d=\sqrt{(x_A-x_B)^2+(y_A-y_B)^2}$ the distance between their centers. If $r_A'+r_B'>d$, the circles clearly don’t intersect. If $d=0$, the circles are concentric, for which see below. Otherwise, the intersections of the circles are equally spaced on a line perpendicular to the line through the two centers. enter image description here

Referring to the illustration above, by the Pythagorean theorem the distance of this line from the center of circle $A$ satisfies $$r_A'^2-w^2=r_B'^2-(d-w)^2$$ from which $$w={d^2+r_A'^2-r_B'^2\over2d}.$$ Again by the Pythagorean theorem, the distance $h$ along the perpendicular line to the intersection points is $h=\sqrt{r_A'^2-w^2}=\sqrt{r_B'^2-(d-w)^2}$. Taking unit vectors in the directions of the two lines, this gives for the $x$- and $y$- coordinates of the intersection points $$\frac wd(x_B-x_A,y_B-y_A)\pm\frac hd(y_A-y_B,x_B-x_A).\tag{*}$$ If $d=0$, then the $x$- and $y$- coordinates of points $A$ and $B$ are equal, so there’s one solution—$(x_A,y_A,z_C)$—when $r_A'=r_B'=0$ (the two spheres are tangent) or an infinite number when $r_A'=r_B'\gt0$. When $r_A'\ne r_B'$, there is, of course, no solution.

As an implementation note, observe that $\frac wd={d^2+r_A'^2-r_B'^2\over d^2}$ and similarly $h/d$ only involves $d^2$ after factoring in the contribution of $w^2$. All of the radii are squared in the actual computations as well, so in practice the only point at which you really need to take a square root is when computing $h$. Range checks can be delayed to avoid taking square roots: If $z_C$ is out of range, one or both of $r_A'^2$ or $r_B'^2$ will end up being negative; if $r_A'+r_B'\gt d$, then $h$ will end up being imaginary.

14
On

amd's answer describes a much better method than what I suggested in this answer.

Simply put, amd pointed out that the problem simplifies to a 2D one. In 2D, the distance between $A$ and $C$ is $\sqrt{AC^2 - (z_C - z_A)^2}$, and the distance between $B$ and $C$ is $\sqrt{BC^2 - (z_C - z_B)^2}$.

amd's answer is very simple to implement. In pseudocode, it is

Function Find_third_point(A_x, A_y, A_z,
                          B_x, B_y, B_z,
                          AC, BC, C_z, epsilon):
    # 2D coordinates
    Bx = B_x - A_x
    By = B_y - A_y
    Ad2 = AC*AC - (C_z - A_z)*(C_z - A_z)
    Bd2 = BC*BC - (C_z - B_z)*(C_z - B_z)
    d2 = Bx*Bx + By*By
    If (d2 <= epsilon):
        Fail: "No solutions, or an infinite number of solutions"
    End if

    w_per_d = (d2 + Ar2 - Br2) / (d2 + d2)
    w2_per_d2 = w_per_d*w_per_d

    If (Ar2 < d2*w2_per_d2):
        Fail: "No solutions"
    End if
    h_per_d = sqrt(Ar2/d2 - w2_per_d2)

    Return Point(A_x + w_per_d*Bx - h_per_d*By,
                 A_y + w_per_d*By + h_per_d*Bx,
                 C_z),
           Point(A_x + w_per_d*Bx + h_per_d*By,
                 A_y + w_per_d*By - h_per_d*Bx,
                 C_z)

which only needs one square root, sixteen multiplications, two divisions, and 21 additions or subtractions; a brilliant solution.

Here is an awk test script that tests a million random points (or NUMBER if you add -v n=NUMBER to the command line):

#!/usr/bin/awk -f
function distance(xp1, yp1, zp1, xp2, yp2, zp2) { return sqrt((xp2-xp1)*(xp2-xp1) + (yp2-yp1)*(yp2-yp1) + (zp2-zp1)*(zp2-zp1)) }
BEGIN {
    srand();

    eps = 0.0005
    xmin = -9.999 ; ymin = -9.999 ; zmin = -9.999
    xmax = +9.999 ; ymax = +9.999 ; zmax = +9.999

    n = int(n)
    if (n < 1)
        n = 1000000;

    for (i = 0; i < n; i++) {

        if (!(i % 10000))
            printf "\r%.1f%% tests completed\033[K\r", 100.0*i/n

        while (1) {
            x1 = xmin + (xmax - xmin) * rand()
            y1 = ymin + (ymax - ymin) * rand()
            z1 = zmin + (zmax - zmin) * rand()
            x2 = xmin + (xmax - xmin) * rand()
            y2 = ymin + (ymax - ymin) * rand()
            z2 = zmin + (zmax - zmin) * rand()
            x3 = xmin + (xmax - xmin) * rand()
            y3 = ymin + (ymax - ymin) * rand()
            z3 = zmin + (zmax - zmin) * rand()
            dd12 = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1)
            dd13 = (x3-x1)*(x3-x1) + (y3-y1)*(y3-y1) + (z3-z1)*(z3-z1)
            dd23 = (x3-x2)*(x3-x2) + (y3-y2)*(y3-y2) + (z3-z2)*(z3-z2)
            if (dd12 > eps*eps &&
                dd13 > eps*eps &&
                dd23 > eps*eps) break
        }

        # Preparation

        Bx = x2 - x1
        By = y2 - y1

        Cz = z3 - z1

        Ar2 = dd13 - (z3 - z1)*(z3 - z1)
        Br2 = dd23 - (z3 - z2)*(z3 - z2)

        d2 = Bx*Bx + By*By
        if (d2 <= eps*eps) {
            printf "A:   %7.3f %7.3f %7.3f\n", x1, y1, z1
            printf "B:   %7.3f %7.3f %7.3f\n", x2, y2, z2
            printf "C:   %7.3f %7.3f %7.3f\n", x3, y3, z3
            printf "AB: %11.6f\n", sqrt(dd12)
            printf "AC: %11.6f\n", sqrt(dd13)
            printf "BC: %11.6f\n", sqrt(dd23)
            printf "\n"
            printf "Cannot solve, A and B differ only by their z coordinates.\n"
            exit(1)
        }

        w_per_d = (d2 + Ar2 - Br2) / (d2 + d2)
        w2_per_d2 = w_per_d * w_per_d
        if (Ar2 < d2*w2_per_d2) {
            printf "A:   %7.3f %7.3f %7.3f\n", x1, y1, z1
            printf "B:   %7.3f %7.3f %7.3f\n", x2, y2, z2
            printf "C:   %7.3f %7.3f %7.3f\n", x3, y3, z3
            printf "AB: %11.6f\n", sqrt(dd12)
            printf "AC: %11.6f\n", sqrt(dd13)
            printf "BC: %11.6f\n", sqrt(dd23)
            printf "\n"
            printf "No solutions.\n"
            exit(1)
        }
        h_per_d = sqrt(Ar2/d2 - w2_per_d2)

        Cx1 = x1 + w_per_d * Bx - h_per_d * By
        Cy1 = y1 + w_per_d * By + h_per_d * Bx
        Cz1 = z3

        Cx2 = x1 + w_per_d * Bx + h_per_d * By
        Cy2 = y1 + w_per_d * By - h_per_d * Bx
        Cz2 = z3

        err11 = distance(Cx1,Cy1,Cz1,x1,y1,z1) - sqrt(dd13)
        err12 = distance(Cx1,Cy1,Cz1,x2,y2,z2) - sqrt(dd23)
        err21 = distance(Cx2,Cy2,Cz2,x1,y1,z1) - sqrt(dd13)
        err22 = distance(Cx2,Cy2,Cz2,x2,y2,z2) - sqrt(dd23)

        if (err11 > eps || err11 < -eps ||
            err12 > eps || err12 < -eps ||
            err21 > eps || err21 < -eps ||
            err22 > eps || err22 < -eps) {
            printf "A:   %7.3f %7.3f %7.3f\n", x1, y1, z1
            printf "B:   %7.3f %7.3f %7.3f\n", x2, y2, z2
            printf "C:   %7.3f %7.3f %7.3f\n", x3, y3, z3
            printf "AB: %11.6f\n", sqrt(dd12)
            printf "AC: %11.6f\n", sqrt(dd13)
            printf "BC: %11.6f\n", sqrt(dd23)
            printf "\n"
            printf "Found: %7.3f %7.3f %7.3f errors: %+.6f %+.6f %+.6f\n", Cx1, Cy1, Cz1, distance(Cx1,Cy1,Cz1,x3,y3,z3), distance(Cx1,Cy1,Cz1,x1,y1,z1) - sqrt(dd13), distance(Cx1,Cy1,Cz1,x2,y2,z2) - sqrt(dd23)
            printf "Found: %7.3f %7.3f %7.3f errors: %+.6f %+.6f %+.6f\n", Cx2, Cy2, Cz2, distance(Cx2,Cy2,Cz2,x3,y3,z3), distance(Cx2,Cy2,Cz2,x1,y1,z1) - sqrt(dd13), distance(Cx2,Cy2,Cz2,x2,y2,z2) - sqrt(dd23)
            exit(1)
        }
    }

    printf "All %d tests successful.\n", n
}


I am honestly ashamed that I didn't see the 2D simplification at all; it took quite a bit of prodding from amd for me to get it.

In the interests of completeness, below is my earlier, comparatively stupid, suggestion, that originally got accepted by the asker. (This is to show that if somebody with good math-fu shows you another way, it always pays to listen and understand: algorithmic enhancements always beat code optimizations.)

If point $A$ is at the origin, then we have $$\begin{cases} C_x^2 + C_y^2 + C_z^2 = AC^2 \\ (C_x - B_x)^2 + (C_y - B_y)^2 + (C_z - B_z)^2 = BC^2 \end{cases} \tag{1a}\label{1a}$$ i.e. $$\begin{cases} C_x^2 + C_y^2 = AC^2 - C_z^2 \\ C_x^2 - 2 B_x C_x + C_y^2 - 2 B_y C_y = BC^2 - (C_z - B_z)^2 - B_x^2 - B_y^2\end{cases}\tag{1b}\label{1b}$$ that we wish to solve for $C_x$ and $C_y$.

Solving $\eqref{1a}$ or $\eqref{1b}$ for $C_x$ and $C_y$ yields two solutions. I used Maple to look at what they are:

C_x = A_x+((B_x-A_x)*(AC^2-BC^2+(B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2-2*(C_z-A_z)*(B_z-A_z))-signum(-B_x+A_x)*signum(-B_y+A_y)*sqrt((B_y-A_y)^2*((B_x-A_x)^4+2*((B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)*(AC^2+BC^2-(B_x-A_x)^2)-(AC^2-BC^2)^2-((B_y-A_y)^2+(B_z-A_z)^2)^2+4*(B_z-A_z)*(C_z-A_z)*(AC^2-BC^2+(B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)-4*AC^2*(B_z-A_z)^2-4*((B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)*(C_z-A_z)^2)))/(2*(B_x-A_x)^2+2*(B_y-A_y)^2)
C_y = A_y+((B_y-A_y)*(AC^2-BC^2+(B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2-2*(C_z-A_z)*(B_z-A_z))+sqrt((B_x-A_x)^2*((B_x-A_x)^4+2*((B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)*(AC^2+BC^2-(B_x-A_x)^2)-(AC^2-BC^2)^2-((B_y-A_y)^2+(B_z-A_z)^2)^2+4*(B_z-A_z)*(C_z-A_z)*(AC^2-BC^2+(B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)-4*AC^2*(B_z-A_z)^2-4*((B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)*(C_z-A_z)^2)))/(2*(B_x-A_x)^2+2*(B_y-A_y)^2)

and

C_x = A_x+((B_x-A_x)*(AC^2-BC^2+(B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2-2*(C_z-A_z)*(B_z-A_z))+signum(-B_x+A_x)*signum(-B_y+A_y)*sqrt((B_y-A_y)^2*((B_x-A_x)^4+2*((B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)*(AC^2+BC^2-(B_x-A_x)^2)-(AC^2-BC^2)^2-((B_y-A_y)^2+(B_z-A_z)^2)^2+4*(B_z-A_z)*(C_z-A_z)*(AC^2-BC^2+(B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)-4*AC^2*(B_z-A_z)^2-4*((B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)*(C_z-A_z)^2)))/(2*(B_x-A_x)^2+2*(B_y-A_y)^2)
C_y = A_y+((B_y-A_y)*(AC^2-BC^2+(B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2-2*(C_z-A_z)*(B_z-A_z))-sqrt((B_x-A_x)^2*((B_x-A_x)^4+2*((B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)*(AC^2+BC^2-(B_x-A_x)^2)-(AC^2-BC^2)^2-((B_y-A_y)^2+(B_z-A_z)^2)^2+4*(B_z-A_z)*(C_z-A_z)*(AC^2-BC^2+(B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)-4*AC^2*(B_z-A_z)^2-4*((B_x-A_x)^2+(B_y-A_y)^2+(B_z-A_z)^2)*(C_z-A_z)^2)))/(2*(B_x-A_x)^2+2*(B_y-A_y)^2)

where ^ denotes exponentiation, not binary exclusive-or operation; and signum() is zero if its argument is zero, -1 if its argument is negative, and +1 if its argument is positive.

Since the above forms are horrible, I fiddled with the expressions a bit to find a form a programmer might use. Mostly, I did this to show that even if the solution looks horrible, it may still be quite easy to use, if you first understand the solution and its requirements. (You do need to avoid things like division by zero, and verify your solutions numerically.)


Let's define

ABAB = AB * AB    # Squared distance from A to B
ACAC = AC * AC    # Squared distance from A to C
BCBC = BC * BC    # Squared distance from B to C
Bx = B_x - A_x    # Location of B relative to A
By = B_y - A_y
Bz = B_z - A_z
BxBx = Bx * Bx
ByBy = By * By
BzBz = Bz * Bz
Cz = C_z - A_z    # Z coordinate of C relative to A
#                   Note: ABAB = BxBx + ByBy + BzBz

so that Bx, By, Bz, and Cz are relative to the location of point $A$; our solutions for Cx and Cy will similarly be relative to $A$, so the actual solutions where $A$ is not at origin will be

C_x = A_x + Cx
C_y = A_y + Cy

Note that I use an _ only in the original coordinate variable names (where point $A$ is not at origin). Coordinate variables without a _ are relative to the location of point $A$.


If one of AB, AC, and AB are all zero, we have a degenerate triangle, a point. Then, C_x = A_x and C_y = A_y.

If AB = 0, but AC ≠ BC, there is no solution.

If AB = 0, and AC = BC > 0, any point $C$ in a circle of radius $AC$ around $A$ works.

If BC = 0, but AC ≠ AB or B_z ≠ C_z, there is no solution.

If BC = 0, AC = AB > 0, and B_z = C_z, then C_x = B_x and C_y = B_y.

If Bx = 0 and By = 0, but AC > 0 and BC > 0, there may be no solution (for example if C_z > AC + A_z). If there are any solutions, they form a circle around (A_x, A_y, C_z), perpendicular to the $z$ axis.

If Bx = 0, the solution simplifies to

    Cy = (ACAC + ABAB - BCBC - 2*Bz*Cz) / (2*By)
    Cx = +- sqrt(ACAC - Cy*Cy - Cz*Cz)

but note that if ACAC = Cy*Cy + Cz*Cz, there is just one solution, Cx = 0, and if ACAC < Cy*Cy + Cz*Cz, there are no (real) solutions.

If By = 0, the solution simplifies to

    Cx = (ACAC + ABAB - BCBC - 2*Bz*Cz) / (2*Bx)
    Cy = +- sqrt(ACAC - Cx*Cx - Cz*Cz)

but note that if ACAC = Cy*Cy + Cz*Cz, there is just one solution, Cy = 0, and if ACAC < Cx*Cx + Cz*Cz, there are no (real) solutions.

In the general case, where neither Bx or By is zero, calculate S:

S = BxBx * BxBx
  - (ACAC - BCBC) * (ACAC - BCBC)
  - (ByBy + BzBz) * (ByBy + BzBz)
  + 2 * ABAB * (ACAC + BCBC - BxBx)
  + 4*( Bz * Cz * (ABAB + ACAC - BCBC) - ACAC*BzBz - ABAB*Cz*Cz )

If S is negative, there are no real solutions. If S is zero, there is exactly one solution (the two solutions are the same). If S is positive, there are two solutions.

(Note that S*BxBx corresponds to the value inside the square root, sqrt(), in the horrible formulas above. I took the term outside, so that we do not try to calculate By/Bx outside the square root.)

Next, calculate temporary values

BB = 2 * (BxBx + ByBy)
BL = ACAC + ABAB - BCBC - 2 * Bz * Cz

If BB is zero, then points $A$ and $B$ have the same $x$ and $y$ coordinates, and there is no solution.

We also need a sign constant:

BS = +1  if Bx and By have the same sign
   = -1  if Bx and By have different signs

Note that if you use floating-point numbers, you do not need to inspect the actual signs; you can just use e.g.

If Bx*By > 0:
    BS = +1
Else:
    Bs = -1
End If

If we define BS so that it is only zero if both are zero, and otherwise only considers the sign of the nonzero value(s), this approach works for the (Bx=0 or By=0) cases as well. That is,

If (Bx > 0):
    If (By > 0):
        BS = +1
    Else if (By < 0):
        BS = -1
    Else:
        BS = +1
    End if
Else if (Bx < 0):
    If (By > 0):
        BS = -1
    Else if (By < 0):
        BS = +1
    Else:
        BS = -1
    End if
Else:
    If (By > 0):
        BS = +1
    Else if (By < 0):
        BS = -1
    Else:
        BS = 0
    End if
End if

The first solution, relative to point $A$, is

Cx1 = (Bx * BL - BS * sqrt( ByBy * S )) / BB
Cy1 = (By * BL + sqrt( BxBx * S )) / BB

and the second solution is

Cx2 = (Bx * BL + BS * sqrt( ByBy * S )) / BB
Cy2 = (By * BL - sqrt( BxBx * S )) / BB

To repeat, if $A$ is not at the origin, the solutions are

C_x = A_x + (Bx * BL - BS * sqrt( ByBy * S )) / BB
C_y = A_y + (By * BL + sqrt( BxBx * S )) / BB

and

C_x = A_x + (Bx * BL + BS * sqrt( ByBy * S )) / BB
C_y = A_y + (By * BL - sqrt( BxBx * S )) / BB

Here is an example Awk script, that generates three random points not too close to each other, and uses the general form above to solve the $x$ and $y$ coordinates for the third point, and verifies that the solution is correct. By default, the test is repeated for a million (different) points; you can specify the number of points to test using -v n=NUMBER.

#!/usr/bin/awk -f
function distance(xp1, yp1, zp1, xp2, yp2, zp2) { return sqrt((xp2-xp1)*(xp2-xp1) + (yp2-yp1)*(yp2-yp1) + (zp2-zp1)*(zp2-zp1)) }
BEGIN {
    srand();

    eps = 0.0005
    xmin = -9.999 ; ymin = -9.999 ; zmin = -9.999
    xmax = +9.999 ; ymax = +9.999 ; zmax = +9.999

    if (int(n) < 1)
        n = 1000000;
    else
        n = int(n);

    for (i = 0; i < n; i++) {

        if (!(i % 10000))
            printf "\r%.1f%% tests completed\033[K\r", 100.0*i/n

        while (1) {
            x1 = xmin + (xmax - xmin) * rand()
            y1 = ymin + (ymax - ymin) * rand()
            z1 = zmin + (zmax - zmin) * rand()
            x2 = xmin + (xmax - xmin) * rand()
            y2 = ymin + (ymax - ymin) * rand()
            z2 = zmin + (zmax - zmin) * rand()
            x3 = xmin + (xmax - xmin) * rand()
            y3 = ymin + (ymax - ymin) * rand()
            z3 = zmin + (zmax - zmin) * rand()
            dd12 = (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1)
            dd13 = (x3-x1)*(x3-x1) + (y3-y1)*(y3-y1) + (z3-z1)*(z3-z1)
            dd23 = (x3-x2)*(x3-x2) + (y3-y2)*(y3-y2) + (z3-z2)*(z3-z2)
            if (dd12 > eps*eps &&
                dd13 > eps*eps &&
                dd23 > eps*eps) break
        }

        # Preparation

        Bx = x2-x1 ; By = y2-y1 ; Bz = z2-z1 ; Cz = z3-z1
        BxBx = Bx*Bx ; ByBy = By*By ; BzBz = Bz*Bz
        ABAB = dd12 ; ACAC = dd13 ; BCBC = dd23

        # Calculations

        S = BxBx * BxBx - (ACAC - BCBC) * (ACAC - BCBC) - (ByBy + BzBz) * (ByBy + BzBz) + 2 * ABAB * (ACAC + BCBC - BxBx) + 4*( Bz * Cz * (ABAB + ACAC - BCBC) - ACAC*BzBz - ABAB*Cz*Cz )
        if (S < 0) {
            printf "A:   %7.3f %7.3f %7.3f\n", x1, y1, z1
            printf "B:   %7.3f %7.3f %7.3f\n", x2, y2, z2
            printf "C:   %7.3f %7.3f %7.3f\n", x3, y3, z3
            printf "AB: %11.6f\n", sqrt(dd12)
            printf "AC: %11.6f\n", sqrt(dd13)
            printf "BC: %11.6f\n", sqrt(dd23)
            printf "\n"
            printf "No solutions.\n"
            exit(1)
        }

        BB = BxBx + BxBx + ByBy + ByBy
        if (BB <= eps*eps) {
            printf "A:   %7.3f %7.3f %7.3f\n", x1, y1, z1
            printf "B:   %7.3f %7.3f %7.3f\n", x2, y2, z2
            printf "C:   %7.3f %7.3f %7.3f\n", x3, y3, z3
            printf "AB: %11.6f\n", sqrt(dd12)
            printf "AC: %11.6f\n", sqrt(dd13)
            printf "BC: %11.6f\n", sqrt(dd23)
            printf "\n"
            printf "Cannot solve, A and B differ only by their z coordinates.\n"
            exit(1)
        }

        BL = ACAC + ABAB - BCBC - 2*Bz*Cz

        if (Bx > 0) {
            if (By > 0)
                BS = +1;
            else
            if (By < 0)
                BS = -1;
            else
                BS = +1;
        } else
        if (Bx < 0) {
            if (By > 0)
                BS = -1;
            else
            if (By < 0)
                BS = +1;
            else
                BS = -1;
        } else {
             if (By > 0)
                BS = +1;
            else
            if (By < 0)
                BS = -1;
            else
                BS = 0;
        }

        Cx1 = x1 + (Bx * BL - BS * sqrt( ByBy * S )) / BB
        Cy1 = y1 + (By * BL + sqrt( BxBx * S )) / BB
        Cz1 = z3
        Cx2 = x1 + (Bx * BL + BS * sqrt( ByBy * S )) / BB
        Cy2 = y1 + (By * BL - sqrt( BxBx * S )) / BB
        Cz2 = z3

        err11 = distance(Cx1,Cy1,Cz1,x1,y1,z1) - sqrt(dd13)
        err12 = distance(Cx1,Cy1,Cz1,x2,y2,z2) - sqrt(dd23)
        err21 = distance(Cx2,Cy2,Cz2,x1,y1,z1) - sqrt(dd13)
        err22 = distance(Cx2,Cy2,Cz2,x2,y2,z2) - sqrt(dd23)

        if (err11 > eps || err11 < -eps ||
            err12 > eps || err12 < -eps ||
            err21 > eps || err21 < -eps ||
            err22 > eps || err22 < -eps) {
            printf "A:   %7.3f %7.3f %7.3f\n", x1, y1, z1
            printf "B:   %7.3f %7.3f %7.3f\n", x2, y2, z2
            printf "C:   %7.3f %7.3f %7.3f\n", x3, y3, z3
            printf "AB: %11.6f\n", sqrt(dd12)
            printf "AC: %11.6f\n", sqrt(dd13)
            printf "BC: %11.6f\n", sqrt(dd23)
            printf "\n"
            printf "Found: %7.3f %7.3f %7.3f errors: %+.6f %+.6f %+.6f\n", Cx1, Cy1, Cz1, distance(Cx1,Cy1,Cz1,x3,y3,z3), distance(Cx1,Cy1,Cz1,x1,y1,z1) - sqrt(dd13), distance(Cx1,Cy1,Cz1,x2,y2,z2) - sqrt(dd23)
            printf "Found: %7.3f %7.3f %7.3f errors: %+.6f %+.6f %+.6f\n", Cx2, Cy2, Cz2, distance(Cx2,Cy2,Cz2,x3,y3,z3), distance(Cx2,Cy2,Cz2,x1,y1,z1) - sqrt(dd13), distance(Cx2,Cy2,Cz2,x2,y2,z2) - sqrt(dd23)
            exit(1)
        }

    }

    printf "All %d tests successful.\n", n
}

In the output, in cases of an issue with the algorithm, the errors reported for the found points are the distance to the initially generated point, followed by the errors in distances to the two initial points.

Testing a few hundred million such points indicates the algorithm should work: that is, it finds the valid solution. If it finds two solutions, one of them is the initially generated point, but the other fulfills the requirements (distances to the other two points and the $z$ coordinate).