84 lines
4.1 KiB
C#
84 lines
4.1 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
|
|
|
/// <summary>一个局部 G2 候选可替换的弧长窗口。</summary>
|
|
internal sealed class LocalG2WindowVariant
|
|
{
|
|
/// <summary>创建同一平滑区域内一个可尝试替换的弧长窗口变体。</summary>
|
|
/// <param name="candidateIndex">区域内候选的稳定从零开始索引。</param>
|
|
/// <param name="startArcLengthMeters">窗口起点弧长，单位 m。</param>
|
|
/// <param name="endArcLengthMeters">窗口终点弧长，单位 m。</param>
|
|
/// <param name="leftWindowLengthMeters">过渡点左侧窗口长度，单位 m。</param>
|
|
/// <param name="rightWindowLengthMeters">过渡点右侧窗口长度，单位 m。</param>
|
|
internal LocalG2WindowVariant(int candidateIndex, double startArcLengthMeters, double endArcLengthMeters,
|
|
double leftWindowLengthMeters, double rightWindowLengthMeters)
|
|
{
|
|
if (candidateIndex < 0 || startArcLengthMeters < 0d || endArcLengthMeters < startArcLengthMeters ||
|
|
leftWindowLengthMeters < 0d || rightWindowLengthMeters < 0d)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(candidateIndex));
|
|
}
|
|
CandidateIndex = candidateIndex;
|
|
StartArcLengthMeters = startArcLengthMeters;
|
|
EndArcLengthMeters = endArcLengthMeters;
|
|
LeftWindowLengthMeters = leftWindowLengthMeters;
|
|
RightWindowLengthMeters = rightWindowLengthMeters;
|
|
}
|
|
|
|
internal int CandidateIndex { get; }
|
|
internal double StartArcLengthMeters { get; }
|
|
internal double EndArcLengthMeters { get; }
|
|
internal double LeftWindowLengthMeters { get; }
|
|
internal double RightWindowLengthMeters { get; }
|
|
}
|
|
|
|
/// <summary>至少存在一个联合合法生成窗口变体的一组曲率过渡。</summary>
|
|
internal sealed class LocalG2SmoothingRegion
|
|
{
|
|
/// <summary>创建一个包含共同可用窗口变体的局部曲率过渡区域。</summary>
|
|
/// <param name="segmentIndex">所属方向段在预处理路径中的从零开始索引。</param>
|
|
/// <param name="transitions">归并到该区域的曲率跳变集合。</param>
|
|
/// <param name="maximumStartArcLengthMeters">所有候选允许的最晚窗口起点，单位 m。</param>
|
|
/// <param name="maximumEndArcLengthMeters">所有候选允许的最早窗口终点，单位 m。</param>
|
|
/// <param name="windowVariants">至少一个合法的窗口变体集合。</param>
|
|
internal LocalG2SmoothingRegion(
|
|
int segmentIndex,
|
|
IReadOnlyList<CurvatureTransition> transitions,
|
|
double maximumStartArcLengthMeters,
|
|
double maximumEndArcLengthMeters,
|
|
IReadOnlyList<LocalG2WindowVariant> windowVariants)
|
|
{
|
|
if (segmentIndex < 0 || transitions == null || transitions.Count == 0 ||
|
|
!NumericGuard.IsFinite(maximumStartArcLengthMeters) ||
|
|
!NumericGuard.IsFinite(maximumEndArcLengthMeters) ||
|
|
maximumStartArcLengthMeters < 0d ||
|
|
maximumEndArcLengthMeters < maximumStartArcLengthMeters ||
|
|
windowVariants == null || windowVariants.Count == 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(transitions));
|
|
}
|
|
SegmentIndex = segmentIndex;
|
|
Transitions = Copy(transitions);
|
|
MaximumStartArcLengthMeters = maximumStartArcLengthMeters;
|
|
MaximumEndArcLengthMeters = maximumEndArcLengthMeters;
|
|
WindowVariants = Copy(windowVariants);
|
|
}
|
|
|
|
internal int SegmentIndex { get; }
|
|
internal IReadOnlyList<CurvatureTransition> Transitions { get; }
|
|
internal double MaximumStartArcLengthMeters { get; }
|
|
internal double MaximumEndArcLengthMeters { get; }
|
|
internal IReadOnlyList<LocalG2WindowVariant> WindowVariants { get; }
|
|
|
|
private static IReadOnlyList<T> Copy<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);
|
|
}
|
|
}
|