zengzy / hhrcs_service · Files

using System;
using System.Collections.Generic;
using System.Linq;
using NetTopologySuite.Geometries;
using NetTopologySuite.IO;
using Rcs.Application.Services;
using Rcs.Application.Services.PathFind.Models;
using Rcs.Domain.Enums;

namespace Rcs.Infrastructure.PathFinding.Services;

/// <summary>
/// 扫掠面覆盖资源展开器：将轨迹中心线和外扩半径映射为覆盖的节点/边编码集合。
/// </summary>
public static class SweptAreaCoverageResolver
{
    public const double DefaultWidth = 0.6d;
    public const double DefaultLength = 1.0d;
    public const double DefaultSafetyDistance = 0.1d;

    private const double Epsilon = 1e-6d;
    private const double CurveSamplingStep = 0.2d;
    private const int MinCurveSamples = 8;
    private const int MaxCurveSamples = 128;
    private const double RotationThresholdRadians = Math.PI / 180d;
    private const double RotationSamplingStepRadians = Math.PI / 36d;

    private static readonly GeometryFactory GeometryFactory = new(new PrecisionModel(), 0);

    public static RobotSweepDimensions ResolveDimensions(
        double? width,
        double? length,
        double? safetyDistance,
        double coordinateScale)
    {
        var scale = coordinateScale > 0 ? coordinateScale : 1d;

        var useDefaultWidth = !width.HasValue || width.Value <= 0;
        var useDefaultLength = !length.HasValue || length.Value <= 0;
        var useDefaultSafety = !safetyDistance.HasValue || safetyDistance.Value < 0;

        var resolvedWidth = useDefaultWidth ? DefaultWidth * scale : width!.Value;
        var resolvedLength = useDefaultLength ? DefaultLength * scale : length!.Value;
        var resolvedSafety = useDefaultSafety ? DefaultSafetyDistance * scale : safetyDistance!.Value;

        return new RobotSweepDimensions(
            Width: resolvedWidth,
            Length: resolvedLength,
            SafetyDistance: resolvedSafety,
            UseDefaultWidth: useDefaultWidth,
            UseDefaultLength: useDefaultLength,
            UseDefaultSafetyDistance: useDefaultSafety);
    }

    public static double CalculateSweepRadius(double length, double width, double safetyDistance)
    {
        var halfLength = Math.Max(length, 0d) / 2d + Math.Max(safetyDistance, 0d);
        var halfWidth = Math.Max(width, 0d) / 2d + Math.Max(safetyDistance, 0d);
        return Math.Sqrt(halfLength * halfLength + halfWidth * halfWidth);
    }

    public static Dictionary<string, SweepPoint> BuildNodeLookup(MapCacheData mapData)
    {
        return mapData.Nodes
            .Where(n => n.Active && !string.IsNullOrWhiteSpace(n.NodeCode))
            .GroupBy(n => n.NodeCode, StringComparer.OrdinalIgnoreCase)
            .ToDictionary(
                g => g.Key,
                g =>
                {
                    var node = g.First();
                    return new SweepPoint(node.X, node.Y);
                },
                StringComparer.OrdinalIgnoreCase);
    }

    public static Dictionary<string, SweepPoint> BuildNodeLookup(PathGraph graph)
    {
        return graph.Nodes.Values
            .Where(n => n.Active && !string.IsNullOrWhiteSpace(n.NodeCode))
            .GroupBy(n => n.NodeCode, StringComparer.OrdinalIgnoreCase)
            .ToDictionary(
                g => g.Key,
                g =>
                {
                    var node = g.First();
                    return new SweepPoint(node.X, node.Y);
                },
                StringComparer.OrdinalIgnoreCase);
    }

    public static Dictionary<string, List<SweepPoint>> BuildEdgePolylineLookup(PathGraph graph)
    {
        var result = new Dictionary<string, List<SweepPoint>>(StringComparer.OrdinalIgnoreCase);
        foreach (var edge in graph.Edges.Values.Where(e => e.Active && !string.IsNullOrWhiteSpace(e.EdgeCode)))
        {
            if (!graph.Nodes.TryGetValue(edge.FromNodeId, out var fromNode) ||
                !graph.Nodes.TryGetValue(edge.ToNodeId, out var toNode))
            {
                continue;
            }

            var fromPoint = new SweepPoint(fromNode.X, fromNode.Y);
            var toPoint = new SweepPoint(toNode.X, toNode.Y);
            result[edge.EdgeCode] = BuildEdgePolyline(
                edge.CurveType,
                edge.Radius,
                edge.CenterX,
                edge.CenterY,
                edge.ControlPoints,
                edge.Degree,
                edge.Weights,
                edge.Knots,
                fromPoint,
                toPoint);
        }

        return result;
    }

    public static List<SweepPoint> BuildPolylineFromSegments(
        IEnumerable<PathSegmentWithCode> segments,
        IReadOnlyDictionary<string, SweepPoint> nodeLookupByCode,
        IReadOnlyDictionary<string, List<SweepPoint>>? edgePolylineLookupByCode = null)
    {
        var result = new List<SweepPoint>();

        foreach (var segment in segments)
        {
            if (string.IsNullOrWhiteSpace(segment.FromNodeCode) || string.IsNullOrWhiteSpace(segment.ToNodeCode))
            {
                continue;
            }

            if (!nodeLookupByCode.TryGetValue(segment.FromNodeCode, out var fromPoint) ||
                !nodeLookupByCode.TryGetValue(segment.ToNodeCode, out var toPoint))
            {
                continue;
            }

            List<SweepPoint> segmentPolyline;
            if (edgePolylineLookupByCode != null &&
                !string.IsNullOrWhiteSpace(segment.EdgeCode) &&
                edgePolylineLookupByCode.TryGetValue(segment.EdgeCode, out var edgePolyline) &&
                edgePolyline.Count > 0)
            {
                segmentPolyline = EnsureEdgePolylineEndpoints(edgePolyline, fromPoint, toPoint);
            }
            else
            {
                segmentPolyline = new List<SweepPoint> { fromPoint, toPoint };
            }

            AppendPolyline(result, segmentPolyline);
        }

        return result;
    }

