85 lines
4.5 KiB
C#
85 lines
4.5 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public static class ReferencePathSegmenter
|
|
{
|
|
public static IReadOnlyList<DirectionSegmentView> Create(PathSmoothingResult referencePath)
|
|
{
|
|
if (referencePath == null || referencePath.Path == null || referencePath.Segments == null ||
|
|
referencePath.Path.Count == 0 || referencePath.Segments.Count == 0)
|
|
throw new ArgumentException("A published reference path with direction segments is required.", nameof(referencePath));
|
|
|
|
var result = new List<DirectionSegmentView>(referencePath.Segments.Count);
|
|
int expectedStart = 0;
|
|
for (int segmentListIndex = 0; segmentListIndex < referencePath.Segments.Count; segmentListIndex++)
|
|
{
|
|
SmoothedPathSegment sourceSegment = referencePath.Segments[segmentListIndex];
|
|
if (sourceSegment == null || sourceSegment.SegmentIndex != segmentListIndex ||
|
|
sourceSegment.StartIndex != expectedStart || sourceSegment.StartIndex < 0 ||
|
|
sourceSegment.EndIndex < sourceSegment.StartIndex || sourceSegment.EndIndex >= referencePath.Path.Count)
|
|
throw new ArgumentException("Reference path segment indexing is invalid.", nameof(referencePath));
|
|
|
|
SmoothedPathPoint firstSourcePoint = referencePath.Path[sourceSegment.StartIndex];
|
|
if (firstSourcePoint == null || firstSourcePoint.Direction != sourceSegment.Direction)
|
|
throw new ArgumentException("Reference path segment start is invalid.", nameof(referencePath));
|
|
|
|
double sourceStartS = firstSourcePoint.ArcLength;
|
|
var rebasedPoints = new List<SmoothedPathPoint>(sourceSegment.EndIndex - sourceSegment.StartIndex + 1);
|
|
double previousLocalS = -1d;
|
|
for (int pathIndex = sourceSegment.StartIndex; pathIndex <= sourceSegment.EndIndex; pathIndex++)
|
|
{
|
|
SmoothedPathPoint sourcePoint = referencePath.Path[pathIndex];
|
|
if (sourcePoint == null || sourcePoint.Direction != sourceSegment.Direction ||
|
|
double.IsNaN(sourcePoint.ArcLength) || double.IsInfinity(sourcePoint.ArcLength))
|
|
throw new ArgumentException("Reference path point is invalid.", nameof(referencePath));
|
|
|
|
double localS = sourcePoint.ArcLength - sourceStartS;
|
|
if (localS < 0d || (pathIndex > sourceSegment.StartIndex && localS <= previousLocalS))
|
|
throw new ArgumentException("Reference path segment arc length must strictly increase.", nameof(referencePath));
|
|
rebasedPoints.Add(CloneAtLocalS(sourcePoint, localS));
|
|
previousLocalS = localS;
|
|
}
|
|
|
|
EmBoundaryType startType = sourceSegment.StartsAtGearSwitch
|
|
? EmBoundaryType.GearSwitchDeparture
|
|
: EmBoundaryType.None;
|
|
EmBoundaryType endType = sourceSegment.EndsAtGearSwitch
|
|
? EmBoundaryType.GearSwitchApproach
|
|
: segmentListIndex == referencePath.Segments.Count - 1
|
|
? EmBoundaryType.Goal
|
|
: EmBoundaryType.None;
|
|
var startBoundary = new ReferenceBoundary(sourceSegment.SegmentIndex, 0d, startType, sourceStartS);
|
|
var endBoundary = new ReferenceBoundary(sourceSegment.SegmentIndex, previousLocalS, endType,
|
|
referencePath.Path[sourceSegment.EndIndex].ArcLength);
|
|
result.Add(new DirectionSegmentView(sourceSegment.SegmentIndex, sourceSegment.Direction, rebasedPoints,
|
|
startBoundary, endBoundary, sourceStartS));
|
|
expectedStart = sourceSegment.EndIndex + 1;
|
|
}
|
|
|
|
if (expectedStart != referencePath.Path.Count)
|
|
throw new ArgumentException("Reference path segments do not cover the full path.", nameof(referencePath));
|
|
return new ReadOnlyCollection<DirectionSegmentView>(result);
|
|
}
|
|
|
|
internal static SmoothedPathPoint CloneAtLocalS(SmoothedPathPoint source, double localS)
|
|
{
|
|
return new SmoothedPathPoint(
|
|
source.X,
|
|
source.Y,
|
|
source.Heading,
|
|
source.UnwrappedHeading,
|
|
localS,
|
|
source.Direction,
|
|
source.GeometricCurvature,
|
|
source.VehicleCurvature,
|
|
source.VehicleCurvatureDerivative,
|
|
source.BodyClearance,
|
|
source.IsGearSwitchPoint,
|
|
source.Source);
|
|
}
|
|
}
|