feat: add SplineConverter with tangent-chained arc fitting
Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
This commit is contained in:
247
OpenNest.Core/Geometry/SplineConverter.cs
Normal file
247
OpenNest.Core/Geometry/SplineConverter.cs
Normal file
@@ -0,0 +1,247 @@
|
||||
using OpenNest.Math;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace OpenNest.Geometry
|
||||
{
|
||||
public static class SplineConverter
|
||||
{
|
||||
private const int MinPointsForArc = 3;
|
||||
|
||||
public static List<Entity> Convert(List<Vector> points, bool isClosed, double tolerance = 0.001)
|
||||
{
|
||||
if (points == null || points.Count < 2)
|
||||
return new List<Entity>();
|
||||
|
||||
var entities = new List<Entity>();
|
||||
var i = 0;
|
||||
var chainedTangent = Vector.Invalid;
|
||||
|
||||
while (i < points.Count - 1)
|
||||
{
|
||||
var result = TryFitArc(points, i, chainedTangent, tolerance);
|
||||
if (result != null)
|
||||
{
|
||||
entities.Add(result.Arc);
|
||||
chainedTangent = result.EndTangent;
|
||||
i = result.EndIndex;
|
||||
}
|
||||
else
|
||||
{
|
||||
entities.Add(new Line(points[i], points[i + 1]));
|
||||
chainedTangent = Vector.Invalid;
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
return entities;
|
||||
}
|
||||
|
||||
private static ArcFitResult TryFitArc(List<Vector> points, int start,
|
||||
Vector chainedTangent, double tolerance)
|
||||
{
|
||||
var minEnd = start + MinPointsForArc - 1;
|
||||
if (minEnd >= points.Count)
|
||||
return null;
|
||||
|
||||
var hasTangent = chainedTangent.IsValid();
|
||||
|
||||
var subPoints = points.GetRange(start, MinPointsForArc);
|
||||
var (center, radius, dev) = hasTangent
|
||||
? FitWithStartTangent(subPoints, chainedTangent)
|
||||
: FitCircumscribed(subPoints);
|
||||
|
||||
if (!center.IsValid() || dev > tolerance)
|
||||
return null;
|
||||
|
||||
var endIdx = minEnd;
|
||||
while (endIdx + 1 < points.Count)
|
||||
{
|
||||
var extPoints = points.GetRange(start, endIdx + 1 - start + 1);
|
||||
var (nc, nr, nd) = hasTangent
|
||||
? FitWithStartTangent(extPoints, chainedTangent)
|
||||
: FitCircumscribed(extPoints);
|
||||
|
||||
if (!nc.IsValid() || nd > tolerance)
|
||||
break;
|
||||
|
||||
endIdx++;
|
||||
center = nc;
|
||||
radius = nr;
|
||||
dev = nd;
|
||||
}
|
||||
|
||||
var finalPoints = points.GetRange(start, endIdx - start + 1);
|
||||
var sweep = System.Math.Abs(SumSignedAngles(center, finalPoints));
|
||||
if (sweep < Angle.ToRadians(5))
|
||||
return null;
|
||||
|
||||
var arc = CreateArc(center, radius, finalPoints);
|
||||
var endTangent = ComputeEndTangent(center, finalPoints);
|
||||
|
||||
return new ArcFitResult(arc, endTangent, endIdx);
|
||||
}
|
||||
|
||||
private static (Vector center, double radius, double deviation) FitCircumscribed(
|
||||
List<Vector> points)
|
||||
{
|
||||
if (points.Count < 3)
|
||||
return (Vector.Invalid, 0, double.MaxValue);
|
||||
|
||||
var p0 = points[0];
|
||||
var pMid = points[points.Count / 2];
|
||||
var pEnd = points[^1];
|
||||
|
||||
// Find circumcenter by intersecting perpendicular bisectors of two chords
|
||||
var (center, radius) = Circumcenter(p0, pMid, pEnd);
|
||||
if (!center.IsValid())
|
||||
return (Vector.Invalid, 0, double.MaxValue);
|
||||
|
||||
return (center, radius, MaxRadialDeviation(points, center.X, center.Y, radius));
|
||||
}
|
||||
|
||||
private static (Vector center, double radius) Circumcenter(Vector a, Vector b, Vector c)
|
||||
{
|
||||
// Perpendicular bisector of chord a-b
|
||||
var m1x = (a.X + b.X) / 2;
|
||||
var m1y = (a.Y + b.Y) / 2;
|
||||
var d1x = -(b.Y - a.Y);
|
||||
var d1y = b.X - a.X;
|
||||
|
||||
// Perpendicular bisector of chord b-c
|
||||
var m2x = (b.X + c.X) / 2;
|
||||
var m2y = (b.Y + c.Y) / 2;
|
||||
var d2x = -(c.Y - b.Y);
|
||||
var d2y = c.X - b.X;
|
||||
|
||||
var det = d1x * d2y - d1y * d2x;
|
||||
if (System.Math.Abs(det) < 1e-10)
|
||||
return (Vector.Invalid, 0);
|
||||
|
||||
var t = ((m2x - m1x) * d2y - (m2y - m1y) * d2x) / det;
|
||||
|
||||
var cx = m1x + t * d1x;
|
||||
var cy = m1y + t * d1y;
|
||||
var radius = System.Math.Sqrt((cx - a.X) * (cx - a.X) + (cy - a.Y) * (cy - a.Y));
|
||||
|
||||
if (radius < 1e-10)
|
||||
return (Vector.Invalid, 0);
|
||||
|
||||
return (new Vector(cx, cy), radius);
|
||||
}
|
||||
|
||||
private static (Vector center, double radius, double deviation) FitWithStartTangent(
|
||||
List<Vector> points, Vector tangent)
|
||||
{
|
||||
if (points.Count < 3)
|
||||
return (Vector.Invalid, 0, double.MaxValue);
|
||||
|
||||
var p1 = points[0];
|
||||
var pn = points[^1];
|
||||
|
||||
var mx = (p1.X + pn.X) / 2;
|
||||
var my = (p1.Y + pn.Y) / 2;
|
||||
var dx = pn.X - p1.X;
|
||||
var dy = pn.Y - p1.Y;
|
||||
var chordLen = System.Math.Sqrt(dx * dx + dy * dy);
|
||||
if (chordLen < 1e-10)
|
||||
return (Vector.Invalid, 0, double.MaxValue);
|
||||
|
||||
var bx = -dy / chordLen;
|
||||
var by = dx / chordLen;
|
||||
|
||||
var tLen = System.Math.Sqrt(tangent.X * tangent.X + tangent.Y * tangent.Y);
|
||||
if (tLen < 1e-10)
|
||||
return (Vector.Invalid, 0, double.MaxValue);
|
||||
|
||||
var nx = -tangent.Y / tLen;
|
||||
var ny = tangent.X / tLen;
|
||||
|
||||
var det = nx * by - ny * bx;
|
||||
if (System.Math.Abs(det) < 1e-10)
|
||||
return (Vector.Invalid, 0, double.MaxValue);
|
||||
|
||||
var s = ((mx - p1.X) * by - (my - p1.Y) * bx) / det;
|
||||
|
||||
var cx = p1.X + s * nx;
|
||||
var cy = p1.Y + s * ny;
|
||||
var radius = System.Math.Sqrt((cx - p1.X) * (cx - p1.X) + (cy - p1.Y) * (cy - p1.Y));
|
||||
|
||||
if (radius < 1e-10)
|
||||
return (Vector.Invalid, 0, double.MaxValue);
|
||||
|
||||
return (new Vector(cx, cy), radius, MaxRadialDeviation(points, cx, cy, radius));
|
||||
}
|
||||
|
||||
private static double MaxRadialDeviation(List<Vector> points, double cx, double cy, double radius)
|
||||
{
|
||||
var maxDev = 0.0;
|
||||
for (var i = 1; i < points.Count - 1; i++)
|
||||
{
|
||||
var px = points[i].X - cx;
|
||||
var py = points[i].Y - cy;
|
||||
var dist = System.Math.Sqrt(px * px + py * py);
|
||||
var dev = System.Math.Abs(dist - radius);
|
||||
if (dev > maxDev) maxDev = dev;
|
||||
}
|
||||
return maxDev;
|
||||
}
|
||||
|
||||
private static double SumSignedAngles(Vector center, List<Vector> points)
|
||||
{
|
||||
var total = 0.0;
|
||||
for (var i = 0; i < points.Count - 1; i++)
|
||||
{
|
||||
var a1 = System.Math.Atan2(points[i].Y - center.Y, points[i].X - center.X);
|
||||
var a2 = System.Math.Atan2(points[i + 1].Y - center.Y, points[i + 1].X - center.X);
|
||||
var da = a2 - a1;
|
||||
while (da > System.Math.PI) da -= Angle.TwoPI;
|
||||
while (da < -System.Math.PI) da += Angle.TwoPI;
|
||||
total += da;
|
||||
}
|
||||
return total;
|
||||
}
|
||||
|
||||
private static Vector ComputeEndTangent(Vector center, List<Vector> points)
|
||||
{
|
||||
var lastPt = points[^1];
|
||||
var totalAngle = SumSignedAngles(center, points);
|
||||
|
||||
var rx = lastPt.X - center.X;
|
||||
var ry = lastPt.Y - center.Y;
|
||||
|
||||
return totalAngle >= 0
|
||||
? new Vector(-ry, rx)
|
||||
: new Vector(ry, -rx);
|
||||
}
|
||||
|
||||
private static Arc CreateArc(Vector center, double radius, List<Vector> points)
|
||||
{
|
||||
var firstPoint = points[0];
|
||||
var lastPoint = points[^1];
|
||||
|
||||
var startAngle = System.Math.Atan2(firstPoint.Y - center.Y, firstPoint.X - center.X);
|
||||
var endAngle = System.Math.Atan2(lastPoint.Y - center.Y, lastPoint.X - center.X);
|
||||
var isReversed = SumSignedAngles(center, points) < 0;
|
||||
|
||||
if (startAngle < 0) startAngle += Angle.TwoPI;
|
||||
if (endAngle < 0) endAngle += Angle.TwoPI;
|
||||
|
||||
return new Arc(center, radius, startAngle, endAngle, isReversed);
|
||||
}
|
||||
|
||||
private sealed class ArcFitResult
|
||||
{
|
||||
public Arc Arc { get; }
|
||||
public Vector EndTangent { get; }
|
||||
public int EndIndex { get; }
|
||||
|
||||
public ArcFitResult(Arc arc, Vector endTangent, int endIndex)
|
||||
{
|
||||
Arc = arc;
|
||||
EndTangent = endTangent;
|
||||
EndIndex = endIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
132
OpenNest.Tests/SplineConverterTests.cs
Normal file
132
OpenNest.Tests/SplineConverterTests.cs
Normal file
@@ -0,0 +1,132 @@
|
||||
using OpenNest.Geometry;
|
||||
using OpenNest.Math;
|
||||
using Xunit;
|
||||
|
||||
namespace OpenNest.Tests;
|
||||
|
||||
public class SplineConverterTests
|
||||
{
|
||||
[Fact]
|
||||
public void Convert_SemicirclePoints_ProducesSingleArc()
|
||||
{
|
||||
var points = new System.Collections.Generic.List<Vector>();
|
||||
for (var i = 0; i <= 50; i++)
|
||||
{
|
||||
var t = System.Math.PI * i / 50;
|
||||
points.Add(new Vector(10 * System.Math.Cos(t), 10 * System.Math.Sin(t)));
|
||||
}
|
||||
|
||||
var result = SplineConverter.Convert(points, isClosed: false, tolerance: 0.001);
|
||||
|
||||
Assert.Single(result);
|
||||
Assert.IsType<Arc>(result[0]);
|
||||
var arc = (Arc)result[0];
|
||||
Assert.InRange(arc.Radius, 9.99, 10.01);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Convert_StraightLinePoints_ProducesSingleLine()
|
||||
{
|
||||
var points = new System.Collections.Generic.List<Vector>();
|
||||
for (var i = 0; i <= 10; i++)
|
||||
points.Add(new Vector(i, 2 * i + 1));
|
||||
|
||||
var result = SplineConverter.Convert(points, isClosed: false, tolerance: 0.001);
|
||||
|
||||
Assert.All(result, e => Assert.IsType<Line>(e));
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Convert_SCurve_ProducesMultipleArcs()
|
||||
{
|
||||
var points = new System.Collections.Generic.List<Vector>();
|
||||
for (var i = 0; i <= 30; i++)
|
||||
{
|
||||
var t = System.Math.PI * i / 30;
|
||||
points.Add(new Vector(10 * System.Math.Cos(t), 10 * System.Math.Sin(t)));
|
||||
}
|
||||
for (var i = 1; i <= 30; i++)
|
||||
{
|
||||
var t = -System.Math.PI * i / 30;
|
||||
points.Add(new Vector(-20 + 10 * System.Math.Cos(t), 10 * System.Math.Sin(t)));
|
||||
}
|
||||
|
||||
var result = SplineConverter.Convert(points, isClosed: false, tolerance: 0.001);
|
||||
|
||||
var arcCount = result.Count(e => e is Arc);
|
||||
Assert.True(arcCount >= 2, $"Expected at least 2 arcs, got {arcCount}");
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Convert_TwoPoints_ProducesSingleLine()
|
||||
{
|
||||
var points = new System.Collections.Generic.List<Vector>
|
||||
{
|
||||
new Vector(0, 0),
|
||||
new Vector(10, 5)
|
||||
};
|
||||
|
||||
var result = SplineConverter.Convert(points, isClosed: false, tolerance: 0.001);
|
||||
|
||||
Assert.Single(result);
|
||||
Assert.IsType<Line>(result[0]);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Convert_EndpointContinuity_EntitiesConnect()
|
||||
{
|
||||
var points = new System.Collections.Generic.List<Vector>();
|
||||
for (var i = 0; i <= 80; i++)
|
||||
{
|
||||
var t = Angle.TwoPI * i / 80;
|
||||
points.Add(new Vector(15 * System.Math.Cos(t), 8 * System.Math.Sin(t)));
|
||||
}
|
||||
|
||||
var result = SplineConverter.Convert(points, isClosed: false, tolerance: 0.001);
|
||||
|
||||
for (var i = 0; i < result.Count - 1; i++)
|
||||
{
|
||||
var endPt = GetEndPoint(result[i]);
|
||||
var startPt = GetStartPoint(result[i + 1]);
|
||||
var gap = endPt.DistanceTo(startPt);
|
||||
Assert.True(gap < 0.001,
|
||||
$"Gap of {gap:F6} between entity {i} and {i + 1}");
|
||||
}
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Convert_EmptyPoints_ReturnsEmpty()
|
||||
{
|
||||
var result = SplineConverter.Convert(new System.Collections.Generic.List<Vector>(),
|
||||
isClosed: false, tolerance: 0.001);
|
||||
Assert.Empty(result);
|
||||
}
|
||||
|
||||
[Fact]
|
||||
public void Convert_SinglePoint_ReturnsEmpty()
|
||||
{
|
||||
var points = new System.Collections.Generic.List<Vector> { new Vector(5, 5) };
|
||||
var result = SplineConverter.Convert(points, isClosed: false, tolerance: 0.001);
|
||||
Assert.Empty(result);
|
||||
}
|
||||
|
||||
private static Vector GetStartPoint(Entity e)
|
||||
{
|
||||
return e switch
|
||||
{
|
||||
Arc a => a.StartPoint(),
|
||||
Line l => l.StartPoint,
|
||||
_ => throw new System.Exception("Unexpected entity type")
|
||||
};
|
||||
}
|
||||
|
||||
private static Vector GetEndPoint(Entity e)
|
||||
{
|
||||
return e switch
|
||||
{
|
||||
Arc a => a.EndPoint(),
|
||||
Line l => l.EndPoint,
|
||||
_ => throw new System.Exception("Unexpected entity type")
|
||||
};
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user