427 lines
16 KiB
C#
427 lines
16 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
|
|
|
/// <summary>在方向段内以局部三次 Bézier 连接替换明显转角。</summary>
|
|
internal sealed class LocalCubicBezierSmoother : IPathSmoother
|
|
{
|
|
private const double WindowToleranceMeters = 1e-9d;
|
|
|
|
/// <inheritdoc />
|
|
public SmoothingMethod Method => SmoothingMethod.LocalCubicBezier;
|
|
|
|
/// <inheritdoc />
|
|
public SmoothingCandidate Smooth(
|
|
SmoothingAlgorithmInput input,
|
|
double effectiveStrength,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (input == null || input.OriginalPath == null || input.Options == null ||
|
|
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
|
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
|
input.MinimumClearanceReserveMeters < 0d)
|
|
{
|
|
return SmoothingCandidate.Failed("Bézier 输入、强度或净空预留无效。");
|
|
}
|
|
|
|
var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
|
|
for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
|
if (!TrySmoothSegment(
|
|
sourceSegment,
|
|
input.Options.BezierCornerHeadingThresholdRadians,
|
|
input.Options.BezierMaximumWindowLengthMeters,
|
|
input.Options.BezierHandleLengthRatio,
|
|
effectiveStrength,
|
|
input.MinimumClearanceReserveMeters,
|
|
cancellationToken,
|
|
out IReadOnlyList<SmoothingPoint2D> points,
|
|
out string reason,
|
|
out SmoothingCandidateStatus status))
|
|
{
|
|
return status == SmoothingCandidateStatus.RetryableInfeasible
|
|
? SmoothingCandidate.RetryableInfeasible(reason)
|
|
: SmoothingCandidate.Failed(reason);
|
|
}
|
|
|
|
candidateSegments.Add(new PreparedDirectionSegment(
|
|
sourceSegment.SegmentIndex,
|
|
sourceSegment.Direction,
|
|
points,
|
|
sourceSegment.StartsAtGearSwitch,
|
|
sourceSegment.EndsAtGearSwitch));
|
|
}
|
|
|
|
return SmoothingCandidate.Success(candidateSegments);
|
|
}
|
|
|
|
private static bool TrySmoothSegment(
|
|
PreparedDirectionSegment sourceSegment,
|
|
double cornerThresholdRadians,
|
|
double maximumWindowLengthMeters,
|
|
double handleLengthRatio,
|
|
double strength,
|
|
double reserveMeters,
|
|
CancellationToken cancellationToken,
|
|
out IReadOnlyList<SmoothingPoint2D> result,
|
|
out string reason,
|
|
out SmoothingCandidateStatus status)
|
|
{
|
|
result = null;
|
|
reason = string.Empty;
|
|
status = SmoothingCandidateStatus.Failed;
|
|
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
|
|
{
|
|
reason = "Bézier 方向段为空。";
|
|
return false;
|
|
}
|
|
|
|
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
|
if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
|
|
if (anchors.Count < 3)
|
|
{
|
|
result = anchors;
|
|
return true;
|
|
}
|
|
|
|
if (!TryCreateMergedWindows(
|
|
anchors,
|
|
cornerThresholdRadians,
|
|
maximumWindowLengthMeters,
|
|
cancellationToken,
|
|
out List<Window> windows,
|
|
out reason))
|
|
{
|
|
return false;
|
|
}
|
|
if (windows.Count == 0)
|
|
{
|
|
result = anchors;
|
|
return true;
|
|
}
|
|
|
|
var output = new List<SmoothingPoint2D>(anchors.Count);
|
|
int anchorIndex = 0;
|
|
for (int windowIndex = 0; windowIndex < windows.Count; windowIndex++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
Window window = windows[windowIndex];
|
|
while (anchorIndex <= window.StartIndex)
|
|
{
|
|
output.Add(anchors[anchorIndex]);
|
|
anchorIndex++;
|
|
}
|
|
|
|
if (!TryAppendWindowInterior(
|
|
anchors,
|
|
window,
|
|
handleLengthRatio,
|
|
strength,
|
|
reserveMeters,
|
|
output,
|
|
cancellationToken,
|
|
out reason,
|
|
out status))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
output.Add(anchors[window.EndIndex]);
|
|
anchorIndex = window.EndIndex + 1;
|
|
}
|
|
|
|
while (anchorIndex < anchors.Count)
|
|
{
|
|
output.Add(anchors[anchorIndex]);
|
|
anchorIndex++;
|
|
}
|
|
|
|
result = output;
|
|
return true;
|
|
}
|
|
|
|
private static bool ValidateAnchors(
|
|
IReadOnlyList<SmoothingPoint2D> anchors,
|
|
CancellationToken cancellationToken,
|
|
out string reason)
|
|
{
|
|
reason = string.Empty;
|
|
for (int index = 0; index < anchors.Count; index++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
SmoothingPoint2D point = anchors[index];
|
|
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
|
!NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
|
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
|
|
!NumericGuard.IsFinite(point.BodyClearance) || point.BodyClearance < 0d ||
|
|
(index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
|
|
{
|
|
reason = "Bézier 方向段包含非有限、非递增弧长或无效净空的锚点。";
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static bool TryCreateMergedWindows(
|
|
IReadOnlyList<SmoothingPoint2D> anchors,
|
|
double cornerThresholdRadians,
|
|
double maximumWindowLengthMeters,
|
|
CancellationToken cancellationToken,
|
|
out List<Window> windows,
|
|
out string reason)
|
|
{
|
|
windows = new List<Window>();
|
|
var candidates = new List<Window>();
|
|
reason = string.Empty;
|
|
for (int cornerIndex = 1; cornerIndex < anchors.Count - 1; cornerIndex++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
if (!TryGetTravelTangent(anchors[cornerIndex - 1], anchors[cornerIndex], out Point2D entryTangent) ||
|
|
!TryGetTravelTangent(anchors[cornerIndex], anchors[cornerIndex + 1], out Point2D exitTangent))
|
|
{
|
|
reason = "Bézier 转角包含零长度或非有限行进切向。";
|
|
return false;
|
|
}
|
|
|
|
double cross = entryTangent.X * exitTangent.Y - entryTangent.Y * exitTangent.X;
|
|
double dot = entryTangent.X * exitTangent.X + entryTangent.Y * exitTangent.Y;
|
|
double turnRadians = Math.Atan2(Math.Abs(cross), dot);
|
|
if (!NumericGuard.IsFinite(turnRadians))
|
|
{
|
|
reason = "Bézier 转角计算产生非有限数值。";
|
|
return false;
|
|
}
|
|
if (turnRadians < cornerThresholdRadians) continue;
|
|
|
|
int startIndex = cornerIndex - 1;
|
|
int endIndex = cornerIndex + 1;
|
|
double windowLength = anchors[endIndex].ArcLength - anchors[startIndex].ArcLength;
|
|
if (!NumericGuard.IsPositiveFinite(windowLength))
|
|
{
|
|
reason = "Bézier 局部窗口弧长无效。";
|
|
return false;
|
|
}
|
|
if (windowLength > maximumWindowLengthMeters + WindowToleranceMeters) continue;
|
|
if (ContainsGearSwitch(anchors, startIndex, endIndex)) continue;
|
|
|
|
candidates.Add(new Window(startIndex, endIndex));
|
|
}
|
|
|
|
MergeBoundedConnectedWindows(anchors, candidates, maximumWindowLengthMeters, windows);
|
|
return true;
|
|
}
|
|
|
|
private static void MergeBoundedConnectedWindows(
|
|
IReadOnlyList<SmoothingPoint2D> anchors,
|
|
IReadOnlyList<Window> candidates,
|
|
double maximumWindowLengthMeters,
|
|
List<Window> windows)
|
|
{
|
|
int candidateIndex = 0;
|
|
while (candidateIndex < candidates.Count)
|
|
{
|
|
Window merged = candidates[candidateIndex];
|
|
candidateIndex++;
|
|
while (candidateIndex < candidates.Count &&
|
|
candidates[candidateIndex].StartIndex <= merged.EndIndex + 1)
|
|
{
|
|
merged = new Window(merged.StartIndex,
|
|
Math.Max(merged.EndIndex, candidates[candidateIndex].EndIndex));
|
|
candidateIndex++;
|
|
}
|
|
|
|
double mergedLength = anchors[merged.EndIndex].ArcLength - anchors[merged.StartIndex].ArcLength;
|
|
if (mergedLength <= maximumWindowLengthMeters + WindowToleranceMeters)
|
|
{
|
|
windows.Add(merged);
|
|
}
|
|
// A connected group that exceeds the cap is declined as a whole. Splitting it into
|
|
// adjacent local curves would introduce unrequested joins; accepting it would violate
|
|
// the maximum-window contract. Its original anchors therefore remain unchanged.
|
|
}
|
|
}
|
|
|
|
private static bool TryAppendWindowInterior(
|
|
IReadOnlyList<SmoothingPoint2D> anchors,
|
|
Window window,
|
|
double handleLengthRatio,
|
|
double strength,
|
|
double reserveMeters,
|
|
List<SmoothingPoint2D> output,
|
|
CancellationToken cancellationToken,
|
|
out string reason,
|
|
out SmoothingCandidateStatus status)
|
|
{
|
|
reason = string.Empty;
|
|
status = SmoothingCandidateStatus.Failed;
|
|
SmoothingPoint2D p0 = anchors[window.StartIndex];
|
|
SmoothingPoint2D p3 = anchors[window.EndIndex];
|
|
if (!TryGetTravelTangent(p0, anchors[window.StartIndex + 1], out Point2D entryTangent) ||
|
|
!TryGetTravelTangent(anchors[window.EndIndex - 1], p3, out Point2D exitTangent))
|
|
{
|
|
reason = "Bézier 窗口端点包含无效行进切向。";
|
|
return false;
|
|
}
|
|
|
|
double arcLength = p3.ArcLength - p0.ArcLength;
|
|
double chordLength = Distance(p0, p3);
|
|
double handleLength = chordLength * handleLengthRatio * strength;
|
|
if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(chordLength) ||
|
|
!NumericGuard.IsPositiveFinite(handleLength))
|
|
{
|
|
reason = "Bézier 窗口弧长、端点弦长或控制柄长度无效。";
|
|
return false;
|
|
}
|
|
|
|
Point2D control1 = new Point2D(
|
|
p0.X + entryTangent.X * handleLength,
|
|
p0.Y + entryTangent.Y * handleLength);
|
|
Point2D control2 = new Point2D(
|
|
p3.X - exitTangent.X * handleLength,
|
|
p3.Y - exitTangent.Y * handleLength);
|
|
if (!IsFinite(control1) || !IsFinite(control2))
|
|
{
|
|
reason = "Bézier 控制点产生非有限数值。";
|
|
return false;
|
|
}
|
|
|
|
for (int index = window.StartIndex + 1; index < window.EndIndex; index++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
SmoothingPoint2D anchor = anchors[index];
|
|
double parameter = (anchor.ArcLength - p0.ArcLength) / arcLength;
|
|
if (!NumericGuard.IsFinite(parameter) || parameter <= 0d || parameter >= 1d)
|
|
{
|
|
reason = "Bézier 窗口参数无效。";
|
|
return false;
|
|
}
|
|
|
|
Point2D evaluated = Evaluate(p0, control1, control2, p3, parameter);
|
|
if (!IsFinite(evaluated))
|
|
{
|
|
reason = "Bézier 评估产生非有限数值。";
|
|
return false;
|
|
}
|
|
|
|
double referenceArcLength = p0.ArcLength + parameter * arcLength;
|
|
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
|
|
anchors,
|
|
referenceArcLength,
|
|
out SmoothingPoint2D reference,
|
|
out reason))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double displacement = Distance(evaluated, reference);
|
|
if (!NumericGuard.IsFinite(displacement))
|
|
{
|
|
reason = "Bézier 评估点位移产生非有限数值。";
|
|
return false;
|
|
}
|
|
double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
|
|
if (displacement > allowedDisplacement)
|
|
{
|
|
reason = "Bézier 评估点超过对应原始弧长参考点的允许移动范围。";
|
|
status = SmoothingCandidateStatus.RetryableInfeasible;
|
|
return false;
|
|
}
|
|
|
|
output.Add(new SmoothingPoint2D(
|
|
evaluated.X,
|
|
evaluated.Y,
|
|
reference.ArcLength,
|
|
reference.Heading,
|
|
reference.UnwrappedHeading,
|
|
reference.BodyClearance,
|
|
false,
|
|
SmoothedPathPointSource.Interpolated));
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static bool ContainsGearSwitch(IReadOnlyList<SmoothingPoint2D> anchors, int startIndex, int endIndex)
|
|
{
|
|
for (int index = startIndex; index <= endIndex; index++)
|
|
{
|
|
if (anchors[index].IsGearSwitchPoint) return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
private static bool TryGetTravelTangent(SmoothingPoint2D start, SmoothingPoint2D end, out Point2D tangent)
|
|
{
|
|
tangent = default;
|
|
double deltaX = end.X - start.X;
|
|
double deltaY = end.Y - start.Y;
|
|
double length = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
|
if (!NumericGuard.IsPositiveFinite(length)) return false;
|
|
tangent = new Point2D(deltaX / length, deltaY / length);
|
|
return IsFinite(tangent);
|
|
}
|
|
|
|
private static Point2D Evaluate(SmoothingPoint2D p0, Point2D p1, Point2D p2, SmoothingPoint2D p3, double parameter)
|
|
{
|
|
double oneMinusParameter = 1d - parameter;
|
|
double p0Weight = oneMinusParameter * oneMinusParameter * oneMinusParameter;
|
|
double p1Weight = 3d * oneMinusParameter * oneMinusParameter * parameter;
|
|
double p2Weight = 3d * oneMinusParameter * parameter * parameter;
|
|
double p3Weight = parameter * parameter * parameter;
|
|
return new Point2D(
|
|
p0Weight * p0.X + p1Weight * p1.X + p2Weight * p2.X + p3Weight * p3.X,
|
|
p0Weight * p0.Y + p1Weight * p1.Y + p2Weight * p2.Y + p3Weight * p3.Y);
|
|
}
|
|
|
|
private static bool IsFinite(Point2D point)
|
|
{
|
|
return NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y);
|
|
}
|
|
|
|
private static double Distance(Point2D point, SmoothingPoint2D reference)
|
|
{
|
|
double deltaX = point.X - reference.X;
|
|
double deltaY = point.Y - reference.Y;
|
|
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
|
}
|
|
|
|
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
|
{
|
|
double deltaX = left.X - right.X;
|
|
double deltaY = left.Y - right.Y;
|
|
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
|
}
|
|
|
|
private readonly struct Window
|
|
{
|
|
internal Window(int startIndex, int endIndex)
|
|
{
|
|
StartIndex = startIndex;
|
|
EndIndex = endIndex;
|
|
}
|
|
|
|
internal int StartIndex { get; }
|
|
|
|
internal int EndIndex { get; }
|
|
}
|
|
|
|
private readonly struct Point2D
|
|
{
|
|
internal Point2D(double x, double y)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
}
|
|
|
|
internal double X { get; }
|
|
|
|
internal double Y { get; }
|
|
}
|
|
}
|