Another cubic solver version

packages/excalidraw/element/collision.ts

@@ -43,6 +43,7 @@ import {
   debugClear,
   debugDrawCubicBezier,
   debugDrawLine,
+  debugDrawPoint,
 } from "../visualdebug";
 
 export const shouldTestInside = (element: ExcalidrawElement) => {
@@ -284,12 +285,46 @@ const intersectRectanguloidWithLine = (
       : [];
 
   debugClear();
+  debugDrawLine(
+    lineSegment(
+      right[1],
+      pointFrom(
+        right[1][0] + (2 / 3) * (r[1][0] - right[1][0]),
+        right[1][1] + (2 / 3) * (r[1][1] - right[1][1]),
+      ),
+    ),
+    { color: "red", permanent: true },
+  );
+  debugDrawLine(
+    lineSegment(
+      bottom[1],
+      pointFrom(
+        bottom[1][0] + (2 / 3) * (r[1][0] - bottom[1][0]),
+        bottom[1][1] + (2 / 3) * (r[1][1] - bottom[1][1]),
+      ),
+    ),
+    { color: "green", permanent: true },
+  );
   sides.forEach((s) => debugDrawLine(s, { color: "red", permanent: true }));
   corners.forEach((s) =>
     debugDrawCubicBezier(s, { color: "green", permanent: true }),
   );
   debugDrawLine(line(rotatedA, rotatedB), { color: "blue", permanent: true });
-  //debugDrawPoint(bottom[0], { color: "white", permanent: true });
+  // console.log(
+  //   curve(
+  //     right[1],
+  //     pointFrom(
+  //       right[1][0] + (2 / 3) * (r[1][0] - right[1][0]),
+  //       right[1][1] + (2 / 3) * (r[1][1] - right[1][1]),
+  //     ),
+  //     pointFrom(
+  //       bottom[1][0] + (2 / 3) * (r[1][0] - bottom[1][0]),
+  //       bottom[1][1] + (2 / 3) * (r[1][1] - bottom[1][1]),
+  //     ),
+  //     bottom[1],
+  //   ),
+  //   line<GlobalPoint>(rotatedA, rotatedB),
+  // );
 
   const sideIntersections: GlobalPoint[] = sides
     .map((s) =>
@@ -303,9 +338,12 @@ const intersectRectanguloidWithLine = (
     .filter((i) => i != null)
     .map((j) => pointRotateRads(j, center, element.angle));
 
-  // [...sideIntersections, ...cornerIntersections].forEach((p) =>
+  [
-  //   debugDrawPoint(p, { color: "purple", permanent: true }),
+    //...sideIntersections,
-  // );
+    ...cornerIntersections,
+  ].forEach((p) => {
+    debugDrawPoint(p, { color: "purple", permanent: true });
+  });
 
   return (
     [...sideIntersections, ...cornerIntersections]

packages/math/complex.ts

@@ -0,0 +1,133 @@
+import type { Complex } from "./types";
+
+export function complex(real: number, imag?: number): Complex {
+  return [real, imag ?? 0] as Complex;
+}
+
+export function add(a: Complex, b: Complex) {
+  return complex(a[0] + b[0], a[1] + b[1]);
+}
+
+export function sub(a: Complex, b: Complex): Complex {
+  return complex(a[0] - b[0], a[1] - b[1]);
+}
+
+export function mul(a: Complex, b: Complex) {
+  return complex(a[0] * b[0] - a[1] * b[1], a[0] * b[1] + a[1] * b[0]);
+}
+
+export function conjugate(a: Complex): Complex {
+  return complex(a[0], -a[1]);
+}
+
+export function pow(a: Complex, b: Complex): Complex {
+  const tIsZero = a[0] === 0 && a[1] === 0;
+  const zIsZero = b[0] === 0 && b[1] === 0;
+
+  if (zIsZero) {
+    return complex(1);
+  }
+
+  // If the exponent is real
+  if (b[1] === 0) {
+    if (b[1] === 0 && a[0] > 0) {
+      return complex(Math.pow(a[0], b[0]), 0);
+    } else if (a[0] === 0) {
+      // If base is fully imaginary
+
+      switch (((b[0] % 4) + 4) % 4) {
+        case 0:
+          return complex(Math.pow(a[1], b[0]), 0);
+        case 1:
+          return complex(0, Math.pow(a[1], b[0]));
+        case 2:
+          return complex(-Math.pow(a[1], b[0]), 0);
+        case 3:
+          return complex(0, -Math.pow(b[1], a[0]));
+      }
+    }
+  }
+
+  if (tIsZero && b[0] > 0) {
+    // Same behavior as Wolframalpha, Zero if real part is zero
+    return complex(0);
+  }
+
+  const arg = Math.atan2(a[1], a[0]);
+  const loh = logHypot(a[0], a[1]);
+
+  const re = Math.exp(b[0] * loh - b[1] * arg);
+  const im = b[1] * loh + b[0] * arg;
+
+  return complex(re * Math.cos(im), re * Math.sin(im));
+}
+
+export function sqrt([a, b]: Complex): Complex {
+  if (b === 0) {
+    // Real number case
+    if (a >= 0) {
+      return complex(Math.sqrt(a), 0);
+    }
+
+    return complex(0, Math.sqrt(-a));
+  }
+
+  const r = hypot(a, b);
+
+  const re = Math.sqrt(0.5 * (r + Math.abs(a))); // sqrt(2x) / 2 = sqrt(x / 2)
+  const im = Math.abs(b) / (2 * re);
+
+  if (a >= 0) {
+    return complex(re, b < 0 ? -im : im);
+  }
+
+  return complex(im, b < 0 ? -re : re);
+}
+
+const hypot = function (x: number, y: number) {
+  x = Math.abs(x);
+  y = Math.abs(y);
+
+  // Ensure `x` is the larger value
+  if (x < y) {
+    [x, y] = [y, x];
+  }
+
+  // If both are below the threshold, use straightforward Pythagoras
+  if (x < 1e8) {
+    return Math.sqrt(x * x + y * y);
+  }
+
+  // For larger values, scale to avoid overflow
+  y /= x;
+  return x * Math.sqrt(1 + y * y);
+};
+
+/**
+ * Calculates log(sqrt(a^2+b^2)) in a way to avoid overflows
+ *
+ * @param {number} a
+ * @param {number} b
+ * @returns {number}
+ */
+function logHypot(a: number, b: number) {
+  const _a = Math.abs(a);
+  const _b = Math.abs(b);
+
+  if (a === 0) {
+    return Math.log(_b);
+  }
+
+  if (b === 0) {
+    return Math.log(_a);
+  }
+
+  if (_a < 3000 && _b < 3000) {
+    return Math.log(a * a + b * b) * 0.5;
+  }
+
+  a = a * 0.5;
+  b = b * 0.5;
+
+  return 0.5 * Math.log(a * a + b * b) + Math.LN2;
+}

packages/math/curve.test.ts

@@ -49,6 +49,21 @@ describe("Math curve", () => {
         [10.970762294018797, 98.78654713247653],
       ]);
     });
+
+    it("regression 1", () => {
+      const c = curve(
+        pointFrom(41.028864759926016, 12.226249068355052),
+        pointFrom(41.028864759926016, 33.55958240168839),
+        pointFrom(30.362198093259348, 44.22624906835505),
+        pointFrom(9.028864759926016, 44.22624906835505),
+      );
+      const l = line(
+        pointFrom(188.2149592542487, 134.75505940984908),
+        pointFrom(-82.30963544324186, -41.19949363038283),
+      );
+
+      expect(curveIntersectLine(c, l)).toCloselyEqualPoints([[8.5, 87.5]]);
+    });
   });
 
   describe("point closest to other", () => {

packages/math/curve.ts

@@ -1,3 +1,4 @@
+import { add, complex, conjugate, mul, pow, sqrt, sub } from "./complex";
 import { isPoint, pointDistance, pointFrom } from "./point";
 import type { Curve, GlobalPoint, Line, LocalPoint } from "./types";
 
@@ -18,129 +19,195 @@ export function curve<Point extends GlobalPoint | LocalPoint>(
   return [a, b, c, d] as Curve<Point>;
 }
 
+export function curveIntersectLine<Point extends GlobalPoint | LocalPoint>(
+  p: Curve<Point>,
+  l: Line<Point>,
+): Point[] {
+  const A = l[0];
+  const B = l[1];
+  const C0 = p[0];
+  const C1 = p[1];
+  const C2 = p[2];
+  const C3 = p[3];
+
+  const res = [];
+
+  const Ax = 3 * (C1[0] - C2[0]) + C3[0] - C0[0];
+  const Ay = 3 * (C1[1] - C2[1]) + C3[1] - C0[1];
+
+  const Bx = 3 * (C0[0] - 2 * C1[0] + C2[0]);
+  const By = 3 * (C0[1] - 2 * C1[1] + C2[1]);
+
+  const Cx = 3 * (C1[0] - C0[0]);
+  const Cy = 3 * (C1[1] - C0[1]);
+
+  const Dx = C0[0];
+  const Dy = C0[0];
+
+  const vx = B[1] - A[1];
+  const vy = A[0] - B[0];
+
+  const d = A[0] * vx + A[1] * vy;
+
+  const roots = cubicRoots(
+    vx * Ax + vy * Ay,
+    vx * Bx + vy * By,
+    vx * Cx + vy * Cy,
+    vx * Dx + vy * Dy - d,
+  );
+  console.log(...roots.map((r) => Math.round(r[0])));
+  for (const [re, im] of roots) {
+    if (0 < re || re > 1 || Math.abs(im) > 1e-5) {
+      continue;
+    }
+
+    res.push(
+      pointFrom<Point>(
+        ((Ax * re + Bx) * re + Cx) * re + Dx,
+        ((Ay * re + By) * re + Cy) * re + Dy,
+      ),
+    );
+  }
+
+  return res;
+}
+
 /**
  * Computes intersection between a cubic spline and a line segment
  *
  * @href https://www.particleincell.com/2013/cubic-line-intersection/
  */
-export function curveIntersectLine<Point extends GlobalPoint | LocalPoint>(
+// export function curveIntersectLine<Point extends GlobalPoint | LocalPoint>(
-  p: Curve<Point>,
+//   p: Curve<Point>,
-  l: Line<Point>,
+//   l: Line<Point>,
-): Point[] {
+// ): Point[] {
-  const A = l[1][1] - l[0][1]; //A=y2-y1
+//   const A = l[1][1] - l[0][1]; //A=y2-y1
-  const B = l[0][0] - l[1][0]; //B=x1-x2
+//   const B = l[0][0] - l[1][0]; //B=x1-x2
-  const C = l[0][0] * (l[0][1] - l[1][1]) + l[0][1] * (l[1][0] - l[0][0]); //C=x1*(y1-y2)+y1*(x2-x1)
+//   const C = l[0][0] * (l[0][1] - l[1][1]) + l[0][1] * (l[1][0] - l[0][0]); //C=x1*(y1-y2)+y1*(x2-x1)
-
+
-  const bx = [
+//   const bx = [
-    -p[0][0] + 3 * p[1][0] + -3 * p[2][0] + p[3][0],
+//     -p[0][0] + 3 * p[1][0] + -3 * p[2][0] + p[3][0],
-    3 * p[0][0] - 6 * p[1][0] + 3 * p[2][0],
+//     3 * p[0][0] - 6 * p[1][0] + 3 * p[2][0],
-    -3 * p[0][0] + 3 * p[1][0],
+//     -3 * p[0][0] + 3 * p[1][0],
-    p[0][0],
+//     p[0][0],
-  ];
+//   ];
-  const by = [
+//   const by = [
-    -p[0][1] + 3 * p[1][1] + -3 * p[2][1] + p[3][1],
+//     -p[0][1] + 3 * p[1][1] + -3 * p[2][1] + p[3][1],
-    3 * p[0][1] - 6 * p[1][1] + 3 * p[2][1],
+//     3 * p[0][1] - 6 * p[1][1] + 3 * p[2][1],
-    -3 * p[0][1] + 3 * p[1][1],
+//     -3 * p[0][1] + 3 * p[1][1],
-    p[0][1],
+//     p[0][1],
-  ];
+//   ];
-
+
-  const P: [number, number, number, number] = [
+//   const P: [number, number, number, number] = [
-    A * bx[0] + B * by[0] /*t^3*/,
+//     A * bx[0] + B * by[0] /*t^3*/,
-    A * bx[1] + B * by[1] /*t^2*/,
+//     A * bx[1] + B * by[1] /*t^2*/,
-    A * bx[2] + B * by[2] /*t*/,
+//     A * bx[2] + B * by[2] /*t*/,
-    A * bx[3] + B * by[3] + C /*1*/,
+//     A * bx[3] + B * by[3] + C /*1*/,
-  ];
+//   ];
-
+
-  const r = cubicRoots(P);
+//   const r = cubicRoots(P);
-
+
-  /*verify the roots are in bounds of the linear segment*/
+//   /*verify the roots are in bounds of the linear segment*/
-  return r
+//   return r
-    .map((t) => {
+//     .map((t) => {
-      const x = pointFrom<Point>(
+//       const x = pointFrom<Point>(
-        bx[0] * t ** 3 + bx[1] * t ** 2 + bx[2] * t + bx[3],
+//         bx[0] * t ** 3 + bx[1] * t ** 2 + bx[2] * t + bx[3],
-        by[0] * t ** 3 + by[1] * t ** 2 + by[2] * t + by[3],
+//         by[0] * t ** 3 + by[1] * t ** 2 + by[2] * t + by[3],
-      );
+//       );
-
+
-      /*above is intersection point assuming infinitely long line segment,
+//       /*in bounds?*/
-          make sure we are also in bounds of the line*/
+//       if (t < 0 || t > 1.0) {
-      let s;
+//         return null;
-      if (l[1][0] - l[0][0] !== 0) {
+//       }
-        /*if not vertical line*/
+
-        s = (x[0] - l[0][0]) / (l[1][0] - l[0][0]);
+//       return x;
-      } else {
+//     })
-        s = (x[1] - l[0][1]) / (l[1][1] - l[0][1]);
+//     .filter((x): x is Point => x !== null);
-      }
+// }
-
+
-      /*in bounds?*/
+function cubicRoots(a: number, b: number, c: number, d: number) {
-      if (t < 0 || t > 1.0 || s < 0 || s > 1.0) {
+  // Make a depressed cubic of the form x^3 + px + q = 0
-        return null;
+  const p = (3 * a * c - b * b) / (3 * a * a);
-      }
+  const q = (2 * b * b * b - 9 * a * b * c + 27 * a * a * d) / (27 * a * a * a);
-
+
-      return x;
+  // Calculate cube roots of complex numbers
-    })
+  const D = complex(Math.pow(q / 2, 2) + Math.pow(p / 3, 3));
-    .filter((x): x is Point => x !== null);
+  const sqrtD = sqrt(D);
+  const u = pow(add(complex(-q / 2), sqrtD), complex(1 / 3));
+  const v = pow(sub(complex(-q / 2), sqrtD), complex(1 / 3));
+
+  // Calculate the roots in t
+  const omega = complex(-0.5, Math.sqrt(3) / 2);
+
+  const t1 = add(u, v);
+  const t2 = add(mul(v, conjugate(omega)), mul(u, omega));
+  const t3 = add(mul(u, omega), mul(v, omega));
+
+  // Transform back to the original variable x
+  const shift = complex(-b / (3 * a));
+  return [add(t1, shift), add(t2, shift), add(t3, shift)];
 }
 
 /*
  * Based on http://mysite.verizon.net/res148h4j/javascript/script_exact_cubic.html#the%20source%20code
  */
-function cubicRoots(P: [number, number, number, number]) {
+// function cubicRoots(P: [number, number, number, number]) {
-  const a = P[0];
+//   const a = P[0];
-  const b = P[1];
+//   const b = P[1];
-  const c = P[2];
+//   const c = P[2];
-  const d = P[3];
+//   const d = P[3];
-
-  const A = b / a;
-  const B = c / a;
-  const C = d / a;
-
-  let Im;
-
-  const Q = (3 * B - Math.pow(A, 2)) / 9;
-  const R = (9 * A * B - 27 * C - 2 * Math.pow(A, 3)) / 54;
-  const D = Math.pow(Q, 3) + Math.pow(R, 2); // polynomial discriminant
-
-  let t = [];
-
-  if (D >= 0) {
-    // complex or duplicate roots
-    const S =
-      Math.sign(R + Math.sqrt(D)) * Math.pow(Math.abs(R + Math.sqrt(D)), 1 / 3);
-    const T =
-      Math.sign(R - Math.sqrt(D)) * Math.pow(Math.abs(R - Math.sqrt(D)), 1 / 3);
-
-    t[0] = -A / 3 + (S + T); // real root
-    t[1] = -A / 3 - (S + T) / 2; // real part of complex root
-    t[2] = -A / 3 - (S + T) / 2; // real part of complex root
-    Im = Math.abs((Math.sqrt(3) * (S - T)) / 2); // complex part of root pair
-
-    /*discard complex roots*/
-    if (Im !== 0) {
-      t[1] = -1;
-      t[2] = -1;
-    }
-  } // distinct real roots
-  else {
-    const th = Math.acos(R / Math.sqrt(-Math.pow(Q, 3)));
-
-    t[0] = 2 * Math.sqrt(-Q) * Math.cos(th / 3) - A / 3;
-    t[1] = 2 * Math.sqrt(-Q) * Math.cos((th + 2 * Math.PI) / 3) - A / 3;
-    t[2] = 2 * Math.sqrt(-Q) * Math.cos((th + 4 * Math.PI) / 3) - A / 3;
-    Im = 0.0;
-  }
 
-  /*discard out of spec roots*/
+//   const A = b / a;
-  for (let i = 0; i < 3; i++) {
+//   const B = c / a;
-    if (t[i] < 0 || t[i] > 1.0) {
+//   const C = d / a;
-      t[i] = -1;
-    }
-  }
 
-  // sort but place -1 at the end
+//   let Im;
-  t = t.sort((a, b) => (a === -1 ? 1 : b === -1 ? -1 : a - b));
 
-  return t;
+//   const Q = (3 * B - Math.pow(A, 2)) / 9;
-}
+//   const R = (9 * A * B - 27 * C - 2 * Math.pow(A, 3)) / 54;
+//   const D = Math.pow(Q, 3) + Math.pow(R, 2); // polynomial discriminant
+
+//   let t = [];
+
+//   if (D >= 0) {
+//     // complex or duplicate roots
+//     const S =
+//       Math.sign(R + Math.sqrt(D)) * Math.pow(Math.abs(R + Math.sqrt(D)), 1 / 3);
+//     const T =
+//       Math.sign(R - Math.sqrt(D)) * Math.pow(Math.abs(R - Math.sqrt(D)), 1 / 3);
+
+//     t[0] = -A / 3 + (S + T); // real root
+//     t[1] = -A / 3 - (S + T) / 2; // real part of complex root
+//     t[2] = -A / 3 - (S + T) / 2; // real part of complex root
+//     Im = Math.abs((Math.sqrt(3) * (S - T)) / 2); // complex part of root pair
+
+//     /*discard complex roots*/
+//     if (Im !== 0) {
+//       t[1] = -1;
+//       t[2] = -1;
+//     }
+//   } // distinct real roots
+//   else {
+//     const th = Math.acos(R / Math.sqrt(-Math.pow(Q, 3)));
+
+//     t[0] = 2 * Math.sqrt(-Q) * Math.cos(th / 3) - A / 3;
+//     t[1] = 2 * Math.sqrt(-Q) * Math.cos((th + 2 * Math.PI) / 3) - A / 3;
+//     t[2] = 2 * Math.sqrt(-Q) * Math.cos((th + 4 * Math.PI) / 3) - A / 3;
+//     Im = 0.0;
+//   }
+
+//   /*discard out of spec roots*/
+//   for (let i = 0; i < 3; i++) {
+//     if (t[i] < 0 || t[i] > 1.0) {
+//       t[i] = -1;
+//     }
+//   }
+
+//   // sort but place -1 at the end
+//   t = t.sort((a, b) => (a === -1 ? 1 : b === -1 ? -1 : a - b));
+
+//   return t;
+// }
 
 /**
  * Finds the closest point on the Bezier curve from another point

packages/math/types.ts

@@ -153,3 +153,7 @@ export type Ellipse<Point extends GlobalPoint | LocalPoint> = {
 } & {
   _brand: "excalimath_ellipse";
 };
+
+export type Complex = [real: number, imag: number] & {
+  _brand: "excalimath_complex";
+};