    public static SweptAreaCoverageResult ExpandFromMap(
        MapCacheData mapData,
        IReadOnlyList<SweepPoint> centerLine,
        double radius)
    {
        var result = new SweptAreaCoverageResult();
        if (mapData.Nodes.Count == 0 || centerLine.Count == 0 || radius < 0)
        {
            return result;
        }

        var nodeById = mapData.Nodes
            .Where(n => n.Active)
            .ToDictionary(n => n.NodeId);

        foreach (var node in nodeById.Values)
        {
            if (string.IsNullOrWhiteSpace(node.NodeCode))
            {
                continue;
            }

            if (DistancePointToPolyline(node.X, node.Y, centerLine) <= radius + Epsilon)
            {
                result.NodeCodes.Add(node.NodeCode);
            }
        }

        foreach (var edge in mapData.Edges.Where(e => e.Active && !string.IsNullOrWhiteSpace(e.EdgeCode)))
        {
            if (!nodeById.TryGetValue(edge.FromNode, out var fromNode) ||
                !nodeById.TryGetValue(edge.ToNode, out var toNode))
            {
                continue;
            }

            var edgePolyline = BuildEdgePolyline(
                edge.CurveType,
                edge.Radius,
                edge.CenterX,
                edge.CenterY,
                edge.ControlPoints,
                edge.Degree,
                edge.Weights,
                edge.Knots,
                new SweepPoint(fromNode.X, fromNode.Y),
                new SweepPoint(toNode.X, toNode.Y));

            var distance = DistancePolylineToPolyline(edgePolyline, centerLine);
            if (distance <= radius + Epsilon)
            {
                result.EdgeCodes.Add(edge.EdgeCode);
            }
        }

        return result;
    }

    public static SweptAreaCoverageResult ExpandFromGraph(
        PathGraph graph,
        IReadOnlyList<SweepPoint> centerLine,
        double radius)
    {
        var result = new SweptAreaCoverageResult();
        if (graph.Nodes.Count == 0 || centerLine.Count == 0 || radius < 0)
        {
            return result;
        }

        foreach (var node in graph.Nodes.Values.Where(n => n.Active && !string.IsNullOrWhiteSpace(n.NodeCode)))
        {
            if (DistancePointToPolyline(node.X, node.Y, centerLine) <= radius + Epsilon)
            {
                result.NodeCodes.Add(node.NodeCode);
            }
        }

        foreach (var edge in graph.Edges.Values.Where(e => e.Active && !string.IsNullOrWhiteSpace(e.EdgeCode)))
        {
            if (!graph.Nodes.TryGetValue(edge.FromNodeId, out var fromNode) ||
                !graph.Nodes.TryGetValue(edge.ToNodeId, out var toNode))
            {
                continue;
            }

            var edgePolyline = BuildEdgePolyline(
                edge.CurveType,
                edge.Radius,
                edge.CenterX,
                edge.CenterY,
                edge.ControlPoints,
                edge.Degree,
                edge.Weights,
                edge.Knots,
                new SweepPoint(fromNode.X, fromNode.Y),
                new SweepPoint(toNode.X, toNode.Y));

            var distance = DistancePolylineToPolyline(edgePolyline, centerLine);
            if (distance <= radius + Epsilon)
            {
                result.EdgeCodes.Add(edge.EdgeCode);
            }
        }

        return result;
    }

    /// <summary>
    /// 从实际规划段构建矩形车体扫掠几何。
    /// </summary>
    public static Geometry BuildSweptGeometryFromSegments(
        IReadOnlyList<PathSegmentWithCode> segments,
        PathGraph graph,
        RobotSweepDimensions dimensions)
    {
        if (segments.Count == 0 || graph.Nodes.Count == 0)
        {
            return GeometryFactory.CreateGeometryCollection(Array.Empty<Geometry>());
        }

        var nodeLookup = BuildNodeLookup(graph);
        var edgePolylineLookup = BuildEdgePolylineLookup(graph);
        var geometries = new List<Geometry>();

        for (var i = 0; i < segments.Count; i++)
        {
            var segment = segments[i];
            if (!nodeLookup.TryGetValue(segment.FromNodeCode, out var fromPoint) ||
                !nodeLookup.TryGetValue(segment.ToNodeCode, out var toPoint))
            {
                continue;
            }

            var polyline = ResolveSegmentPolyline(segment, fromPoint, toPoint, edgePolylineLookup);
            var travelGeometry = BuildTravelSweptGeometry(
                polyline,
                segment.StartTheta,
                segment.EndTheta,
                dimensions);
            if (!travelGeometry.IsEmpty)
            {
                geometries.Add(travelGeometry);
            }

            var segmentRotation = BuildRotationSweptGeometry(
                toPoint,
                segment.StartTheta,
                segment.EndTheta,
                dimensions);
            if (!segmentRotation.IsEmpty)
            {
                geometries.Add(segmentRotation);
            }

            if (i + 1 < segments.Count &&
                nodeLookup.TryGetValue(segments[i + 1].FromNodeCode, out var nextStartPoint))
            {
                var nodePoint = ArePointsEqual(toPoint, nextStartPoint) ? toPoint : nextStartPoint;
                var connectionRotation = BuildRotationSweptGeometry(
                    nodePoint,
                    segment.EndTheta,
                    segments[i + 1].StartTheta,
                    dimensions);
                if (!connectionRotation.IsEmpty)
                {
                    geometries.Add(connectionRotation);
                }
            }
        }

        return UnionGeometries(geometries);
    }

    /// <summary>
    /// 构建当前位置实际车体扫掠几何，可合并同地图上一帧到当前帧之间的短距离行驶扫掠。
    /// </summary>
    public static Geometry BuildLiveActualGeometry(
        SweepPoint currentCenter,
        double currentTheta,
        RobotSweepDimensions dimensions,
        SweepPoint? previousCenter = null,
        double? previousTheta = null)
    {
        if (previousCenter.HasValue && !ArePointsEqual(previousCenter.Value, currentCenter))
        {
            return BuildTravelSweptGeometry(
                new List<SweepPoint> { previousCenter.Value, currentCenter },
                previousTheta ?? currentTheta,
                currentTheta,
                dimensions);
        }

        return BuildFootprintGeometry(currentCenter, currentTheta, dimensions);
    }

    /// <summary>
    /// 按车体中心点和姿态角生成矩形 footprint。
    /// </summary>
    public static Geometry BuildFootprintGeometry(
        SweepPoint center,
        double theta,
        RobotSweepDimensions dimensions)
    {
        var effectiveLength = Math.Max(0d, dimensions.Length + dimensions.SafetyDistance);
        var effectiveWidth = Math.Max(0d, dimensions.Width + dimensions.SafetyDistance);
        if (effectiveLength <= Epsilon || effectiveWidth <= Epsilon)
        {
            return GeometryFactory.CreatePoint(new Coordinate(center.X, center.Y));
        }

        var halfLength = effectiveLength / 2d;
        var halfWidth = effectiveWidth / 2d;
        var cos = Math.Cos(theta);
        var sin = Math.Sin(theta);

        var localCorners = new[]
        {
            (X: halfLength, Y: halfWidth),
            (X: halfLength, Y: -halfWidth),
            (X: -halfLength, Y: -halfWidth),
            (X: -halfLength, Y: halfWidth)
        };

        var coordinates = localCorners
            .Select(point => new Coordinate(
                center.X + point.X * cos - point.Y * sin,
                center.Y + point.X * sin + point.Y * cos))
            .ToList();
        coordinates.Add(coordinates[0].Copy());

        return GeometryFactory.CreatePolygon(coordinates.ToArray());
    }

