103 lines
3.7 KiB
C#
103 lines
3.7 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
|
|
/// <summary>按方向段局部弧长插值平滑算法的原始路径参考。</summary>
|
|
internal static class PathReferenceInterpolator
|
|
{
|
|
internal static bool TryInterpolateByArcLength(
|
|
IReadOnlyList<SmoothingPoint2D> points,
|
|
double targetArcLength,
|
|
out SmoothingPoint2D reference,
|
|
out string reason)
|
|
{
|
|
reference = null;
|
|
reason = string.Empty;
|
|
if (points == null || points.Count == 0 || !NumericGuard.IsFinite(targetArcLength))
|
|
{
|
|
reason = "原始路径参考点或目标弧长无效。";
|
|
return false;
|
|
}
|
|
|
|
for (int index = 0; index < points.Count; index++)
|
|
{
|
|
SmoothingPoint2D point = points[index];
|
|
if (!IsValid(point) || (index > 0 && point.ArcLength < points[index - 1].ArcLength))
|
|
{
|
|
reason = "原始路径参考点包含非法数值或非递增弧长。";
|
|
return false;
|
|
}
|
|
}
|
|
|
|
SmoothingPoint2D first = points[0];
|
|
SmoothingPoint2D last = points[points.Count - 1];
|
|
if (targetArcLength < first.ArcLength || targetArcLength > last.ArcLength)
|
|
{
|
|
reason = "目标弧长不在原始方向段范围内。";
|
|
return false;
|
|
}
|
|
|
|
if (targetArcLength == first.ArcLength)
|
|
{
|
|
reference = first;
|
|
return true;
|
|
}
|
|
if (targetArcLength == last.ArcLength)
|
|
{
|
|
reference = last;
|
|
return true;
|
|
}
|
|
|
|
for (int rightIndex = 1; rightIndex < points.Count; rightIndex++)
|
|
{
|
|
SmoothingPoint2D left = points[rightIndex - 1];
|
|
SmoothingPoint2D right = points[rightIndex];
|
|
if (targetArcLength > right.ArcLength) continue;
|
|
if (targetArcLength == right.ArcLength)
|
|
{
|
|
reference = right;
|
|
return true;
|
|
}
|
|
|
|
double interval = right.ArcLength - left.ArcLength;
|
|
if (!NumericGuard.IsPositiveFinite(interval))
|
|
{
|
|
reason = "原始路径参考点包含无法插值的重复弧长。";
|
|
return false;
|
|
}
|
|
|
|
double ratio = (targetArcLength - left.ArcLength) / interval;
|
|
reference = new SmoothingPoint2D(
|
|
left.X + ratio * (right.X - left.X),
|
|
left.Y + ratio * (right.Y - left.Y),
|
|
targetArcLength,
|
|
left.Heading + ratio * (right.Heading - left.Heading),
|
|
left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
|
|
left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
|
|
false,
|
|
SmoothedPathPointSource.Interpolated);
|
|
if (!IsValid(reference))
|
|
{
|
|
reference = null;
|
|
reason = "原始路径参考插值产生非法数值。";
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
reason = "原始路径参考无法定位目标弧长。";
|
|
return false;
|
|
}
|
|
|
|
private static bool IsValid(SmoothingPoint2D point)
|
|
{
|
|
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
|
NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.Heading) &&
|
|
NumericGuard.IsFinite(point.UnwrappedHeading) && NumericGuard.IsFinite(point.BodyClearance) &&
|
|
point.ArcLength >= 0d && point.BodyClearance >= 0d;
|
|
}
|
|
}
|