From 1f5375ef3799ae0890b54dd3b7e21174349badb7 Mon Sep 17 00:00:00 2001 From: Mark Tolmacs Date: Wed, 5 Feb 2025 11:18:16 +0100 Subject: [PATCH] Another cubic solver version --- packages/excalidraw/element/collision.ts | 46 +++- packages/math/complex.ts | 133 +++++++++++ packages/math/curve.test.ts | 15 ++ packages/math/curve.ts | 287 ++++++++++++++--------- packages/math/types.ts | 4 + 5 files changed, 371 insertions(+), 114 deletions(-) create mode 100644 packages/math/complex.ts diff --git a/packages/excalidraw/element/collision.ts b/packages/excalidraw/element/collision.ts index 656207487f..27bd00db77 100644 --- a/packages/excalidraw/element/collision.ts +++ b/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(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] diff --git a/packages/math/complex.ts b/packages/math/complex.ts new file mode 100644 index 0000000000..12e211fcf5 --- /dev/null +++ b/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; +} diff --git a/packages/math/curve.test.ts b/packages/math/curve.test.ts index 164d74a648..6abb0a2370 100644 --- a/packages/math/curve.test.ts +++ b/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", () => { diff --git a/packages/math/curve.ts b/packages/math/curve.ts index 7ae6ff037d..fcd0c129c2 100644 --- a/packages/math/curve.ts +++ b/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( return [a, b, c, d] as Curve; } +export function curveIntersectLine( + p: Curve, + l: Line, +): 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( + ((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( - p: Curve, - l: Line, -): Point[] { - const A = l[1][1] - l[0][1]; //A=y2-y1 - 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 bx = [ - -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] + 3 * p[1][0], - p[0][0], - ]; - const by = [ - -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] + 3 * p[1][1], - p[0][1], - ]; - - const P: [number, number, number, number] = [ - A * bx[0] + B * by[0] /*t^3*/, - A * bx[1] + B * by[1] /*t^2*/, - A * bx[2] + B * by[2] /*t*/, - A * bx[3] + B * by[3] + C /*1*/, - ]; - - const r = cubicRoots(P); - - /*verify the roots are in bounds of the linear segment*/ - return r - .map((t) => { - const x = pointFrom( - 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], - ); - - /*above is intersection point assuming infinitely long line segment, - make sure we are also in bounds of the line*/ - let s; - if (l[1][0] - l[0][0] !== 0) { - /*if not vertical line*/ - s = (x[0] - l[0][0]) / (l[1][0] - l[0][0]); - } else { - s = (x[1] - l[0][1]) / (l[1][1] - l[0][1]); - } - - /*in bounds?*/ - if (t < 0 || t > 1.0 || s < 0 || s > 1.0) { - return null; - } - - return x; - }) - .filter((x): x is Point => x !== null); +// export function curveIntersectLine( +// p: Curve, +// l: Line, +// ): Point[] { +// const A = l[1][1] - l[0][1]; //A=y2-y1 +// 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 bx = [ +// -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] + 3 * p[1][0], +// p[0][0], +// ]; +// const by = [ +// -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] + 3 * p[1][1], +// p[0][1], +// ]; + +// const P: [number, number, number, number] = [ +// A * bx[0] + B * by[0] /*t^3*/, +// A * bx[1] + B * by[1] /*t^2*/, +// A * bx[2] + B * by[2] /*t*/, +// A * bx[3] + B * by[3] + C /*1*/, +// ]; + +// const r = cubicRoots(P); + +// /*verify the roots are in bounds of the linear segment*/ +// return r +// .map((t) => { +// const x = pointFrom( +// 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], +// ); + +// /*in bounds?*/ +// if (t < 0 || t > 1.0) { +// return null; +// } + +// return x; +// }) +// .filter((x): x is Point => x !== null); +// } + +function cubicRoots(a: number, b: number, c: number, d: number) { + // Make a depressed cubic of the form x^3 + px + q = 0 + 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); + + // Calculate cube roots of complex numbers + const D = complex(Math.pow(q / 2, 2) + Math.pow(p / 3, 3)); + 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]) { - const a = P[0]; - const b = P[1]; - const c = P[2]; - 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; - } +// function cubicRoots(P: [number, number, number, number]) { +// const a = P[0]; +// const b = P[1]; +// const c = P[2]; +// const d = P[3]; - /*discard out of spec roots*/ - for (let i = 0; i < 3; i++) { - if (t[i] < 0 || t[i] > 1.0) { - t[i] = -1; - } - } +// const A = b / a; +// const B = c / a; +// const C = d / a; - // sort but place -1 at the end - t = t.sort((a, b) => (a === -1 ? 1 : b === -1 ? -1 : a - b)); +// let Im; - 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 diff --git a/packages/math/types.ts b/packages/math/types.ts index b42e809b48..d467a82b76 100644 --- a/packages/math/types.ts +++ b/packages/math/types.ts @@ -153,3 +153,7 @@ export type Ellipse = { } & { _brand: "excalimath_ellipse"; }; + +export type Complex = [real: number, imag: number] & { + _brand: "excalimath_complex"; +};