    /// <summary>
    /// 沿原子路径采样，按每个采样点的车体姿态生成矩形并做 Union。
    /// </summary>
    public static Geometry BuildTravelSweptGeometry(
        IReadOnlyList<SweepPoint> centerLine,
        double startTheta,
        double endTheta,
        RobotSweepDimensions dimensions)
    {
        if (centerLine.Count == 0)
        {
            return GeometryFactory.CreateGeometryCollection(Array.Empty<Geometry>());
        }

        if (centerLine.Count == 1)
        {
            return BuildFootprintGeometry(centerLine[0], startTheta, dimensions);
        }

        var samples = SamplePolyline(centerLine, CurveSamplingStep);
        if (samples.Count == 0)
        {
            return GeometryFactory.CreateGeometryCollection(Array.Empty<Geometry>());
        }

        var thetaDelta = NormalizeAngleDiff(endTheta - startTheta);
        var geometries = new List<Geometry>(samples.Count);
        foreach (var sample in samples)
        {
            var theta = NormalizeAngle(startTheta + thetaDelta * sample.T);
            geometries.Add(BuildFootprintGeometry(sample.Point, theta, dimensions));
        }

        return UnionGeometries(geometries);
    }

    /// <summary>
    /// 原地旋转扫掠区，按角度步长采样多个矩形并做 Union。
    /// </summary>
    public static Geometry BuildRotationSweptGeometry(
        SweepPoint center,
        double fromTheta,
        double toTheta,
        RobotSweepDimensions dimensions)
    {
        var delta = NormalizeAngleDiff(toTheta - fromTheta);
        if (Math.Abs(delta) <= RotationThresholdRadians)
        {
            return GeometryFactory.CreateGeometryCollection(Array.Empty<Geometry>());
        }

        var sampleCount = Math.Max(1, (int)Math.Ceiling(Math.Abs(delta) / RotationSamplingStepRadians));
        var geometries = new List<Geometry>(sampleCount + 1);
        for (var i = 0; i <= sampleCount; i++)
        {
            var t = i / (double)sampleCount;
            geometries.Add(BuildFootprintGeometry(center, NormalizeAngle(fromTheta + delta * t), dimensions));
        }

        return UnionGeometries(geometries);
    }

    /// <summary>
    /// 用几何结果反查覆盖的 node/edge，仅用于粗筛、可视化和日志。
    /// </summary>
    public static SweptAreaCoverageResult ExpandCoverageFromGeometry(PathGraph graph, Geometry geometry)
    {
        var result = new SweptAreaCoverageResult();
        if (geometry.IsEmpty)
        {
            return result;
        }

        foreach (var node in graph.Nodes.Values.Where(n => n.Active && !string.IsNullOrWhiteSpace(n.NodeCode)))
        {
            var point = GeometryFactory.CreatePoint(new Coordinate(node.X, node.Y));
            if (geometry.Covers(point))
            {
                result.NodeCodes.Add(node.NodeCode);
            }
        }

        foreach (var edge in graph.Edges.Values.Where(e => e.Active && !string.IsNullOrWhiteSpace(e.EdgeCode)))
        {
            if (!graph.Nodes.TryGetValue(edge.FromNodeId, out var fromNode) ||
                !graph.Nodes.TryGetValue(edge.ToNodeId, out var toNode))
            {
                continue;
            }

            var edgePolyline = BuildEdgePolyline(
                edge.CurveType,
                edge.Radius,
                edge.CenterX,
                edge.CenterY,
                edge.ControlPoints,
                edge.Degree,
                edge.Weights,
                edge.Knots,
                new SweepPoint(fromNode.X, fromNode.Y),
                new SweepPoint(toNode.X, toNode.Y));
            var line = GeometryFactory.CreateLineString(edgePolyline.Select(p => new Coordinate(p.X, p.Y)).ToArray());
            if (geometry.Intersects(line))
            {
                result.EdgeCodes.Add(edge.EdgeCode);
            }
        }

        return result;
    }

    public static SweptAreaCoverageResult ExpandCoverageFromGeometry(MapCacheData mapData, Geometry geometry)
    {
        var result = new SweptAreaCoverageResult();
        if (geometry.IsEmpty)
        {
            return result;
        }

        var nodeById = mapData.Nodes
            .Where(n => n.Active)
            .ToDictionary(n => n.NodeId);

        foreach (var node in nodeById.Values.Where(n => !string.IsNullOrWhiteSpace(n.NodeCode)))
        {
            var point = GeometryFactory.CreatePoint(new Coordinate(node.X, node.Y));
            if (geometry.Covers(point))
            {
                result.NodeCodes.Add(node.NodeCode);
            }
        }

        foreach (var edge in mapData.Edges.Where(e => e.Active && !string.IsNullOrWhiteSpace(e.EdgeCode)))
        {
            if (!nodeById.TryGetValue(edge.FromNode, out var fromNode) ||
                !nodeById.TryGetValue(edge.ToNode, out var toNode))
            {
                continue;
            }

            var edgePolyline = BuildEdgePolyline(
                edge.CurveType,
                edge.Radius,
                edge.CenterX,
                edge.CenterY,
                edge.ControlPoints,
                edge.Degree,
                edge.Weights,
                edge.Knots,
                new SweepPoint(fromNode.X, fromNode.Y),
                new SweepPoint(toNode.X, toNode.Y));
            var line = GeometryFactory.CreateLineString(edgePolyline.Select(p => new Coordinate(p.X, p.Y)).ToArray());
            if (geometry.Intersects(line))
            {
                result.EdgeCodes.Add(edge.EdgeCode);
            }
        }

        return result;
    }

    public static SweptGeometryDescriptor ToDescriptor(
        Geometry geometry,
        SweptAreaCoverageResult coverage,
        IEnumerable<VdaLockCoveredEdge> coveredEdges,
        SweptGeometryKind kind)
    {
        var envelope = geometry.EnvelopeInternal;
        return new SweptGeometryDescriptor
        {
            GeometryWkbBase64 = Convert.ToBase64String(new WKBWriter().Write(geometry)),
            MinX = envelope.MinX,
            MinY = envelope.MinY,
            MaxX = envelope.MaxX,
            MaxY = envelope.MaxY,
            CoveredNodeCodes = coverage.NodeCodes
                .Where(code => !string.IsNullOrWhiteSpace(code))
                .OrderBy(code => code, StringComparer.OrdinalIgnoreCase)
                .ToList(),
            CoveredEdges = coveredEdges
                .Where(edge => !string.IsNullOrWhiteSpace(edge.EdgeCode))
                .GroupBy(edge => edge.EdgeCode, StringComparer.OrdinalIgnoreCase)
                .Select(group => group.First())
                .OrderBy(edge => edge.EdgeCode, StringComparer.OrdinalIgnoreCase)
                .ToList(),
            Kind = kind
        };
    }

