47 lines
2.8 KiB
C#
47 lines
2.8 KiB
C#
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal static class SegmentationChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
var referencePath = EMPlannerVerificationHost.EmFixtureFactory.CreateGearPairReferencePath();
|
|
var segments = ReferencePathSegmenter.Create(referencePath);
|
|
|
|
EMPlannerVerificationHost.Verification.Equal(2, segments.Count, "segment count");
|
|
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
|
|
segments[0].EndBoundary.BoundaryType, "forward end boundary");
|
|
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchDeparture,
|
|
segments[1].StartBoundary.BoundaryType, "reverse start boundary");
|
|
EMPlannerVerificationHost.Verification.True(!segments[0].EndBoundary.Equals(segments[1].StartBoundary),
|
|
"gear-pair boundary identities differ");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[2].ArcLength, "forward source arc length");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(2d, referencePath.Path[3].ArcLength, "reverse source arc length");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0d, segments[1].Points[0].ArcLength, "rebased reverse start");
|
|
|
|
ReferenceHorizonSlice rolling = ReferenceHorizonSlicer.Slice(segments[0], 1.95d);
|
|
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.RollingSafetyStop,
|
|
rolling.TerminalBoundary.BoundaryType, "rolling terminal type");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.TerminalBoundary.SegmentLocalS,
|
|
"rolling terminal local s");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].ArcLength,
|
|
"rolling anchor path s");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1.95d, rolling.Points[rolling.Points.Count - 1].X,
|
|
"rolling anchor x");
|
|
|
|
ReferenceHorizonSlice gearSwitch = ReferenceHorizonSlicer.Slice(segments[0], 2.05d);
|
|
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach,
|
|
gearSwitch.TerminalBoundary.BoundaryType, "gear terminal type");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.TerminalBoundary.SegmentLocalS,
|
|
"gear terminal local s");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(2d, gearSwitch.Points[gearSwitch.Points.Count - 1].ArcLength,
|
|
"gear anchor path s");
|
|
for (int index = 0; index < gearSwitch.Points.Count; index++)
|
|
{
|
|
EMPlannerVerificationHost.Verification.Equal(segments[0].Direction, gearSwitch.Points[index].Direction,
|
|
"horizon point direction");
|
|
}
|
|
}
|
|
}
|