56 lines
2.4 KiB
C#
56 lines
2.4 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
|
|
|
/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
|
|
internal sealed class CurvatureTransition
|
|
{
|
|
internal CurvatureTransition(
|
|
int segmentIndex,
|
|
int leftCoarsePathIndex,
|
|
int rightCoarsePathIndex,
|
|
double localArcLengthMeters,
|
|
double x,
|
|
double y,
|
|
double vehicleHeadingRadians,
|
|
double leftVehicleCurvaturePerMeter,
|
|
double rightVehicleCurvaturePerMeter)
|
|
{
|
|
if (segmentIndex < 0 || leftCoarsePathIndex < 0 || rightCoarsePathIndex != leftCoarsePathIndex + 1)
|
|
throw new ArgumentOutOfRangeException(nameof(leftCoarsePathIndex));
|
|
if (!NumericGuard.IsFinite(localArcLengthMeters) || localArcLengthMeters < 0d ||
|
|
!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(vehicleHeadingRadians) ||
|
|
!NumericGuard.IsFinite(leftVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(rightVehicleCurvaturePerMeter))
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(localArcLengthMeters));
|
|
}
|
|
|
|
double curvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter);
|
|
if (!NumericGuard.IsFinite(curvatureJumpPerMeter))
|
|
throw new ArgumentOutOfRangeException(nameof(rightVehicleCurvaturePerMeter));
|
|
|
|
SegmentIndex = segmentIndex;
|
|
LeftCoarsePathIndex = leftCoarsePathIndex;
|
|
RightCoarsePathIndex = rightCoarsePathIndex;
|
|
LocalArcLengthMeters = localArcLengthMeters;
|
|
X = x;
|
|
Y = y;
|
|
VehicleHeadingRadians = vehicleHeadingRadians;
|
|
LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter;
|
|
RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter;
|
|
CurvatureJumpPerMeter = curvatureJumpPerMeter;
|
|
}
|
|
|
|
internal int SegmentIndex { get; }
|
|
internal int LeftCoarsePathIndex { get; }
|
|
internal int RightCoarsePathIndex { get; }
|
|
internal double LocalArcLengthMeters { get; }
|
|
internal double X { get; }
|
|
internal double Y { get; }
|
|
internal double VehicleHeadingRadians { get; }
|
|
internal double LeftVehicleCurvaturePerMeter { get; }
|
|
internal double RightVehicleCurvaturePerMeter { get; }
|
|
internal double CurvatureJumpPerMeter { get; }
|
|
}
|