    public static Geometry ReadGeometryFromWkbBase64(string wkbBase64)
    {
        if (string.IsNullOrWhiteSpace(wkbBase64))
        {
            return GeometryFactory.CreateGeometryCollection(Array.Empty<Geometry>());
        }

        return new WKBReader().Read(Convert.FromBase64String(wkbBase64));
    }

    /// <summary>
    /// 判断两组扫掠区域在连续几何意义上是否相交。
    /// </summary>
    public static bool ZonesIntersect(
        IReadOnlyList<SweptZone> leftZones,
        IReadOnlyList<SweptZone> rightZones)
    {
        if (leftZones.Count == 0 || rightZones.Count == 0)
        {
            return false;
        }

        foreach (var left in leftZones)
        {
            if (left.CenterLine.Count == 0)
            {
                continue;
            }

            foreach (var right in rightZones)
            {
                if (right.CenterLine.Count == 0)
                {
                    continue;
                }

                var distance = DistancePolylineToPolyline(left.CenterLine, right.CenterLine);
                if (distance <= left.Radius + right.Radius + Epsilon)
                {
                    return true;
                }
            }
        }

        return false;
    }

    private static List<SweepPoint> BuildEdgePolyline(
        MapEdgeCurveType curveType,
        double? radius,
        double? centerX,
        double? centerY,
        IReadOnlyList<PointCache>? controlPoints,
        int? degree,
        IReadOnlyList<double>? weights,
        IReadOnlyList<double>? knots,
        SweepPoint fromPoint,
        SweepPoint toPoint)
    {
        if (curveType == MapEdgeCurveType.Straight)
        {
            return new List<SweepPoint> { fromPoint, toPoint };
        }

        if (curveType == MapEdgeCurveType.Arc &&
            TrySampleArc(fromPoint, toPoint, radius, centerX, centerY, out var arcPoints))
        {
            return arcPoints;
        }

        if (controlPoints == null || controlPoints.Count < 2)
        {
            return new List<SweepPoint> { fromPoint, toPoint };
        }

        if (TrySampleNurbs(controlPoints, degree, weights, knots, out var nurbsPoints))
        {
            return EnsureEdgePolylineEndpoints(nurbsPoints, fromPoint, toPoint);
        }

        if (TrySampleRationalBezier(controlPoints, weights, out var rationalBezierPoints))
        {
            return EnsureEdgePolylineEndpoints(rationalBezierPoints, fromPoint, toPoint);
        }

        if (TrySampleBezier(controlPoints, out var bezierPoints))
        {
            return EnsureEdgePolylineEndpoints(bezierPoints, fromPoint, toPoint);
        }

        var fallback = controlPoints.Select(p => new SweepPoint(p.X, p.Y)).ToList();
        return EnsureEdgePolylineEndpoints(fallback, fromPoint, toPoint);
    }

    private static bool TrySampleArc(
        SweepPoint fromPoint,
        SweepPoint toPoint,
        double? radius,
        double? centerX,
        double? centerY,
        out List<SweepPoint> points)
    {
        points = new List<SweepPoint>();
        if (!centerX.HasValue || !centerY.HasValue)
        {
            return false;
        }

        var cx = centerX.Value;
        var cy = centerY.Value;
        var r = radius.GetValueOrDefault();
        if (r <= Epsilon)
        {
            r = DistancePointToPoint(fromPoint.X, fromPoint.Y, cx, cy);
        }

        if (r <= Epsilon)
        {
            return false;
        }

        var startAngle = Math.Atan2(fromPoint.Y - cy, fromPoint.X - cx);
        var endAngle = Math.Atan2(toPoint.Y - cy, toPoint.X - cx);
        var cross = (fromPoint.X - cx) * (toPoint.Y - cy) - (fromPoint.Y - cy) * (toPoint.X - cx);

        double delta;
        if (cross >= 0)
        {
            delta = NormalizePositiveAngle(endAngle - startAngle);
        }
        else
        {
            delta = -NormalizePositiveAngle(startAngle - endAngle);
        }

        var arcLength = Math.Abs(delta) * r;
        var sampleCount = Math.Clamp((int)Math.Ceiling(arcLength / CurveSamplingStep), MinCurveSamples, MaxCurveSamples);
        points = new List<SweepPoint>(sampleCount + 1);
        for (var i = 0; i <= sampleCount; i++)
        {
            var t = i / (double)sampleCount;
            var angle = startAngle + delta * t;
            points.Add(new SweepPoint(cx + r * Math.Cos(angle), cy + r * Math.Sin(angle)));
        }

        points[0] = fromPoint;
        points[^1] = toPoint;
        return true;
    }

    private static double NormalizePositiveAngle(double angle)
    {
        var twoPi = Math.PI * 2d;
        var normalized = angle % twoPi;
        if (normalized < 0)
        {
            normalized += twoPi;
        }

        return normalized;
    }

    private static double NormalizeAngle(double angle)
    {
        var normalized = NormalizeAngleDiff(angle);
        return Math.Abs(normalized + Math.PI) <= Epsilon ? Math.PI : normalized;
    }

    private static bool TrySampleBezier(IReadOnlyList<PointCache> controlPoints, out List<SweepPoint> points)
    {
        points = new List<SweepPoint>();
        if (controlPoints.Count < 2)
        {
            return false;
        }

        var cps = controlPoints.Select(p => new SweepPoint(p.X, p.Y)).ToList();
        var sampleCount = ResolveSampleCount(cps);
        points = new List<SweepPoint>(sampleCount + 1);

        for (var i = 0; i <= sampleCount; i++)
        {
            var t = i / (double)sampleCount;
            points.Add(EvaluateBezier(cps, t));
        }

        return points.Count >= 2;
    }

