66 lines
3.3 KiB
C#
66 lines
3.3 KiB
C#
using System;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
|
|
|
/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
|
|
internal sealed class CurvatureTransition
|
|
{
|
|
/// <summary>记录同一方向段内相邻粗路径采样点之间的车辆曲率跳变。</summary>
|
|
/// <param name="segmentIndex">所属方向段在预处理路径中的从零开始索引。</param>
|
|
/// <param name="leftCoarsePathIndex">跳变左侧粗路径采样点索引。</param>
|
|
/// <param name="rightCoarsePathIndex">跳变右侧粗路径采样点索引，必须等于左索引加一。</param>
|
|
/// <param name="localArcLengthMeters">跳变位置在方向段内的局部弧长，单位 m。</param>
|
|
/// <param name="x">跳变位置世界 X 坐标，单位 m。</param>
|
|
/// <param name="y">跳变位置世界 Y 坐标，单位 m。</param>
|
|
/// <param name="vehicleHeadingRadians">跳变位置的车辆航向，单位 rad。</param>
|
|
/// <param name="leftVehicleCurvaturePerMeter">左侧车辆曲率，单位 1/m。</param>
|
|
/// <param name="rightVehicleCurvaturePerMeter">右侧车辆曲率，单位 1/m。</param>
|
|
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; }
|
|
}
|