diff --git a/OpenNest.Core/Geometry/Clearance.cs b/OpenNest.Core/Geometry/Clearance.cs new file mode 100644 index 0000000..0e8afba --- /dev/null +++ b/OpenNest.Core/Geometry/Clearance.cs @@ -0,0 +1,318 @@ +using System; +using System.Collections.Generic; +using OpenNest.Math; + +namespace OpenNest.Geometry +{ + /// + /// Signed clearance between two closed polygons, plus the unit direction that + /// increases it by moving the first polygon. + /// + public struct ClearanceResult + { + /// + /// > 0: minimum boundary distance. 0: touching. < 0: penetration depth + /// (the translation of a along needed to end + /// contact). + /// + public double Distance; + + /// + /// Unit direction for translating a away from b. For penetration + /// this is the minimum-translation direction. Never zero-length; degenerate + /// (coincident-centroid) penetration resolves to a deterministic axis. + /// + public Vector Direction; + + public ClearanceResult(double distance, Vector direction) + { + Distance = distance; + Direction = direction; + } + } + + /// + /// Omnidirectional clearance between two closed, lines-only polygons. + /// Complements (movement along a + /// fixed ray) with the all-directions minimum distance and separating direction, + /// and (boolean overlap) with depth and direction. + /// + /// Reference quality, not hot-loop quality: separation is a brute-force + /// segment-pair minimum with a bounding-box reject, penetration is a + /// separating-axis sweep over both polygons' edge normals. The overlap verdict + /// defers to + /// so callers that validate with Collision never see a disagreeing kernel. + /// Rings with holes are handled by the caller: pass every ring pair (a part's + /// material boundary is its outer ring plus its hole rings). + /// + /// + public static class Clearance + { + public static ClearanceResult Between(Polygon a, Polygon b) + { + var linesA = a.ToLines(); + var linesB = b.ToLines(); + + if (linesA.Count == 0 || linesB.Count == 0) + return new ClearanceResult(0, new Vector(1, 0)); + + if (Collision.HasOverlap(a, b)) + return Penetration(linesA, linesB); + + return Separation(linesA, linesB); + } + + /// + /// Minimum boundary distance between two non-overlapping rings and the + /// direction that translates away from + /// at the closest contact. + /// + private static ClearanceResult Separation(List linesA, List linesB) + { + var minDist = double.MaxValue; + var pa = Vector.Zero; + var pb = Vector.Zero; + + var boxes = new Box[linesB.Count]; + for (var i = 0; i < linesB.Count; i++) + boxes[i] = SegmentBox(linesB[i]); + + foreach (var la in linesA) + { + var boxA = SegmentBox(la); + + for (var i = 0; i < linesB.Count; i++) + { + if (!BoxesWithin(boxA, boxes[i], minDist)) + continue; + + var d = SegmentDistance(la, linesB[i], out var qa, out var qb); + if (d < minDist) + { + minDist = d; + pa = qa; + pb = qb; + } + } + } + + var dir = pa - pb; + var len = Magnitude(dir); + + if (len <= Tolerance.Epsilon) + dir = CentroidAway(linesA, linesB); + else + dir = dir / len; + + return new ClearanceResult(minDist, dir); + } + + /// + /// Penetration depth and minimum-translation direction along the separating- + /// axis candidates of both rings. Per candidate axis the true translation + /// depth is used (exit distance to the far side), so containment reports the + /// depth that actually ends contact, not the interval-intersection length. + /// Depth is reported as a negative clearance. + /// + private static ClearanceResult Penetration(List linesA, List linesB) + { + var ca = Centroid(linesA); + var cb = Centroid(linesB); + + var bestDepth = double.MaxValue; + var bestDir = new Vector(1, 0); + + var bestAxis = -1; + + for (var axis = 0; axis < 2; axis++) + { + var lines = axis == 0 ? linesA : linesB; + + foreach (var line in lines) + { + var edge = line.pt2 - line.pt1; + var n = new Vector(edge.Y, -edge.X); + var len = Magnitude(n); + if (len <= Tolerance.Epsilon) + continue; + n = n / len; + + var (minA, maxA) = Project(linesA, n); + var (minB, maxB) = Project(linesB, n); + + if (maxA <= minB || maxB <= minA) + continue; // separating axis found + + // Depth pushing a away from b along ±n. + var forward = maxB - minA; // move a in +n until minA >= maxB + var backward = maxA - minB; // move a in -n until maxA <= minB + + double depth; + Vector dir; + if (forward <= backward) + { + depth = forward; + dir = n; + } + else + { + depth = backward; + dir = -n; + } + + if (depth < bestDepth - Tolerance.Epsilon || bestAxis < 0) + { + bestDepth = depth; + bestDir = dir; + bestAxis = axis; + } + } + } + + if (bestAxis < 0) + { + // No candidate axis (degenerate rings): deterministic fallback. + var away = ca - cb; + var len = Magnitude(away); + bestDir = len > Tolerance.Epsilon ? away / len : new Vector(1, 0); + bestDepth = 0; + } + + return new ClearanceResult(-bestDepth, bestDir); + } + + private static Vector CentroidAway(List linesA, List linesB) + { + var away = Centroid(linesA) - Centroid(linesB); + var len = Magnitude(away); + return len > Tolerance.Epsilon ? away / len : new Vector(1, 0); + } + + private static Vector Centroid(List lines) + { + var sum = Vector.Zero; + foreach (var line in lines) + { + sum += line.pt1; + sum += line.pt2; + } + return sum / (2 * lines.Count); + } + + private static (double Min, double Max) Project(List lines, Vector n) + { + var min = double.MaxValue; + var max = double.MinValue; + + foreach (var line in lines) + { + var d1 = line.pt1.DotProduct(n); + var d2 = line.pt2.DotProduct(n); + if (d1 < min) + min = d1; + if (d1 > max) + max = d1; + if (d2 < min) + min = d2; + if (d2 > max) + max = d2; + } + + return (min, max); + } + + /// + /// Minimum distance between two segments with the closest points. + /// Non-parallel segments use the classic clamped closest-point solve; + /// (near-)parallel segments fall back to the four endpoint-to-segment + /// distances, which is where the minimum always lies. + /// + private static double SegmentDistance(Line a, Line b, out Vector pa, out Vector pb) + { + var p = a.pt1; + var r = a.pt2 - a.pt1; + var q = b.pt1; + var s = b.pt2 - b.pt1; + + var rxr = r.DotProduct(r); + var sxs = s.DotProduct(s); + var rxs = r.DotProduct(s); + + const double eps = 1e-12; + + var denom = rxr * sxs - rxs * rxs; + if (denom > eps && rxr > eps && sxs > eps) + { + // Minimize |(p + r t) - (q + s u)|^2; setting both partials to + // zero and solving (Cramer) with d0 = p - q: + // t = ((r.s)(d0.s) - (d0.r)(s.s)) / (rr.ss - (r.s)^2) + // u = ((r.r)(d0.s) - (r.s)(d0.r)) / (rr.ss - (r.s)^2) + var d0 = p - q; + var d0r = d0.DotProduct(r); + var d0s = d0.DotProduct(s); + + var t = Clamp((rxs * d0s - d0r * sxs) / denom, 0, 1); + var u = Clamp((rxs * t + d0s) / sxs, 0, 1); // nearest u on b for clamped t + t = Clamp((rxs * u - d0r) / rxr, 0, 1); // re-solve t for clamped u + + pa = p + r * t; + pb = q + s * u; + return pa.DistanceTo(pb); + } + + // Degenerate or parallel: the minimum is attained at an endpoint. + var bestPa = p; + var bestPb = q; + var best = double.MaxValue; + + void Consider(Vector pt, Line seg, bool ptOnA) + { + var d = seg.pt2 - seg.pt1; + var len2 = d.DotProduct(d); + var u = len2 <= eps ? 0 : Clamp((pt - seg.pt1).DotProduct(d) / len2, 0, 1); + var on = seg.pt1 + d * u; + var dist = pt.DistanceTo(on); + if (dist < best) + { + best = dist; + bestPa = ptOnA ? pt : on; + bestPb = ptOnA ? on : pt; + } + } + + Consider(p, b, true); + Consider(a.pt2, b, true); + Consider(q, a, false); + Consider(b.pt2, a, false); + + pa = bestPa; + pb = bestPb; + return best; + } + + private static double Clamp(double v, double lo, double hi) => + v < lo ? lo : (v > hi ? hi : v); + + private static double Magnitude(Vector v) => System.Math.Sqrt(v.X * v.X + v.Y * v.Y); + + private static Box SegmentBox(Line line) + { + return new Box( + System.Math.Min(line.pt1.X, line.pt2.X), + System.Math.Min(line.pt1.Y, line.pt2.Y), + System.Math.Abs(line.pt2.X - line.pt1.X), + System.Math.Abs(line.pt2.Y - line.pt1.Y) + ); + } + + private static bool BoxesWithin(Box a, Box b, double distance) + { + return !( + a.Right + distance < b.Left + || b.Right + distance < a.Left + || a.Top + distance < b.Bottom + || b.Top + distance < a.Bottom + ); + } + } +} diff --git a/OpenNest.Tests/Geometry/ClearanceTests.cs b/OpenNest.Tests/Geometry/ClearanceTests.cs new file mode 100644 index 0000000..c7708d7 --- /dev/null +++ b/OpenNest.Tests/Geometry/ClearanceTests.cs @@ -0,0 +1,286 @@ +using System; +using System.Collections.Generic; +using OpenNest.Geometry; + +namespace OpenNest.Tests.Geometry; + +public class ClearanceTests +{ + private const double Tol = 1e-9; + + private static Polygon Square(double x, double y, double w, double h) + { + var p = new Polygon + { + Vertices = new List + { + new Vector(x, y), + new Vector(x + w, y), + new Vector(x + w, y + h), + new Vector(x, y + h), + } + }; + p.Close(); + p.UpdateBounds(); + return p; + } + + private static Polygon Triangle(params double[] xy) + { + var p = new Polygon(); + for (var i = 0; i + 1 < xy.Length; i += 2) + p.Vertices.Add(new Vector(xy[i], xy[i + 1])); + p.Close(); + p.UpdateBounds(); + return p; + } + + // ---- Separation ---- + + [Fact] + public void Between_SeparatedHorizontally_DistanceAndDirection() + { + var a = Square(0, 0, 1, 1); + var b = Square(3, 0, 1, 1); + + var r = Clearance.Between(a, b); + + Assert.Equal(2.0, r.Distance, 6); + // Pushing a away from b means moving left. + Assert.Equal(-1.0, r.Direction.X, 6); + Assert.Equal(0.0, r.Direction.Y, 6); + } + + [Fact] + public void Between_SeparatedDiagonally_CornerDistance() + { + var a = Square(0, 0, 1, 1); + var b = Square(2, 2, 1, 1); + + var r = Clearance.Between(a, b); + + Assert.Equal(System.Math.Sqrt(2.0), r.Distance, 6); + Assert.Equal(-1 / System.Math.Sqrt(2), r.Direction.X, 6); + Assert.Equal(-1 / System.Math.Sqrt(2), r.Direction.Y, 6); + } + + [Fact] + public void Between_Touching_ZeroDistance() + { + var a = Square(0, 0, 1, 1); + var b = Square(1, 0, 2, 1); + + var r = Clearance.Between(a, b); + + Assert.True(System.Math.Abs(r.Distance) < 1e-6, $"expected ~0, got {r.Distance}"); + var mag = System.Math.Sqrt( + r.Direction.X * r.Direction.X + r.Direction.Y * r.Direction.Y + ); + Assert.Equal(1.0, mag, 6); + } + + [Fact] + public void Between_VertexToEdge_DistanceIsPerpendicular() + { + // Triangle above a wide square; the base sits 3 above the square's top edge. + var a = Triangle(1, 3, 3, 3, 2, 4); + var b = Square(0, -4, 10, 4); // top edge at y = 0 + + var r = Clearance.Between(a, b); + + Assert.Equal(3.0, r.Distance, 6); // base y=3 to y=0 + Assert.Equal(0.0, r.Direction.X, 6); + Assert.Equal(1.0, r.Direction.Y, 6); + } + + [Fact] + public void Between_ParallelStaggeredEdges_MinimumAcrossAllPairs() + { + // Two L-ish shapes (as simple polys) offset so the true minimum is + // between mid-edges, not vertices. + var a = Square(0, 0, 4, 1); + var b = Square(1, 2, 1, 3); + + var r = Clearance.Between(a, b); + + Assert.Equal(1.0, r.Distance, 6); + Assert.Equal(-1.0, r.Direction.Y, 6); + } + + // ---- Penetration ---- + + [Fact] + public void Between_OverlappingSquares_MinimumTranslationAxis() + { + // Overlap 0.5 in X, 1.0 in Y -> cheapest exit is X. + var a = Square(0, 0, 1, 1); + var b = Square(0.5, 0, 1.5, 1); + + var r = Clearance.Between(a, b); + + Assert.Equal(-0.5, r.Distance, 6); + Assert.Equal(-1.0, r.Direction.X, 6); // push a left, out of b + Assert.Equal(0.0, r.Direction.Y, 6); + } + + [Fact] + public void Between_OverlappingVerticallyCheaper_ExitsInY() + { + // Overlap 0.8 in X, 0.2 in Y -> cheapest exit is Y. + var a = Square(0, 0, 1, 1); + var b = Square(0.2, 0.8, 1.2, 1.8); + + var r = Clearance.Between(a, b); + + Assert.Equal(-0.2, r.Distance, 6); + Assert.Equal(0.0, r.Direction.X, 6); + Assert.Equal(-1.0, r.Direction.Y, 6); + } + + [Fact] + public void Between_ContainedSquare_ExitsThroughNearestWall() + { + // Inner square near the left wall: the translation that ENDS the overlap + // carries its right edge (x=1.2) past the outer's left edge (x=0). + var outer = Square(0, 0, 10, 10); + var inner = Square(0.2, 4, 1, 1); + + var r = Clearance.Between(inner, outer); + + Assert.Equal(-1.2, r.Distance, 6); + Assert.Equal(-1.0, r.Direction.X, 6); + } + + [Fact] + public void Between_ConcentricSquares_DepthIsExitTranslation() + { + var outer = Square(0, 0, 10, 10); + var inner = Square(2, 2, 4, 4); // spans [2,6]; leftmost exit carries 6 to 0 + + var r = Clearance.Between(inner, outer); + + Assert.Equal(-6.0, r.Distance, 6); + Assert.Equal(0.0, r.Direction.X, 6); + Assert.Equal(-1.0, r.Direction.Y, 6); + } + + [Fact] + public void Between_TrianglesPenetrating_ReportsNegativeDepth() + { + var a = Triangle(0, 0, 4, 0, 2, 3); + var b = Triangle(1, 0, 5, 0, 3, 3); + + var r = Clearance.Between(a, b); + + Assert.True(r.Distance < 0, $"expected penetration, got {r.Distance}"); + } + + // ---- Direction is actionable: moving a by -Distance * dir clears contact ---- + + [Fact] + public void Between_PenetrationApplyingDirection_EndsContact() + { + var a = Square(0, 0, 1, 1); + var b = Square(0.3, 0, 1.6, 2); + + var r = Clearance.Between(a, b); + + var moved = (Polygon)a.Clone(); + moved.Offset(r.Direction * (-r.Distance + 0.001)); + moved.UpdateBounds(); + + Assert.False(Collision.HasOverlap(moved, b)); + } + + [Fact] + public void Between_SeparationApplyingDirection_NeverReducesDistance() + { + var a = Square(0, 0, 1, 1); + var b = Square(4, 1, 2, 2); + + var r = Clearance.Between(a, b); + Assert.True(r.Distance > 0); + + // A tiny step along the reported direction must not move closer. + var moved = (Polygon)a.Clone(); + moved.Offset(r.Direction * (r.Distance / 2)); + moved.UpdateBounds(); + + var r2 = Clearance.Between(moved, b); + Assert.True( + r2.Distance >= r.Distance - Tol, + $"moving along dir reduced clearance {r.Distance} -> {r2.Distance}" + ); + } + + // ---- Determinism ---- + + [Fact] + public void Between_RepeatedCalls_IdenticalResult() + { + var a = Square(0, 0, 1, 1); + var b = Square(0.5, 0.25, 2, 1.5); + + var r1 = Clearance.Between(a, b); + var r2 = Clearance.Between(a, b); + + Assert.Equal(r1.Distance, r2.Distance); + Assert.Equal(r1.Direction.X, r2.Direction.X); + Assert.Equal(r1.Direction.Y, r2.Direction.Y); + } + + [Fact] + public void Between_SymmetricSwap_MirrorsDirection() + { + var a = Square(0, 0, 1, 1); + var b = Square(0.5, 0, 1.5, 1); + + var ab = Clearance.Between(a, b); + var ba = Clearance.Between(b, a); + + Assert.Equal(ab.Distance, ba.Distance, 6); + Assert.Equal(-ab.Direction.X, ba.Direction.X, 6); + Assert.Equal(-ab.Direction.Y, ba.Direction.Y, 6); + } + + // ---- Agreement with the Collision oracle ---- + + [Fact] + public void Between_SignMatchesCollisionVerdict() + { + var polygons = new List + { + Square(0, 0, 1, 1), + Square(1, 0, 2, 1), + Square(0.5, 0, 1.5, 1), + Square(0.25, 0.25, 0.75, 0.75), + Square(5, 5, 6, 6), + Triangle(0, 0, 2, 0, 1, 2), + Triangle(0.5, -1, 2.5, -1, 1.5, 1), + }; + + for (var i = 0; i < polygons.Count; i++) + { + for (var j = i + 1; j < polygons.Count; j++) + { + var overlaps = Collision.HasOverlap(polygons[i], polygons[j]); + var r = Clearance.Between(polygons[i], polygons[j]); + + if (overlaps) + { + Assert.True( + r.Distance <= Tol, + $"pair {i},{j}: Collision overlaps but clearance {r.Distance}" + ); + } + else + { + Assert.True( + r.Distance >= -Tol, + $"pair {i},{j}: Collision clear but clearance {r.Distance}" + ); + } + } + } + } +}