    private static bool TrySampleRationalBezier(
        IReadOnlyList<PointCache> controlPoints,
        IReadOnlyList<double>? weights,
        out List<SweepPoint> points)
    {
        points = new List<SweepPoint>();
        if (controlPoints.Count < 2 || weights == null || weights.Count != controlPoints.Count)
        {
            return false;
        }

        var cps = controlPoints.Select(p => new SweepPoint(p.X, p.Y)).ToList();
        var sampleCount = ResolveSampleCount(cps);
        points = new List<SweepPoint>(sampleCount + 1);
        var n = cps.Count - 1;

        for (var i = 0; i <= sampleCount; i++)
        {
            var t = i / (double)sampleCount;
            double numeratorX = 0d;
            double numeratorY = 0d;
            double denominator = 0d;

            for (var j = 0; j <= n; j++)
            {
                var basis = Bernstein(n, j, t);
                var weightedBasis = basis * weights[j];
                numeratorX += weightedBasis * cps[j].X;
                numeratorY += weightedBasis * cps[j].Y;
                denominator += weightedBasis;
            }

            if (Math.Abs(denominator) <= Epsilon)
            {
                return false;
            }

            points.Add(new SweepPoint(numeratorX / denominator, numeratorY / denominator));
        }

        return points.Count >= 2;
    }

    private static bool TrySampleNurbs(
        IReadOnlyList<PointCache> controlPoints,
        int? degree,
        IReadOnlyList<double>? weights,
        IReadOnlyList<double>? knots,
        out List<SweepPoint> points)
    {
        points = new List<SweepPoint>();

        if (controlPoints.Count < 2 || knots == null || knots.Count == 0)
        {
            return false;
        }

        var n = controlPoints.Count - 1;
        var p = Math.Clamp(degree ?? Math.Min(3, n), 1, n);
        var expectedKnotCount = n + p + 2;
        if (knots.Count != expectedKnotCount)
        {
            return false;
        }

        var curveWeights = weights != null && weights.Count == controlPoints.Count
            ? weights.ToArray()
            : Enumerable.Repeat(1d, controlPoints.Count).ToArray();

        var uStart = knots[p];
        var uEnd = knots[n + 1];
        if (uEnd - uStart <= Epsilon)
        {
            return false;
        }

        var cps = controlPoints.Select(cp => new SweepPoint(cp.X, cp.Y)).ToArray();
        var sampleCount = ResolveSampleCount(cps);
        points = new List<SweepPoint>(sampleCount + 1);

        for (var i = 0; i <= sampleCount; i++)
        {
            var u = i == sampleCount
                ? uEnd - Epsilon
                : uStart + (uEnd - uStart) * i / sampleCount;

            if (!TryEvaluateNurbsPoint(cps, curveWeights, knots, p, u, out var point))
            {
                return false;
            }

            points.Add(point);
        }

        return points.Count >= 2;
    }

    private static bool TryEvaluateNurbsPoint(
        IReadOnlyList<SweepPoint> controlPoints,
        IReadOnlyList<double> weights,
        IReadOnlyList<double> knots,
        int degree,
        double u,
        out SweepPoint point)
    {
        point = default;
        var n = controlPoints.Count - 1;
        var span = FindKnotSpan(n, degree, u, knots);
        if (span < degree || span > n)
        {
            return false;
        }

        var d = new (double Xw, double Yw, double W)[degree + 1];
        for (var j = 0; j <= degree; j++)
        {
            var idx = span - degree + j;
            var weight = weights[idx];
            d[j] = (controlPoints[idx].X * weight, controlPoints[idx].Y * weight, weight);
        }

        for (var r = 1; r <= degree; r++)
        {
            for (var j = degree; j >= r; j--)
            {
                var i = span - degree + j;
                var left = knots[i];
                var right = knots[i + degree - r + 1];
                var denominator = right - left;
                var alpha = Math.Abs(denominator) <= Epsilon ? 0d : (u - left) / denominator;

                d[j] = (
                    (1d - alpha) * d[j - 1].Xw + alpha * d[j].Xw,
                    (1d - alpha) * d[j - 1].Yw + alpha * d[j].Yw,
                    (1d - alpha) * d[j - 1].W + alpha * d[j].W);
            }
        }

        var final = d[degree];
        if (Math.Abs(final.W) <= Epsilon)
        {
            return false;
        }

        point = new SweepPoint(final.Xw / final.W, final.Yw / final.W);
        return true;
    }

    private static int FindKnotSpan(int n, int degree, double u, IReadOnlyList<double> knots)
    {
        if (u >= knots[n + 1] - Epsilon)
        {
            return n;
        }

        if (u <= knots[degree] + Epsilon)
        {
            return degree;
        }

        var low = degree;
        var high = n + 1;
        var mid = (low + high) / 2;

        while (u < knots[mid] || u >= knots[mid + 1])
        {
            if (u < knots[mid])
            {
                high = mid;
            }
            else
            {
                low = mid;
            }

            mid = (low + high) / 2;
        }

        return mid;
    }

    private static List<SweepPoint> EnsureEdgePolylineEndpoints(
        IReadOnlyList<SweepPoint> polyline,
        SweepPoint fromPoint,
        SweepPoint toPoint)
    {
        if (polyline.Count == 0)
        {
            return new List<SweepPoint> { fromPoint, toPoint };
        }

        var result = polyline.ToList();
        if (result.Count == 1)
        {
            if (!ArePointsEqual(result[0], fromPoint))
            {
                result.Insert(0, fromPoint);
            }

            if (!ArePointsEqual(result[^1], toPoint))
            {
                result.Add(toPoint);
            }

            return result;
        }

        result[0] = fromPoint;
        result[^1] = toPoint;
        return result;
    }

    private static int ResolveSampleCount(IReadOnlyList<SweepPoint> points)
    {
        var length = 0d;
        for (var i = 1; i < points.Count; i++)
        {
            length += DistancePointToPoint(points[i - 1].X, points[i - 1].Y, points[i].X, points[i].Y);
        }

        if (length <= Epsilon)
        {
            return MinCurveSamples;
        }

        var samples = (int)Math.Ceiling(length / CurveSamplingStep);
        return Math.Clamp(samples, MinCurveSamples, MaxCurveSamples);
    }

    private static SweepPoint EvaluateBezier(IReadOnlyList<SweepPoint> controlPoints, double t)
    {
        var work = controlPoints.ToArray();
        for (var r = 1; r < work.Length; r++)
        {
            for (var i = 0; i < work.Length - r; i++)
            {
                work[i] = new SweepPoint(
                    (1d - t) * work[i].X + t * work[i + 1].X,
                    (1d - t) * work[i].Y + t * work[i + 1].Y);
            }
        }

        return work[0];
    }

    private static double Bernstein(int n, int i, double t)
    {
        var coefficient = BinomialCoefficient(n, i);
        return coefficient * Math.Pow(t, i) * Math.Pow(1d - t, n - i);
    }

    private static double BinomialCoefficient(int n, int k)
    {
        if (k < 0 || k > n)
        {
            return 0d;
        }

        if (k == 0 || k == n)
        {
            return 1d;
        }

        k = Math.Min(k, n - k);
        var result = 1d;
        for (var i = 1; i <= k; i++)
        {
            result *= (n - (k - i));
            result /= i;
        }

        return result;
    }

