OpenNest/OpenNest.Engine/BestFit/CpuDistanceComputer.cs

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

namespace OpenNest.Engine.BestFit
{
    public class CpuDistanceComputer : IDistanceComputer
    {
        public double[] ComputeDistances(
            List<Line> stationaryLines,
            List<Line> movingTemplateLines,
            SlideOffset[] offsets
        )
        {
            var count = offsets.Length;
            var results = new double[count];

            var allMovingVerts = ExtractUniqueVertices(movingTemplateLines);
            var allStationaryVerts = ExtractUniqueVertices(stationaryLines);

            var vertexCache =
                new Dictionary<(double, double), (Vector[] leading, Vector[] facing)>();

            foreach (var offset in offsets)
            {
                var key = (offset.DirX, offset.DirY);
                if (vertexCache.ContainsKey(key))
                    continue;

                var leading = FilterVerticesByProjection(
                    allMovingVerts,
                    offset.DirX,
                    offset.DirY,
                    keepHigh: true
                );
                var facing = FilterVerticesByProjection(
                    allStationaryVerts,
                    offset.DirX,
                    offset.DirY,
                    keepHigh: false
                );
                vertexCache[key] = (leading, facing);
            }

            System.Threading.Tasks.Parallel.For(
                0,
                count,
                i =>
                {
                    var offset = offsets[i];
                    var dirX = offset.DirX;
                    var dirY = offset.DirY;
                    var oppX = -dirX;
                    var oppY = -dirY;

                    var (leadingMoving, facingStationary) = vertexCache[(dirX, dirY)];

                    var minDist = double.MaxValue;

                    for (var v = 0; v < leadingMoving.Length; v++)
                    {
                        var vx = leadingMoving[v].X + offset.Dx;
                        var vy = leadingMoving[v].Y + offset.Dy;

                        for (var j = 0; j < stationaryLines.Count; j++)
                        {
                            var e = stationaryLines[j];
                            var d = SpatialQuery.RayEdgeDistance(
                                vx,
                                vy,
                                e.StartPoint.X,
                                e.StartPoint.Y,
                                e.EndPoint.X,
                                e.EndPoint.Y,
                                dirX,
                                dirY
                            );

                            if (d < minDist)
                            {
                                minDist = d;
                                if (d <= 0)
                                {
                                    results[i] = 0;
                                    return;
                                }
                            }
                        }
                    }

                    for (var v = 0; v < facingStationary.Length; v++)
                    {
                        var svx = facingStationary[v].X;
                        var svy = facingStationary[v].Y;

                        for (var j = 0; j < movingTemplateLines.Count; j++)
                        {
                            var e = movingTemplateLines[j];
                            var d = SpatialQuery.RayEdgeDistance(
                                svx,
                                svy,
                                e.StartPoint.X + offset.Dx,
                                e.StartPoint.Y + offset.Dy,
                                e.EndPoint.X + offset.Dx,
                                e.EndPoint.Y + offset.Dy,
                                oppX,
                                oppY
                            );

                            if (d < minDist)
                            {
                                minDist = d;
                                if (d <= 0)
                                {
                                    results[i] = 0;
                                    return;
                                }
                            }
                        }
                    }

                    results[i] = minDist;
                }
            );

            return results;
        }

