3D circle center through 3 points

src/util.typ

@@ -99,53 +99,54 @@
   return (calc.cos(angle) * rx + x, calc.sin(angle) * ry + y, z)
 }
 
-/// Calculates the center of a circle from 3 points. The z coordinate is taken from point a.
+/// Calculates the center of a circle from 3 points. The points are 3D, the center is in the plane containing the 3 points. It fails if the points are aligned and distinct. If two of the points are the same, the circle center is not unique; we take the midpoint.
+/// The center p is computed by solving for scalars lambda, mu such that p=a+lambda(b-a)+mu(c-a). Using the scalar product we obtain that lambda=||w||^2(||v||^2-scal(v,w))/denom where v=b-a, w=c-a and denom=2(||v||^2||w||^2-scal(v,w)^2), and mu has the reverse formula. denom is zero iff a, b, c are aligned.
 ///
 /// - a (vector): Point 1
 /// - b (vector): Point 2
 /// - c (vector): Point 3
 /// -> vector
 #let calculate-circle-center-3pt(a, b, c) = {
-  let m-ab = line-pt(a, b, .5)
-  let m-bc = line-pt(b, c, .5)
-  let m-cd = line-pt(c, a, .5)
-
-  let args = () // a, c, b, d
-  for i in range(0, 3) {
-    let (p1, p2) = ((a,b,c).at(calc.rem(i,3)),
-                    (b,c,a).at(calc.rem(i,3)))
-    let m = line-pt(p1, p2, .5)
-    let n = line-normal(p1, p2)
-
-    // Find a line with a non upwards normal
-    if n.at(0) == 0 { continue }
-
-    let la = n.at(1) / n.at(0)
-    args.push(la)
-    args.push(m.at(1) - la * m.at(0))
-
-    // We need only 2 lines
-    if args.len() == 4 { break }
-  }
+  // If two points are the same we take the midpoint with the two other.
+  if a == b or b == c { return ligne-pt(a, c, 0.5) } else if a == c { return line-pt(a, b, 0.5) }
+  // we compute the vectors b-a and c-a and the norm of their cross product
+  let (vx, vy, vz) = (0, 1, 2).map(i => b.at(i) - a.at(i)) // v=b-a
+  let (wx, wy, wz) = (0, 1, 2).map(i => c.at(i) - a.at(i)) // w=c-a
+  let v2 = (vx * vx + vy * vy + vz * vz) // ||v||^2
+  let w2 = (wx * wx + wy * wy + wz * wz) // ||w||^2
+  let vw = vx * wx + vy * wy + vz * wz // <v, w>
+  let denom = 2 * (v2 * w2 - calc.pow(vw, 2)) // 2*norm of "v cross w"
+  // if the points are aligned, we fail with error message returning the coordinates of the points
+  assert(
+    denom != 0,
+    message: "The points are aligned, and no two points are equal, so the circle center is at infinity.
+    Coordinates: a=( "
+      + str(a.at(0))
+      + ", "
+      + str(a.at(1))
+      + ", "
+      + str(a.at(2))
+      + ") and b =( "
+      + str(b.at(0))
+      + ", "
+      + str(b.at(1))
+      + ", "
+      + str(b.at(2))
+      + "), and c = ( "
+      + str(c.at(0))
+      + ", "
+      + str(c.at(1))
+      + ", "
+      + str(c.at(2))
+      + ") ",
+  )
+let lambda = w2 * (v2 - vw) / denom
+  let mu = v2 * (w2 - vw) / denom
+  let p = (0, 1, 2).map(i => lambda * (vx, vy, vz).at(i) + mu * (wx, wy, wz).at(i) + a.at(i)) // p=lambda v+ mu w+a
+  return p
+}
 
-  // Find intersection point of two 2d lines
-  // L1: a*x + c
-  // L2: b*x + d
-  let line-intersection-2d(a, c, b, d) = {
-    if a - b == 0 {
-      if c == d {
-        return (0, c, 0)
-      }
-      return none
-    }
-    let x = (d - c)/(a - b)
-    let y = a * x + c
-    return (x, y)
-  }
 
-  assert(args.len() == 4, message: "Could not find circle center")
-  return vector.as-vec(line-intersection-2d(..args), init: (0, 0, a.at(2)))
-}
 
 /// Converts a {{number}} to "canvas units"
 /// - ctx (context): The current context object.