77 lines
3.6 KiB
C#
77 lines
3.6 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
|
|
|
/// <summary>一个尚未经过安全和质量评价的局部 G2 替换几何。</summary>
|
|
internal sealed class LocalG2CandidateGeometry
|
|
{
|
|
internal LocalG2CandidateGeometry(
|
|
int candidateIndex,
|
|
int segmentIndex,
|
|
double startArcLengthMeters,
|
|
double endArcLengthMeters,
|
|
double leftWindowLengthMeters,
|
|
double rightWindowLengthMeters,
|
|
IReadOnlyList<SmoothingPoint2D> regionPoints,
|
|
double startVehicleCurvaturePerMeter,
|
|
double endVehicleCurvaturePerMeter,
|
|
double startGeometricCurvaturePerMeter,
|
|
double endGeometricCurvaturePerMeter,
|
|
bool internalConnectionsAreG2)
|
|
{
|
|
if (candidateIndex < 0 || segmentIndex < 0 || !NumericGuard.IsFinite(startArcLengthMeters) ||
|
|
!NumericGuard.IsFinite(endArcLengthMeters) || startArcLengthMeters < 0d ||
|
|
endArcLengthMeters < startArcLengthMeters || !NumericGuard.IsFinite(leftWindowLengthMeters) ||
|
|
!NumericGuard.IsFinite(rightWindowLengthMeters) || leftWindowLengthMeters < 0d ||
|
|
rightWindowLengthMeters < 0d || regionPoints == null || regionPoints.Count < 2 ||
|
|
!NumericGuard.IsFinite(startVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(endVehicleCurvaturePerMeter) ||
|
|
!NumericGuard.IsFinite(startGeometricCurvaturePerMeter) || !NumericGuard.IsFinite(endGeometricCurvaturePerMeter))
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(regionPoints));
|
|
}
|
|
|
|
CandidateIndex = candidateIndex;
|
|
SegmentIndex = segmentIndex;
|
|
StartArcLengthMeters = startArcLengthMeters;
|
|
EndArcLengthMeters = endArcLengthMeters;
|
|
LeftWindowLengthMeters = leftWindowLengthMeters;
|
|
RightWindowLengthMeters = rightWindowLengthMeters;
|
|
RegionPoints = Copy(regionPoints);
|
|
StartVehicleCurvaturePerMeter = startVehicleCurvaturePerMeter;
|
|
EndVehicleCurvaturePerMeter = endVehicleCurvaturePerMeter;
|
|
StartGeometricCurvaturePerMeter = startGeometricCurvaturePerMeter;
|
|
EndGeometricCurvaturePerMeter = endGeometricCurvaturePerMeter;
|
|
InternalConnectionsAreG2 = internalConnectionsAreG2;
|
|
}
|
|
|
|
internal int CandidateIndex { get; }
|
|
internal int SegmentIndex { get; }
|
|
internal double StartArcLengthMeters { get; }
|
|
internal double EndArcLengthMeters { get; }
|
|
internal double LeftWindowLengthMeters { get; }
|
|
internal double RightWindowLengthMeters { get; }
|
|
internal IReadOnlyList<SmoothingPoint2D> RegionPoints { get; }
|
|
|
|
// 这些边界状态供候选评价和窄范围反射验证使用，不能替代统一几何分析器的最终统计。
|
|
internal double StartVehicleCurvaturePerMeter { get; }
|
|
internal double EndVehicleCurvaturePerMeter { get; }
|
|
internal double StartGeometricCurvaturePerMeter { get; }
|
|
internal double EndGeometricCurvaturePerMeter { get; }
|
|
internal bool InternalConnectionsAreG2 { get; }
|
|
|
|
private static IReadOnlyList<SmoothingPoint2D> Copy(IReadOnlyList<SmoothingPoint2D> source)
|
|
{
|
|
var copy = new List<SmoothingPoint2D>(source.Count);
|
|
for (int index = 0; index < source.Count; index++)
|
|
{
|
|
if (source[index] == null) throw new ArgumentOutOfRangeException(nameof(source));
|
|
copy.Add(source[index]);
|
|
}
|
|
return new ReadOnlyCollection<SmoothingPoint2D>(copy);
|
|
}
|
|
}
|