92 lines
3.2 KiB
C#
92 lines
3.2 KiB
C#
using System;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.Trajectory
|
|
{
|
|
/// <summary>
|
|
/// 保存车体中心投影到二维参考轨迹后得到的只读结果。
|
|
/// </summary>
|
|
public readonly struct TrajectoryProjection
|
|
{
|
|
/// <summary>
|
|
/// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。
|
|
/// </summary>
|
|
public TrajectoryProjection(
|
|
int segmentStartIndex,
|
|
TrajectoryPoint referencePoint,
|
|
double lateralErrorMeters,
|
|
double headingErrorRadians,
|
|
double distanceToTrajectoryMeters,
|
|
double remainingDistanceMeters)
|
|
{
|
|
if (segmentStartIndex < 0)
|
|
{
|
|
throw new ArgumentOutOfRangeException(
|
|
nameof(segmentStartIndex),
|
|
"投影线段起点索引不能为负数。");
|
|
}
|
|
|
|
NumericGuard.EnsureFinite(
|
|
lateralErrorMeters,
|
|
nameof(lateralErrorMeters));
|
|
NumericGuard.EnsureFinite(
|
|
headingErrorRadians,
|
|
nameof(headingErrorRadians));
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
distanceToTrajectoryMeters,
|
|
nameof(distanceToTrajectoryMeters));
|
|
NumericGuard.EnsureFiniteNonNegative(
|
|
remainingDistanceMeters,
|
|
nameof(remainingDistanceMeters));
|
|
|
|
SegmentStartIndex = segmentStartIndex;
|
|
ReferencePoint = referencePoint;
|
|
LateralErrorMeters = lateralErrorMeters;
|
|
HeadingErrorRadians =
|
|
AngleMath.NormalizeRadians(
|
|
headingErrorRadians);
|
|
DistanceToTrajectoryMeters =
|
|
distanceToTrajectoryMeters;
|
|
RemainingDistanceMeters =
|
|
remainingDistanceMeters;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。
|
|
/// </summary>
|
|
public int SegmentStartIndex { get; }
|
|
|
|
/// <summary>
|
|
/// 获取投影位置插值得到的车体中心参考轨迹点。
|
|
/// </summary>
|
|
public TrajectoryPoint ReferencePoint { get; }
|
|
|
|
/// <summary>
|
|
/// 获取相对累计弧长增加方向的有符号横向误差，单位为m，参考轨迹位于该方向左侧时为正。
|
|
/// </summary>
|
|
public double LateralErrorMeters { get; }
|
|
|
|
/// <summary>
|
|
/// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
|
|
/// </summary>
|
|
public double HeadingErrorRadians { get; }
|
|
|
|
/// <summary>
|
|
/// 获取车体中心到投影点的欧氏距离，单位为m。
|
|
/// </summary>
|
|
public double DistanceToTrajectoryMeters { get; }
|
|
|
|
/// <summary>
|
|
/// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。
|
|
/// </summary>
|
|
public double RemainingDistanceMeters { get; }
|
|
|
|
/// <summary>
|
|
/// 获取投影位置从轨迹起点累计的弧长，单位为m。
|
|
/// </summary>
|
|
public double ArcLengthMeters =>
|
|
ReferencePoint.ArcLengthMeters;
|
|
|
|
}
|
|
}
|