OpenNest/OpenNest.Core/Geometry/EllipseConverter.cs

using System;
using System.Collections.Generic;
using OpenNest.Math;

namespace OpenNest.Geometry
{
    public static class EllipseConverter
    {
        private const int MaxSubdivisionDepth = 12;
        private const int DeviationSamples = 20;

        internal static Vector EvaluatePoint(
            double semiMajor,
            double semiMinor,
            double rotation,
            Vector center,
            double t
        )
        {
            var x = semiMajor * System.Math.Cos(t);
            var y = semiMinor * System.Math.Sin(t);

            var cos = System.Math.Cos(rotation);
            var sin = System.Math.Sin(rotation);

            return new Vector(center.X + x * cos - y * sin, center.Y + x * sin + y * cos);
        }

        internal static Vector EvaluateTangent(
            double semiMajor,
            double semiMinor,
            double rotation,
            double t
        )
        {
            var tx = -semiMajor * System.Math.Sin(t);
            var ty = semiMinor * System.Math.Cos(t);

            var cos = System.Math.Cos(rotation);
            var sin = System.Math.Sin(rotation);

            return new Vector(tx * cos - ty * sin, tx * sin + ty * cos);
        }

        internal static Vector EvaluateNormal(
            double semiMajor,
            double semiMinor,
            double rotation,
            double t
        )
        {
            // Inward normal: perpendicular to tangent, pointing toward center of curvature.
            // In local coords: N(t) = (-b*cos(t), -a*sin(t))
            var nx = -semiMinor * System.Math.Cos(t);
            var ny = -semiMajor * System.Math.Sin(t);

            var cos = System.Math.Cos(rotation);
            var sin = System.Math.Sin(rotation);

            return new Vector(nx * cos - ny * sin, nx * sin + ny * cos);
        }

        internal static Vector IntersectNormals(Vector p1, Vector n1, Vector p2, Vector n2)
        {
            // Solve: p1 + s*n1 = p2 + t*n2
            var det = n1.X * (-n2.Y) - (-n2.X) * n1.Y;
            if (System.Math.Abs(det) < 1e-10)
                return Vector.Invalid;

            var dx = p2.X - p1.X;
            var dy = p2.Y - p1.Y;
            var s = (dx * (-n2.Y) - dy * (-n2.X)) / det;

            return new Vector(p1.X + s * n1.X, p1.Y + s * n1.Y);
        }

        internal static Vector Circumcenter(Vector a, Vector b, Vector c)
        {
            var ax = a.X - c.X;
            var ay = a.Y - c.Y;
            var bx = b.X - c.X;
            var by = b.Y - c.Y;
            var D = 2.0 * (ax * by - ay * bx);

            if (System.Math.Abs(D) < 1e-10)
                return Vector.Invalid;

            var a2 = ax * ax + ay * ay;
            var b2 = bx * bx + by * by;
            var ux = (by * a2 - ay * b2) / D;
            var uy = (ax * b2 - bx * a2) / D;

            return new Vector(ux + c.X, uy + c.Y);
        }

        public static List<Entity> Convert(
            Vector center,
            double semiMajor,
            double semiMinor,
            double rotation,
            double startParam,
            double endParam,
            double tolerance = 0.001
        )
        {
            if (tolerance <= 0)
                throw new ArgumentOutOfRangeException(
                    nameof(tolerance),
                    "Tolerance must be positive."
                );
            if (semiMajor <= 0 || semiMinor <= 0)
                throw new ArgumentOutOfRangeException("Semi-axis lengths must be positive.");

            if (endParam <= startParam)
                endParam += Angle.TwoPI;

            // True circle — emit a single arc (or two for full circle)
            if (System.Math.Abs(semiMajor - semiMinor) < Tolerance.Epsilon)
                return ConvertCircle(center, semiMajor, rotation, startParam, endParam);

            var splits = GetInitialSplits(startParam, endParam);

            var entities = new List<Entity>();
            for (var i = 0; i < splits.Count - 1; i++)
                FitSegment(
                    center,
                    semiMajor,
                    semiMinor,
                    rotation,
                    splits[i],
                    splits[i + 1],
                    tolerance,
                    entities,
                    0
                );

            return entities;
        }

        private static List<Entity> ConvertCircle(
            Vector center,
            double radius,
            double rotation,
            double startParam,
            double endParam
        )
        {
            var sweep = endParam - startParam;
            var isFull = System.Math.Abs(sweep - Angle.TwoPI) < 0.01;

            if (isFull)
            {
                var startAngle1 = Angle.NormalizeRad(startParam + rotation);
                var midAngle = Angle.NormalizeRad(startParam + System.Math.PI + rotation);
                var endAngle2 = startAngle1;

                return new List<Entity>
                {
                    new Arc(center, radius, startAngle1, midAngle, false),
                    new Arc(center, radius, midAngle, endAngle2, false),
                };
            }

            var sa = Angle.NormalizeRad(startParam + rotation);
            var ea = Angle.NormalizeRad(endParam + rotation);
            return new List<Entity> { new Arc(center, radius, sa, ea, false) };
        }