    private static void AppendPolyline(List<SweepPoint> target, IReadOnlyList<SweepPoint> source)
    {
        if (source.Count == 0)
        {
            return;
        }

        if (target.Count == 0)
        {
            target.AddRange(source);
            return;
        }

        var startIndex = ArePointsEqual(target[^1], source[0]) ? 1 : 0;
        for (var i = startIndex; i < source.Count; i++)
        {
            target.Add(source[i]);
        }
    }

    private static List<SweepPoint> ResolveSegmentPolyline(
        PathSegmentWithCode segment,
        SweepPoint fromPoint,
        SweepPoint toPoint,
        IReadOnlyDictionary<string, List<SweepPoint>> edgePolylineLookupByCode)
    {
        if (!string.IsNullOrWhiteSpace(segment.EdgeCode) &&
            edgePolylineLookupByCode.TryGetValue(segment.EdgeCode, out var edgePolyline) &&
            edgePolyline.Count > 0)
        {
            return EnsureEdgePolylineEndpoints(edgePolyline, fromPoint, toPoint);
        }

        return new List<SweepPoint> { fromPoint, toPoint };
    }

    private static List<(SweepPoint Point, double T)> SamplePolyline(IReadOnlyList<SweepPoint> polyline, double step)
    {
        var totalLength = 0d;
        for (var i = 1; i < polyline.Count; i++)
        {
            totalLength += DistancePointToPoint(polyline[i - 1].X, polyline[i - 1].Y, polyline[i].X, polyline[i].Y);
        }

        if (totalLength <= Epsilon)
        {
            return new List<(SweepPoint Point, double T)> { (polyline[0], 0d) };
        }

        var sampleCount = Math.Max(1, (int)Math.Ceiling(totalLength / Math.Max(step, Epsilon)));
        var samples = new List<(SweepPoint Point, double T)>(sampleCount + 1);
        for (var i = 0; i <= sampleCount; i++)
        {
            var distance = totalLength * i / sampleCount;
            samples.Add((InterpolateAlongPolyline(polyline, distance), i / (double)sampleCount));
        }

        return samples;
    }

    private static SweepPoint InterpolateAlongPolyline(IReadOnlyList<SweepPoint> polyline, double targetDistance)
    {
        var traversed = 0d;
        for (var i = 1; i < polyline.Count; i++)
        {
            var start = polyline[i - 1];
            var end = polyline[i];
            var segmentLength = DistancePointToPoint(start.X, start.Y, end.X, end.Y);
            if (segmentLength <= Epsilon)
            {
                continue;
            }

            if (traversed + segmentLength >= targetDistance)
            {
                var t = Math.Clamp((targetDistance - traversed) / segmentLength, 0d, 1d);
                return new SweepPoint(
                    start.X + (end.X - start.X) * t,
                    start.Y + (end.Y - start.Y) * t);
            }

            traversed += segmentLength;
        }

        return polyline[^1];
    }

    private static Geometry UnionGeometries(IReadOnlyList<Geometry> geometries)
    {
        var nonEmpty = geometries.Where(g => !g.IsEmpty).ToArray();
        if (nonEmpty.Length == 0)
        {
            return GeometryFactory.CreateGeometryCollection(Array.Empty<Geometry>());
        }

        var union = GeometryFactory.CreateGeometryCollection(nonEmpty).Union();
        return union.IsValid ? union : union.Buffer(0);
    }

    private static bool ArePointsEqual(SweepPoint a, SweepPoint b)
    {
        return Math.Abs(a.X - b.X) <= Epsilon && Math.Abs(a.Y - b.Y) <= Epsilon;
    }

    private static double DistancePointToPolyline(double x, double y, IReadOnlyList<SweepPoint> polyline)
    {
        if (polyline.Count == 0)
        {
            return double.MaxValue;
        }

        if (polyline.Count == 1)
        {
            var point = polyline[0];
            return DistancePointToPoint(x, y, point.X, point.Y);
        }

        var minDistance = double.MaxValue;
        for (var i = 1; i < polyline.Count; i++)
        {
            var p1 = polyline[i - 1];
            var p2 = polyline[i];
            var distance = DistancePointToSegment(x, y, p1.X, p1.Y, p2.X, p2.Y);
            if (distance < minDistance)
            {
                minDistance = distance;
            }
        }

        return minDistance;
    }

    private static double DistancePolylineToPolyline(
        IReadOnlyList<SweepPoint> polylineA,
        IReadOnlyList<SweepPoint> polylineB)
    {
        if (polylineA.Count == 0 || polylineB.Count == 0)
        {
            return double.MaxValue;
        }

        if (polylineA.Count == 1)
        {
            var p = polylineA[0];
            return DistancePointToPolyline(p.X, p.Y, polylineB);
        }

        if (polylineB.Count == 1)
        {
            var p = polylineB[0];
            return DistancePointToPolyline(p.X, p.Y, polylineA);
        }

        var minDistance = double.MaxValue;
        for (var i = 1; i < polylineA.Count; i++)
        {
            var a1 = polylineA[i - 1];
            var a2 = polylineA[i];

            for (var j = 1; j < polylineB.Count; j++)
            {
                var b1 = polylineB[j - 1];
                var b2 = polylineB[j];
                var distance = DistanceSegmentToSegment(a1.X, a1.Y, a2.X, a2.Y, b1.X, b1.Y, b2.X, b2.Y);
                if (distance < minDistance)
                {
                    minDistance = distance;
                }
            }
        }

        return minDistance;
    }

    private static double DistanceSegmentToSegment(
        double ax1,
        double ay1,
        double ax2,
        double ay2,
        double bx1,
        double by1,
        double bx2,
        double by2)
    {
        if (SegmentsIntersect(ax1, ay1, ax2, ay2, bx1, by1, bx2, by2))
        {
            return 0d;
        }

        var d1 = DistancePointToSegment(ax1, ay1, bx1, by1, bx2, by2);
        var d2 = DistancePointToSegment(ax2, ay2, bx1, by1, bx2, by2);
        var d3 = DistancePointToSegment(bx1, by1, ax1, ay1, ax2, ay2);
        var d4 = DistancePointToSegment(bx2, by2, ax1, ay1, ax2, ay2);

        return Math.Min(Math.Min(d1, d2), Math.Min(d3, d4));
    }

