65 lines
2.6 KiB
C#
65 lines
2.6 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public sealed class DirectionSegmentView
|
|
{
|
|
public DirectionSegmentView(
|
|
int segmentIndex,
|
|
TravelDirection direction,
|
|
IReadOnlyList<SmoothedPathPoint> points,
|
|
ReferenceBoundary startBoundary,
|
|
ReferenceBoundary endBoundary,
|
|
double sourceStartArcLength)
|
|
{
|
|
if (segmentIndex < 0)
|
|
throw new ArgumentOutOfRangeException(nameof(segmentIndex));
|
|
if (points == null || points.Count == 0)
|
|
throw new ArgumentException("A direction segment requires points.", nameof(points));
|
|
if (startBoundary == null || endBoundary == null)
|
|
throw new ArgumentNullException(startBoundary == null ? nameof(startBoundary) : nameof(endBoundary));
|
|
if (startBoundary.SegmentIndex != segmentIndex || endBoundary.SegmentIndex != segmentIndex)
|
|
throw new ArgumentException("Boundary segment identity must match the segment.");
|
|
if (points[0] == null || Math.Abs(points[0].ArcLength) > 1e-12d)
|
|
throw new ArgumentException("A direction segment must begin at local S zero.", nameof(points));
|
|
|
|
var copy = new List<SmoothedPathPoint>(points.Count);
|
|
double previousS = -1d;
|
|
for (int index = 0; index < points.Count; index++)
|
|
{
|
|
SmoothedPathPoint point = points[index];
|
|
if (point == null || point.Direction != direction || point.ArcLength < 0d || point.ArcLength < previousS)
|
|
throw new ArgumentException("Direction segment points are invalid.", nameof(points));
|
|
copy.Add(point);
|
|
previousS = point.ArcLength;
|
|
}
|
|
if (Math.Abs(endBoundary.SegmentLocalS - previousS) > 1e-12d)
|
|
throw new ArgumentException("End boundary must match the final local S.", nameof(endBoundary));
|
|
|
|
SegmentIndex = segmentIndex;
|
|
Direction = direction;
|
|
Points = new ReadOnlyCollection<SmoothedPathPoint>(copy);
|
|
StartBoundary = startBoundary;
|
|
EndBoundary = endBoundary;
|
|
SourceStartArcLength = sourceStartArcLength;
|
|
}
|
|
|
|
public int SegmentIndex { get; }
|
|
|
|
public TravelDirection Direction { get; }
|
|
|
|
public IReadOnlyList<SmoothedPathPoint> Points { get; }
|
|
|
|
public ReferenceBoundary StartBoundary { get; }
|
|
|
|
public ReferenceBoundary EndBoundary { get; }
|
|
|
|
public double SourceStartArcLength { get; }
|
|
|
|
public double LengthMeters { get { return EndBoundary.SegmentLocalS; } }
|
|
}
|