82 lines
4.0 KiB
C#
82 lines
4.0 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
|
|
/// <summary>已校验、已按单一行驶方向分割并重采样的路径段。</summary>
|
|
public sealed class PreparedDirectionSegment
|
|
{
|
|
/// <summary>创建不可变方向段。</summary>
|
|
/// <param name="segmentIndex">在 <see cref="PreparedPath.Segments"/> 中从零开始的段索引。</param>
|
|
/// <param name="direction">本段唯一的车辆行驶方向。</param>
|
|
/// <param name="points">本段的非空采样点集合；构造后会复制为只读快照。</param>
|
|
/// <param name="startsAtGearSwitch">若首点紧随换向，则为 <see langword="true"/>。</param>
|
|
/// <param name="endsAtGearSwitch">若末点紧邻换向，则为 <see langword="true"/>。</param>
|
|
public PreparedDirectionSegment(
|
|
int segmentIndex,
|
|
TravelDirection direction,
|
|
IReadOnlyList<SmoothingPoint2D> points,
|
|
bool startsAtGearSwitch,
|
|
bool endsAtGearSwitch)
|
|
: this(segmentIndex, direction, points, startsAtGearSwitch, endsAtGearSwitch, null)
|
|
{
|
|
}
|
|
|
|
/// <summary>创建带有真实起始车辆曲率边界状态的不可变方向段。</summary>
|
|
/// <param name="segmentIndex">在 <see cref="PreparedPath.Segments"/> 中从零开始的段索引。</param>
|
|
/// <param name="direction">本段唯一的车辆行驶方向。</param>
|
|
/// <param name="points">本段的非空采样点集合；构造后会复制为只读快照。</param>
|
|
/// <param name="startsAtGearSwitch">若首点紧随换向，则为 <see langword="true"/>。</param>
|
|
/// <param name="endsAtGearSwitch">若末点紧邻换向，则为 <see langword="true"/>。</param>
|
|
/// <param name="startVehicleCurvaturePerMeter">物理起点车辆曲率边界，单位 1/m；未知时为 <see langword="null"/>。</param>
|
|
public PreparedDirectionSegment(
|
|
int segmentIndex,
|
|
TravelDirection direction,
|
|
IReadOnlyList<SmoothingPoint2D> points,
|
|
bool startsAtGearSwitch,
|
|
bool endsAtGearSwitch,
|
|
double? startVehicleCurvaturePerMeter)
|
|
{
|
|
if (segmentIndex < 0) throw new ArgumentOutOfRangeException(nameof(segmentIndex));
|
|
if (points == null || points.Count == 0) throw new ArgumentException("A prepared segment requires points.", nameof(points));
|
|
if (startVehicleCurvaturePerMeter.HasValue && !IsFinite(startVehicleCurvaturePerMeter.Value))
|
|
throw new ArgumentOutOfRangeException(nameof(startVehicleCurvaturePerMeter));
|
|
|
|
SegmentIndex = segmentIndex;
|
|
Direction = direction;
|
|
Points = CopyReadOnly(points);
|
|
StartsAtGearSwitch = startsAtGearSwitch;
|
|
EndsAtGearSwitch = endsAtGearSwitch;
|
|
StartVehicleCurvaturePerMeter = startVehicleCurvaturePerMeter;
|
|
}
|
|
|
|
/// <summary>从零开始的分段序号；在 <see cref="PreparedPath.Segments"/> 中必须与其位置一致。</summary>
|
|
public int SegmentIndex { get; }
|
|
|
|
/// <summary>该段的唯一行驶方向。</summary>
|
|
public TravelDirection Direction { get; }
|
|
|
|
/// <summary>不包含相邻段点的本段不可变采样点。</summary>
|
|
public IReadOnlyList<SmoothingPoint2D> Points { get; }
|
|
|
|
/// <summary>本段首点是否为换向后保留的新方向点。</summary>
|
|
public bool StartsAtGearSwitch { get; }
|
|
|
|
/// <summary>本段末点之后是否紧邻换向点。</summary>
|
|
public bool EndsAtGearSwitch { get; }
|
|
|
|
/// <summary>原始车辆在本段物理起点的曲率边界状态，单位 1/m。</summary>
|
|
public double? StartVehicleCurvaturePerMeter { get; }
|
|
|
|
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
|
|
|
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);
|
|
}
|
|
}
|