    private static bool SegmentsIntersect(
        double ax1,
        double ay1,
        double ax2,
        double ay2,
        double bx1,
        double by1,
        double bx2,
        double by2)
    {
        var o1 = Orientation(ax1, ay1, ax2, ay2, bx1, by1);
        var o2 = Orientation(ax1, ay1, ax2, ay2, bx2, by2);
        var o3 = Orientation(bx1, by1, bx2, by2, ax1, ay1);
        var o4 = Orientation(bx1, by1, bx2, by2, ax2, ay2);

        if (o1 * o2 < 0 && o3 * o4 < 0)
        {
            return true;
        }

        if (Math.Abs(o1) <= Epsilon && IsPointOnSegment(bx1, by1, ax1, ay1, ax2, ay2)) return true;
        if (Math.Abs(o2) <= Epsilon && IsPointOnSegment(bx2, by2, ax1, ay1, ax2, ay2)) return true;
        if (Math.Abs(o3) <= Epsilon && IsPointOnSegment(ax1, ay1, bx1, by1, bx2, by2)) return true;
        if (Math.Abs(o4) <= Epsilon && IsPointOnSegment(ax2, ay2, bx1, by1, bx2, by2)) return true;

        return false;
    }

    private static double Orientation(
        double ax,
        double ay,
        double bx,
        double by,
        double cx,
        double cy)
    {
        return (bx - ax) * (cy - ay) - (by - ay) * (cx - ax);
    }

    private static bool IsPointOnSegment(
        double px,
        double py,
        double x1,
        double y1,
        double x2,
        double y2)
    {
        if (Math.Abs(Orientation(x1, y1, x2, y2, px, py)) > Epsilon)
        {
            return false;
        }

        var minX = Math.Min(x1, x2) - Epsilon;
        var maxX = Math.Max(x1, x2) + Epsilon;
        var minY = Math.Min(y1, y2) - Epsilon;
        var maxY = Math.Max(y1, y2) + Epsilon;
        return px >= minX && px <= maxX && py >= minY && py <= maxY;
    }

    private static double DistancePointToSegment(
        double px,
        double py,
        double x1,
        double y1,
        double x2,
        double y2)
    {
        var dx = x2 - x1;
        var dy = y2 - y1;
        if (Math.Abs(dx) < Epsilon && Math.Abs(dy) < Epsilon)
        {
            return DistancePointToPoint(px, py, x1, y1);
        }

        var t = ((px - x1) * dx + (py - y1) * dy) / (dx * dx + dy * dy);
        t = Math.Clamp(t, 0d, 1d);
        var projectionX = x1 + t * dx;
        var projectionY = y1 + t * dy;
        return DistancePointToPoint(px, py, projectionX, projectionY);
    }

    private static double DistancePointToPoint(double x1, double y1, double x2, double y2)
    {
        var dx = x1 - x2;
        var dy = y1 - y2;
        return Math.Sqrt(dx * dx + dy * dy);
    }

    /// <summary>
    /// 节点旋转判定阈值（弧度），约3°。
    /// 入射角与出射角差值超过此阈值时视为需要旋转。
    /// @author zzy
    /// 2026-04-19 创建
    /// </summary>
    private const double NodeRotationThreshold = 0.052d;

    /// <summary>
    /// 计算边上行驶时的扫掠半径。
    /// 根据机器人车头与边方向的夹角，计算旋转矩形在垂直于边方向上的投影半宽。
    /// 公式：|halfLength × sin(θ)| + |halfWidth × cos(θ)| + safetyDistance
    /// @author zzy
    /// 2026-04-19 创建
    /// </summary>
    /// <param name="length">机器人长度</param>
    /// <param name="width">机器人宽度</param>
    /// <param name="safetyDistance">安全距离</param>
    /// <param name="headingAngle">车头与边方向的夹角（弧度）</param>
    /// <returns>边扫掠半径</returns>
    public static double CalculateEdgeSweepRadius(double length, double width, double safetyDistance, double headingAngle)
    {
        var halfLength = Math.Max(length, 0d) / 2d;
        var halfWidth = Math.Max(width, 0d) / 2d;
        var safety = Math.Max(safetyDistance, 0d);
        var sinA = Math.Abs(Math.Sin(headingAngle));
        var cosA = Math.Abs(Math.Cos(headingAngle));
        return halfLength * sinA + halfWidth * cosA + safety;
    }

    /// <summary>
    /// 计算节点处的扫掠半径。
    /// 比较入射角和出射角判断是否需要旋转：
    /// - 需要旋转：使用外接圆半径（对角线/2 + 安全距离）
    /// - 不需要旋转：使用入射边和出射边投影半宽中的较大值
    /// @author zzy
    /// 2026-04-19 创建
    /// </summary>
    /// <param name="length">机器人长度</param>
    /// <param name="width">机器人宽度</param>
    /// <param name="safetyDistance">安全距离</param>
    /// <param name="incomingTheta">入射全局航向角（弧度），null 表示起点无入射</param>
    /// <param name="outgoingTheta">出射全局航向角（弧度），null 表示终点无出射</param>
    /// <param name="incomingEdgeAngle">入射边的车头夹角（弧度），null 表示起点无入射</param>
    /// <param name="outgoingEdgeAngle">出射边的车头夹角（弧度），null 表示终点无出射</param>
    /// <returns>节点扫掠半径</returns>
    public static double CalculateNodeSweepRadius(
        double length,
        double width,
        double safetyDistance,
        double? incomingTheta,
        double? outgoingTheta,
        double? incomingEdgeAngle,
        double? outgoingEdgeAngle)
    {
        var needsRotation = false;
        if (incomingTheta.HasValue && outgoingTheta.HasValue)
        {
            var angleDiff = Math.Abs(NormalizeAngleDiff(outgoingTheta.Value - incomingTheta.Value));
            needsRotation = angleDiff > NodeRotationThreshold;
        }

        if (needsRotation)
        {
            return CalculateSweepRadius(length, width, safetyDistance);
        }

        var edgeRadius1 = incomingEdgeAngle.HasValue
            ? CalculateEdgeSweepRadius(length, width, safetyDistance, incomingEdgeAngle.Value)
            : 0d;
        var edgeRadius2 = outgoingEdgeAngle.HasValue
            ? CalculateEdgeSweepRadius(length, width, safetyDistance, outgoingEdgeAngle.Value)
            : 0d;
        var narrowRadius = Math.Max(edgeRadius1, edgeRadius2);
        return narrowRadius > 0d ? narrowRadius : CalculateSweepRadius(length, width, safetyDistance);
    }

    /// <summary>
    /// 将角度差归一化到 [-π, π] 范围。
    /// @author zzy
    /// 2026-04-19 创建
    /// </summary>
    private static double NormalizeAngleDiff(double angle)
    {
        while (angle > Math.PI) angle -= 2d * Math.PI;
        while (angle < -Math.PI) angle += 2d * Math.PI;
        return angle;
    }

