323 lines
13 KiB
C#
323 lines
13 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 EndpointTangentScale = 1d / 3d;
|
|
private const double EndpointProbeParameter = 1e-6d;
|
|
|
|
/// <inheritdoc />
|
|
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
|
|
|
|
/// <inheritdoc />
|
|
public SmoothingCandidate Smooth(
|
|
SmoothingAlgorithmInput input,
|
|
double effectiveStrength,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (input == null || input.OriginalPath == 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,
|
|
cancellationToken, out IReadOnlyList<SmoothingPoint2D> points, out string reason))
|
|
{
|
|
return 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 strength,
|
|
double reserveMeters,
|
|
CancellationToken cancellationToken,
|
|
out IReadOnlyList<SmoothingPoint2D> result,
|
|
out string reason)
|
|
{
|
|
result = null;
|
|
reason = string.Empty;
|
|
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;
|
|
}
|
|
|
|
Point2D[] controls = CreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
|
|
cancellationToken);
|
|
if (controls == null)
|
|
{
|
|
reason = "B 样条控制点构造产生非法数值。";
|
|
return false;
|
|
}
|
|
|
|
double[] knots = CreateClampedKnots(controls.Length);
|
|
var sampled = new List<SmoothingPoint2D>();
|
|
int spanCount = controls.Length - Degree;
|
|
int uniformIntervals = spanCount * SamplesPerSpan;
|
|
AddSample(0d, anchors, controls, knots, sampled, cancellationToken);
|
|
AddSample(EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken);
|
|
for (int index = 1; index < uniformIntervals; index++)
|
|
{
|
|
AddSample((double)index / uniformIntervals, anchors, controls, knots, sampled, cancellationToken);
|
|
}
|
|
AddSample(1d - EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken);
|
|
AddSample(1d, anchors, controls, knots, sampled, cancellationToken);
|
|
|
|
result = sampled;
|
|
return true;
|
|
}
|
|
|
|
private static Point2D[] CreateControls(
|
|
IReadOnlyList<SmoothingPoint2D> anchors,
|
|
TravelDirection direction,
|
|
double strength,
|
|
double reserveMeters,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
var 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);
|
|
Point2D startProposed = new Point2D(
|
|
anchors[0].X + startHandleLength * Math.Cos(startTravelHeading),
|
|
anchors[0].Y + startHandleLength * Math.Sin(startTravelHeading));
|
|
controls[1] = ClampDisplacement(anchors[1], startProposed, GetAllowedRadius(anchors[1], reserveMeters));
|
|
|
|
int finalIndex = anchors.Count - 1;
|
|
double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * EndpointTangentScale * strength;
|
|
double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
|
|
Point2D endProposed = new Point2D(
|
|
anchors[finalIndex].X - endHandleLength * Math.Cos(endTravelHeading),
|
|
anchors[finalIndex].Y - endHandleLength * Math.Sin(endTravelHeading));
|
|
controls[finalIndex - 1] = ClampDisplacement(
|
|
anchors[finalIndex - 1], endProposed, GetAllowedRadius(anchors[finalIndex - 1], reserveMeters));
|
|
|
|
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))
|
|
return null;
|
|
}
|
|
return controls;
|
|
}
|
|
|
|
private static void AddSample(
|
|
double parameter,
|
|
IReadOnlyList<SmoothingPoint2D> anchors,
|
|
Point2D[] controls,
|
|
double[] knots,
|
|
List<SmoothingPoint2D> output,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
Point2D evaluated = Evaluate(controls, knots, parameter);
|
|
SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter);
|
|
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));
|
|
}
|
|
|
|
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 SmoothingPoint2D InterpolateAnchor(IReadOnlyList<SmoothingPoint2D> anchors, double parameter)
|
|
{
|
|
if (parameter <= 0d) return anchors[0];
|
|
if (parameter >= 1d) return anchors[anchors.Count - 1];
|
|
|
|
double scaled = parameter * (anchors.Count - 1);
|
|
int leftIndex = (int)Math.Floor(scaled);
|
|
double ratio = scaled - leftIndex;
|
|
SmoothingPoint2D left = anchors[leftIndex];
|
|
SmoothingPoint2D right = anchors[leftIndex + 1];
|
|
return new SmoothingPoint2D(
|
|
left.X + ratio * (right.X - left.X),
|
|
left.Y + ratio * (right.Y - left.Y),
|
|
left.ArcLength + ratio * (right.ArcLength - left.ArcLength),
|
|
left.Heading + ratio * (right.Heading - left.Heading),
|
|
left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
|
|
left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
|
|
false,
|
|
SmoothedPathPointSource.Interpolated);
|
|
}
|
|
|
|
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 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);
|
|
}
|
|
}
|
|
}
|