diff --git a/OpenNest.Core/Geometry/SpatialQuery.cs b/OpenNest.Core/Geometry/SpatialQuery.cs index cded1a9..c7bac71 100644 --- a/OpenNest.Core/Geometry/SpatialQuery.cs +++ b/OpenNest.Core/Geometry/SpatialQuery.cs @@ -631,19 +631,46 @@ namespace OpenNest.Geometry { for (var i = 0; i < arcEntities.Count; i++) { - if (arcEntities[i] is Arc arc) + if (arcEntities[i] is not Arc arc) + continue; + + var cx = arc.Center.X; + var cy = arc.Center.Y; + var r = arc.Radius; + + for (var j = 0; j < lineEntities.Count; j++) { - for (var j = 0; j < lineEntities.Count; j++) + if (lineEntities[j] is not Line line) + continue; + + var p1x = line.pt1.X; + var p1y = line.pt1.Y; + var ex = line.pt2.X - p1x; + var ey = line.pt2.Y - p1y; + + var det = ex * dirY - ey * dirX; + if (System.Math.Abs(det) < Tolerance.Epsilon) + continue; + + // The directional distance from an arc point at angle θ to the + // line is t(θ) = [A + r·(ey·cosθ − ex·sinθ)] / det. + // dt/dθ = 0 at θ = atan2(−ex, ey) and θ + π. + var theta1 = Angle.NormalizeRad(System.Math.Atan2(-ex, ey)); + var theta2 = Angle.NormalizeRad(theta1 + System.Math.PI); + + for (var k = 0; k < 2; k++) { - if (lineEntities[j] is Line line) - { - var linePt = line.ClosestPointTo(arc.Center); - var arcPt = arc.ClosestPointTo(linePt); - var d = RayEdgeDistance(arcPt.X, arcPt.Y, - line.pt1.X, line.pt1.Y, line.pt2.X, line.pt2.Y, - dirX, dirY); - if (d < minDist) { minDist = d; if (d <= 0) return 0; } - } + var theta = k == 0 ? theta1 : theta2; + + if (!Angle.IsBetweenRad(theta, arc.StartAngle, arc.EndAngle, arc.IsReversed)) + continue; + + var qx = cx + r * System.Math.Cos(theta); + var qy = cy + r * System.Math.Sin(theta); + + var d = RayEdgeDistance(qx, qy, p1x, p1y, line.pt2.X, line.pt2.Y, + dirX, dirY); + if (d < minDist) { minDist = d; if (d <= 0) return 0; } } } } diff --git a/OpenNest.Tests/Fill/CompactorTests.cs b/OpenNest.Tests/Fill/CompactorTests.cs index 21e30b4..4c27df8 100644 --- a/OpenNest.Tests/Fill/CompactorTests.cs +++ b/OpenNest.Tests/Fill/CompactorTests.cs @@ -8,6 +8,76 @@ namespace OpenNest.Tests.Fill { public class CompactorTests { + [Fact] + public void DirectionalDistance_ArcVsInclinedLine_DoesNotOverPush() + { + // Arc (top semicircle) pushed upward toward a 45° inclined line. + // The critical angle on the arc gives a shorter distance than any + // sampled vertex (endpoints + cardinal extremes). + var arc = new Arc(5, 0, 2, 0, System.Math.PI); + var line = new Line(new Vector(3, 4), new Vector(7, 6)); + + var moving = new List { arc }; + var stationary = new List { line }; + var direction = new Vector(0, 1); // push up + + var dist = SpatialQuery.DirectionalDistance(moving, stationary, direction); + + // Move the arc up by the computed distance, then verify no overlap. + // The topmost reachable point on the arc at the critical angle θ ≈ 2.034 + // (between π/2 and π) should just touch the line. + Assert.True(dist < double.MaxValue, "Should find a finite distance"); + Assert.True(dist > 0, "Should be a positive distance"); + + // Verify: after moving, the closest point on the arc should be within + // tolerance of the line, not past it. + var theta = System.Math.Atan2( + line.pt2.X - line.pt1.X, -(line.pt2.Y - line.pt1.Y)); + theta = OpenNest.Math.Angle.NormalizeRad(theta + System.Math.PI); + var qx = arc.Center.X + arc.Radius * System.Math.Cos(theta); + var qy = arc.Center.Y + arc.Radius * System.Math.Sin(theta) + dist; + + // The moved point should be on or just touching the line, not past it. + // Line equation: (y - 4) / (x - 3) = (6 - 4) / (7 - 3) = 0.5 + // y = 0.5x + 2.5 + var lineYAtQx = 0.5 * qx + 2.5; + Assert.True(qy <= lineYAtQx + 0.001, + $"Arc point ({qx:F4}, {qy:F4}) should not be past line (line Y={lineYAtQx:F4} at X={qx:F4}). " + + $"dist={dist:F6}, overshot by {qy - lineYAtQx:F6}"); + } + + [Fact] + public void DirectionalDistance_ArcVsInclinedLine_BetterThanVertexSampling() + { + // Same geometry — verify the analytical Phase 3 finds a shorter + // distance than the Phase 1/2 vertex sampling alone would. + var arc = new Arc(5, 0, 2, 0, System.Math.PI); + var line = new Line(new Vector(3, 4), new Vector(7, 6)); + + // Phase 1/2 vertex-only distance: sample arc endpoints + cardinal extreme. + var vertices = new[] + { + new Vector(7, 0), // arc endpoint θ=0 + new Vector(3, 0), // arc endpoint θ=π + new Vector(5, 2), // cardinal extreme θ=π/2 + }; + + var vertexMin = double.MaxValue; + foreach (var v in vertices) + { + var d = SpatialQuery.RayEdgeDistance(v.X, v.Y, + line.pt1.X, line.pt1.Y, line.pt2.X, line.pt2.Y, 0, 1); + if (d < vertexMin) vertexMin = d; + } + + // Full directional distance (includes Phase 3 arc-to-line). + var moving = new List { arc }; + var stationary = new List { line }; + var fullDist = SpatialQuery.DirectionalDistance(moving, stationary, new Vector(0, 1)); + + Assert.True(fullDist < vertexMin, + $"Full distance ({fullDist:F6}) should be less than vertex-only ({vertexMin:F6})"); + } private static Drawing MakeRectDrawing(double w, double h) { var pgm = new OpenNest.CNC.Program();