        public double[] ComputeDistances(
            List<Entity> stationaryEntities,
            List<Entity> movingEntities,
            SlideOffset[] offsets
        )
        {
            var count = offsets.Length;
            var results = new double[count];

            var allMovingVerts = ExtractVerticesFromEntities(movingEntities);
            var allStationaryVerts = ExtractVerticesFromEntities(stationaryEntities);

            var movingCurves = ExtractCurveParams(movingEntities);
            var stationaryCurves = ExtractCurveParams(stationaryEntities);

            var vertexCache =
                new Dictionary<(double, double), (Vector[] leading, Vector[] facing)>();

            foreach (var offset in offsets)
            {
                var key = (offset.DirX, offset.DirY);
                if (vertexCache.ContainsKey(key))
                    continue;

                var leading = FilterVerticesByProjection(
                    allMovingVerts,
                    offset.DirX,
                    offset.DirY,
                    keepHigh: true
                );
                var facing = FilterVerticesByProjection(
                    allStationaryVerts,
                    offset.DirX,
                    offset.DirY,
                    keepHigh: false
                );
                vertexCache[key] = (leading, facing);
            }

            System.Threading.Tasks.Parallel.For(
                0,
                count,
                i =>
                {
                    var offset = offsets[i];
                    var dirX = offset.DirX;
                    var dirY = offset.DirY;
                    var oppX = -dirX;
                    var oppY = -dirY;

                    var (leadingMoving, facingStationary) = vertexCache[(dirX, dirY)];

                    var minDist = double.MaxValue;

                    // Case 1: Leading moving vertices → stationary entities
                    for (var v = 0; v < leadingMoving.Length; v++)
                    {
                        var vx = leadingMoving[v].X + offset.Dx;
                        var vy = leadingMoving[v].Y + offset.Dy;

                        for (var j = 0; j < stationaryEntities.Count; j++)
                        {
                            var d = RayEntityDistance(
                                vx,
                                vy,
                                stationaryEntities[j],
                                0,
                                0,
                                dirX,
                                dirY
                            );

                            if (d < minDist)
                            {
                                minDist = d;
                                if (d <= 0)
                                {
                                    results[i] = 0;
                                    return;
                                }
                            }
                        }
                    }

                    // Case 2: Facing stationary vertices → moving entities (opposite direction)
                    for (var v = 0; v < facingStationary.Length; v++)
                    {
                        var svx = facingStationary[v].X;
                        var svy = facingStationary[v].Y;

                        for (var j = 0; j < movingEntities.Count; j++)
                        {
                            var d = RayEntityDistance(
                                svx,
                                svy,
                                movingEntities[j],
                                offset.Dx,
                                offset.Dy,
                                oppX,
                                oppY
                            );

                            if (d < minDist)
                            {
                                minDist = d;
                                if (d <= 0)
                                {
                                    results[i] = 0;
                                    return;
                                }
                            }
                        }
                    }

                    // Phase 3: Curve-to-curve direct distance.
                    // Vertex sampling misses the true contact between two curved entities
                    // when the approach angle doesn't align with a sampled vertex.
                    for (var m = 0; m < movingCurves.Length; m++)
                    {
                        var mc = movingCurves[m];
                        var mcx = mc.Cx + offset.Dx;
                        var mcy = mc.Cy + offset.Dy;

                        for (var s = 0; s < stationaryCurves.Length; s++)
                        {
                            var sc = stationaryCurves[s];
                            var d = SpatialQuery.RayCircleDistance(
                                mcx,
                                mcy,
                                sc.Cx,
                                sc.Cy,
                                mc.Radius + sc.Radius,
                                dirX,
                                dirY
                            );

                            if (d >= minDist || d == double.MaxValue)
                                continue;

                            if (mc.Entity is Arc || sc.Entity is Arc)
                            {
                                var mx = mcx + d * dirX;
                                var my = mcy + d * dirY;
                                var toCx = sc.Cx - mx;
                                var toCy = sc.Cy - my;

                                if (mc.Entity is Arc mArc)
                                {
                                    var angle = Angle.NormalizeRad(System.Math.Atan2(toCy, toCx));
                                    if (
                                        !Angle.IsBetweenRad(
                                            angle,
                                            mArc.StartAngle,
                                            mArc.EndAngle,
                                            mArc.IsReversed
                                        )
                                    )
                                        continue;
                                }

                                if (sc.Entity is Arc sArc)
                                {
                                    var angle = Angle.NormalizeRad(System.Math.Atan2(-toCy, -toCx));
                                    if (
                                        !Angle.IsBetweenRad(
                                            angle,
                                            sArc.StartAngle,
                                            sArc.EndAngle,
                                            sArc.IsReversed
                                        )
                                    )
                                        continue;
                                }
                            }

                            minDist = d;
                            if (d <= 0)
                            {
                                results[i] = 0;
                                return;
                            }
                        }
                    }

                    results[i] = minDist;
                }
            );

            return results;
        }

        private readonly struct CurveParams
        {
            public readonly Entity Entity;
            public readonly double Cx,
                Cy,
                Radius;

            public CurveParams(Entity entity, double cx, double cy, double radius)
            {
                Entity = entity;
                Cx = cx;
                Cy = cy;
                Radius = radius;
            }
        }

        private static CurveParams[] ExtractCurveParams(List<Entity> entities)
        {
            var curves = new List<CurveParams>();
            for (var i = 0; i < entities.Count; i++)
            {
                if (entities[i] is Circle circle)
                    curves.Add(
                        new CurveParams(circle, circle.Center.X, circle.Center.Y, circle.Radius)
                    );
                else if (entities[i] is Arc arc)
                    curves.Add(new CurveParams(arc, arc.Center.X, arc.Center.Y, arc.Radius));
            }
            return curves.ToArray();
        }