    /// <summary>
    /// 根据路径段列表构建逐段扫掠区域。
    /// 每条边生成一个边区域（使用投影半宽），每个节点生成一个节点区域（根据旋转判定选择半径）。
    /// @author zzy
    /// 2026-04-19 创建
    /// </summary>
    /// <param name="segments">路径段列表</param>
    /// <param name="dimensions">机器人尺寸</param>
    /// <param name="nodeLookupByCode">节点编码到坐标的映射</param>
    /// <param name="edgePolylineLookupByCode">边编码到多段线的映射</param>
    /// <returns>扫掠区域列表</returns>
    public static List<SweptZone> BuildSweptZones(
        IReadOnlyList<PathSegmentWithCode> segments,
        RobotSweepDimensions dimensions,
        IReadOnlyDictionary<string, SweepPoint> nodeLookupByCode,
        IReadOnlyDictionary<string, List<SweepPoint>>? edgePolylineLookupByCode = null)
    {
        var zones = new List<SweptZone>();
        if (segments.Count == 0) return zones;

        var length = dimensions.Length;
        var width = dimensions.Width;
        var safety = dimensions.SafetyDistance;

        for (var i = 0; i < segments.Count; i++)
        {
            var seg = segments[i];
            if (string.IsNullOrWhiteSpace(seg.FromNodeCode) || string.IsNullOrWhiteSpace(seg.ToNodeCode))
                continue;
            if (!nodeLookupByCode.TryGetValue(seg.FromNodeCode, out var fromPoint) ||
                !nodeLookupByCode.TryGetValue(seg.ToNodeCode, out var toPoint))
                continue;

            // 边区域
            List<SweepPoint> edgePolyline;
            if (edgePolylineLookupByCode != null &&
                !string.IsNullOrWhiteSpace(seg.EdgeCode) &&
                edgePolylineLookupByCode.TryGetValue(seg.EdgeCode, out var polyline) &&
                polyline.Count > 0)
            {
                edgePolyline = EnsureEdgePolylineEndpoints(polyline, fromPoint, toPoint);
            }
            else
            {
                edgePolyline = new List<SweepPoint> { fromPoint, toPoint };
            }

            var edgeRadius = CalculateEdgeSweepRadius(length, width, safety, seg.Angle);
            zones.Add(new SweptZone(edgePolyline, edgeRadius));

            // 起点节点区域（仅首段添加）
            if (i == 0)
            {
                double? prevTheta = null;
                double? prevEdgeAngle = null;
                var nodeRadius = CalculateNodeSweepRadius(
                    length, width, safety,
                    prevTheta, seg.StartTheta,
                    prevEdgeAngle, seg.Angle);
                zones.Add(new SweptZone(
                    new List<SweepPoint> { fromPoint },
                    nodeRadius));
            }

            // 终点节点区域
            double? nextTheta = (i + 1 < segments.Count) ? segments[i + 1].StartTheta : (double?)null;
            double? nextEdgeAngle = (i + 1 < segments.Count) ? segments[i + 1].Angle : (double?)null;
            var endNodeRadius = CalculateNodeSweepRadius(
                length, width, safety,
                seg.EndTheta, nextTheta,
                seg.Angle, nextEdgeAngle);
            zones.Add(new SweptZone(
                new List<SweepPoint> { toPoint },
                endNodeRadius));
        }

        return zones;
    }

    /// <summary>
    /// 使用多个扫掠区域从图结构中展开覆盖的节点和边。
    /// 节点/边只要落入任一区域即纳入覆盖结果。
    /// @author zzy
    /// 2026-04-19 创建
    /// </summary>
    /// <param name="graph">路径图结构</param>
    /// <param name="zones">扫掠区域列表</param>
    /// <returns>覆盖结果</returns>
    public static SweptAreaCoverageResult ExpandFromGraphWithZones(
        PathGraph graph,
        IReadOnlyList<SweptZone> zones)
    {
        var result = new SweptAreaCoverageResult();
        if (graph.Nodes.Count == 0 || zones.Count == 0)
        {
            return result;
        }

        foreach (var node in graph.Nodes.Values.Where(n => n.Active && !string.IsNullOrWhiteSpace(n.NodeCode)))
        {
            if (result.NodeCodes.Contains(node.NodeCode)) continue;
            foreach (var zone in zones)
            {
                if (DistancePointToPolyline(node.X, node.Y, zone.CenterLine) <= zone.Radius + Epsilon)
                {
                    result.NodeCodes.Add(node.NodeCode);
                    break;
                }
            }
        }

        foreach (var edge in graph.Edges.Values.Where(e => e.Active && !string.IsNullOrWhiteSpace(e.EdgeCode)))
        {
            if (result.EdgeCodes.Contains(edge.EdgeCode)) continue;
            if (!graph.Nodes.TryGetValue(edge.FromNodeId, out var fromNode) ||
                !graph.Nodes.TryGetValue(edge.ToNodeId, out var toNode))
            {
                continue;
            }

            var edgePolyline = BuildEdgePolyline(
                edge.CurveType,
                edge.Radius,
                edge.CenterX,
                edge.CenterY,
                edge.ControlPoints,
                edge.Degree,
                edge.Weights,
                edge.Knots,
                new SweepPoint(fromNode.X, fromNode.Y),
                new SweepPoint(toNode.X, toNode.Y));

            foreach (var zone in zones)
            {
                var distance = DistancePolylineToPolyline(edgePolyline, zone.CenterLine);
                if (distance <= zone.Radius + Epsilon)
                {
                    result.EdgeCodes.Add(edge.EdgeCode);
                    break;
                }
            }
        }

        return result;
    }
}

public readonly record struct SweepPoint(double X, double Y);

public readonly record struct RobotSweepDimensions(
    double Width,
    double Length,
    double SafetyDistance,
    bool UseDefaultWidth,
    bool UseDefaultLength,
    bool UseDefaultSafetyDistance);

public sealed class SweptAreaCoverageResult
{
    public HashSet<string> NodeCodes { get; } = new(StringComparer.OrdinalIgnoreCase);
    public HashSet<string> EdgeCodes { get; } = new(StringComparer.OrdinalIgnoreCase);
}

/// <summary>
/// 扫掠区域：中心线多段线 + 扫掠半径。
/// @author zzy
/// 2026-04-19 创建
/// </summary>
/// <param name="CenterLine">中心线多段线点集</param>
/// <param name="Radius">扫掠半径</param>
public sealed class SweptZone
{
    public SweptZone(IReadOnlyList<SweepPoint> centerLine, double radius)
    {
        CenterLine = centerLine;
        Radius = radius;
    }

    public IReadOnlyList<SweepPoint> CenterLine { get; }
    public double Radius { get; }
}