45 lines
2.1 KiB
C#
45 lines
2.1 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
namespace EMPlannerVerificationHost;
|
|
|
|
internal static class EmFixtureFactory
|
|
{
|
|
public static PathSmoothingResult CreateGearPairReferencePath(double firstSegmentLength = 2d)
|
|
{
|
|
if (firstSegmentLength <= 0d)
|
|
throw new ArgumentOutOfRangeException(nameof(firstSegmentLength));
|
|
double midpoint = 0.5d * firstSegmentLength;
|
|
var points = new List<SmoothedPathPoint>
|
|
{
|
|
Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
|
Point(midpoint, midpoint, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
|
Point(firstSegmentLength, firstSegmentLength, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
|
Point(firstSegmentLength, firstSegmentLength, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
|
|
Point(midpoint, firstSegmentLength + midpoint, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
|
|
Point(0d, 2d * firstSegmentLength, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
|
|
};
|
|
var segments = new List<SmoothedPathSegment>
|
|
{
|
|
new SmoothedPathSegment(0, TravelDirection.Forward, 0, 2, false, true),
|
|
new SmoothedPathSegment(1, TravelDirection.Reverse, 3, 5, true, false),
|
|
};
|
|
var metrics = new PathQualityMetrics(true, 2d * firstSegmentLength, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
|
|
return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
|
|
new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
|
|
}
|
|
|
|
private static SmoothedPathPoint Point(
|
|
double x,
|
|
double arcLength,
|
|
TravelDirection direction,
|
|
bool isGearSwitchPoint,
|
|
SmoothedPathPointSource source)
|
|
{
|
|
return new SmoothedPathPoint(x, 0d, 0d, 0d, arcLength, direction, 0d, 0d, 0d, 1d,
|
|
isGearSwitchPoint, source);
|
|
}
|
|
}
|