108 lines
3.7 KiB
C#
108 lines
3.7 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
|
|
|
/// <summary>把稳定报告顺序转换为不会使后续原始局部弧长错位的工作顺序。</summary>
|
|
internal sealed class LocalG2RegionWorkOrder
|
|
{
|
|
internal bool TryCreate(
|
|
IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
|
|
out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
|
|
out string reason)
|
|
{
|
|
workOrder = Empty();
|
|
reason = string.Empty;
|
|
if (reportOrder == null)
|
|
{
|
|
reason = "局部 G2 区域工作顺序输入无效。";
|
|
return false;
|
|
}
|
|
|
|
var indexed = new List<IndexedRegion>(reportOrder.Count);
|
|
for (int index = 0; index < reportOrder.Count; index++)
|
|
{
|
|
LocalG2SmoothingRegion region = reportOrder[index];
|
|
if (!TryValidate(region, out double firstArc, out reason))
|
|
return false;
|
|
indexed.Add(new IndexedRegion(region, index, firstArc));
|
|
}
|
|
|
|
indexed.Sort(Compare);
|
|
var ordered = new List<LocalG2SmoothingRegion>(indexed.Count);
|
|
for (int index = 0; index < indexed.Count; index++)
|
|
ordered.Add(indexed[index].Region);
|
|
workOrder = new ReadOnlyCollection<LocalG2SmoothingRegion>(ordered);
|
|
return true;
|
|
}
|
|
|
|
private static bool TryValidate(
|
|
LocalG2SmoothingRegion region,
|
|
out double firstArc,
|
|
out string reason)
|
|
{
|
|
firstArc = 0d;
|
|
reason = string.Empty;
|
|
if (region == null || region.SegmentIndex < 0 ||
|
|
region.Transitions == null || region.Transitions.Count == 0 ||
|
|
region.WindowVariants == null || region.WindowVariants.Count == 0)
|
|
{
|
|
reason = "局部 G2 工作顺序包含空区域或空窗口集。";
|
|
return false;
|
|
}
|
|
|
|
double previousArc = -1d;
|
|
for (int index = 0; index < region.Transitions.Count; index++)
|
|
{
|
|
CurvatureTransition transition = region.Transitions[index];
|
|
if (transition == null ||
|
|
transition.SegmentIndex != region.SegmentIndex ||
|
|
!NumericGuard.IsFinite(transition.LocalArcLengthMeters) ||
|
|
transition.LocalArcLengthMeters < previousArc)
|
|
{
|
|
reason = "局部 G2 工作顺序要求区域事件有限、同段且按弧长升序。";
|
|
return false;
|
|
}
|
|
previousArc = transition.LocalArcLengthMeters;
|
|
}
|
|
|
|
firstArc = region.Transitions[0].LocalArcLengthMeters;
|
|
return true;
|
|
}
|
|
|
|
private static int Compare(IndexedRegion left, IndexedRegion right)
|
|
{
|
|
int segment = left.Region.SegmentIndex.CompareTo(right.Region.SegmentIndex);
|
|
if (segment != 0) return segment;
|
|
int descendingArc = right.FirstArc.CompareTo(left.FirstArc);
|
|
return descendingArc != 0
|
|
? descendingArc
|
|
: left.ReportIndex.CompareTo(right.ReportIndex);
|
|
}
|
|
|
|
private static IReadOnlyList<LocalG2SmoothingRegion> Empty()
|
|
{
|
|
return new ReadOnlyCollection<LocalG2SmoothingRegion>(
|
|
new List<LocalG2SmoothingRegion>());
|
|
}
|
|
|
|
private sealed class IndexedRegion
|
|
{
|
|
internal IndexedRegion(
|
|
LocalG2SmoothingRegion region,
|
|
int reportIndex,
|
|
double firstArc)
|
|
{
|
|
Region = region;
|
|
ReportIndex = reportIndex;
|
|
FirstArc = firstArc;
|
|
}
|
|
|
|
internal LocalG2SmoothingRegion Region { get; }
|
|
internal int ReportIndex { get; }
|
|
internal double FirstArc { get; }
|
|
}
|
|
}
|