72 lines
2.9 KiB
C#
72 lines
2.9 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
|
|
{
|
|
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
|
|
{
|
|
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);
|
|
}
|
|
}
|