        private static double RayEntityDistance(
            double vx,
            double vy,
            Entity entity,
            double entityOffsetX,
            double entityOffsetY,
            double dirX,
            double dirY
        )
        {
            if (entity is Line line)
            {
                return SpatialQuery.RayEdgeDistance(
                    vx,
                    vy,
                    line.StartPoint.X + entityOffsetX,
                    line.StartPoint.Y + entityOffsetY,
                    line.EndPoint.X + entityOffsetX,
                    line.EndPoint.Y + entityOffsetY,
                    dirX,
                    dirY
                );
            }

            if (entity is Arc arc)
            {
                return SpatialQuery.RayArcDistance(
                    vx,
                    vy,
                    arc.Center.X + entityOffsetX,
                    arc.Center.Y + entityOffsetY,
                    arc.Radius,
                    arc.StartAngle,
                    arc.EndAngle,
                    arc.IsReversed,
                    dirX,
                    dirY
                );
            }

            if (entity is Circle circle)
            {
                return SpatialQuery.RayCircleDistance(
                    vx,
                    vy,
                    circle.Center.X + entityOffsetX,
                    circle.Center.Y + entityOffsetY,
                    circle.Radius,
                    dirX,
                    dirY
                );
            }

            return double.MaxValue;
        }

        private static Vector[] ExtractVerticesFromEntities(List<Entity> entities)
        {
            var vertices = new HashSet<Vector>();

            for (var i = 0; i < entities.Count; i++)
            {
                var entity = entities[i];

                if (entity is Line line)
                {
                    vertices.Add(line.StartPoint);
                    vertices.Add(line.EndPoint);
                }
                else if (entity is Arc arc)
                {
                    vertices.Add(arc.StartPoint());
                    vertices.Add(arc.EndPoint());
                    AddArcExtremes(vertices, arc);
                }
                else if (entity is Circle circle)
                {
                    // Four cardinal points
                    vertices.Add(new Vector(circle.Center.X + circle.Radius, circle.Center.Y));
                    vertices.Add(new Vector(circle.Center.X - circle.Radius, circle.Center.Y));
                    vertices.Add(new Vector(circle.Center.X, circle.Center.Y + circle.Radius));
                    vertices.Add(new Vector(circle.Center.X, circle.Center.Y - circle.Radius));
                }
            }

            return vertices.ToArray();
        }

        private static void AddArcExtremes(HashSet<Vector> points, Arc arc)
        {
            var a1 = arc.StartAngle;
            var a2 = arc.EndAngle;
            var reversed = arc.IsReversed;

            if (reversed)
                Generic.Swap(ref a1, ref a2);

            // Right (0°)
            if (Angle.IsBetweenRad(Angle.TwoPI, a1, a2))
                points.Add(new Vector(arc.Center.X + arc.Radius, arc.Center.Y));

            // Top (90°)
            if (Angle.IsBetweenRad(Angle.HalfPI, a1, a2))
                points.Add(new Vector(arc.Center.X, arc.Center.Y + arc.Radius));

            // Left (180°)
            if (Angle.IsBetweenRad(System.Math.PI, a1, a2))
                points.Add(new Vector(arc.Center.X - arc.Radius, arc.Center.Y));

            // Bottom (270°)
            if (Angle.IsBetweenRad(System.Math.PI * 1.5, a1, a2))
                points.Add(new Vector(arc.Center.X, arc.Center.Y - arc.Radius));
        }

        private static Vector[] ExtractUniqueVertices(List<Line> lines)
        {
            var vertices = new HashSet<Vector>();
            for (var i = 0; i < lines.Count; i++)
            {
                vertices.Add(lines[i].StartPoint);
                vertices.Add(lines[i].EndPoint);
            }
            return vertices.ToArray();
        }

        private static Vector[] FilterVerticesByProjection(
            Vector[] vertices,
            double dirX,
            double dirY,
            bool keepHigh
        )
        {
            if (vertices.Length == 0)
                return vertices;

            var projections = new double[vertices.Length];
            var min = double.MaxValue;
            var max = double.MinValue;

            for (var i = 0; i < vertices.Length; i++)
            {
                projections[i] = vertices[i].X * dirX + vertices[i].Y * dirY;
                if (projections[i] < min)
                    min = projections[i];
                if (projections[i] > max)
                    max = projections[i];
            }

            var midpoint = (min + max) / 2;
            var count = 0;

            for (var i = 0; i < vertices.Length; i++)
            {
                if (keepHigh ? projections[i] >= midpoint : projections[i] <= midpoint)
                    count++;
            }

            var result = new Vector[count];
            var idx = 0;

            for (var i = 0; i < vertices.Length; i++)
            {
                if (keepHigh ? projections[i] >= midpoint : projections[i] <= midpoint)
                    result[idx++] = vertices[i];
            }

            return result;
        }
    }
}