413 lines
17 KiB
C#
413 lines
17 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
|
|
|
/// <summary>按方向段独立生成夹持三次 B 样条原始几何候选。</summary>
|
|
internal sealed class CubicBSplineSmoother : IPathSmoother
|
|
{
|
|
private const int Degree = 3;
|
|
private const int SamplesPerSpan = 64;
|
|
private const double StraightToleranceMeters = 1e-9d;
|
|
private const double EndpointProbeParameter = 1e-6d;
|
|
private const double MinimumTangentHandleLengthMeters = 1e-10d;
|
|
|
|
/// <inheritdoc />
|
|
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
|
|
|
|
/// <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 样条输入、强度或净空预留无效。");
|
|
}
|
|
|
|
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,
|
|
effectiveStrength,
|
|
input.MinimumClearanceReserveMeters,
|
|
input.Options.CubicBSplineEndpointTangentScale,
|
|
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,
|
|
sourceSegment.StartVehicleCurvaturePerMeter));
|
|
}
|
|
|
|
return SmoothingCandidate.Success(candidateSegments);
|
|
}
|
|
|
|
private static bool TrySmoothSegment(
|
|
PreparedDirectionSegment sourceSegment,
|
|
double strength,
|
|
double reserveMeters,
|
|
double endpointTangentScale,
|
|
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 样条方向段为空。";
|
|
return false;
|
|
}
|
|
|
|
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
|
for (int index = 0; index < anchors.Count; index++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
if (!IsValidAnchor(anchors[index]))
|
|
{
|
|
reason = "B 样条方向段包含非法锚点。";
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (anchors.Count <= Degree || IsStraight(anchors))
|
|
{
|
|
result = anchors;
|
|
return true;
|
|
}
|
|
|
|
if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, endpointTangentScale,
|
|
cancellationToken, out Point2D[] controls, out reason))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double[] knots = CreateClampedKnots(controls.Length);
|
|
var sampled = new List<SmoothingPoint2D>();
|
|
int spanCount = controls.Length - Degree;
|
|
int uniformIntervals = spanCount * SamplesPerSpan;
|
|
if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
|
|
return false;
|
|
if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
|
sampled, cancellationToken, out reason, out status))
|
|
return false;
|
|
for (int index = 1; index < uniformIntervals; index++)
|
|
{
|
|
if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters,
|
|
sampled, cancellationToken, out reason, out status))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
|
sampled, cancellationToken, out reason, out status))
|
|
return false;
|
|
if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
|
|
return false;
|
|
|
|
result = sampled;
|
|
return true;
|
|
}
|
|
|
|
private static bool TryCreateControls(
|
|
IReadOnlyList<SmoothingPoint2D> anchors,
|
|
TravelDirection direction,
|
|
double strength,
|
|
double reserveMeters,
|
|
double endpointTangentScale,
|
|
CancellationToken cancellationToken,
|
|
out Point2D[] controls,
|
|
out string reason)
|
|
{
|
|
reason = string.Empty;
|
|
controls = new Point2D[anchors.Count];
|
|
controls[0] = Point2D.FromAnchor(anchors[0]);
|
|
controls[controls.Length - 1] = Point2D.FromAnchor(anchors[anchors.Count - 1]);
|
|
|
|
double startHandleLength = Distance(anchors[0], anchors[1]) * endpointTangentScale * strength;
|
|
double startTravelHeading = GetTravelHeading(anchors[0], direction);
|
|
if (!TryConstrainTangentHandle(
|
|
Point2D.FromAnchor(anchors[0]),
|
|
Math.Cos(startTravelHeading),
|
|
Math.Sin(startTravelHeading),
|
|
startHandleLength,
|
|
anchors[1],
|
|
GetAllowedRadius(anchors[1], reserveMeters),
|
|
out controls[1]))
|
|
{
|
|
reason = "B 样条起点切向手柄无法同时满足相邻锚点移动范围。";
|
|
return false;
|
|
}
|
|
|
|
int finalIndex = anchors.Count - 1;
|
|
double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * endpointTangentScale * strength;
|
|
double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
|
|
if (!TryConstrainTangentHandle(
|
|
Point2D.FromAnchor(anchors[finalIndex]),
|
|
-Math.Cos(endTravelHeading),
|
|
-Math.Sin(endTravelHeading),
|
|
endHandleLength,
|
|
anchors[finalIndex - 1],
|
|
GetAllowedRadius(anchors[finalIndex - 1], reserveMeters),
|
|
out controls[finalIndex - 1]))
|
|
{
|
|
reason = "B 样条终点切向手柄无法同时满足相邻锚点移动范围。";
|
|
return false;
|
|
}
|
|
|
|
for (int index = 2; index < finalIndex - 1; index++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
SmoothingPoint2D previous = anchors[index - 1];
|
|
SmoothingPoint2D current = anchors[index];
|
|
SmoothingPoint2D next = anchors[index + 1];
|
|
Point2D target = new Point2D(
|
|
(previous.X + current.X + next.X) / 3d,
|
|
(previous.Y + current.Y + next.Y) / 3d);
|
|
Point2D proposed = new Point2D(
|
|
current.X + strength * (target.X - current.X),
|
|
current.Y + strength * (target.Y - current.Y));
|
|
controls[index] = ClampDisplacement(current, proposed, GetAllowedRadius(current, reserveMeters));
|
|
}
|
|
|
|
for (int index = 0; index < controls.Length; index++)
|
|
{
|
|
if (!NumericGuard.IsFinite(controls[index].X) || !NumericGuard.IsFinite(controls[index].Y))
|
|
{
|
|
reason = "B 样条控制点构造产生非法数值。";
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static bool TryAddSample(
|
|
double parameter,
|
|
IReadOnlyList<SmoothingPoint2D> anchors,
|
|
Point2D[] controls,
|
|
double[] knots,
|
|
double reserveMeters,
|
|
List<SmoothingPoint2D> output,
|
|
CancellationToken cancellationToken,
|
|
out string reason,
|
|
out SmoothingCandidateStatus status)
|
|
{
|
|
reason = string.Empty;
|
|
status = SmoothingCandidateStatus.Failed;
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
Point2D evaluated = Evaluate(controls, knots, parameter);
|
|
if (!NumericGuard.IsFinite(evaluated.X) || !NumericGuard.IsFinite(evaluated.Y))
|
|
{
|
|
reason = "B 样条评估产生非法数值。";
|
|
return false;
|
|
}
|
|
|
|
double targetArcLength = parameter * anchors[anchors.Count - 1].ArcLength;
|
|
if (!PathReferenceInterpolator.TryInterpolateByArcLength(anchors, targetArcLength,
|
|
out SmoothingPoint2D reference, out reason))
|
|
{
|
|
return false;
|
|
}
|
|
double displacement = Distance(evaluated, reference);
|
|
if (!NumericGuard.IsFinite(displacement))
|
|
{
|
|
reason = "B 样条评估点位移产生非法数值。";
|
|
return false;
|
|
}
|
|
if (displacement > GetAllowedRadius(reference, reserveMeters))
|
|
{
|
|
reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
|
|
status = SmoothingCandidateStatus.RetryableInfeasible;
|
|
return false;
|
|
}
|
|
bool endpoint = parameter == 0d || parameter == 1d;
|
|
output.Add(new SmoothingPoint2D(
|
|
evaluated.X,
|
|
evaluated.Y,
|
|
reference.ArcLength,
|
|
reference.Heading,
|
|
reference.UnwrappedHeading,
|
|
reference.BodyClearance,
|
|
endpoint && reference.IsGearSwitchPoint,
|
|
endpoint ? reference.Source : SmoothedPathPointSource.Interpolated));
|
|
return true;
|
|
}
|
|
|
|
private static bool TryConstrainTangentHandle(
|
|
Point2D endpoint,
|
|
double rayDirectionX,
|
|
double rayDirectionY,
|
|
double desiredLength,
|
|
SmoothingPoint2D adjacentAnchor,
|
|
double allowedRadius,
|
|
out Point2D control)
|
|
{
|
|
control = default;
|
|
double offsetX = adjacentAnchor.X - endpoint.X;
|
|
double offsetY = adjacentAnchor.Y - endpoint.Y;
|
|
double projectedLength = offsetX * rayDirectionX + offsetY * rayDirectionY;
|
|
double perpendicularX = offsetX - projectedLength * rayDirectionX;
|
|
double perpendicularY = offsetY - projectedLength * rayDirectionY;
|
|
double discriminant = allowedRadius * allowedRadius -
|
|
(perpendicularX * perpendicularX + perpendicularY * perpendicularY);
|
|
if (!NumericGuard.IsFinite(discriminant) || discriminant < 0d) return false;
|
|
|
|
double halfInterval = Math.Sqrt(discriminant);
|
|
double minimumLength = Math.Max(MinimumTangentHandleLengthMeters, projectedLength - halfInterval);
|
|
double maximumLength = projectedLength + halfInterval;
|
|
if (!NumericGuard.IsFinite(maximumLength) || maximumLength < minimumLength) return false;
|
|
|
|
double constrainedLength = Math.Max(minimumLength, Math.Min(desiredLength, maximumLength));
|
|
control = new Point2D(
|
|
endpoint.X + constrainedLength * rayDirectionX,
|
|
endpoint.Y + constrainedLength * rayDirectionY);
|
|
return NumericGuard.IsFinite(control.X) && NumericGuard.IsFinite(control.Y);
|
|
}
|
|
|
|
private static Point2D Evaluate(Point2D[] controls, double[] knots, double parameter)
|
|
{
|
|
if (parameter <= 0d) return controls[0];
|
|
if (parameter >= 1d) return controls[controls.Length - 1];
|
|
|
|
var point = new Point2D(0d, 0d);
|
|
for (int index = 0; index < controls.Length; index++)
|
|
{
|
|
double basis = EvaluateBasis(index, Degree, parameter, knots);
|
|
point = new Point2D(point.X + basis * controls[index].X, point.Y + basis * controls[index].Y);
|
|
}
|
|
return point;
|
|
}
|
|
|
|
private static double EvaluateBasis(int index, int degree, double parameter, double[] knots)
|
|
{
|
|
if (degree == 0)
|
|
return knots[index] <= parameter && parameter < knots[index + 1] ? 1d : 0d;
|
|
|
|
double left = 0d;
|
|
double leftDenominator = knots[index + degree] - knots[index];
|
|
if (leftDenominator > 0d)
|
|
left = (parameter - knots[index]) / leftDenominator * EvaluateBasis(index, degree - 1, parameter, knots);
|
|
|
|
double right = 0d;
|
|
double rightDenominator = knots[index + degree + 1] - knots[index + 1];
|
|
if (rightDenominator > 0d)
|
|
right = (knots[index + degree + 1] - parameter) / rightDenominator *
|
|
EvaluateBasis(index + 1, degree - 1, parameter, knots);
|
|
return left + right;
|
|
}
|
|
|
|
private static double[] CreateClampedKnots(int controlCount)
|
|
{
|
|
var knots = new double[controlCount + Degree + 1];
|
|
for (int index = Degree + 1; index < controlCount; index++)
|
|
knots[index] = (double)(index - Degree) / (controlCount - Degree);
|
|
for (int index = controlCount; index < knots.Length; index++) knots[index] = 1d;
|
|
return knots;
|
|
}
|
|
|
|
private static Point2D ClampDisplacement(SmoothingPoint2D anchor, Point2D proposed, double allowedRadius)
|
|
{
|
|
double deltaX = proposed.X - anchor.X;
|
|
double deltaY = proposed.Y - anchor.Y;
|
|
double distance = Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
|
if (!NumericGuard.IsFinite(distance) || distance <= allowedRadius) return proposed;
|
|
if (distance == 0d || allowedRadius == 0d) return Point2D.FromAnchor(anchor);
|
|
double scale = allowedRadius / distance;
|
|
return new Point2D(anchor.X + deltaX * scale, anchor.Y + deltaY * scale);
|
|
}
|
|
|
|
private static bool IsStraight(IReadOnlyList<SmoothingPoint2D> anchors)
|
|
{
|
|
if (anchors.Count < 3) return true;
|
|
SmoothingPoint2D first = anchors[0];
|
|
SmoothingPoint2D last = anchors[anchors.Count - 1];
|
|
double directionX = last.X - first.X;
|
|
double directionY = last.Y - first.Y;
|
|
double length = Math.Sqrt(directionX * directionX + directionY * directionY);
|
|
if (!NumericGuard.IsFinite(length) || length <= StraightToleranceMeters) return false;
|
|
for (int index = 1; index < anchors.Count - 1; index++)
|
|
{
|
|
double offsetX = anchors[index].X - first.X;
|
|
double offsetY = anchors[index].Y - first.Y;
|
|
double perpendicularDeviation = Math.Abs(directionX * offsetY - directionY * offsetX) / length;
|
|
if (perpendicularDeviation >= StraightToleranceMeters) return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static bool IsValidAnchor(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.BodyClearance >= 0d;
|
|
}
|
|
|
|
private static double GetAllowedRadius(SmoothingPoint2D anchor, double reserveMeters)
|
|
{
|
|
return Math.Max(0d, anchor.BodyClearance - reserveMeters);
|
|
}
|
|
|
|
private static double GetTravelHeading(SmoothingPoint2D point, TravelDirection direction)
|
|
{
|
|
return direction == TravelDirection.Forward ? point.Heading : point.Heading - Math.PI;
|
|
}
|
|
|
|
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
|
{
|
|
double deltaX = right.X - left.X;
|
|
double deltaY = right.Y - left.Y;
|
|
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
|
}
|
|
|
|
private static double Distance(Point2D left, SmoothingPoint2D right)
|
|
{
|
|
double deltaX = right.X - left.X;
|
|
double deltaY = right.Y - left.Y;
|
|
return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
|
|
}
|
|
|
|
private readonly struct Point2D
|
|
{
|
|
internal Point2D(double x, double y)
|
|
{
|
|
X = x;
|
|
Y = y;
|
|
}
|
|
|
|
internal double X { get; }
|
|
internal double Y { get; }
|
|
|
|
internal static Point2D FromAnchor(SmoothingPoint2D anchor)
|
|
{
|
|
return new Point2D(anchor.X, anchor.Y);
|
|
}
|
|
}
|
|
}
|