50 lines
2.0 KiB
C#
50 lines
2.0 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
|
|
/// <summary>已校验并按方向拆分的粗路径输入快照。</summary>
|
|
public sealed class PreparedPath
|
|
{
|
|
/// <summary>创建不可变预处理路径。</summary>
|
|
public PreparedPath(IReadOnlyList<PreparedDirectionSegment> segments)
|
|
{
|
|
if (segments == null || segments.Count == 0)
|
|
throw new ArgumentException("A prepared path requires direction segments.", nameof(segments));
|
|
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
|
{
|
|
if (segments[segmentIndex] == null)
|
|
throw new ArgumentException("A prepared path cannot contain null direction segments.", nameof(segments));
|
|
}
|
|
|
|
Segments = CopyReadOnly(segments);
|
|
Points = Flatten(Segments);
|
|
}
|
|
|
|
/// <summary>按原始前进/倒车拓扑排列的方向段。</summary>
|
|
public IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
|
|
|
/// <summary>将所有方向段顺序拼接后的点；换向重复点保留两次。</summary>
|
|
public IReadOnlyList<SmoothingPoint2D> Points { get; }
|
|
|
|
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
|
{
|
|
var copy = new List<T>(source.Count);
|
|
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
|
return new ReadOnlyCollection<T>(copy);
|
|
}
|
|
|
|
private static IReadOnlyList<SmoothingPoint2D> Flatten(IReadOnlyList<PreparedDirectionSegment> segments)
|
|
{
|
|
var points = new List<SmoothingPoint2D>();
|
|
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
|
{
|
|
PreparedDirectionSegment segment = segments[segmentIndex];
|
|
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
|
points.Add(segment.Points[pointIndex]);
|
|
}
|
|
return new ReadOnlyCollection<SmoothingPoint2D>(points);
|
|
}
|
|
}
|