        private static List<double> GetInitialSplits(double startParam, double endParam)
        {
            var splits = new List<double> { startParam };

            var firstQuadrant =
                System.Math.Ceiling(startParam / (System.Math.PI / 2)) * (System.Math.PI / 2);
            for (var q = firstQuadrant; q < endParam; q += System.Math.PI / 2)
            {
                if (q > startParam + 1e-10 && q < endParam - 1e-10)
                    splits.Add(q);
            }

            splits.Add(endParam);
            return splits;
        }

        private static void FitSegment(
            Vector center,
            double semiMajor,
            double semiMinor,
            double rotation,
            double t0,
            double t1,
            double tolerance,
            List<Entity> results,
            int depth
        )
        {
            var p0 = EvaluatePoint(semiMajor, semiMinor, rotation, center, t0);
            var p1 = EvaluatePoint(semiMajor, semiMinor, rotation, center, t1);

            if (p0.DistanceTo(p1) < 1e-10)
                return;

            var n0 = EvaluateNormal(semiMajor, semiMinor, rotation, t0);
            var n1 = EvaluateNormal(semiMajor, semiMinor, rotation, t1);

            var arcCenter = IntersectNormals(p0, n0, p1, n1);

            if (!arcCenter.IsValid() || depth >= MaxSubdivisionDepth)
            {
                results.Add(new Line(p0, p1));
                return;
            }

            var radius = p0.DistanceTo(arcCenter);
            var maxDev = MeasureDeviation(
                center,
                semiMajor,
                semiMinor,
                rotation,
                t0,
                t1,
                arcCenter,
                radius
            );

            if (maxDev <= tolerance)
            {
                var arc = CreateArc(
                    arcCenter,
                    radius,
                    center,
                    semiMajor,
                    semiMinor,
                    rotation,
                    t0,
                    t1
                );
                if (arc.SweepAngle() < Tolerance.Epsilon)
                    results.Add(new Line(p0, p1));
                else
                    results.Add(arc);
            }
            else
            {
                var tMid = (t0 + t1) / 2.0;
                FitSegment(
                    center,
                    semiMajor,
                    semiMinor,
                    rotation,
                    t0,
                    tMid,
                    tolerance,
                    results,
                    depth + 1
                );
                FitSegment(
                    center,
                    semiMajor,
                    semiMinor,
                    rotation,
                    tMid,
                    t1,
                    tolerance,
                    results,
                    depth + 1
                );
            }
        }

        private static double MeasureDeviation(
            Vector center,
            double semiMajor,
            double semiMinor,
            double rotation,
            double t0,
            double t1,
            Vector arcCenter,
            double radius
        )
        {
            var maxDev = 0.0;
            for (var i = 1; i <= DeviationSamples; i++)
            {
                var t = t0 + (t1 - t0) * i / DeviationSamples;
                var p = EvaluatePoint(semiMajor, semiMinor, rotation, center, t);
                var dist = p.DistanceTo(arcCenter);
                var dev = System.Math.Abs(dist - radius);
                if (dev > maxDev)
                    maxDev = dev;
            }
            return maxDev;
        }

        private static Arc CreateArc(
            Vector arcCenter,
            double radius,
            Vector ellipseCenter,
            double semiMajor,
            double semiMinor,
            double rotation,
            double t0,
            double t1
        )
        {
            var p0 = EvaluatePoint(semiMajor, semiMinor, rotation, ellipseCenter, t0);
            var p1 = EvaluatePoint(semiMajor, semiMinor, rotation, ellipseCenter, t1);
            var pMid = EvaluatePoint(semiMajor, semiMinor, rotation, ellipseCenter, (t0 + t1) / 2);

            // Use circumcircle of (p0, pMid, p1) so the arc passes through both
            // endpoints exactly, eliminating gaps between adjacent arcs.
            var cc = Circumcenter(p0, pMid, p1);
            if (cc.IsValid())
            {
                arcCenter = cc;
                radius = p0.DistanceTo(cc);
            }

            var startAngle = System.Math.Atan2(p0.Y - arcCenter.Y, p0.X - arcCenter.X);
            var endAngle = System.Math.Atan2(p1.Y - arcCenter.Y, p1.X - arcCenter.X);

            var points = new List<Vector> { p0, pMid, p1 };
            var isReversed = SumSignedAngles(arcCenter, points) < 0;

            if (startAngle < 0)
                startAngle += Angle.TwoPI;
            if (endAngle < 0)
                endAngle += Angle.TwoPI;

            return new Arc(arcCenter, radius, startAngle, endAngle, isReversed);
        }

        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;
        }
    }
}