zengzy / hhrcs_service · Files

using System;
using System.Collections.Generic;
using System.Linq;
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;

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