chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,412 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>单一平滑方法生成原始几何候选的内部契约。</summary>
|
||||
internal interface IPathSmoother
|
||||
{
|
||||
SmoothingMethod Method { get; }
|
||||
|
||||
SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken);
|
||||
}
|
||||
@@ -0,0 +1,427 @@
|
||||
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,
|
||||
sourceSegment.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
|
||||
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; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,557 @@
|
||||
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>按方向段局部弧长构造 C2 连续的分段五次 Hermite 原始几何候选。</summary>
|
||||
internal sealed class PiecewiseQuinticSmoother : IPathSmoother
|
||||
{
|
||||
private const int SamplesPerInterval = 8;
|
||||
private const double DoubleMachineEpsilon = 2.2204460492503131e-16d;
|
||||
private const double EndpointNormalizationUlps = 32d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.PiecewiseQuintic;
|
||||
|
||||
/// <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 ||
|
||||
!NumericGuard.IsPositiveFinite(input.Options.QuinticKnotSpacingMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(input.Options.QuinticMinimumKnotSpacingMeters) ||
|
||||
input.Options.QuinticKnotSpacingMeters < input.Options.QuinticMinimumKnotSpacingMeters)
|
||||
{
|
||||
return SmoothingCandidate.Failed("五次 Hermite 输入、强度、净空预留或结点间距无效。");
|
||||
}
|
||||
|
||||
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.QuinticKnotSpacingMeters,
|
||||
input.Options.QuinticMinimumKnotSpacingMeters,
|
||||
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 effectiveStrength,
|
||||
double reserveMeters,
|
||||
double knotSpacingMeters,
|
||||
double minimumKnotSpacingMeters,
|
||||
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 < 2)
|
||||
{
|
||||
reason = "五次 Hermite 方向段至少需要两个锚点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
||||
if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
|
||||
|
||||
if (!TryCreateKnots(
|
||||
anchors,
|
||||
sourceSegment.Direction,
|
||||
effectiveStrength,
|
||||
knotSpacingMeters,
|
||||
minimumKnotSpacingMeters,
|
||||
cancellationToken,
|
||||
out List<Knot> knots,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Each physical acceleration is blended once into its shared knot and then scaled by
|
||||
// the left/right local interval independently. Reusing this value is what makes the
|
||||
// curve C2 with respect to local arc length, even for nonuniform final intervals.
|
||||
if (!TryAssignSharedAccelerations(knots, out reason)) return false;
|
||||
if (!TryCreateIntervals(knots, out List<QuinticInterval> intervals, out reason)) return false;
|
||||
|
||||
var sampled = new List<SmoothingPoint2D>(1 + intervals.Count * SamplesPerInterval);
|
||||
for (int intervalIndex = 0; intervalIndex < intervals.Count; intervalIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
QuinticInterval interval = intervals[intervalIndex];
|
||||
int firstSample = intervalIndex == 0 ? 0 : 1;
|
||||
for (int sampleIndex = firstSample; sampleIndex <= SamplesPerInterval; sampleIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
double parameter = (double)sampleIndex / SamplesPerInterval;
|
||||
double referenceArcLength = interval.Start.ArcLength + parameter * interval.Length;
|
||||
if (!NumericGuard.IsFinite(referenceArcLength))
|
||||
{
|
||||
reason = "五次 Hermite 采样参考弧长无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
|
||||
anchors,
|
||||
referenceArcLength,
|
||||
out SmoothingPoint2D reference,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D evaluated;
|
||||
if (sampleIndex == 0)
|
||||
evaluated = interval.Start.Position;
|
||||
else if (sampleIndex == SamplesPerInterval)
|
||||
evaluated = interval.End.Position;
|
||||
else if (!interval.TryEvaluate(parameter, out evaluated, out Point2D derivative, out Point2D secondDerivative))
|
||||
{
|
||||
reason = "五次 Hermite 采样产生非有限位置或导数。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double displacement = Distance(evaluated, reference);
|
||||
if (!NumericGuard.IsFinite(displacement))
|
||||
{
|
||||
reason = "五次 Hermite 采样位移无效。";
|
||||
return false;
|
||||
}
|
||||
double allowedDisplacement = Math.Max(0d, reference.BodyClearance - reserveMeters);
|
||||
if (displacement > allowedDisplacement)
|
||||
{
|
||||
reason = "五次 Hermite 采样点超过对应局部弧长参考点的允许移动范围。";
|
||||
status = SmoothingCandidateStatus.RetryableInfeasible;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool firstEndpoint = intervalIndex == 0 && sampleIndex == 0;
|
||||
bool lastEndpoint = intervalIndex == intervals.Count - 1 && sampleIndex == SamplesPerInterval;
|
||||
if (firstEndpoint)
|
||||
{
|
||||
sampled.Add(anchors[0]);
|
||||
}
|
||||
else if (lastEndpoint)
|
||||
{
|
||||
sampled.Add(anchors[anchors.Count - 1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sampled.Add(new SmoothingPoint2D(
|
||||
evaluated.X,
|
||||
evaluated.Y,
|
||||
reference.ArcLength,
|
||||
reference.Heading,
|
||||
reference.UnwrappedHeading,
|
||||
reference.BodyClearance,
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
result = sampled;
|
||||
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) || !NumericGuard.IsFinite(point.Heading) ||
|
||||
!NumericGuard.IsFinite(point.UnwrappedHeading) || !NumericGuard.IsFinite(point.BodyClearance) ||
|
||||
point.ArcLength < 0d || point.BodyClearance < 0d ||
|
||||
(index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
|
||||
{
|
||||
reason = "五次 Hermite 方向段包含非有限、非递增弧长或无效净空的锚点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateKnots(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
TravelDirection direction,
|
||||
double effectiveStrength,
|
||||
double knotSpacingMeters,
|
||||
double minimumKnotSpacingMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out List<Knot> knots,
|
||||
out string reason)
|
||||
{
|
||||
knots = new List<Knot>();
|
||||
reason = string.Empty;
|
||||
double startArcLength = anchors[0].ArcLength;
|
||||
double endArcLength = anchors[anchors.Count - 1].ArcLength;
|
||||
double totalLength = endArcLength - startArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(totalLength) || totalLength < minimumKnotSpacingMeters)
|
||||
{
|
||||
reason = "五次 Hermite 方向段短于配置的最小结点间距。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryCreateKnot(anchors[0], direction, effectiveStrength, out Knot first))
|
||||
{
|
||||
reason = "五次 Hermite 起点结点或行进切向无效。";
|
||||
return false;
|
||||
}
|
||||
knots.Add(first);
|
||||
|
||||
double endpointTolerance = GetEndpointNormalizationTolerance(
|
||||
startArcLength,
|
||||
endArcLength,
|
||||
knotSpacingMeters);
|
||||
double previousArcLength = startArcLength;
|
||||
for (long knotOrdinal = 1L; ; knotOrdinal++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
double targetArcLength = startArcLength + knotOrdinal * knotSpacingMeters;
|
||||
if (!NumericGuard.IsFinite(targetArcLength))
|
||||
{
|
||||
reason = "五次 Hermite 内部结点弧长无效。";
|
||||
return false;
|
||||
}
|
||||
if (targetArcLength >= endArcLength - endpointTolerance) break;
|
||||
if (targetArcLength <= previousArcLength)
|
||||
{
|
||||
reason = "五次 Hermite 内部结点无法在浮点弧长尺度上保持递增。";
|
||||
return false;
|
||||
}
|
||||
if (!PathReferenceInterpolator.TryInterpolateByArcLength(
|
||||
anchors,
|
||||
targetArcLength,
|
||||
out SmoothingPoint2D reference,
|
||||
out reason) ||
|
||||
!TryCreateKnot(reference, direction, effectiveStrength, out Knot knot))
|
||||
{
|
||||
if (string.IsNullOrEmpty(reason)) reason = "五次 Hermite 内部结点或行进切向无效。";
|
||||
return false;
|
||||
}
|
||||
knots.Add(knot);
|
||||
previousArcLength = targetArcLength;
|
||||
}
|
||||
|
||||
if (!TryCreateKnot(anchors[anchors.Count - 1], direction, effectiveStrength, out Knot last))
|
||||
{
|
||||
reason = "五次 Hermite 终点结点或行进切向无效。";
|
||||
return false;
|
||||
}
|
||||
knots.Add(last);
|
||||
|
||||
for (int index = 1; index < knots.Count; index++)
|
||||
{
|
||||
double intervalLength = knots[index].ArcLength - knots[index - 1].ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(intervalLength) || intervalLength < minimumKnotSpacingMeters)
|
||||
{
|
||||
reason = "五次 Hermite 结点间隔无效或短于配置的最小间距。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double GetEndpointNormalizationTolerance(
|
||||
double startArcLength,
|
||||
double endArcLength,
|
||||
double knotSpacingMeters)
|
||||
{
|
||||
double magnitude = Math.Max(
|
||||
Math.Abs(startArcLength),
|
||||
Math.Max(Math.Abs(endArcLength), Math.Abs(knotSpacingMeters)));
|
||||
return EndpointNormalizationUlps * DoubleMachineEpsilon * magnitude;
|
||||
}
|
||||
|
||||
private static bool TryCreateKnot(
|
||||
SmoothingPoint2D reference,
|
||||
TravelDirection direction,
|
||||
double effectiveStrength,
|
||||
out Knot knot)
|
||||
{
|
||||
knot = default;
|
||||
if (reference == null || !NumericGuard.IsFinite(reference.X) || !NumericGuard.IsFinite(reference.Y) ||
|
||||
!NumericGuard.IsFinite(reference.ArcLength) || !NumericGuard.IsFinite(reference.Heading))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
double travelHeading = direction == TravelDirection.Forward
|
||||
? reference.Heading
|
||||
: reference.Heading - Math.PI;
|
||||
double tangentX = Math.Cos(travelHeading);
|
||||
double tangentY = Math.Sin(travelHeading);
|
||||
if (!NumericGuard.IsFinite(tangentX) || !NumericGuard.IsFinite(tangentY)) return false;
|
||||
|
||||
var velocity = new Point2D(tangentX * effectiveStrength, tangentY * effectiveStrength);
|
||||
if (!velocity.IsFinite) return false;
|
||||
knot = new Knot(reference.ArcLength, new Point2D(reference.X, reference.Y), velocity);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAssignSharedAccelerations(List<Knot> knots, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int index = 0; index < knots.Count; index++)
|
||||
{
|
||||
Point2D acceleration;
|
||||
if (index == 0)
|
||||
{
|
||||
if (!TryAcceleration(knots[0], knots[1], out acceleration))
|
||||
{
|
||||
reason = "五次 Hermite 起点加速度无效。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (index == knots.Count - 1)
|
||||
{
|
||||
if (!TryAcceleration(knots[index - 1], knots[index], out acceleration))
|
||||
{
|
||||
reason = "五次 Hermite 终点加速度无效。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (!TryAcceleration(knots[index - 1], knots[index], out Point2D left) ||
|
||||
!TryAcceleration(knots[index], knots[index + 1], out Point2D right))
|
||||
{
|
||||
reason = "五次 Hermite 共享结点加速度无效。";
|
||||
return false;
|
||||
}
|
||||
acceleration = new Point2D((left.X + right.X) / 2d, (left.Y + right.Y) / 2d);
|
||||
if (!acceleration.IsFinite)
|
||||
{
|
||||
reason = "五次 Hermite 共享结点加速度混合产生非有限数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
knots[index] = knots[index].WithAcceleration(acceleration);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAcceleration(Knot start, Knot end, out Point2D acceleration)
|
||||
{
|
||||
acceleration = default;
|
||||
double intervalLength = end.ArcLength - start.ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(intervalLength)) return false;
|
||||
acceleration = new Point2D(
|
||||
(end.Velocity.X - start.Velocity.X) / intervalLength,
|
||||
(end.Velocity.Y - start.Velocity.Y) / intervalLength);
|
||||
return acceleration.IsFinite;
|
||||
}
|
||||
|
||||
private static bool TryCreateIntervals(
|
||||
IReadOnlyList<Knot> knots,
|
||||
out List<QuinticInterval> intervals,
|
||||
out string reason)
|
||||
{
|
||||
intervals = new List<QuinticInterval>(knots.Count - 1);
|
||||
reason = string.Empty;
|
||||
for (int index = 1; index < knots.Count; index++)
|
||||
{
|
||||
if (!QuinticInterval.TryCreate(knots[index - 1], knots[index], out QuinticInterval interval))
|
||||
{
|
||||
reason = "五次 Hermite 系数、端点导数或结点区间无效。";
|
||||
return false;
|
||||
}
|
||||
intervals.Add(interval);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
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 readonly struct Knot
|
||||
{
|
||||
internal Knot(double arcLength, Point2D position, Point2D velocity)
|
||||
{
|
||||
ArcLength = arcLength;
|
||||
Position = position;
|
||||
Velocity = velocity;
|
||||
Acceleration = default;
|
||||
}
|
||||
|
||||
internal double ArcLength { get; }
|
||||
|
||||
internal Point2D Position { get; }
|
||||
|
||||
internal Point2D Velocity { get; }
|
||||
|
||||
internal Point2D Acceleration { get; }
|
||||
|
||||
internal Knot WithAcceleration(Point2D acceleration)
|
||||
{
|
||||
return new Knot(ArcLength, Position, Velocity, acceleration);
|
||||
}
|
||||
|
||||
private Knot(double arcLength, Point2D position, Point2D velocity, Point2D acceleration)
|
||||
{
|
||||
ArcLength = arcLength;
|
||||
Position = position;
|
||||
Velocity = velocity;
|
||||
Acceleration = acceleration;
|
||||
}
|
||||
}
|
||||
|
||||
private readonly struct QuinticInterval
|
||||
{
|
||||
private QuinticInterval(Knot start, Knot end, Point2D c0, Point2D c1, Point2D c2, Point2D c3, Point2D c4, Point2D c5)
|
||||
{
|
||||
Start = start;
|
||||
End = end;
|
||||
Length = end.ArcLength - start.ArcLength;
|
||||
_c0 = c0;
|
||||
_c1 = c1;
|
||||
_c2 = c2;
|
||||
_c3 = c3;
|
||||
_c4 = c4;
|
||||
_c5 = c5;
|
||||
}
|
||||
|
||||
private readonly Point2D _c0;
|
||||
private readonly Point2D _c1;
|
||||
private readonly Point2D _c2;
|
||||
private readonly Point2D _c3;
|
||||
private readonly Point2D _c4;
|
||||
private readonly Point2D _c5;
|
||||
|
||||
internal Knot Start { get; }
|
||||
|
||||
internal Knot End { get; }
|
||||
|
||||
internal double Length { get; }
|
||||
|
||||
internal static bool TryCreate(Knot start, Knot end, out QuinticInterval interval)
|
||||
{
|
||||
interval = default;
|
||||
double length = end.ArcLength - start.ArcLength;
|
||||
if (!NumericGuard.IsPositiveFinite(length) || !start.Position.IsFinite || !end.Position.IsFinite ||
|
||||
!start.Velocity.IsFinite || !end.Velocity.IsFinite ||
|
||||
!start.Acceleration.IsFinite || !end.Acceleration.IsFinite)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Point2D c0 = start.Position;
|
||||
Point2D c1 = Scale(start.Velocity, length);
|
||||
Point2D c2 = Scale(start.Acceleration, length * length / 2d);
|
||||
Point2D difference = Subtract(end.Position, start.Position);
|
||||
Point2D endVelocity = Scale(end.Velocity, length);
|
||||
Point2D startAcceleration = Scale(start.Acceleration, length * length);
|
||||
Point2D endAcceleration = Scale(end.Acceleration, length * length);
|
||||
Point2D c3 = Add(
|
||||
Add(Scale(difference, 10d), Scale(c1, -6d)),
|
||||
Add(Scale(endVelocity, -4d), Add(Scale(startAcceleration, -1.5d), Scale(endAcceleration, 0.5d))));
|
||||
Point2D c4 = Add(
|
||||
Add(Scale(difference, -15d), Scale(c1, 8d)),
|
||||
Add(Scale(endVelocity, 7d), Add(Scale(startAcceleration, 1.5d), Scale(endAcceleration, -1d))));
|
||||
Point2D c5 = Add(
|
||||
Add(Scale(difference, 6d), Add(Scale(c1, -3d), Scale(endVelocity, -3d))),
|
||||
Add(Scale(startAcceleration, -0.5d), Scale(endAcceleration, 0.5d)));
|
||||
if (!c0.IsFinite || !c1.IsFinite || !c2.IsFinite || !c3.IsFinite || !c4.IsFinite || !c5.IsFinite)
|
||||
return false;
|
||||
|
||||
interval = new QuinticInterval(start, end, c0, c1, c2, c3, c4, c5);
|
||||
return interval.TryEvaluate(0d, out _, out _, out _) && interval.TryEvaluate(1d, out _, out _, out _);
|
||||
}
|
||||
|
||||
internal bool TryEvaluate(double parameter, out Point2D position, out Point2D derivative, out Point2D secondDerivative)
|
||||
{
|
||||
position = default;
|
||||
derivative = default;
|
||||
secondDerivative = default;
|
||||
if (!NumericGuard.IsFinite(parameter) || parameter < 0d || parameter > 1d) return false;
|
||||
|
||||
double t2 = parameter * parameter;
|
||||
double t3 = t2 * parameter;
|
||||
double t4 = t3 * parameter;
|
||||
double t5 = t4 * parameter;
|
||||
position = Add(Add(Add(_c0, Scale(_c1, parameter)), Add(Scale(_c2, t2), Scale(_c3, t3))),
|
||||
Add(Scale(_c4, t4), Scale(_c5, t5)));
|
||||
derivative = Add(Add(_c1, Scale(_c2, 2d * parameter)),
|
||||
Add(Scale(_c3, 3d * t2), Add(Scale(_c4, 4d * t3), Scale(_c5, 5d * t4))));
|
||||
secondDerivative = Add(Scale(_c2, 2d),
|
||||
Add(Scale(_c3, 6d * parameter), Add(Scale(_c4, 12d * t2), Scale(_c5, 20d * t3))));
|
||||
return position.IsFinite && derivative.IsFinite && secondDerivative.IsFinite;
|
||||
}
|
||||
}
|
||||
|
||||
private readonly struct Point2D
|
||||
{
|
||||
internal Point2D(double x, double y)
|
||||
{
|
||||
X = x;
|
||||
Y = y;
|
||||
}
|
||||
|
||||
internal double X { get; }
|
||||
|
||||
internal double Y { get; }
|
||||
|
||||
internal bool IsFinite => NumericGuard.IsFinite(X) && NumericGuard.IsFinite(Y);
|
||||
}
|
||||
|
||||
private static Point2D Add(Point2D left, Point2D right)
|
||||
{
|
||||
return new Point2D(left.X + right.X, left.Y + right.Y);
|
||||
}
|
||||
|
||||
private static Point2D Subtract(Point2D left, Point2D right)
|
||||
{
|
||||
return new Point2D(left.X - right.X, left.Y - right.Y);
|
||||
}
|
||||
|
||||
private static Point2D Scale(Point2D point, double scale)
|
||||
{
|
||||
return new Point2D(point.X * scale, point.Y * scale);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>单次算法运行共享的已预处理路径和独立复核上下文。</summary>
|
||||
internal sealed class SmoothingAlgorithmInput
|
||||
{
|
||||
internal SmoothingAlgorithmInput(
|
||||
PreparedPath originalPath,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
double maximumCollisionCheckStepMeters,
|
||||
double minimumClearanceReserveMeters,
|
||||
SmoothingOptionsSnapshot options)
|
||||
{
|
||||
OriginalPath = originalPath ?? throw new ArgumentNullException(nameof(originalPath));
|
||||
Map = map ?? throw new ArgumentNullException(nameof(map));
|
||||
Vehicle = vehicle ?? throw new ArgumentNullException(nameof(vehicle));
|
||||
MaximumCollisionCheckStepMeters = maximumCollisionCheckStepMeters;
|
||||
MinimumClearanceReserveMeters = minimumClearanceReserveMeters;
|
||||
Options = options ?? throw new ArgumentNullException(nameof(options));
|
||||
}
|
||||
|
||||
/// <summary>已校验并按方向分段的原始路径。</summary>
|
||||
internal PreparedPath OriginalPath { get; }
|
||||
|
||||
/// <summary>用于完整车体复核的不可变规划地图。</summary>
|
||||
internal PlanningGridMap Map { get; }
|
||||
|
||||
/// <summary>用于曲率和足迹复核的车辆参数快照。</summary>
|
||||
internal VehicleParameters Vehicle { get; }
|
||||
|
||||
/// <summary>连续车体碰撞检查的最大步长，单位 m。</summary>
|
||||
internal double MaximumCollisionCheckStepMeters { get; }
|
||||
|
||||
/// <summary>候选几何必须从原始保守净空中预留的最小安全余量，单位 m。</summary>
|
||||
internal double MinimumClearanceReserveMeters { get; }
|
||||
|
||||
/// <summary>本次算法运行使用的已验证方法选项快照。</summary>
|
||||
internal SmoothingOptionsSnapshot Options { get; }
|
||||
}
|
||||
@@ -0,0 +1,443 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>在有限强度计划内运行单一算法，并以共享分析和安全复核决定是否接受候选。</summary>
|
||||
internal sealed class SmoothingAlgorithmRunner
|
||||
{
|
||||
private static readonly double[] RetryStrengthScales = { 1d, 0.75d, 0.50d, 0.25d };
|
||||
private readonly PathGeometryAnalyzer _analyzer;
|
||||
private readonly SmoothedPathValidator _validator;
|
||||
|
||||
internal SmoothingAlgorithmRunner()
|
||||
: this(new PathGeometryAnalyzer(), new SmoothedPathValidator())
|
||||
{
|
||||
}
|
||||
|
||||
internal SmoothingAlgorithmRunner(PathGeometryAnalyzer analyzer, SmoothedPathValidator validator)
|
||||
{
|
||||
_analyzer = analyzer ?? throw new ArgumentNullException(nameof(analyzer));
|
||||
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 依次尝试配置的有限强度比例。取消直接向上传播，由门面转换为最终状态；
|
||||
/// 只有算法明确标记为可重试的不可行性才会使用较低强度；终止失败和统一复核失败均不重试。
|
||||
/// </summary>
|
||||
internal AlgorithmRunResult Run(
|
||||
IPathSmoother smoother,
|
||||
SmoothingAlgorithmInput input,
|
||||
PathSmoothingConfiguration configuration,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var attemptedStrengths = new List<double>();
|
||||
var failureReasons = new List<string>();
|
||||
if (smoother == null || input == null || input.Options == null || configuration == null)
|
||||
return AlgorithmRunResult.Failed("平滑算法、输入或配置无效。", attemptedStrengths, failureReasons);
|
||||
|
||||
foreach (double scale in RetryStrengthScales)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
double effectiveStrength = configuration.SmoothingStrength * scale;
|
||||
if (!IsPositiveFinite(effectiveStrength))
|
||||
return AlgorithmRunResult.Failed("平滑强度或重试比例无效。", attemptedStrengths, failureReasons);
|
||||
|
||||
attemptedStrengths.Add(effectiveStrength);
|
||||
SmoothingCandidate candidate = smoother.Smooth(input, effectiveStrength, cancellationToken);
|
||||
if (candidate == null)
|
||||
return AlgorithmRunResult.Failed("平滑算法未返回候选。", attemptedStrengths, failureReasons);
|
||||
if (candidate.Status == SmoothingCandidateStatus.RetryableInfeasible)
|
||||
{
|
||||
failureReasons.Add(candidate.Reason);
|
||||
continue;
|
||||
}
|
||||
if (candidate.Status == SmoothingCandidateStatus.Failed)
|
||||
{
|
||||
failureReasons.Add(candidate.Reason);
|
||||
return AlgorithmRunResult.Failed(candidate.Reason, attemptedStrengths, failureReasons);
|
||||
}
|
||||
if (candidate.Status != SmoothingCandidateStatus.Success)
|
||||
{
|
||||
const string unknownStatusReason = "平滑算法返回未知候选状态。";
|
||||
failureReasons.Add(unknownStatusReason);
|
||||
return AlgorithmRunResult.Failed(unknownStatusReason, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
if (!_analyzer.TryAnalyze(candidate.Segments, configuration.OutputSpacingMeters,
|
||||
out PathGeometryAnalysis analysis, out string reason))
|
||||
{
|
||||
failureReasons.Add(reason);
|
||||
return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
if (_validator.TryValidate(analysis.Path, analysis.Segments, input.OriginalPath, input.Map, input.Vehicle,
|
||||
input.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearanceMeters, out reason))
|
||||
{
|
||||
return AlgorithmRunResult.Success(safePath, analysis.Segments,
|
||||
CreateMetrics(analysis, minimumClearanceMeters), effectiveStrength,
|
||||
attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
failureReasons.Add(reason);
|
||||
return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
return AlgorithmRunResult.Infeasible(null, attemptedStrengths, failureReasons);
|
||||
}
|
||||
|
||||
private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
|
||||
{
|
||||
return new PathQualityMetrics(
|
||||
true,
|
||||
analysis.PathLengthMeters,
|
||||
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
analysis.RootMeanSquareVehicleCurvaturePerMeter,
|
||||
analysis.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
analysis.CurvatureVariationEnergy,
|
||||
minimumClearanceMeters,
|
||||
0d,
|
||||
0d,
|
||||
0d,
|
||||
0d);
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Reflection-only deterministic coverage seam. It is nested in an internal runner and intentionally
|
||||
/// does not construct or register a production smoothing method.
|
||||
/// </summary>
|
||||
public static class TestHooks
|
||||
{
|
||||
/// <summary>执行一个固定的内部假平滑器场景并返回可反射读取的快照。</summary>
|
||||
public static RunnerTestSnapshot Execute(string scenario)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
|
||||
|
||||
var cancellationSource = new CancellationTokenSource();
|
||||
var smoother = new DeterministicTestSmoother(ParseScenario(scenario), cancellationSource);
|
||||
var runner = new SmoothingAlgorithmRunner();
|
||||
SmoothingAlgorithmInput input = CreateTestInput();
|
||||
var configuration = new PathSmoothingConfiguration();
|
||||
try
|
||||
{
|
||||
AlgorithmRunResult result = runner.Run(smoother, input, configuration, cancellationSource.Token);
|
||||
return RunnerTestSnapshot.FromResult(result, false);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return new RunnerTestSnapshot(
|
||||
"OperationCanceledException",
|
||||
smoother.AttemptedStrengths,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
true);
|
||||
}
|
||||
finally
|
||||
{
|
||||
cancellationSource.Dispose();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>供 PowerShell 断言使用的不可变执行摘要。</summary>
|
||||
public sealed class RunnerTestSnapshot
|
||||
{
|
||||
internal RunnerTestSnapshot(
|
||||
string status,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
int acceptedPathPointCount,
|
||||
int rejectedComparisonCandidatePointCount,
|
||||
int failureCount,
|
||||
bool cancellationPropagated)
|
||||
{
|
||||
Status = status ?? string.Empty;
|
||||
AttemptedStrengths = CopyReadOnly(attemptedStrengths);
|
||||
AcceptedPathPointCount = acceptedPathPointCount;
|
||||
RejectedComparisonCandidatePointCount = rejectedComparisonCandidatePointCount;
|
||||
FailureCount = failureCount;
|
||||
CancellationPropagated = cancellationPropagated;
|
||||
}
|
||||
|
||||
public string Status { get; }
|
||||
public IReadOnlyList<double> AttemptedStrengths { get; }
|
||||
public int AcceptedPathPointCount { get; }
|
||||
public int RejectedComparisonCandidatePointCount { get; }
|
||||
public int FailureCount { get; }
|
||||
public bool CancellationPropagated { get; }
|
||||
|
||||
internal static RunnerTestSnapshot FromResult(AlgorithmRunResult result, bool cancellationPropagated)
|
||||
{
|
||||
int rejectedPointCount = result.RejectedComparisonCandidate == null
|
||||
? 0
|
||||
: CountPoints(result.RejectedComparisonCandidate.Segments);
|
||||
return new RunnerTestSnapshot(
|
||||
result.Status.ToString(),
|
||||
result.AttemptedStrengths,
|
||||
result.Path.Count,
|
||||
rejectedPointCount,
|
||||
result.FailureReasons.Count,
|
||||
cancellationPropagated);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
|
||||
private enum TestScenario
|
||||
{
|
||||
RetryableInfeasible,
|
||||
AcceptFirst,
|
||||
TerminalFailed,
|
||||
CancelBeforeNextAttempt,
|
||||
}
|
||||
|
||||
private sealed class DeterministicTestSmoother : IPathSmoother
|
||||
{
|
||||
private readonly TestScenario _scenario;
|
||||
private readonly CancellationTokenSource _cancellationSource;
|
||||
|
||||
internal DeterministicTestSmoother(TestScenario scenario, CancellationTokenSource cancellationSource)
|
||||
{
|
||||
_scenario = scenario;
|
||||
_cancellationSource = cancellationSource;
|
||||
AttemptedStrengths = new List<double>();
|
||||
}
|
||||
|
||||
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
|
||||
|
||||
internal List<double> AttemptedStrengths { get; }
|
||||
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
AttemptedStrengths.Add(effectiveStrength);
|
||||
if (_scenario == TestScenario.TerminalFailed)
|
||||
return SmoothingCandidate.Failed("确定性数值退化。");
|
||||
if (_scenario == TestScenario.AcceptFirst)
|
||||
return CreateAcceptedCandidate();
|
||||
|
||||
if (_scenario == TestScenario.CancelBeforeNextAttempt)
|
||||
_cancellationSource.Cancel();
|
||||
return SmoothingCandidate.RetryableInfeasible("确定性可重试不可行。" );
|
||||
}
|
||||
}
|
||||
|
||||
private static TestScenario ParseScenario(string scenario)
|
||||
{
|
||||
if (string.Equals(scenario, nameof(TestScenario.RetryableInfeasible), StringComparison.Ordinal)) return TestScenario.RetryableInfeasible;
|
||||
if (string.Equals(scenario, nameof(TestScenario.AcceptFirst), StringComparison.Ordinal)) return TestScenario.AcceptFirst;
|
||||
if (string.Equals(scenario, nameof(TestScenario.TerminalFailed), StringComparison.Ordinal)) return TestScenario.TerminalFailed;
|
||||
if (string.Equals(scenario, nameof(TestScenario.CancelBeforeNextAttempt), StringComparison.Ordinal)) return TestScenario.CancelBeforeNextAttempt;
|
||||
throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
|
||||
private static SmoothingAlgorithmInput CreateTestInput()
|
||||
{
|
||||
var mapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 5000f, 0f, 5000f),
|
||||
ResolutionMm = 50f,
|
||||
AllowExplicitEmptyMap = true,
|
||||
};
|
||||
PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(mapRequest);
|
||||
if (!mapResult.Succeeded || mapResult.Map == null)
|
||||
throw new InvalidOperationException("The runner test hook could not create its empty map.");
|
||||
|
||||
var originalSegments = new List<PreparedDirectionSegment>
|
||||
{
|
||||
new PreparedDirectionSegment(
|
||||
0,
|
||||
TravelDirection.Forward,
|
||||
new List<SmoothingPoint2D>
|
||||
{
|
||||
CreatePoint(0.5d, 0.5d, 0d),
|
||||
CreatePoint(1.5d, 0.5d, 1d),
|
||||
},
|
||||
false,
|
||||
false),
|
||||
};
|
||||
var vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.20d,
|
||||
WidthMeters = 0.20d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 100d,
|
||||
MinimumTurningRadiusMeters = 0.01d,
|
||||
};
|
||||
return new SmoothingAlgorithmInput(
|
||||
new PreparedPath(originalSegments),
|
||||
mapResult.Map,
|
||||
vehicle,
|
||||
0.05d,
|
||||
0.02d,
|
||||
new SmoothingOptionsSnapshot(new PathSmoothingConfiguration()));
|
||||
}
|
||||
|
||||
private static SmoothingCandidate CreateAcceptedCandidate()
|
||||
{
|
||||
return SmoothingCandidate.Success(new List<PreparedDirectionSegment>
|
||||
{
|
||||
new PreparedDirectionSegment(
|
||||
0,
|
||||
TravelDirection.Forward,
|
||||
new List<SmoothingPoint2D>
|
||||
{
|
||||
CreatePoint(0.5d, 0.5d, 0d),
|
||||
CreatePoint(1.5d, 0.5d, 1d),
|
||||
},
|
||||
false,
|
||||
false),
|
||||
});
|
||||
}
|
||||
|
||||
private static SmoothingPoint2D CreatePoint(double x, double y, double arcLength)
|
||||
{
|
||||
return new SmoothingPoint2D(
|
||||
x,
|
||||
y,
|
||||
arcLength,
|
||||
0d,
|
||||
0d,
|
||||
1d,
|
||||
false,
|
||||
SmoothedPathPointSource.Anchor);
|
||||
}
|
||||
|
||||
private static int CountPoints(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
int count = 0;
|
||||
for (int index = 0; index < segments.Count; index++) count += segments[index].Points.Count;
|
||||
return count;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>内部运行结果；只有成功路径可被正式门面发布，拒绝候选仅供比较诊断读取。</summary>
|
||||
internal sealed class AlgorithmRunResult
|
||||
{
|
||||
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
||||
new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
||||
private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
|
||||
new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
|
||||
|
||||
private AlgorithmRunResult(
|
||||
PathSmoothingStatus status,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathQualityMetrics metrics,
|
||||
double acceptedStrength,
|
||||
SmoothingCandidate rejectedComparisonCandidate,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
IReadOnlyList<string> failureReasons,
|
||||
string reason)
|
||||
{
|
||||
Status = status;
|
||||
Path = path ?? EmptyPath;
|
||||
Segments = segments ?? EmptySegments;
|
||||
Metrics = metrics ?? new PathQualityMetrics();
|
||||
AcceptedStrength = acceptedStrength;
|
||||
RejectedComparisonCandidate = rejectedComparisonCandidate;
|
||||
AttemptedStrengths = CopyReadOnly(attemptedStrengths);
|
||||
FailureReasons = CopyReadOnly(failureReasons);
|
||||
Reason = reason ?? string.Empty;
|
||||
}
|
||||
|
||||
internal PathSmoothingStatus Status { get; }
|
||||
internal IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
internal PathQualityMetrics Metrics { get; }
|
||||
internal double AcceptedStrength { get; }
|
||||
internal SmoothingCandidate RejectedComparisonCandidate { get; }
|
||||
internal IReadOnlyList<double> AttemptedStrengths { get; }
|
||||
internal IReadOnlyList<string> FailureReasons { get; }
|
||||
internal string Reason { get; }
|
||||
|
||||
internal static AlgorithmRunResult Success(
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathQualityMetrics metrics,
|
||||
double acceptedStrength,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
IReadOnlyList<string> failureReasons)
|
||||
{
|
||||
return new AlgorithmRunResult(
|
||||
PathSmoothingStatus.Success,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
metrics,
|
||||
acceptedStrength,
|
||||
null,
|
||||
attemptedStrengths,
|
||||
failureReasons,
|
||||
string.Empty);
|
||||
}
|
||||
|
||||
internal static AlgorithmRunResult Infeasible(
|
||||
SmoothingCandidate rejectedComparisonCandidate,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
IReadOnlyList<string> failureReasons)
|
||||
{
|
||||
string reason = failureReasons == null || failureReasons.Count == 0
|
||||
? "所有有限平滑尝试均未通过复核。"
|
||||
: failureReasons[failureReasons.Count - 1];
|
||||
return new AlgorithmRunResult(
|
||||
PathSmoothingStatus.Infeasible,
|
||||
null,
|
||||
null,
|
||||
null,
|
||||
0d,
|
||||
rejectedComparisonCandidate,
|
||||
attemptedStrengths,
|
||||
failureReasons,
|
||||
reason);
|
||||
}
|
||||
|
||||
internal static AlgorithmRunResult Failed(
|
||||
string reason,
|
||||
IReadOnlyList<double> attemptedStrengths,
|
||||
IReadOnlyList<string> failureReasons)
|
||||
{
|
||||
return new AlgorithmRunResult(
|
||||
PathSmoothingStatus.Failed,
|
||||
null,
|
||||
null,
|
||||
null,
|
||||
0d,
|
||||
null,
|
||||
attemptedStrengths,
|
||||
failureReasons,
|
||||
reason);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
internal enum SmoothingCandidateStatus
|
||||
{
|
||||
Success,
|
||||
RetryableInfeasible,
|
||||
Failed,
|
||||
}
|
||||
|
||||
/// <summary>平滑方法产生的原始方向段候选，尚未经过统一几何或安全复核。</summary>
|
||||
internal sealed class SmoothingCandidate
|
||||
{
|
||||
private SmoothingCandidate(
|
||||
SmoothingCandidateStatus status,
|
||||
IReadOnlyList<PreparedDirectionSegment> segments,
|
||||
string reason)
|
||||
{
|
||||
Status = status;
|
||||
if (status == SmoothingCandidateStatus.Success)
|
||||
{
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("A successful smoothing candidate requires direction segments.", nameof(segments));
|
||||
for (int index = 0; index < segments.Count; index++)
|
||||
{
|
||||
if (segments[index] == null || segments[index].Points == null || segments[index].Points.Count == 0)
|
||||
throw new ArgumentException("A successful smoothing candidate requires complete direction segments.", nameof(segments));
|
||||
}
|
||||
Segments = CopyReadOnly(segments);
|
||||
Reason = string.Empty;
|
||||
return;
|
||||
}
|
||||
|
||||
if (string.IsNullOrWhiteSpace(reason))
|
||||
throw new ArgumentException("An unsuccessful smoothing candidate requires a reason.", nameof(reason));
|
||||
Segments = CopyReadOnly<PreparedDirectionSegment>(null);
|
||||
Reason = reason;
|
||||
}
|
||||
|
||||
/// <summary>候选是否成功产生有限的原始几何。</summary>
|
||||
internal bool Succeeded => Status == SmoothingCandidateStatus.Success;
|
||||
|
||||
/// <summary>候选的可重试性和终止性状态。</summary>
|
||||
internal SmoothingCandidateStatus Status { get; }
|
||||
|
||||
/// <summary>候选方向段；失败候选始终为空。</summary>
|
||||
internal IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
||||
|
||||
/// <summary>失败或退化时的稳定说明；成功时为空。</summary>
|
||||
internal string Reason { get; }
|
||||
|
||||
/// <summary>创建待统一分析和验证的成功候选。</summary>
|
||||
internal static SmoothingCandidate Success(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
return new SmoothingCandidate(SmoothingCandidateStatus.Success, segments, string.Empty);
|
||||
}
|
||||
|
||||
/// <summary>创建可由较低平滑强度重新尝试的不可行候选。</summary>
|
||||
internal static SmoothingCandidate RetryableInfeasible(string reason)
|
||||
{
|
||||
return new SmoothingCandidate(SmoothingCandidateStatus.RetryableInfeasible, null, reason);
|
||||
}
|
||||
|
||||
/// <summary>创建不应重试的数值或构造失败候选。</summary>
|
||||
internal static SmoothingCandidate Failed(string reason)
|
||||
{
|
||||
return new SmoothingCandidate(SmoothingCandidateStatus.Failed, null, reason);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>供单次平滑算法运行使用的、已校验的不可变方法选项快照。</summary>
|
||||
internal sealed class SmoothingOptionsSnapshot
|
||||
{
|
||||
internal SmoothingOptionsSnapshot(PathSmoothingConfiguration configuration)
|
||||
{
|
||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||
|
||||
CubicBSplineEndpointTangentScale = configuration.CubicBSpline.EndpointTangentScale;
|
||||
BezierCornerHeadingThresholdRadians = configuration.LocalCubicBezier.CornerHeadingThresholdRadians;
|
||||
BezierMaximumWindowLengthMeters = configuration.LocalCubicBezier.MaximumWindowLengthMeters;
|
||||
BezierHandleLengthRatio = configuration.LocalCubicBezier.HandleLengthRatio;
|
||||
QuinticKnotSpacingMeters = configuration.PiecewiseQuintic.KnotSpacingMeters;
|
||||
QuinticMinimumKnotSpacingMeters = configuration.PiecewiseQuintic.MinimumKnotSpacingMeters;
|
||||
|
||||
ValidatePositiveFinite(CubicBSplineEndpointTangentScale, nameof(CubicBSplineEndpointTangentScale));
|
||||
if (!NumericGuard.IsFinite(BezierCornerHeadingThresholdRadians) ||
|
||||
BezierCornerHeadingThresholdRadians <= 0d || BezierCornerHeadingThresholdRadians > Math.PI)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(BezierCornerHeadingThresholdRadians));
|
||||
}
|
||||
ValidatePositiveFinite(BezierMaximumWindowLengthMeters, nameof(BezierMaximumWindowLengthMeters));
|
||||
ValidatePositiveFinite(BezierHandleLengthRatio, nameof(BezierHandleLengthRatio));
|
||||
ValidatePositiveFinite(QuinticKnotSpacingMeters, nameof(QuinticKnotSpacingMeters));
|
||||
ValidatePositiveFinite(QuinticMinimumKnotSpacingMeters, nameof(QuinticMinimumKnotSpacingMeters));
|
||||
if (QuinticKnotSpacingMeters < QuinticMinimumKnotSpacingMeters)
|
||||
throw new ArgumentOutOfRangeException(nameof(QuinticKnotSpacingMeters));
|
||||
}
|
||||
|
||||
internal double CubicBSplineEndpointTangentScale { get; }
|
||||
|
||||
internal double BezierCornerHeadingThresholdRadians { get; }
|
||||
|
||||
internal double BezierMaximumWindowLengthMeters { get; }
|
||||
|
||||
internal double BezierHandleLengthRatio { get; }
|
||||
|
||||
internal double QuinticKnotSpacingMeters { get; }
|
||||
|
||||
internal double QuinticMinimumKnotSpacingMeters { get; }
|
||||
|
||||
private static void ValidatePositiveFinite(double value, string name)
|
||||
{
|
||||
if (!NumericGuard.IsPositiveFinite(value)) throw new ArgumentOutOfRangeException(name);
|
||||
}
|
||||
}
|
||||
+132
@@ -0,0 +1,132 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>原始基线或一种平滑方法的不可变比较条目。</summary>
|
||||
public sealed class PathSmoothingComparisonEntry
|
||||
{
|
||||
/// <summary>为测试、离线分析和排序创建不携带路径几何的候选条目。</summary>
|
||||
public PathSmoothingComparisonEntry(
|
||||
SmoothingMethod method,
|
||||
PathSmoothingStatus status,
|
||||
PathQualityMetrics metrics,
|
||||
SmoothingTimingSummary timing,
|
||||
string stableGeometryDigest,
|
||||
string diagnostic)
|
||||
: this(method, false, status, metrics, timing, stableGeometryDigest, diagnostic, 0, 0d, Empty<SmoothedPathPoint>(), Empty<SmoothedPathSegment>())
|
||||
{
|
||||
}
|
||||
|
||||
private PathSmoothingComparisonEntry(
|
||||
SmoothingMethod? method,
|
||||
bool isRawPathBaseline,
|
||||
PathSmoothingStatus status,
|
||||
PathQualityMetrics metrics,
|
||||
SmoothingTimingSummary timing,
|
||||
string stableGeometryDigest,
|
||||
string diagnostic,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments)
|
||||
{
|
||||
Method = method;
|
||||
IsRawPathBaseline = isRawPathBaseline;
|
||||
Status = status;
|
||||
Metrics = metrics ?? new PathQualityMetrics();
|
||||
Timing = timing ?? new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, "未提供计时结果。");
|
||||
StableGeometryDigest = stableGeometryDigest ?? string.Empty;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
RetryCount = retryCount < 0 ? 0 : retryCount;
|
||||
AcceptedStrength = acceptedStrength;
|
||||
Path = CopyReadOnly(path);
|
||||
Segments = CopyReadOnly(segments);
|
||||
}
|
||||
|
||||
/// <summary>候选所代表的方法；原始粗路径基线为空。</summary>
|
||||
public SmoothingMethod? Method { get; }
|
||||
|
||||
/// <summary>是否为单独分析的原始粗路径基线。</summary>
|
||||
public bool IsRawPathBaseline { get; }
|
||||
|
||||
/// <summary>本条目的最终状态。</summary>
|
||||
public PathSmoothingStatus Status { get; }
|
||||
|
||||
/// <summary>使用原始基线规范化后的质量指标。</summary>
|
||||
public PathQualityMetrics Metrics { get; }
|
||||
|
||||
/// <summary>方法的五次测量计时；基线不参与计时排名。</summary>
|
||||
public SmoothingTimingSummary Timing { get; }
|
||||
|
||||
/// <summary>由状态、分段元数据和完整路径 IEEE 754 位模式生成的 SHA-256 摘要。</summary>
|
||||
public string StableGeometryDigest { get; }
|
||||
|
||||
/// <summary>面向报告和诊断的稳定说明。</summary>
|
||||
public string Diagnostic { get; }
|
||||
|
||||
/// <summary>规范平滑调用记录的重试次数；原始粗路径基线为零。</summary>
|
||||
public int RetryCount { get; }
|
||||
|
||||
/// <summary>规范平滑调用接受的强度；未接受候选和原始粗路径基线为零。</summary>
|
||||
public double AcceptedStrength { get; }
|
||||
|
||||
/// <summary>仅供比较与报告读取的正式路径；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>覆盖 <see cref="Path"/> 的方向段；失败时为空。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>条目能否参与方法推荐。</summary>
|
||||
public bool IsEligibleForRecommendation =>
|
||||
!IsRawPathBaseline &&
|
||||
Status == PathSmoothingStatus.Success &&
|
||||
Metrics.IsFeasible &&
|
||||
Timing.IsDeterministic;
|
||||
|
||||
internal static PathSmoothingComparisonEntry CreateCandidate(
|
||||
SmoothingMethod method,
|
||||
PathSmoothingStatus status,
|
||||
PathQualityMetrics metrics,
|
||||
SmoothingTimingSummary timing,
|
||||
string stableGeometryDigest,
|
||||
string diagnostic,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments)
|
||||
{
|
||||
return new PathSmoothingComparisonEntry(
|
||||
method, false, status, metrics, timing, stableGeometryDigest, diagnostic, retryCount, acceptedStrength, path, segments);
|
||||
}
|
||||
|
||||
internal static PathSmoothingComparisonEntry CreateRawPathBaseline(
|
||||
PathSmoothingStatus status,
|
||||
PathQualityMetrics metrics,
|
||||
string stableGeometryDigest,
|
||||
string diagnostic,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments)
|
||||
{
|
||||
return new PathSmoothingComparisonEntry(
|
||||
null, true, status, metrics,
|
||||
new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, true, string.Empty),
|
||||
stableGeometryDigest, diagnostic, 0, 0d, path, segments);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> Empty<T>()
|
||||
{
|
||||
return new ReadOnlyCollection<T>(new List<T>());
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>同一粗路径的离线平滑比较请求。</summary>
|
||||
public sealed class PathSmoothingComparisonRequest
|
||||
{
|
||||
private static readonly SmoothingMethod[] DefaultMethods =
|
||||
{
|
||||
SmoothingMethod.CubicBSpline,
|
||||
SmoothingMethod.LocalCubicBezier,
|
||||
SmoothingMethod.PiecewiseQuintic,
|
||||
};
|
||||
|
||||
/// <summary>创建比较请求，并固定原始输入与方法顺序。</summary>
|
||||
public PathSmoothingComparisonRequest(
|
||||
PathSmoothingRequest smoothingRequest,
|
||||
IReadOnlyList<SmoothingMethod> methods = null)
|
||||
{
|
||||
SmoothingRequest = CopyRequest(smoothingRequest);
|
||||
Methods = CopyMethods(methods ?? DefaultMethods);
|
||||
}
|
||||
|
||||
/// <summary>所有方法共享的不可变粗路径、地图、车辆和配置快照。</summary>
|
||||
public PathSmoothingRequest SmoothingRequest { get; }
|
||||
|
||||
/// <summary>按调用方指定稳定顺序运行的方法集合。</summary>
|
||||
public IReadOnlyList<SmoothingMethod> Methods { get; }
|
||||
|
||||
private static PathSmoothingRequest CopyRequest(PathSmoothingRequest source)
|
||||
{
|
||||
if (source == null) return null;
|
||||
return new PathSmoothingRequest(
|
||||
source.CoarsePath,
|
||||
source.Segments,
|
||||
source.Map,
|
||||
source.Vehicle,
|
||||
source.Configuration);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingMethod> CopyMethods(IReadOnlyList<SmoothingMethod> source)
|
||||
{
|
||||
var copy = new List<SmoothingMethod>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
SmoothingMethod method = source[index];
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), method))
|
||||
throw new ArgumentOutOfRangeException(nameof(source), "比较方法无效。");
|
||||
if (copy.Contains(method))
|
||||
throw new ArgumentException("比较方法不能重复。", nameof(source));
|
||||
copy.Add(method);
|
||||
}
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingMethod>(copy);
|
||||
}
|
||||
}
|
||||
+49
@@ -0,0 +1,49 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>一次离线比较的不可变基线、方法条目和推荐结论。</summary>
|
||||
public sealed class PathSmoothingComparisonResult
|
||||
{
|
||||
internal PathSmoothingComparisonResult(
|
||||
PathSmoothingComparisonEntry rawPathBaseline,
|
||||
IReadOnlyList<PathSmoothingComparisonEntry> entries,
|
||||
SmoothingMethod? recommendedMethod,
|
||||
bool isCancelled,
|
||||
string diagnostic)
|
||||
{
|
||||
RawPathBaseline = rawPathBaseline ?? throw new ArgumentNullException(nameof(rawPathBaseline));
|
||||
Entries = CopyReadOnly(entries);
|
||||
RecommendedMethod = recommendedMethod;
|
||||
IsCancelled = isCancelled;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>独立分析的原始粗路径；不属于任何候选方法。</summary>
|
||||
public PathSmoothingComparisonEntry RawPathBaseline { get; }
|
||||
|
||||
/// <summary>每个请求方法恰有一个条目；取消时可能只包含已完成的方法。</summary>
|
||||
public IReadOnlyList<PathSmoothingComparisonEntry> Entries { get; }
|
||||
|
||||
/// <summary>按公开字典序选择的方法；没有合格方法或取消时为空。</summary>
|
||||
public SmoothingMethod? RecommendedMethod { get; }
|
||||
|
||||
/// <summary>比较是否在启动后续方法前被取消。</summary>
|
||||
public bool IsCancelled { get; }
|
||||
|
||||
/// <summary>整个比较的稳定状态说明。</summary>
|
||||
public string Diagnostic { get; }
|
||||
|
||||
private static IReadOnlyList<PathSmoothingComparisonEntry> CopyReadOnly(
|
||||
IReadOnlyList<PathSmoothingComparisonEntry> source)
|
||||
{
|
||||
var copy = new List<PathSmoothingComparisonEntry>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<PathSmoothingComparisonEntry>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>按公开字典序选择唯一的推荐平滑方法。</summary>
|
||||
public static class SmoothingMethodRanker
|
||||
{
|
||||
/// <summary>从当前场景的可行且确定性候选中选择最佳方法；没有合格候选时返回空。</summary>
|
||||
public static SmoothingMethod? Rank(IReadOnlyList<PathSmoothingComparisonEntry> entries)
|
||||
{
|
||||
PathSmoothingComparisonEntry best = null;
|
||||
if (entries == null) return null;
|
||||
|
||||
for (int index = 0; index < entries.Count; index++)
|
||||
{
|
||||
PathSmoothingComparisonEntry candidate = entries[index];
|
||||
if (candidate == null || !candidate.IsEligibleForRecommendation) continue;
|
||||
if (best == null || Compare(candidate, best) < 0) best = candidate;
|
||||
}
|
||||
return best == null ? (SmoothingMethod?)null : best.Method;
|
||||
}
|
||||
|
||||
private static int Compare(PathSmoothingComparisonEntry left, PathSmoothingComparisonEntry right)
|
||||
{
|
||||
int comparison = CompareAscending(left.Metrics.CurvatureVariationEnergy, right.Metrics.CurvatureVariationEnergy);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(
|
||||
left.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
right.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareDescending(left.Metrics.MinimumBodyClearanceMeters, right.Metrics.MinimumBodyClearanceMeters);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(left.Metrics.LengthChangePercent, right.Metrics.LengthChangePercent);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
comparison = CompareAscending(left.Timing.MedianElapsedMilliseconds, right.Timing.MedianElapsedMilliseconds);
|
||||
if (comparison != 0) return comparison;
|
||||
|
||||
return ((int)left.Method.Value).CompareTo((int)right.Method.Value);
|
||||
}
|
||||
|
||||
private static int CompareAscending(double left, double right)
|
||||
{
|
||||
return Normalize(left).CompareTo(Normalize(right));
|
||||
}
|
||||
|
||||
private static int CompareDescending(double left, double right)
|
||||
{
|
||||
return Normalize(right).CompareTo(Normalize(left));
|
||||
}
|
||||
|
||||
private static double Normalize(double value)
|
||||
{
|
||||
return double.IsNaN(value) || double.IsInfinity(value) ? double.PositiveInfinity : value;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>一个方法的固定五次计时样本和确定性结论。</summary>
|
||||
public sealed class SmoothingTimingSummary
|
||||
{
|
||||
/// <summary>创建一个只接受五个已测量样本的计时汇总。</summary>
|
||||
public SmoothingTimingSummary(
|
||||
IReadOnlyList<double> measuredElapsedMilliseconds,
|
||||
bool isDeterministic,
|
||||
string diagnostic)
|
||||
{
|
||||
if (measuredElapsedMilliseconds == null || measuredElapsedMilliseconds.Count != 5)
|
||||
throw new ArgumentException("计时汇总必须包含五个已测量样本。", nameof(measuredElapsedMilliseconds));
|
||||
|
||||
var copy = new List<double>(measuredElapsedMilliseconds.Count);
|
||||
for (int index = 0; index < measuredElapsedMilliseconds.Count; index++)
|
||||
{
|
||||
double value = measuredElapsedMilliseconds[index];
|
||||
if (double.IsNaN(value) || double.IsInfinity(value) || value < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(measuredElapsedMilliseconds), "计时样本必须为有限非负数。");
|
||||
copy.Add(value);
|
||||
}
|
||||
|
||||
MeasuredElapsedMilliseconds = new ReadOnlyCollection<double>(copy);
|
||||
MedianElapsedMilliseconds = Median(copy);
|
||||
IsDeterministic = isDeterministic;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>不含预热执行的五个实测耗时，单位 ms。</summary>
|
||||
public IReadOnlyList<double> MeasuredElapsedMilliseconds { get; }
|
||||
|
||||
/// <summary>五个实测耗时的稳定中位数，单位 ms。</summary>
|
||||
public double MedianElapsedMilliseconds { get; }
|
||||
|
||||
/// <summary>五次输出是否具有相同的状态和稳定几何摘要。</summary>
|
||||
public bool IsDeterministic { get; }
|
||||
|
||||
/// <summary>非确定性或测量失败的稳定诊断说明。</summary>
|
||||
public string Diagnostic { get; }
|
||||
|
||||
/// <summary>从五次测量结果中生成计时汇总，并拒绝状态或几何不稳定的输出。</summary>
|
||||
public static SmoothingTimingSummary FromMeasurements(
|
||||
IReadOnlyList<double> measuredElapsedMilliseconds,
|
||||
IReadOnlyList<PathSmoothingResult> measuredResults)
|
||||
{
|
||||
if (measuredResults == null || measuredResults.Count != 5)
|
||||
throw new ArgumentException("确定性检查必须包含五个测量结果。", nameof(measuredResults));
|
||||
for (int index = 0; index < measuredResults.Count; index++)
|
||||
{
|
||||
if (measuredResults[index] == null)
|
||||
throw new ArgumentException("确定性检查不能包含空测量结果。", nameof(measuredResults));
|
||||
}
|
||||
|
||||
PathSmoothingResult canonical = measuredResults[0];
|
||||
string canonicalDigest = StableGeometryDigest.Compute(canonical);
|
||||
for (int index = 1; index < measuredResults.Count; index++)
|
||||
{
|
||||
PathSmoothingResult measured = measuredResults[index];
|
||||
string digest = StableGeometryDigest.Compute(measured);
|
||||
if (measured.Status != canonical.Status ||
|
||||
measured.Path.Count != canonical.Path.Count ||
|
||||
measured.Segments.Count != canonical.Segments.Count ||
|
||||
!string.Equals(digest, canonicalDigest, StringComparison.Ordinal))
|
||||
{
|
||||
return new SmoothingTimingSummary(
|
||||
measuredElapsedMilliseconds,
|
||||
false,
|
||||
"五次测量的状态、点数、分段数或稳定几何摘要不一致。");
|
||||
}
|
||||
}
|
||||
|
||||
return new SmoothingTimingSummary(measuredElapsedMilliseconds, true, string.Empty);
|
||||
}
|
||||
|
||||
internal static double Median(IReadOnlyList<double> values)
|
||||
{
|
||||
var sorted = new double[values.Count];
|
||||
for (int index = 0; index < values.Count; index++) sorted[index] = values[index];
|
||||
Array.Sort(sorted);
|
||||
int middle = sorted.Length / 2;
|
||||
return sorted.Length % 2 == 1
|
||||
? sorted[middle]
|
||||
: (sorted[middle - 1] + sorted[middle]) / 2d;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,111 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Security.Cryptography;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
/// <summary>为重复执行结果生成与进程无关的稳定几何 SHA-256 摘要。</summary>
|
||||
public static class StableGeometryDigest
|
||||
{
|
||||
/// <summary>计算正式平滑结果的状态、方法、分段和路径位模式摘要。</summary>
|
||||
public static string Compute(PathSmoothingResult result)
|
||||
{
|
||||
if (result == null) throw new ArgumentNullException(nameof(result));
|
||||
return Compute(result.Status, result.Method, result.Path, result.Segments);
|
||||
}
|
||||
|
||||
/// <summary>计算任意已分析路径的稳定摘要。</summary>
|
||||
public static string Compute(
|
||||
PathSmoothingStatus status,
|
||||
SmoothingMethod? method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments)
|
||||
{
|
||||
using (var stream = new MemoryStream())
|
||||
{
|
||||
WriteInt32(stream, 1);
|
||||
WriteInt32(stream, (int)status);
|
||||
WriteBoolean(stream, method.HasValue);
|
||||
if (method.HasValue) WriteInt32(stream, (int)method.Value);
|
||||
|
||||
WriteInt32(stream, path == null ? 0 : path.Count);
|
||||
if (path != null)
|
||||
{
|
||||
for (int index = 0; index < path.Count; index++) WritePoint(stream, path[index]);
|
||||
}
|
||||
|
||||
WriteInt32(stream, segments == null ? 0 : segments.Count);
|
||||
if (segments != null)
|
||||
{
|
||||
for (int index = 0; index < segments.Count; index++) WriteSegment(stream, segments[index]);
|
||||
}
|
||||
|
||||
using (SHA256 sha256 = SHA256.Create())
|
||||
{
|
||||
byte[] hash = sha256.ComputeHash(stream.ToArray());
|
||||
var builder = new StringBuilder(hash.Length * 2);
|
||||
for (int index = 0; index < hash.Length; index++) builder.Append(hash[index].ToString("x2"));
|
||||
return builder.ToString();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void WritePoint(Stream stream, SmoothedPathPoint point)
|
||||
{
|
||||
if (point == null) throw new ArgumentException("稳定路径摘要不能包含空点。", nameof(point));
|
||||
WriteDouble(stream, point.X);
|
||||
WriteDouble(stream, point.Y);
|
||||
WriteDouble(stream, point.Heading);
|
||||
WriteDouble(stream, point.UnwrappedHeading);
|
||||
WriteDouble(stream, point.ArcLength);
|
||||
WriteInt32(stream, (int)point.Direction);
|
||||
WriteDouble(stream, point.GeometricCurvature);
|
||||
WriteDouble(stream, point.VehicleCurvature);
|
||||
WriteDouble(stream, point.BodyClearance);
|
||||
WriteBoolean(stream, point.IsGearSwitchPoint);
|
||||
WriteInt32(stream, (int)point.Source);
|
||||
}
|
||||
|
||||
private static void WriteSegment(Stream stream, SmoothedPathSegment segment)
|
||||
{
|
||||
if (segment == null) throw new ArgumentException("稳定路径摘要不能包含空方向段。", nameof(segment));
|
||||
WriteInt32(stream, segment.SegmentIndex);
|
||||
WriteInt32(stream, (int)segment.Direction);
|
||||
WriteInt32(stream, segment.StartIndex);
|
||||
WriteInt32(stream, segment.EndIndex);
|
||||
WriteBoolean(stream, segment.StartsAtGearSwitch);
|
||||
WriteBoolean(stream, segment.EndsAtGearSwitch);
|
||||
}
|
||||
|
||||
private static void WriteDouble(Stream stream, double value)
|
||||
{
|
||||
WriteInt64(stream, BitConverter.DoubleToInt64Bits(value));
|
||||
}
|
||||
|
||||
private static void WriteBoolean(Stream stream, bool value)
|
||||
{
|
||||
stream.WriteByte(value ? (byte)1 : (byte)0);
|
||||
}
|
||||
|
||||
private static void WriteInt32(Stream stream, int value)
|
||||
{
|
||||
unchecked
|
||||
{
|
||||
stream.WriteByte((byte)value);
|
||||
stream.WriteByte((byte)(value >> 8));
|
||||
stream.WriteByte((byte)(value >> 16));
|
||||
stream.WriteByte((byte)(value >> 24));
|
||||
}
|
||||
}
|
||||
|
||||
private static void WriteInt64(Stream stream, long value)
|
||||
{
|
||||
unchecked
|
||||
{
|
||||
ulong bits = (ulong)value;
|
||||
for (int index = 0; index < 8; index++) stream.WriteByte((byte)(bits >> (index * 8)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>三次 B 样条平滑参数。</summary>
|
||||
public sealed class CubicBSplineOptions
|
||||
{
|
||||
/// <summary>端点切向控制柄相对于相邻弦长的比例。</summary>
|
||||
public double EndpointTangentScale { get; set; } = 1d / 3d;
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>局部三次 Bézier 平滑参数。</summary>
|
||||
public sealed class LocalCubicBezierOptions
|
||||
{
|
||||
/// <summary>判定为明显转角的最小航向变化，单位 rad。</summary>
|
||||
public double CornerHeadingThresholdRadians { get; set; } = Math.PI / 18d;
|
||||
|
||||
/// <summary>单个局部平滑窗口的最大弧长，单位 m。</summary>
|
||||
public double MaximumWindowLengthMeters { get; set; } = 0.60d;
|
||||
|
||||
/// <summary>控制柄相对于窗口局部弦长的比例。</summary>
|
||||
public double HandleLengthRatio { get; set; } = 1d / 3d;
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>局部 G2 五次过渡的可配置阈值。</summary>
|
||||
public sealed class LocalG2QuinticOptions
|
||||
{
|
||||
public double MinimumWindowLengthMeters { get; set; } = 0.20d;
|
||||
public double PreferredWindowLengthMeters { get; set; } = 0.50d;
|
||||
public double MaximumWindowLengthMeters { get; set; } = 0.80d;
|
||||
public double MaximumDeviationMeters { get; set; } = 0.10d;
|
||||
public double AbsoluteCurvatureJumpFloorPerMeter { get; set; } = 0.001d;
|
||||
public double CurvatureJumpRatioOfMaximum { get; set; } = 0.05d;
|
||||
public double MinimumPeakGradientImprovementRatio { get; set; } = 0.20d;
|
||||
public double MaximumVariationCostRegressionRatio { get; set; } = 0.02d;
|
||||
public int MaximumCandidatesPerRegion { get; set; } = 12;
|
||||
}
|
||||
@@ -0,0 +1,108 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>一条原始或平滑路径的不可变质量指标。</summary>
|
||||
public sealed class PathQualityMetrics
|
||||
{
|
||||
/// <summary>创建全零、不可行的质量指标。</summary>
|
||||
public PathQualityMetrics()
|
||||
: this(false, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建完整的质量指标快照。</summary>
|
||||
public PathQualityMetrics(
|
||||
bool isFeasible,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters,
|
||||
double lengthChangePercent,
|
||||
double peakCurvatureChangePercent,
|
||||
double curvatureVariationChangePercent,
|
||||
double minimumClearanceChangeMeters)
|
||||
: this(
|
||||
isFeasible,
|
||||
pathLengthMeters,
|
||||
maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
0d,
|
||||
rootMeanSquareVehicleCurvaturePerMeter,
|
||||
totalAbsoluteCurvatureVariationPerMeter,
|
||||
curvatureVariationEnergy,
|
||||
minimumBodyClearanceMeters,
|
||||
lengthChangePercent,
|
||||
peakCurvatureChangePercent,
|
||||
curvatureVariationChangePercent,
|
||||
minimumClearanceChangeMeters)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建带有曲率导数峰值的完整质量指标快照。</summary>
|
||||
public PathQualityMetrics(
|
||||
bool isFeasible,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters,
|
||||
double lengthChangePercent,
|
||||
double peakCurvatureChangePercent,
|
||||
double curvatureVariationChangePercent,
|
||||
double minimumClearanceChangeMeters)
|
||||
{
|
||||
IsFeasible = isFeasible;
|
||||
PathLengthMeters = pathLengthMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter = maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter;
|
||||
RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
|
||||
TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
|
||||
CurvatureVariationEnergy = curvatureVariationEnergy;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
LengthChangePercent = lengthChangePercent;
|
||||
PeakCurvatureChangePercent = peakCurvatureChangePercent;
|
||||
CurvatureVariationChangePercent = curvatureVariationChangePercent;
|
||||
MinimumClearanceChangeMeters = minimumClearanceChangeMeters;
|
||||
}
|
||||
|
||||
/// <summary>该路径是否通过完整安全和运动学复核。</summary>
|
||||
public bool IsFeasible { get; }
|
||||
|
||||
/// <summary>路径总弧长，单位 m。</summary>
|
||||
public double PathLengthMeters { get; }
|
||||
|
||||
/// <summary>绝对车辆曲率峰值，单位 1/m。</summary>
|
||||
public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>绝对车辆曲率导数峰值，单位 1/m²。</summary>
|
||||
public double MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter { get; }
|
||||
|
||||
/// <summary>车辆曲率均方根，单位 1/m。</summary>
|
||||
public double RootMeanSquareVehicleCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>逐方向段累加的绝对曲率变化，单位 1/m。</summary>
|
||||
public double TotalAbsoluteCurvatureVariationPerMeter { get; }
|
||||
|
||||
/// <summary>逐方向段计算的曲率变化能量。</summary>
|
||||
public double CurvatureVariationEnergy { get; }
|
||||
|
||||
/// <summary>曲率变化代价的兼容名称。</summary>
|
||||
public double CurvatureVariationCost => CurvatureVariationEnergy;
|
||||
|
||||
/// <summary>完整扩大车体的最小保守净空，单位 m。</summary>
|
||||
public double MinimumBodyClearanceMeters { get; }
|
||||
|
||||
/// <summary>相对原始粗路径的长度变化百分比。</summary>
|
||||
public double LengthChangePercent { get; }
|
||||
|
||||
/// <summary>相对原始粗路径的峰值曲率变化百分比。</summary>
|
||||
public double PeakCurvatureChangePercent { get; }
|
||||
|
||||
/// <summary>相对原始粗路径的曲率变化百分比。</summary>
|
||||
public double CurvatureVariationChangePercent { get; }
|
||||
|
||||
/// <summary>相对原始粗路径的最小净空变化，单位 m。</summary>
|
||||
public double MinimumClearanceChangeMeters { get; }
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的公共配置；所有距离使用 m。</summary>
|
||||
public sealed class PathSmoothingConfiguration
|
||||
{
|
||||
/// <summary>创建带有安全默认值的平滑配置。</summary>
|
||||
public PathSmoothingConfiguration()
|
||||
{
|
||||
OutputSpacingMeters = 0.025d;
|
||||
MaximumCollisionCheckStepMeters = 0.025d;
|
||||
MinimumClearanceReserveMeters = 0.02d;
|
||||
SmoothingStrength = 1d;
|
||||
AllowFallbackToCoarsePath = true;
|
||||
RetryStrengthScales = new ReadOnlyCollection<double>(
|
||||
new List<double> { 1d, 0.75d, 0.50d, 0.25d });
|
||||
}
|
||||
|
||||
/// <summary>正式单算法入口使用的方法。</summary>
|
||||
public SmoothingMethod Method { get; set; }
|
||||
|
||||
/// <summary>输出路径的目标弧长采样间距，单位 m。</summary>
|
||||
public double OutputSpacingMeters { get; set; }
|
||||
|
||||
/// <summary>扫掠碰撞检查的最大步长，单位 m。</summary>
|
||||
public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
|
||||
/// <summary>平滑候选必须在最小净空之外保留的额外余量，单位 m。</summary>
|
||||
public double MinimumClearanceReserveMeters { get; set; }
|
||||
|
||||
/// <summary>算法初始平滑强度。</summary>
|
||||
public double SmoothingStrength { get; set; }
|
||||
|
||||
/// <summary>所有平滑尝试失败时是否允许发布经过复核的原粗路径。</summary>
|
||||
public bool AllowFallbackToCoarsePath { get; set; }
|
||||
|
||||
/// <summary>有限且严格递减的平滑强度重试比例。</summary>
|
||||
public IReadOnlyList<double> RetryStrengthScales { get; }
|
||||
|
||||
/// <summary>三次 B 样条专用参数。</summary>
|
||||
public CubicBSplineOptions CubicBSpline { get; } = new CubicBSplineOptions();
|
||||
|
||||
/// <summary>局部三次 Bézier 专用参数。</summary>
|
||||
public LocalCubicBezierOptions LocalCubicBezier { get; } = new LocalCubicBezierOptions();
|
||||
|
||||
/// <summary>分段五次多项式专用参数。</summary>
|
||||
public PiecewiseQuinticOptions PiecewiseQuintic { get; } = new PiecewiseQuinticOptions();
|
||||
|
||||
/// <summary>局部 G2 五次过渡专用参数。</summary>
|
||||
public LocalG2QuinticOptions LocalG2Quintic { get; } = new LocalG2QuinticOptions();
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>一次平滑尝试的不可变诊断信息。</summary>
|
||||
public sealed class PathSmoothingDiagnostics
|
||||
{
|
||||
/// <summary>创建不含路径指标的默认诊断信息。</summary>
|
||||
public PathSmoothingDiagnostics()
|
||||
: this(new PathQualityMetrics(), TimeSpan.Zero, 0, 0d, string.Empty)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建完整的平滑诊断快照。</summary>
|
||||
public PathSmoothingDiagnostics(
|
||||
PathQualityMetrics metrics,
|
||||
TimeSpan elapsed,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string terminationReason = null)
|
||||
{
|
||||
Metrics = metrics ?? new PathQualityMetrics();
|
||||
Elapsed = elapsed;
|
||||
RetryCount = retryCount;
|
||||
AcceptedStrength = acceptedStrength;
|
||||
TerminationReason = terminationReason ?? string.Empty;
|
||||
}
|
||||
|
||||
/// <summary>使用所有测量量创建平滑诊断快照。</summary>
|
||||
public PathSmoothingDiagnostics(
|
||||
bool isFeasible,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters,
|
||||
double lengthChangePercent,
|
||||
double peakCurvatureChangePercent,
|
||||
double curvatureVariationChangePercent,
|
||||
double minimumClearanceChangeMeters,
|
||||
TimeSpan elapsed,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string terminationReason = null)
|
||||
: this(
|
||||
new PathQualityMetrics(
|
||||
isFeasible,
|
||||
pathLengthMeters,
|
||||
maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
rootMeanSquareVehicleCurvaturePerMeter,
|
||||
totalAbsoluteCurvatureVariationPerMeter,
|
||||
curvatureVariationEnergy,
|
||||
minimumBodyClearanceMeters,
|
||||
lengthChangePercent,
|
||||
peakCurvatureChangePercent,
|
||||
curvatureVariationChangePercent,
|
||||
minimumClearanceChangeMeters),
|
||||
elapsed,
|
||||
retryCount,
|
||||
acceptedStrength,
|
||||
terminationReason)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>路径质量指标；始终非空。</summary>
|
||||
public PathQualityMetrics Metrics { get; }
|
||||
|
||||
/// <summary>从算法入口到返回诊断的耗时。</summary>
|
||||
public TimeSpan Elapsed { get; }
|
||||
|
||||
/// <summary>已执行的安全强度重试次数。</summary>
|
||||
public int RetryCount { get; }
|
||||
|
||||
/// <summary>通过复核的平滑强度；未接受候选时为零。</summary>
|
||||
public double AcceptedStrength { get; }
|
||||
|
||||
/// <summary>面向调用方的稳定终止说明。</summary>
|
||||
public string TerminationReason { get; }
|
||||
}
|
||||
+17
@@ -0,0 +1,17 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>局部 G2 区域未替换原始路径的稳定原因。</summary>
|
||||
public enum PathSmoothingRegionFailureReason
|
||||
{
|
||||
None,
|
||||
WindowUnavailable,
|
||||
CandidateGenerationFailed,
|
||||
Collision,
|
||||
InsufficientClearance,
|
||||
CurvatureExceeded,
|
||||
CurvatureOvershoot,
|
||||
DeviationExceeded,
|
||||
InsufficientImprovement,
|
||||
VariationCostRegression,
|
||||
GlobalValidationRollback,
|
||||
}
|
||||
@@ -0,0 +1,83 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>单个局部 G2 平滑区域的不可变发布报告。</summary>
|
||||
public sealed class PathSmoothingRegionReport
|
||||
{
|
||||
public PathSmoothingRegionReport(
|
||||
int segmentIndex,
|
||||
double startArcLengthMeters,
|
||||
double endArcLengthMeters,
|
||||
IReadOnlyList<double> curvatureJumpsPerMeter,
|
||||
double plannedWindowLengthMeters,
|
||||
double actualWindowLengthMeters,
|
||||
double leftWindowLengthMeters,
|
||||
double rightWindowLengthMeters,
|
||||
int candidateCount,
|
||||
int selectedCandidateIndex,
|
||||
PathSmoothingRegionStatus status,
|
||||
PathSmoothingRegionFailureReason failureReason,
|
||||
double rawPeakCurvatureDerivativePerSquareMeter,
|
||||
double resultPeakCurvatureDerivativePerSquareMeter,
|
||||
double rawCurvatureVariationCost,
|
||||
double resultCurvatureVariationCost,
|
||||
double maximumDeviationMeters,
|
||||
double minimumBodyClearanceMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter)
|
||||
{
|
||||
SegmentIndex = segmentIndex;
|
||||
StartArcLengthMeters = startArcLengthMeters;
|
||||
EndArcLengthMeters = endArcLengthMeters;
|
||||
CurvatureJumpsPerMeter = CopyReadOnly(curvatureJumpsPerMeter);
|
||||
PlannedWindowLengthMeters = plannedWindowLengthMeters;
|
||||
ActualWindowLengthMeters = actualWindowLengthMeters;
|
||||
LeftWindowLengthMeters = leftWindowLengthMeters;
|
||||
RightWindowLengthMeters = rightWindowLengthMeters;
|
||||
CandidateCount = candidateCount;
|
||||
SelectedCandidateIndex = status == PathSmoothingRegionStatus.Improved
|
||||
? selectedCandidateIndex
|
||||
: -1;
|
||||
Status = status;
|
||||
FailureReason = failureReason;
|
||||
RawPeakCurvatureDerivativePerSquareMeter = rawPeakCurvatureDerivativePerSquareMeter;
|
||||
ResultPeakCurvatureDerivativePerSquareMeter = resultPeakCurvatureDerivativePerSquareMeter;
|
||||
RawCurvatureVariationCost = rawCurvatureVariationCost;
|
||||
ResultCurvatureVariationCost = resultCurvatureVariationCost;
|
||||
MaximumDeviationMeters = maximumDeviationMeters;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
}
|
||||
|
||||
public int SegmentIndex { get; }
|
||||
public double StartArcLengthMeters { get; }
|
||||
public double EndArcLengthMeters { get; }
|
||||
public IReadOnlyList<double> CurvatureJumpsPerMeter { get; }
|
||||
public double PlannedWindowLengthMeters { get; }
|
||||
public double ActualWindowLengthMeters { get; }
|
||||
public double LeftWindowLengthMeters { get; }
|
||||
public double RightWindowLengthMeters { get; }
|
||||
public int CandidateCount { get; }
|
||||
public int SelectedCandidateIndex { get; }
|
||||
public PathSmoothingRegionStatus Status { get; }
|
||||
public PathSmoothingRegionFailureReason FailureReason { get; }
|
||||
public double RawPeakCurvatureDerivativePerSquareMeter { get; }
|
||||
public double ResultPeakCurvatureDerivativePerSquareMeter { get; }
|
||||
public double RawCurvatureVariationCost { get; }
|
||||
public double ResultCurvatureVariationCost { get; }
|
||||
public double MaximumDeviationMeters { get; }
|
||||
public double MinimumBodyClearanceMeters { get; }
|
||||
public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
|
||||
|
||||
private static IReadOnlyList<double> CopyReadOnly(IReadOnlyList<double> source)
|
||||
{
|
||||
var copy = new List<double>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<double>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>单个局部 G2 平滑区域的处理结果。</summary>
|
||||
public enum PathSmoothingRegionStatus
|
||||
{
|
||||
Improved,
|
||||
RetainedOriginal,
|
||||
}
|
||||
@@ -0,0 +1,97 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑所需的原始粗路径、复核上下文和配置。</summary>
|
||||
public sealed class PathSmoothingRequest
|
||||
{
|
||||
private readonly VehicleParameters _vehicle;
|
||||
private readonly PathSmoothingConfiguration _configuration;
|
||||
|
||||
/// <summary>创建路径平滑请求，并复制粗路径和方向分段集合。</summary>
|
||||
public PathSmoothingRequest(
|
||||
IReadOnlyList<CoarsePathPoint> coarsePath,
|
||||
IReadOnlyList<PathSegment> segments,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
PathSmoothingConfiguration configuration)
|
||||
{
|
||||
CoarsePath = CopyReadOnly(coarsePath);
|
||||
Segments = CopyReadOnly(segments);
|
||||
Map = map;
|
||||
_vehicle = CopyVehicle(vehicle);
|
||||
_configuration = CopyConfiguration(configuration);
|
||||
}
|
||||
|
||||
/// <summary>原始粗路径的不可变快照。</summary>
|
||||
public IReadOnlyList<CoarsePathPoint> CoarsePath { get; }
|
||||
|
||||
/// <summary>原始粗路径方向分段的不可变快照。</summary>
|
||||
public IReadOnlyList<PathSegment> Segments { get; }
|
||||
|
||||
/// <summary>用于平滑后完整车体复核的规划栅格地图。</summary>
|
||||
public PlanningGridMap Map { get; }
|
||||
|
||||
/// <summary>车辆几何与最大曲率约束的不可变快照副本。</summary>
|
||||
public VehicleParameters Vehicle => CopyVehicle(_vehicle);
|
||||
|
||||
/// <summary>本次平滑配置的不可变快照副本。</summary>
|
||||
public PathSmoothingConfiguration Configuration => CopyConfiguration(_configuration);
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
|
||||
private static VehicleParameters CopyVehicle(VehicleParameters source)
|
||||
{
|
||||
if (source == null) return null;
|
||||
return new VehicleParameters
|
||||
{
|
||||
LengthMeters = source.LengthMeters,
|
||||
WidthMeters = source.WidthMeters,
|
||||
SafetyMarginMeters = source.SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
|
||||
MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
|
||||
};
|
||||
}
|
||||
|
||||
private static PathSmoothingConfiguration CopyConfiguration(PathSmoothingConfiguration source)
|
||||
{
|
||||
if (source == null) return null;
|
||||
var copy = new PathSmoothingConfiguration
|
||||
{
|
||||
Method = source.Method,
|
||||
OutputSpacingMeters = source.OutputSpacingMeters,
|
||||
MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
|
||||
MinimumClearanceReserveMeters = source.MinimumClearanceReserveMeters,
|
||||
SmoothingStrength = source.SmoothingStrength,
|
||||
AllowFallbackToCoarsePath = source.AllowFallbackToCoarsePath,
|
||||
};
|
||||
copy.CubicBSpline.EndpointTangentScale = source.CubicBSpline.EndpointTangentScale;
|
||||
copy.LocalCubicBezier.CornerHeadingThresholdRadians = source.LocalCubicBezier.CornerHeadingThresholdRadians;
|
||||
copy.LocalCubicBezier.MaximumWindowLengthMeters = source.LocalCubicBezier.MaximumWindowLengthMeters;
|
||||
copy.LocalCubicBezier.HandleLengthRatio = source.LocalCubicBezier.HandleLengthRatio;
|
||||
copy.PiecewiseQuintic.KnotSpacingMeters = source.PiecewiseQuintic.KnotSpacingMeters;
|
||||
copy.PiecewiseQuintic.MinimumKnotSpacingMeters = source.PiecewiseQuintic.MinimumKnotSpacingMeters;
|
||||
copy.LocalG2Quintic.MinimumWindowLengthMeters = source.LocalG2Quintic.MinimumWindowLengthMeters;
|
||||
copy.LocalG2Quintic.PreferredWindowLengthMeters = source.LocalG2Quintic.PreferredWindowLengthMeters;
|
||||
copy.LocalG2Quintic.MaximumWindowLengthMeters = source.LocalG2Quintic.MaximumWindowLengthMeters;
|
||||
copy.LocalG2Quintic.MaximumDeviationMeters = source.LocalG2Quintic.MaximumDeviationMeters;
|
||||
copy.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = source.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter;
|
||||
copy.LocalG2Quintic.CurvatureJumpRatioOfMaximum = source.LocalG2Quintic.CurvatureJumpRatioOfMaximum;
|
||||
copy.LocalG2Quintic.MinimumPeakGradientImprovementRatio = source.LocalG2Quintic.MinimumPeakGradientImprovementRatio;
|
||||
copy.LocalG2Quintic.MaximumVariationCostRegressionRatio = source.LocalG2Quintic.MaximumVariationCostRegressionRatio;
|
||||
copy.LocalG2Quintic.MaximumCandidatesPerRegion = source.LocalG2Quintic.MaximumCandidatesPerRegion;
|
||||
return copy;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,154 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的最终不可变结果。</summary>
|
||||
public sealed class PathSmoothingResult
|
||||
{
|
||||
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
||||
new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
||||
private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
|
||||
new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
|
||||
private static readonly IReadOnlyList<PathSmoothingRegionReport> EmptyRegionReports =
|
||||
new ReadOnlyCollection<PathSmoothingRegionReport>(new List<PathSmoothingRegionReport>());
|
||||
|
||||
private PathSmoothingResult(
|
||||
PathSmoothingStatus status,
|
||||
SmoothingMethod? method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics,
|
||||
IReadOnlyList<PathSmoothingRegionReport> regionReports)
|
||||
{
|
||||
Status = status;
|
||||
Method = method;
|
||||
Path = path;
|
||||
Segments = segments;
|
||||
RegionReports = regionReports;
|
||||
Diagnostics = diagnostics ?? new PathSmoothingDiagnostics(
|
||||
new PathQualityMetrics(),
|
||||
TimeSpan.Zero,
|
||||
0,
|
||||
0d,
|
||||
"No smoothing diagnostics were supplied.");
|
||||
}
|
||||
|
||||
/// <summary>最终发布状态。</summary>
|
||||
public PathSmoothingStatus Status { get; }
|
||||
|
||||
/// <summary>成功或回退时的实际（或尝试）平滑方法；失败时为空。</summary>
|
||||
public SmoothingMethod? Method { get; }
|
||||
|
||||
/// <summary>成功或经过复核的回退路径；其他状态始终为空且不可变。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>覆盖 <see cref="Path"/> 的方向分段；其他状态始终为空且不可变。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>局部 G2 各检测区域的不可变报告；传统算法结果为空。</summary>
|
||||
public IReadOnlyList<PathSmoothingRegionReport> RegionReports { get; }
|
||||
|
||||
/// <summary>本次平滑的质量和终止诊断；始终非空。</summary>
|
||||
public PathSmoothingDiagnostics Diagnostics { get; }
|
||||
|
||||
/// <summary>创建已通过所有复核的平滑结果。</summary>
|
||||
public static PathSmoothingResult Success(
|
||||
SmoothingMethod method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
ValidatePublishedResult(method, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
PathSmoothingStatus.Success,
|
||||
method,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
EmptyRegionReports);
|
||||
}
|
||||
|
||||
/// <summary>创建经过完整复核的原始粗路径回退结果。</summary>
|
||||
public static PathSmoothingResult Fallback(
|
||||
SmoothingMethod attemptedMethod,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
ValidatePublishedResult(attemptedMethod, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
PathSmoothingStatus.FallbackToCoarsePath,
|
||||
attemptedMethod,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
EmptyRegionReports);
|
||||
}
|
||||
|
||||
/// <summary>发布经过完整复核的局部 G2 预平滑结果。</summary>
|
||||
public static PathSmoothingResult PublishLocalG2(
|
||||
PathSmoothingStatus status,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics,
|
||||
IReadOnlyList<PathSmoothingRegionReport> regionReports)
|
||||
{
|
||||
if (status != PathSmoothingStatus.Complete &&
|
||||
status != PathSmoothingStatus.PartialImprovement &&
|
||||
status != PathSmoothingStatus.NotNeeded &&
|
||||
status != PathSmoothingStatus.Unchanged)
|
||||
throw new ArgumentException("Use a Local G2 publication status.", nameof(status));
|
||||
if (regionReports == null)
|
||||
throw new ArgumentNullException(nameof(regionReports));
|
||||
|
||||
ValidatePublishedResult(SmoothingMethod.LocalG2Quintic, path, segments, diagnostics);
|
||||
return new PathSmoothingResult(
|
||||
status,
|
||||
SmoothingMethod.LocalG2Quintic,
|
||||
CopyReadOnly(path),
|
||||
CopyReadOnly(segments),
|
||||
diagnostics,
|
||||
CopyReadOnly(regionReports));
|
||||
}
|
||||
|
||||
/// <summary>创建不发布路径的失败、不可行、取消或输入无效结果。</summary>
|
||||
public static PathSmoothingResult Failure(PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
if (status == PathSmoothingStatus.Success ||
|
||||
status == PathSmoothingStatus.FallbackToCoarsePath ||
|
||||
status == PathSmoothingStatus.Complete ||
|
||||
status == PathSmoothingStatus.PartialImprovement ||
|
||||
status == PathSmoothingStatus.NotNeeded ||
|
||||
status == PathSmoothingStatus.Unchanged)
|
||||
throw new ArgumentException("Use Success or Fallback to publish a path.", nameof(status));
|
||||
if (!Enum.IsDefined(typeof(PathSmoothingStatus), status))
|
||||
throw new ArgumentOutOfRangeException(nameof(status));
|
||||
return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics, EmptyRegionReports);
|
||||
}
|
||||
|
||||
private static void ValidatePublishedResult(
|
||||
SmoothingMethod method,
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathSmoothingDiagnostics diagnostics)
|
||||
{
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), method))
|
||||
throw new ArgumentOutOfRangeException(nameof(method));
|
||||
if (path == null || path.Count == 0)
|
||||
throw new ArgumentException("Published smoothing results require a non-empty path.", nameof(path));
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("Published smoothing results require non-empty segments.", nameof(segments));
|
||||
if (diagnostics == null || diagnostics.Metrics == null || !diagnostics.Metrics.IsFeasible)
|
||||
throw new ArgumentException("Published smoothing results require feasible diagnostics.", nameof(diagnostics));
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>路径平滑的最终发布状态。</summary>
|
||||
public enum PathSmoothingStatus
|
||||
{
|
||||
Success,
|
||||
FallbackToCoarsePath,
|
||||
InvalidInput,
|
||||
Infeasible,
|
||||
Failed,
|
||||
Cancelled,
|
||||
Complete,
|
||||
PartialImprovement,
|
||||
NotNeeded,
|
||||
Unchanged,
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>分段五次 Hermite 平滑参数。</summary>
|
||||
public sealed class PiecewiseQuinticOptions
|
||||
{
|
||||
/// <summary>相邻内部结点的目标距离，单位 m。</summary>
|
||||
public double KnotSpacingMeters { get; set; } = 0.50d;
|
||||
|
||||
/// <summary>允许创建内部结点的最小间距，单位 m。</summary>
|
||||
public double MinimumKnotSpacingMeters { get; set; } = 0.10d;
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>平滑空间路径上的不可变采样点；位置和长度单位为 m，航向为 rad，曲率为 1/m。</summary>
|
||||
public sealed class SmoothedPathPoint
|
||||
{
|
||||
public SmoothedPathPoint(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double headingRadians,
|
||||
double unwrappedHeadingRadians,
|
||||
double arcLengthMeters,
|
||||
TravelDirection direction,
|
||||
double geometricCurvaturePerMeter,
|
||||
double vehicleCurvaturePerMeter,
|
||||
double bodyClearanceMeters,
|
||||
bool isGearSwitchPoint,
|
||||
SmoothedPathPointSource source)
|
||||
: this(
|
||||
xMeters,
|
||||
yMeters,
|
||||
headingRadians,
|
||||
unwrappedHeadingRadians,
|
||||
arcLengthMeters,
|
||||
direction,
|
||||
geometricCurvaturePerMeter,
|
||||
vehicleCurvaturePerMeter,
|
||||
0d,
|
||||
bodyClearanceMeters,
|
||||
isGearSwitchPoint,
|
||||
source)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建带有车辆曲率对弧长导数的不可变采样点。</summary>
|
||||
public SmoothedPathPoint(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double headingRadians,
|
||||
double unwrappedHeadingRadians,
|
||||
double arcLengthMeters,
|
||||
TravelDirection direction,
|
||||
double geometricCurvaturePerMeter,
|
||||
double vehicleCurvaturePerMeter,
|
||||
double vehicleCurvatureDerivativePerSquareMeter,
|
||||
double bodyClearanceMeters,
|
||||
bool isGearSwitchPoint,
|
||||
SmoothedPathPointSource source)
|
||||
{
|
||||
X = xMeters;
|
||||
Y = yMeters;
|
||||
Heading = headingRadians;
|
||||
UnwrappedHeading = unwrappedHeadingRadians;
|
||||
ArcLength = arcLengthMeters;
|
||||
Direction = direction;
|
||||
GeometricCurvature = geometricCurvaturePerMeter;
|
||||
VehicleCurvature = vehicleCurvaturePerMeter;
|
||||
VehicleCurvatureDerivative = vehicleCurvatureDerivativePerSquareMeter;
|
||||
BodyClearance = bodyClearanceMeters;
|
||||
IsGearSwitchPoint = isGearSwitchPoint;
|
||||
Source = source;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 坐标，单位 m。</summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>世界 Y 坐标，单位 m。</summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>归一化的车辆航向，单位 rad。</summary>
|
||||
public double Heading { get; }
|
||||
|
||||
/// <summary>连续展开的车辆航向，单位 rad。</summary>
|
||||
public double UnwrappedHeading { get; }
|
||||
|
||||
/// <summary>从完整路径起点累计的弧长，单位 m。</summary>
|
||||
public double ArcLength { get; }
|
||||
|
||||
/// <summary>该点所在连续方向段的行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>按几何弧长计算的有符号路径曲率，单位 1/m。</summary>
|
||||
public double GeometricCurvature { get; }
|
||||
|
||||
/// <summary>车辆模型使用的有符号曲率，单位 1/m。</summary>
|
||||
public double VehicleCurvature { get; }
|
||||
|
||||
/// <summary>车辆曲率对弧长的导数 dκ/ds，单位 1/m²。</summary>
|
||||
public double VehicleCurvatureDerivative { get; }
|
||||
|
||||
/// <summary>扩大车体后的保守净空下界，单位 m。</summary>
|
||||
public double BodyClearance { get; }
|
||||
|
||||
/// <summary>该点是否为新方向段开始的换向点。</summary>
|
||||
public bool IsGearSwitchPoint { get; }
|
||||
|
||||
/// <summary>该点在平滑流程中的来源。</summary>
|
||||
public SmoothedPathPointSource Source { get; }
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>平滑路径点的来源。</summary>
|
||||
public enum SmoothedPathPointSource
|
||||
{
|
||||
Anchor,
|
||||
Interpolated,
|
||||
GearSwitch,
|
||||
CoarsePathFallback,
|
||||
LocalG2Transition,
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>平滑路径中方向一致的连续点范围；起止索引均包含在内。</summary>
|
||||
public sealed class SmoothedPathSegment
|
||||
{
|
||||
public SmoothedPathSegment(
|
||||
int segmentIndex,
|
||||
TravelDirection direction,
|
||||
int startIndex,
|
||||
int endIndex,
|
||||
bool startsAtGearSwitch,
|
||||
bool endsAtGearSwitch)
|
||||
{
|
||||
SegmentIndex = segmentIndex;
|
||||
Direction = direction;
|
||||
StartIndex = startIndex;
|
||||
EndIndex = endIndex;
|
||||
StartsAtGearSwitch = startsAtGearSwitch;
|
||||
EndsAtGearSwitch = endsAtGearSwitch;
|
||||
}
|
||||
|
||||
/// <summary>从零开始的分段序号。</summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>本段行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>本段在平滑路径中的起始包含式索引。</summary>
|
||||
public int StartIndex { get; }
|
||||
|
||||
/// <summary>本段在平滑路径中的结束包含式索引。</summary>
|
||||
public int EndIndex { get; }
|
||||
|
||||
/// <summary>本段首点是否为换向后保留的新方向点。</summary>
|
||||
public bool StartsAtGearSwitch { get; }
|
||||
|
||||
/// <summary>本段末点是否紧邻下一方向段的换向对。</summary>
|
||||
public bool EndsAtGearSwitch { get; }
|
||||
}
|
||||
@@ -0,0 +1,10 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
/// <summary>支持的粗路径平滑方法。</summary>
|
||||
public enum SmoothingMethod
|
||||
{
|
||||
CubicBSpline,
|
||||
LocalCubicBezier,
|
||||
PiecewiseQuintic,
|
||||
LocalG2Quintic,
|
||||
}
|
||||
+231
@@ -0,0 +1,231 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
/// <summary>以固定预热和五次测量隔离比较所有请求平滑方法的离线入口。</summary>
|
||||
public sealed class PathSmoothingComparisonService
|
||||
{
|
||||
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
|
||||
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
||||
|
||||
/// <summary>比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。</summary>
|
||||
public PathSmoothingComparisonResult Compare(
|
||||
PathSmoothingComparisonRequest request,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
PathSmoothingComparisonEntry baseline = CreateRawPathBaseline(request, out string baselineReason);
|
||||
var entries = new List<PathSmoothingComparisonEntry>();
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Cancelled(baseline, entries);
|
||||
|
||||
if (request == null || request.SmoothingRequest == null)
|
||||
return new PathSmoothingComparisonResult(baseline, entries, null, false, baselineReason);
|
||||
|
||||
for (int methodIndex = 0; methodIndex < request.Methods.Count; methodIndex++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Cancelled(baseline, entries);
|
||||
|
||||
SmoothingMethod method = request.Methods[methodIndex];
|
||||
if (!TryCompareMethod(
|
||||
request.SmoothingRequest,
|
||||
method,
|
||||
baseline.Metrics,
|
||||
cancellationToken,
|
||||
out PathSmoothingComparisonEntry entry))
|
||||
return Cancelled(baseline, entries);
|
||||
entries.Add(entry);
|
||||
}
|
||||
|
||||
return new PathSmoothingComparisonResult(
|
||||
baseline,
|
||||
entries,
|
||||
SmoothingMethodRanker.Rank(entries),
|
||||
false,
|
||||
baselineReason);
|
||||
}
|
||||
|
||||
private bool TryCompareMethod(
|
||||
PathSmoothingRequest sourceRequest,
|
||||
SmoothingMethod method,
|
||||
PathQualityMetrics rawMetrics,
|
||||
CancellationToken cancellationToken,
|
||||
out PathSmoothingComparisonEntry entry)
|
||||
{
|
||||
entry = null;
|
||||
try
|
||||
{
|
||||
PathSmoothingRequest methodRequest = CreateMethodRequest(sourceRequest, method);
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PathSmoothingResult warmup = _smoothingService.Smooth(methodRequest, cancellationToken);
|
||||
if (warmup.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
|
||||
|
||||
var timings = new List<double>(5);
|
||||
var measuredResults = new List<PathSmoothingResult>(5);
|
||||
for (int sampleIndex = 0; sampleIndex < 5; sampleIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Stopwatch stopwatch = Stopwatch.StartNew();
|
||||
PathSmoothingResult result = _smoothingService.Smooth(methodRequest, cancellationToken);
|
||||
stopwatch.Stop();
|
||||
if (result.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
|
||||
timings.Add(stopwatch.Elapsed.TotalMilliseconds);
|
||||
measuredResults.Add(result);
|
||||
}
|
||||
|
||||
PathSmoothingResult canonical = measuredResults[0];
|
||||
string digest = StableGeometryDigest.Compute(canonical);
|
||||
SmoothingTimingSummary timing = SmoothingTimingSummary.FromMeasurements(timings, measuredResults);
|
||||
string diagnostic = string.IsNullOrWhiteSpace(timing.Diagnostic)
|
||||
? canonical.Diagnostics.TerminationReason
|
||||
: timing.Diagnostic;
|
||||
|
||||
PathQualityMetrics metrics = canonical.Status == PathSmoothingStatus.Success
|
||||
? NormalizeMetrics(canonical.Diagnostics.Metrics, rawMetrics)
|
||||
: new PathQualityMetrics();
|
||||
entry = PathSmoothingComparisonEntry.CreateCandidate(
|
||||
method,
|
||||
canonical.Status,
|
||||
metrics,
|
||||
timing,
|
||||
digest,
|
||||
diagnostic,
|
||||
canonical.Diagnostics.RetryCount,
|
||||
canonical.Diagnostics.AcceptedStrength,
|
||||
canonical.Path,
|
||||
canonical.Segments);
|
||||
return true;
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
entry = PathSmoothingComparisonEntry.CreateCandidate(
|
||||
method,
|
||||
PathSmoothingStatus.Failed,
|
||||
new PathQualityMetrics(),
|
||||
new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, exception.GetType().Name),
|
||||
string.Empty,
|
||||
exception.Message,
|
||||
0,
|
||||
0d,
|
||||
null,
|
||||
null);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
private PathSmoothingComparisonEntry CreateRawPathBaseline(
|
||||
PathSmoothingComparisonRequest request,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (request == null || request.SmoothingRequest == null)
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求为空。", out reason);
|
||||
|
||||
PathSmoothingRequest smoothingRequest = request.SmoothingRequest;
|
||||
PathSmoothingConfiguration configuration = smoothingRequest.Configuration;
|
||||
if (configuration == null || smoothingRequest.Map == null || smoothingRequest.Vehicle == null)
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求缺少可用的地图、车辆或配置。", out reason);
|
||||
|
||||
if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason))
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
|
||||
if (!RawPathBaselineBuilder.TryCreate(
|
||||
smoothingRequest,
|
||||
preparedPath,
|
||||
_analyzer,
|
||||
configuration.OutputSpacingMeters,
|
||||
_validator,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline rawPath,
|
||||
out reason))
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
|
||||
|
||||
string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, rawPath.Path, rawPath.Segments);
|
||||
return PathSmoothingComparisonEntry.CreateRawPathBaseline(
|
||||
PathSmoothingStatus.Success,
|
||||
rawPath.Metrics,
|
||||
digest,
|
||||
string.Empty,
|
||||
rawPath.Path,
|
||||
rawPath.Segments);
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonEntry FailedBaseline(
|
||||
PathSmoothingStatus status,
|
||||
string failureReason,
|
||||
out string reason)
|
||||
{
|
||||
reason = failureReason ?? string.Empty;
|
||||
return PathSmoothingComparisonEntry.CreateRawPathBaseline(
|
||||
status,
|
||||
new PathQualityMetrics(),
|
||||
StableGeometryDigest.Compute(status, null, null, null),
|
||||
reason,
|
||||
null,
|
||||
null);
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonResult Cancelled(
|
||||
PathSmoothingComparisonEntry baseline,
|
||||
IReadOnlyList<PathSmoothingComparisonEntry> entries)
|
||||
{
|
||||
return new PathSmoothingComparisonResult(baseline, entries, null, true, "路径平滑比较已取消。");
|
||||
}
|
||||
|
||||
private static PathSmoothingRequest CreateMethodRequest(PathSmoothingRequest source, SmoothingMethod method)
|
||||
{
|
||||
PathSmoothingConfiguration configuration = source.Configuration;
|
||||
configuration.Method = method;
|
||||
configuration.AllowFallbackToCoarsePath = false;
|
||||
return new PathSmoothingRequest(
|
||||
source.CoarsePath,
|
||||
source.Segments,
|
||||
source.Map,
|
||||
source.Vehicle,
|
||||
configuration);
|
||||
}
|
||||
|
||||
private static PathQualityMetrics NormalizeMetrics(
|
||||
PathQualityMetrics candidate,
|
||||
PathQualityMetrics raw)
|
||||
{
|
||||
if (candidate == null || raw == null || !candidate.IsFeasible)
|
||||
return new PathQualityMetrics();
|
||||
|
||||
return new PathQualityMetrics(
|
||||
true,
|
||||
candidate.PathLengthMeters,
|
||||
candidate.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
candidate.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
candidate.RootMeanSquareVehicleCurvaturePerMeter,
|
||||
candidate.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
candidate.CurvatureVariationEnergy,
|
||||
candidate.MinimumBodyClearanceMeters,
|
||||
RelativePercentOrAbsoluteDelta(candidate.PathLengthMeters, raw.PathLengthMeters),
|
||||
RelativePercentOrAbsoluteDelta(
|
||||
candidate.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
raw.MaximumAbsoluteVehicleCurvaturePerMeter),
|
||||
RelativePercentOrAbsoluteDelta(
|
||||
candidate.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
raw.TotalAbsoluteCurvatureVariationPerMeter),
|
||||
candidate.MinimumBodyClearanceMeters - raw.MinimumBodyClearanceMeters);
|
||||
}
|
||||
|
||||
private static double RelativePercentOrAbsoluteDelta(double candidate, double raw)
|
||||
{
|
||||
double delta = candidate - raw;
|
||||
return Math.Abs(raw) < 1e-12d ? delta : delta / raw * 100d;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,305 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
/// <summary>正式单算法路径平滑入口，负责输入校验、有限重试与经过复核的粗路径回退。</summary>
|
||||
public sealed class PathSmoothingService
|
||||
{
|
||||
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
|
||||
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
||||
private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
|
||||
private readonly IPathSmoother _bezier = new LocalCubicBezierSmoother();
|
||||
private readonly IPathSmoother _quintic = new PiecewiseQuinticSmoother();
|
||||
private readonly LocalG2PreSmoothingPipeline _localG2Pipeline = new LocalG2PreSmoothingPipeline();
|
||||
|
||||
/// <summary>执行一次经过完整安全复核的单算法平滑。</summary>
|
||||
public PathSmoothingResult Smooth(
|
||||
PathSmoothingRequest request,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
try
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!TryValidateRequest(request, out PathSmoothingConfiguration configuration, out string reason))
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
||||
|
||||
if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
||||
|
||||
if (!RawPathBaselineBuilder.TryCreate(
|
||||
request,
|
||||
preparedPath,
|
||||
_analyzer,
|
||||
configuration.OutputSpacingMeters,
|
||||
_validator,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline rawBaseline,
|
||||
out reason))
|
||||
{
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
||||
}
|
||||
|
||||
if (configuration.Method == SmoothingMethod.LocalG2Quintic)
|
||||
return _localG2Pipeline.Smooth(request, preparedPath, rawBaseline, cancellationToken);
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
var input = new SmoothingAlgorithmInput(
|
||||
preparedPath,
|
||||
request.Map,
|
||||
request.Vehicle,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
configuration.MinimumClearanceReserveMeters,
|
||||
new SmoothingOptionsSnapshot(configuration));
|
||||
SmoothingAlgorithmRunner.AlgorithmRunResult runResult = _runner.Run(
|
||||
Resolve(configuration.Method), input, configuration, cancellationToken);
|
||||
int retryCount = GetRetryCount(runResult.AttemptedStrengths);
|
||||
PathSmoothingDiagnostics diagnostics = new PathSmoothingDiagnostics(
|
||||
runResult.Metrics,
|
||||
stopwatch.Elapsed,
|
||||
retryCount,
|
||||
runResult.AcceptedStrength,
|
||||
runResult.Reason);
|
||||
|
||||
if (runResult.Status == PathSmoothingStatus.Success)
|
||||
{
|
||||
return PathSmoothingResult.Success(
|
||||
configuration.Method,
|
||||
runResult.Path,
|
||||
runResult.Segments,
|
||||
diagnostics);
|
||||
}
|
||||
|
||||
if (!configuration.AllowFallbackToCoarsePath)
|
||||
return PathSmoothingResult.Failure(runResult.Status, diagnostics);
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!TryCreateVerifiedFallback(
|
||||
request,
|
||||
configuration,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothedPathPoint> fallbackPath,
|
||||
out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
|
||||
out PathQualityMetrics fallbackMetrics,
|
||||
out reason))
|
||||
{
|
||||
return PathSmoothingResult.Failure(runResult.Status, diagnostics);
|
||||
}
|
||||
|
||||
var fallbackDiagnostics = new PathSmoothingDiagnostics(
|
||||
fallbackMetrics,
|
||||
stopwatch.Elapsed,
|
||||
retryCount,
|
||||
runResult.AcceptedStrength,
|
||||
runResult.Reason);
|
||||
return PathSmoothingResult.Fallback(
|
||||
configuration.Method,
|
||||
fallbackPath,
|
||||
fallbackSegments,
|
||||
fallbackDiagnostics);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Cancelled, stopwatch, 0, 0d, "路径平滑已取消。");
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, 0, 0d, exception.Message);
|
||||
}
|
||||
}
|
||||
|
||||
private bool TryCreateVerifiedFallback(
|
||||
PathSmoothingRequest request,
|
||||
PathSmoothingConfiguration configuration,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothedPathPoint> fallbackPath,
|
||||
out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
|
||||
out PathQualityMetrics fallbackMetrics,
|
||||
out string reason)
|
||||
{
|
||||
fallbackPath = null;
|
||||
fallbackSegments = null;
|
||||
fallbackMetrics = null;
|
||||
reason = string.Empty;
|
||||
|
||||
// Reprepare from the immutable request instead of reusing the algorithm input: fallback is a
|
||||
// separately published output and must repeat the coarse-path contract validation.
|
||||
if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false;
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
if (!RawPathBaselineBuilder.TryCreate(
|
||||
request,
|
||||
revalidatedPath,
|
||||
_analyzer,
|
||||
configuration.OutputSpacingMeters,
|
||||
_validator,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline rawPath,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
fallbackPath = ToFallbackPoints(rawPath.Path);
|
||||
fallbackSegments = rawPath.Segments;
|
||||
fallbackMetrics = rawPath.Metrics;
|
||||
return true;
|
||||
}
|
||||
|
||||
private IPathSmoother Resolve(SmoothingMethod method)
|
||||
{
|
||||
return method switch
|
||||
{
|
||||
SmoothingMethod.CubicBSpline => _bSpline,
|
||||
SmoothingMethod.LocalCubicBezier => _bezier,
|
||||
SmoothingMethod.PiecewiseQuintic => _quintic,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(method)),
|
||||
};
|
||||
}
|
||||
|
||||
private static bool TryValidateRequest(
|
||||
PathSmoothingRequest request,
|
||||
out PathSmoothingConfiguration configuration,
|
||||
out string reason)
|
||||
{
|
||||
configuration = null;
|
||||
reason = string.Empty;
|
||||
if (request == null)
|
||||
{
|
||||
reason = "平滑请求为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
configuration = request.Configuration;
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
if (configuration == null || request.Map == null || !request.Map.PlanningReady || vehicle == null)
|
||||
{
|
||||
reason = "平滑请求缺少可用的地图、车辆或配置。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
|
||||
!NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _))
|
||||
{
|
||||
reason = "平滑请求中的车辆几何或曲率约束无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), configuration.Method))
|
||||
{
|
||||
reason = "平滑方法无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
|
||||
!NumericGuard.IsFinite(configuration.MinimumClearanceReserveMeters) ||
|
||||
configuration.MinimumClearanceReserveMeters < 0d ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.SmoothingStrength))
|
||||
{
|
||||
reason = "平滑配置包含非法数值或不满足方法契约。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (configuration.Method == SmoothingMethod.LocalG2Quintic)
|
||||
return IsValidLocalG2Options(configuration.LocalG2Quintic, out reason);
|
||||
|
||||
if (
|
||||
!NumericGuard.IsPositiveFinite(configuration.CubicBSpline.EndpointTangentScale) ||
|
||||
!IsValidBezierThreshold(configuration.LocalCubicBezier.CornerHeadingThresholdRadians) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.MaximumWindowLengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.HandleLengthRatio) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.KnotSpacingMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.MinimumKnotSpacingMeters) ||
|
||||
configuration.PiecewiseQuintic.KnotSpacingMeters < configuration.PiecewiseQuintic.MinimumKnotSpacingMeters)
|
||||
{
|
||||
reason = "平滑配置包含非法数值或不满足方法契约。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidLocalG2Options(LocalG2QuinticOptions options, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (options != null &&
|
||||
NumericGuard.IsPositiveFinite(options.MinimumWindowLengthMeters) &&
|
||||
NumericGuard.IsFinite(options.PreferredWindowLengthMeters) &&
|
||||
options.PreferredWindowLengthMeters >= options.MinimumWindowLengthMeters &&
|
||||
NumericGuard.IsFinite(options.MaximumWindowLengthMeters) &&
|
||||
options.MaximumWindowLengthMeters >= options.PreferredWindowLengthMeters &&
|
||||
NumericGuard.IsPositiveFinite(options.MaximumDeviationMeters) &&
|
||||
NumericGuard.IsPositiveFinite(options.AbsoluteCurvatureJumpFloorPerMeter) &&
|
||||
NumericGuard.IsFinite(options.CurvatureJumpRatioOfMaximum) &&
|
||||
options.CurvatureJumpRatioOfMaximum > 0d && options.CurvatureJumpRatioOfMaximum <= 1d &&
|
||||
NumericGuard.IsFinite(options.MinimumPeakGradientImprovementRatio) &&
|
||||
options.MinimumPeakGradientImprovementRatio > 0d && options.MinimumPeakGradientImprovementRatio < 1d &&
|
||||
NumericGuard.IsFinite(options.MaximumVariationCostRegressionRatio) &&
|
||||
options.MaximumVariationCostRegressionRatio >= 0d && options.MaximumCandidatesPerRegion >= 1)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
reason = "局部 G2 配置包含非法数值或不满足方法契约。";
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool IsValidBezierThreshold(double thresholdRadians)
|
||||
{
|
||||
return NumericGuard.IsFinite(thresholdRadians) && thresholdRadians > 0d && thresholdRadians <= Math.PI;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothedPathPoint> ToFallbackPoints(IReadOnlyList<SmoothedPathPoint> path)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>(path.Count);
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint point = path[index];
|
||||
points.Add(new SmoothedPathPoint(
|
||||
point.X,
|
||||
point.Y,
|
||||
point.Heading,
|
||||
point.UnwrappedHeading,
|
||||
point.ArcLength,
|
||||
point.Direction,
|
||||
point.GeometricCurvature,
|
||||
point.VehicleCurvature,
|
||||
point.VehicleCurvatureDerivative,
|
||||
point.BodyClearance,
|
||||
point.IsGearSwitchPoint,
|
||||
SmoothedPathPointSource.CoarsePathFallback));
|
||||
}
|
||||
return points;
|
||||
}
|
||||
|
||||
private static int GetRetryCount(IReadOnlyList<double> attemptedStrengths)
|
||||
{
|
||||
return attemptedStrengths == null || attemptedStrengths.Count == 0 ? 0 : attemptedStrengths.Count - 1;
|
||||
}
|
||||
|
||||
private static PathSmoothingResult Failure(
|
||||
PathSmoothingStatus status,
|
||||
Stopwatch stopwatch,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string reason)
|
||||
{
|
||||
return PathSmoothingResult.Failure(
|
||||
status,
|
||||
new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, retryCount, acceptedStrength, reason));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
|
||||
internal sealed class CurvatureTransition
|
||||
{
|
||||
internal CurvatureTransition(
|
||||
int segmentIndex,
|
||||
int leftCoarsePathIndex,
|
||||
int rightCoarsePathIndex,
|
||||
double localArcLengthMeters,
|
||||
double x,
|
||||
double y,
|
||||
double vehicleHeadingRadians,
|
||||
double leftVehicleCurvaturePerMeter,
|
||||
double rightVehicleCurvaturePerMeter)
|
||||
{
|
||||
if (segmentIndex < 0 || leftCoarsePathIndex < 0 || rightCoarsePathIndex != leftCoarsePathIndex + 1)
|
||||
throw new ArgumentOutOfRangeException(nameof(leftCoarsePathIndex));
|
||||
if (!NumericGuard.IsFinite(localArcLengthMeters) || localArcLengthMeters < 0d ||
|
||||
!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(vehicleHeadingRadians) ||
|
||||
!NumericGuard.IsFinite(leftVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(rightVehicleCurvaturePerMeter))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(localArcLengthMeters));
|
||||
}
|
||||
|
||||
double curvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter);
|
||||
if (!NumericGuard.IsFinite(curvatureJumpPerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(rightVehicleCurvaturePerMeter));
|
||||
|
||||
SegmentIndex = segmentIndex;
|
||||
LeftCoarsePathIndex = leftCoarsePathIndex;
|
||||
RightCoarsePathIndex = rightCoarsePathIndex;
|
||||
LocalArcLengthMeters = localArcLengthMeters;
|
||||
X = x;
|
||||
Y = y;
|
||||
VehicleHeadingRadians = vehicleHeadingRadians;
|
||||
LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter;
|
||||
RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter;
|
||||
CurvatureJumpPerMeter = curvatureJumpPerMeter;
|
||||
}
|
||||
|
||||
internal int SegmentIndex { get; }
|
||||
internal int LeftCoarsePathIndex { get; }
|
||||
internal int RightCoarsePathIndex { get; }
|
||||
internal double LocalArcLengthMeters { get; }
|
||||
internal double X { get; }
|
||||
internal double Y { get; }
|
||||
internal double VehicleHeadingRadians { get; }
|
||||
internal double LeftVehicleCurvaturePerMeter { get; }
|
||||
internal double RightVehicleCurvaturePerMeter { get; }
|
||||
internal double CurvatureJumpPerMeter { get; }
|
||||
}
|
||||
@@ -0,0 +1,260 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>从原始粗路径的同向相邻点中识别恒曲率原语边界。</summary>
|
||||
internal sealed class CurvatureTransitionDetector
|
||||
{
|
||||
internal bool TryDetect(
|
||||
PathSmoothingRequest request,
|
||||
double maximumVehicleCurvaturePerMeter,
|
||||
LocalG2OptionsSnapshot options,
|
||||
out IReadOnlyList<CurvatureTransition> transitions,
|
||||
out string reason)
|
||||
{
|
||||
transitions = Empty<CurvatureTransition>();
|
||||
reason = string.Empty;
|
||||
if (request == null || request.CoarsePath == null || request.Segments == null || options == null ||
|
||||
!NumericGuard.IsPositiveFinite(maximumVehicleCurvaturePerMeter))
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测输入无效。";
|
||||
return false;
|
||||
}
|
||||
if (!ValidatePath(request.CoarsePath, out reason) || !ValidateSegments(request.CoarsePath, request.Segments, out reason))
|
||||
return false;
|
||||
|
||||
double threshold = Math.Max(
|
||||
options.AbsoluteCurvatureJumpFloorPerMeter,
|
||||
options.CurvatureJumpRatioOfMaximum * maximumVehicleCurvaturePerMeter);
|
||||
if (!NumericGuard.IsPositiveFinite(threshold))
|
||||
{
|
||||
reason = "局部 G2 曲率事件阈值无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var detected = new List<CurvatureTransition>();
|
||||
for (int segmentPosition = 0; segmentPosition < request.Segments.Count; segmentPosition++)
|
||||
{
|
||||
PathSegment segment = request.Segments[segmentPosition];
|
||||
double segmentStartArc = request.CoarsePath[segment.StartIndex].ArcLength;
|
||||
for (int leftIndex = segment.StartIndex; leftIndex < segment.EndIndex; leftIndex++)
|
||||
{
|
||||
CoarsePathPoint left = request.CoarsePath[leftIndex];
|
||||
CoarsePathPoint right = request.CoarsePath[leftIndex + 1];
|
||||
double delta = right.VehicleCurvature - left.VehicleCurvature;
|
||||
if (!NumericGuard.IsFinite(delta))
|
||||
{
|
||||
reason = "局部 G2 曲率事件跳变溢出。";
|
||||
return false;
|
||||
}
|
||||
if (Math.Abs(delta) >= threshold)
|
||||
{
|
||||
detected.Add(new CurvatureTransition(
|
||||
segment.SegmentIndex,
|
||||
leftIndex,
|
||||
leftIndex + 1,
|
||||
left.ArcLength - segmentStartArc,
|
||||
left.X,
|
||||
left.Y,
|
||||
left.Heading,
|
||||
left.VehicleCurvature,
|
||||
right.VehicleCurvature));
|
||||
}
|
||||
}
|
||||
}
|
||||
transitions = new ReadOnlyCollection<CurvatureTransition>(detected);
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>反射脚本使用的确定性检测与窗口规划接缝。</summary>
|
||||
public static class TestHooks
|
||||
{
|
||||
public static DetectionTestSnapshot Execute(string scenario)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
|
||||
switch (scenario)
|
||||
{
|
||||
case "SingleTransition": return DetectSingleTransition();
|
||||
case "GearSwitch": return DetectGearSwitch();
|
||||
case "Noise": return DetectNoise();
|
||||
case "Overlap": return PlanOverlap();
|
||||
case "NearStart": return PlanNearStart();
|
||||
default: throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class DetectionTestSnapshot
|
||||
{
|
||||
internal DetectionTestSnapshot(int transitionCount, double maximumJump, int regionCount,
|
||||
int transitionCountInFirstRegion, double startArcLength, double leftWindowLength, double rightWindowLength)
|
||||
{
|
||||
TransitionCount = transitionCount;
|
||||
MaximumJump = maximumJump;
|
||||
RegionCount = regionCount;
|
||||
TransitionCountInFirstRegion = transitionCountInFirstRegion;
|
||||
StartArcLength = startArcLength;
|
||||
LeftWindowLength = leftWindowLength;
|
||||
RightWindowLength = rightWindowLength;
|
||||
}
|
||||
|
||||
public int TransitionCount { get; }
|
||||
public double MaximumJump { get; }
|
||||
public int RegionCount { get; }
|
||||
public int TransitionCountInFirstRegion { get; }
|
||||
public double StartArcLength { get; }
|
||||
public double LeftWindowLength { get; }
|
||||
public double RightWindowLength { get; }
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot DetectSingleTransition()
|
||||
{
|
||||
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||
new[] { Point(0d, 0d), Point(0.1d, 0.4167d), Point(0.2d, 0.4167d) },
|
||||
new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
|
||||
return Snapshot(transitions);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot DetectGearSwitch()
|
||||
{
|
||||
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||
new[]
|
||||
{
|
||||
Point(0d, 0d, TravelDirection.Forward), Point(0.2d, 0d, TravelDirection.Forward),
|
||||
Point(0.2d, 0.4167d, TravelDirection.Reverse, true), Point(0.4d, 0.4167d, TravelDirection.Reverse),
|
||||
},
|
||||
new[]
|
||||
{
|
||||
new PathSegment(0, TravelDirection.Forward, 0, 1, false, true),
|
||||
new PathSegment(1, TravelDirection.Reverse, 2, 3, true, false),
|
||||
}));
|
||||
return Snapshot(transitions);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot DetectNoise()
|
||||
{
|
||||
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||
new[] { Point(0d, 0d), Point(0.1d, 0.01d), Point(0.2d, 0.01d) },
|
||||
new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
|
||||
return Snapshot(transitions);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot PlanOverlap()
|
||||
{
|
||||
LocalG2OptionsSnapshot options = CreateOptions();
|
||||
var transitions = new[]
|
||||
{
|
||||
new CurvatureTransition(0, 1, 2, 0.4d, 0.4d, 0d, 0d, 0d, 0.5d),
|
||||
new CurvatureTransition(0, 2, 3, 0.7d, 0.7d, 0d, 0d, 0.5d, 0d),
|
||||
};
|
||||
var planner = new LocalG2WindowPlanner();
|
||||
if (!planner.TryPlan(CreatePreparedPath(1.4d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
|
||||
throw new InvalidOperationException(reason);
|
||||
return new DetectionTestSnapshot(2, 0.5d, regions.Count, regions[0].Transitions.Count, 0d, 0d, 0d);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot PlanNearStart()
|
||||
{
|
||||
LocalG2OptionsSnapshot options = CreateOptions();
|
||||
var transitions = new[] { new CurvatureTransition(0, 0, 1, 0.1d, 0.1d, 0d, 0d, 0d, 0.5d) };
|
||||
var planner = new LocalG2WindowPlanner();
|
||||
if (!planner.TryPlan(CreatePreparedPath(1d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
|
||||
throw new InvalidOperationException(reason);
|
||||
LocalG2WindowVariant first = regions[0].WindowVariants[0];
|
||||
return new DetectionTestSnapshot(1, 0.5d, regions.Count, 1,
|
||||
first.StartArcLengthMeters, first.LeftWindowLengthMeters, first.RightWindowLengthMeters);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CurvatureTransition> Detect(PathSmoothingRequest request)
|
||||
{
|
||||
var detector = new CurvatureTransitionDetector();
|
||||
if (!detector.TryDetect(request, 0.8333d, CreateOptions(), out IReadOnlyList<CurvatureTransition> transitions, out string reason))
|
||||
throw new InvalidOperationException(reason);
|
||||
return transitions;
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot Snapshot(IReadOnlyList<CurvatureTransition> transitions)
|
||||
{
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < transitions.Count; index++) maximum = Math.Max(maximum, transitions[index].CurvatureJumpPerMeter);
|
||||
return new DetectionTestSnapshot(transitions.Count, maximum, 0, 0, 0d, 0d, 0d);
|
||||
}
|
||||
|
||||
private static PathSmoothingRequest CreateRequest(IReadOnlyList<CoarsePathPoint> points, IReadOnlyList<PathSegment> segments)
|
||||
{
|
||||
return new PathSmoothingRequest(points, segments, null, null, new PathSmoothingConfiguration());
|
||||
}
|
||||
|
||||
private static CoarsePathPoint Point(double arcLength, double curvature, TravelDirection direction = TravelDirection.Forward, bool gearSwitch = false)
|
||||
{
|
||||
return new CoarsePathPoint(arcLength, 0d, 0d, 0d, arcLength, direction, curvature, 1d, gearSwitch,
|
||||
CoarsePathPointSource.MotionPrimitive);
|
||||
}
|
||||
|
||||
private static LocalG2OptionsSnapshot CreateOptions() => new LocalG2OptionsSnapshot(new PathSmoothingConfiguration());
|
||||
|
||||
private static Processing.PreparedPath CreatePreparedPath(double length)
|
||||
{
|
||||
var points = new[]
|
||||
{
|
||||
new Processing.SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
new Processing.SmoothingPoint2D(length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
var segment = new Processing.PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false);
|
||||
return new Processing.PreparedPath(new[] { segment });
|
||||
}
|
||||
}
|
||||
|
||||
private static bool ValidatePath(IReadOnlyList<CoarsePathPoint> path, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (path.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测需要粗路径点。";
|
||||
return false;
|
||||
}
|
||||
double previousArc = -1d;
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint point = path[index];
|
||||
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!NumericGuard.IsFinite(point.VehicleCurvature) || point.ArcLength < previousArc)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测要求有限且非递减的粗路径。";
|
||||
return false;
|
||||
}
|
||||
previousArc = point.ArcLength;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateSegments(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int position = 0; position < segments.Count; position++)
|
||||
{
|
||||
PathSegment segment = segments[position];
|
||||
if (segment == null || segment.SegmentIndex != position || segment.StartIndex < 0 ||
|
||||
segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测的方向分段无效。";
|
||||
return false;
|
||||
}
|
||||
for (int index = segment.StartIndex; index <= segment.EndIndex; index++)
|
||||
{
|
||||
if (path[index].Direction != segment.Direction)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测的方向分段包含换向点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,559 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>对一个局部 G2 替换候选执行区域质量门和完整路径安全复核。</summary>
|
||||
internal sealed class LocalG2CandidateEvaluator
|
||||
{
|
||||
private const double CurvatureRangeTolerance = 1e-6d;
|
||||
private const double WindowPointToleranceMeters = 1e-10d;
|
||||
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
||||
new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
||||
private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
|
||||
new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
|
||||
private readonly PathGeometryAnalyzer _analyzer;
|
||||
private readonly LocalG2PathSplicer _splicer;
|
||||
private readonly SmoothedPathValidator _validator;
|
||||
|
||||
internal LocalG2CandidateEvaluator()
|
||||
: this(new PathGeometryAnalyzer(), new LocalG2PathSplicer(), new SmoothedPathValidator())
|
||||
{
|
||||
}
|
||||
|
||||
internal LocalG2CandidateEvaluator(
|
||||
PathGeometryAnalyzer analyzer,
|
||||
LocalG2PathSplicer splicer,
|
||||
SmoothedPathValidator validator)
|
||||
{
|
||||
_analyzer = analyzer ?? throw new ArgumentNullException(nameof(analyzer));
|
||||
_splicer = splicer ?? throw new ArgumentNullException(nameof(splicer));
|
||||
_validator = validator ?? throw new ArgumentNullException(nameof(validator));
|
||||
}
|
||||
|
||||
internal LocalG2CandidateEvaluation Evaluate(
|
||||
PreparedPath rawPath,
|
||||
PreparedPath currentPath,
|
||||
LocalG2SmoothingRegion region,
|
||||
LocalG2CandidateGeometry candidate,
|
||||
PathSmoothingRequest request,
|
||||
LocalG2OptionsSnapshot options,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
int candidateIndex = candidate == null ? -1 : candidate.CandidateIndex;
|
||||
if (!HasUsableInput(rawPath, currentPath, region, candidate, request, options))
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选评价输入无效。");
|
||||
|
||||
PreparedDirectionSegment currentSegment = currentPath.Segments[candidate.SegmentIndex];
|
||||
if (!TryExtractWindow(currentSegment, candidate.StartArcLengthMeters, candidate.EndArcLengthMeters,
|
||||
out IReadOnlyList<SmoothingPoint2D> rawWindow, out string reason) ||
|
||||
!TryAnalyzeWindow(currentSegment, rawWindow, request.Configuration.OutputSpacingMeters,
|
||||
out PathGeometryAnalysis rawAnalysis, out reason) ||
|
||||
!TryAnalyzeWindow(currentSegment, candidate.RegionPoints, request.Configuration.OutputSpacingMeters,
|
||||
out PathGeometryAnalysis candidateAnalysis, out reason) ||
|
||||
!HasFiniteMetrics(rawAnalysis) || !HasFiniteMetrics(candidateAnalysis))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
|
||||
string.IsNullOrEmpty(reason) ? "局部 G2 区域几何分析失败。" : reason);
|
||||
}
|
||||
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double vehicleMaximumCurvature))
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "车辆曲率约束无效。");
|
||||
if (candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter > vehicleMaximumCurvature + CurvatureRangeTolerance)
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureExceeded, "局部 G2 候选超过车辆曲率上限。");
|
||||
|
||||
GetCurvatureRange(rawAnalysis.Path, out double rawMinimumCurvature, out double rawMaximumCurvature);
|
||||
if (ExceedsRawCurvatureRange(candidateAnalysis.Path, rawMinimumCurvature, rawMaximumCurvature))
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CurvatureOvershoot, "局部 G2 候选超出原始区域曲率范围。");
|
||||
|
||||
double maximumDeviation = MaximumDistanceToPolyline(candidate.RegionPoints, rawWindow);
|
||||
if (!NumericGuard.IsFinite(maximumDeviation))
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "局部 G2 候选偏差计算失败。");
|
||||
if (maximumDeviation > options.MaximumDeviationMeters)
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.DeviationExceeded, "局部 G2 候选偏离原始窗口过远。");
|
||||
|
||||
if (!_splicer.TryReplace(currentPath, candidate, out PreparedPath spliced, out reason) ||
|
||||
!_analyzer.TryAnalyze(spliced.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis fullAnalysis, out reason) ||
|
||||
!HasFiniteMetrics(fullAnalysis))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
|
||||
string.IsNullOrEmpty(reason) ? "局部 G2 完整路径几何分析失败。" : reason);
|
||||
}
|
||||
|
||||
if (!_validator.TryValidate(fullAnalysis.Path, fullAnalysis.Segments, rawPath, request.Map, request.Vehicle,
|
||||
request.Configuration.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearance, out reason))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.Collision,
|
||||
string.IsNullOrEmpty(reason) ? "局部 G2 完整路径安全复核失败。" : reason);
|
||||
}
|
||||
if (!NumericGuard.IsFinite(minimumClearance) || minimumClearance < request.Configuration.MinimumClearanceReserveMeters)
|
||||
return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientClearance, candidateIndex,
|
||||
null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
|
||||
candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选净空不足。");
|
||||
|
||||
if (candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter >
|
||||
rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter *
|
||||
(1d - options.MinimumPeakGradientImprovementRatio))
|
||||
{
|
||||
return new LocalG2CandidateEvaluation(false, PathSmoothingRegionFailureReason.InsufficientImprovement, candidateIndex,
|
||||
null, EmptyPath, EmptySegments, rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter, rawAnalysis.CurvatureVariationCost,
|
||||
candidateAnalysis.CurvatureVariationCost, maximumDeviation, minimumClearance,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter, 0d, "局部 G2 候选曲率导数峰值改善不足。");
|
||||
}
|
||||
if (candidateAnalysis.CurvatureVariationCost > rawAnalysis.CurvatureVariationCost *
|
||||
(1d + options.MaximumVariationCostRegressionRatio))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.VariationCostRegression, "局部 G2 候选曲率变化代价回退。");
|
||||
}
|
||||
|
||||
if (!_analyzer.TryAnalyze(rawPath.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis rawFullAnalysis, out reason) ||
|
||||
!HasFiniteMetrics(rawFullAnalysis))
|
||||
{
|
||||
return Rejected(candidateIndex, PathSmoothingRegionFailureReason.CandidateGenerationFailed,
|
||||
string.IsNullOrEmpty(reason) ? "原始完整路径几何分析失败。" : reason);
|
||||
}
|
||||
|
||||
return new LocalG2CandidateEvaluation(
|
||||
true,
|
||||
PathSmoothingRegionFailureReason.None,
|
||||
candidateIndex,
|
||||
spliced,
|
||||
safePath,
|
||||
fullAnalysis.Segments,
|
||||
rawAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
rawAnalysis.CurvatureVariationCost,
|
||||
candidateAnalysis.CurvatureVariationCost,
|
||||
maximumDeviation,
|
||||
minimumClearance,
|
||||
candidateAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
Math.Abs(fullAnalysis.PathLengthMeters - rawFullAnalysis.PathLengthMeters),
|
||||
"Accepted");
|
||||
}
|
||||
|
||||
internal static LocalG2CandidateEvaluation SelectBest(IReadOnlyList<LocalG2CandidateEvaluation> evaluations)
|
||||
{
|
||||
if (evaluations == null) throw new ArgumentNullException(nameof(evaluations));
|
||||
LocalG2CandidateEvaluation best = null;
|
||||
for (int index = 0; index < evaluations.Count; index++)
|
||||
{
|
||||
LocalG2CandidateEvaluation evaluation = evaluations[index];
|
||||
if (evaluation == null || !evaluation.Accepted) continue;
|
||||
if (best == null || Compare(evaluation, best) < 0) best = evaluation;
|
||||
}
|
||||
return best ?? Rejected(-1, PathSmoothingRegionFailureReason.CandidateGenerationFailed, "没有通过质量门的局部 G2 候选。");
|
||||
}
|
||||
|
||||
private static int Compare(LocalG2CandidateEvaluation left, LocalG2CandidateEvaluation right)
|
||||
{
|
||||
int result = left.MaximumDeviationMeters.CompareTo(right.MaximumDeviationMeters);
|
||||
if (result != 0) return result;
|
||||
result = left.ResultPeakCurvatureDerivativePerSquareMeter.CompareTo(right.ResultPeakCurvatureDerivativePerSquareMeter);
|
||||
if (result != 0) return result;
|
||||
result = left.ResultCurvatureVariationCost.CompareTo(right.ResultCurvatureVariationCost);
|
||||
if (result != 0) return result;
|
||||
result = left.AbsolutePathLengthChangeMeters.CompareTo(right.AbsolutePathLengthChangeMeters);
|
||||
return result != 0 ? result : left.CandidateIndex.CompareTo(right.CandidateIndex);
|
||||
}
|
||||
|
||||
private static bool HasUsableInput(PreparedPath rawPath, PreparedPath currentPath, LocalG2SmoothingRegion region,
|
||||
LocalG2CandidateGeometry candidate, PathSmoothingRequest request, LocalG2OptionsSnapshot options)
|
||||
{
|
||||
return rawPath != null && currentPath != null && region != null && candidate != null && request != null && options != null &&
|
||||
request.Configuration != null && request.Map != null && request.Vehicle != null && candidate.SegmentIndex == region.SegmentIndex &&
|
||||
candidate.SegmentIndex >= 0 && candidate.SegmentIndex < currentPath.Segments.Count &&
|
||||
NumericGuard.IsPositiveFinite(request.Configuration.OutputSpacingMeters) &&
|
||||
NumericGuard.IsPositiveFinite(request.Configuration.MaximumCollisionCheckStepMeters) &&
|
||||
NumericGuard.IsFinite(request.Configuration.MinimumClearanceReserveMeters) &&
|
||||
request.Configuration.MinimumClearanceReserveMeters >= 0d;
|
||||
}
|
||||
|
||||
private static bool TryExtractWindow(PreparedDirectionSegment segment, double startArcLength, double endArcLength,
|
||||
out IReadOnlyList<SmoothingPoint2D> window, out string reason)
|
||||
{
|
||||
window = null;
|
||||
reason = string.Empty;
|
||||
if (segment == null || !PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, startArcLength, out SmoothingPoint2D start, out reason) ||
|
||||
!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, endArcLength, out SmoothingPoint2D end, out reason)) return false;
|
||||
var points = new List<SmoothingPoint2D> { start };
|
||||
for (int index = 0; index < segment.Points.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = segment.Points[index];
|
||||
if (point.ArcLength > startArcLength && point.ArcLength < endArcLength &&
|
||||
!SamePosition(points[points.Count - 1], point))
|
||||
{
|
||||
points.Add(point);
|
||||
}
|
||||
}
|
||||
if (SamePosition(points[points.Count - 1], end))
|
||||
points[points.Count - 1] = end;
|
||||
else
|
||||
points.Add(end);
|
||||
window = new ReadOnlyCollection<SmoothingPoint2D>(points);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
if (left == null || right == null) return false;
|
||||
double x = right.X - left.X;
|
||||
double y = right.Y - left.Y;
|
||||
return x * x + y * y <= WindowPointToleranceMeters * WindowPointToleranceMeters;
|
||||
}
|
||||
|
||||
private bool TryAnalyzeWindow(PreparedDirectionSegment source, IReadOnlyList<SmoothingPoint2D> points, double spacing,
|
||||
out PathGeometryAnalysis analysis, out string reason)
|
||||
{
|
||||
var segment = new PreparedDirectionSegment(0, source.Direction, points, false, false,
|
||||
source.Points[0].ArcLength == points[0].ArcLength ? source.StartVehicleCurvaturePerMeter : null);
|
||||
return _analyzer.TryAnalyze(new[] { segment }, spacing, out analysis, out reason);
|
||||
}
|
||||
|
||||
private static bool HasFiniteMetrics(PathGeometryAnalysis analysis)
|
||||
{
|
||||
return analysis != null && NumericGuard.IsFinite(analysis.PathLengthMeters) &&
|
||||
NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvaturePerMeter) &&
|
||||
NumericGuard.IsFinite(analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter) &&
|
||||
NumericGuard.IsFinite(analysis.CurvatureVariationCost) && analysis.Path != null && analysis.Path.Count >= 2;
|
||||
}
|
||||
|
||||
private static void GetCurvatureRange(IReadOnlyList<SmoothedPathPoint> path, out double minimum, out double maximum)
|
||||
{
|
||||
minimum = double.PositiveInfinity;
|
||||
maximum = double.NegativeInfinity;
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
minimum = Math.Min(minimum, path[index].VehicleCurvature);
|
||||
maximum = Math.Max(maximum, path[index].VehicleCurvature);
|
||||
}
|
||||
}
|
||||
|
||||
private static bool ExceedsRawCurvatureRange(IReadOnlyList<SmoothedPathPoint> candidate, double minimum, double maximum)
|
||||
{
|
||||
for (int index = 0; index < candidate.Count; index++)
|
||||
{
|
||||
double curvature = candidate[index].VehicleCurvature;
|
||||
if (curvature < minimum - CurvatureRangeTolerance || curvature > maximum + CurvatureRangeTolerance) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static double MaximumDistanceToPolyline(IReadOnlyList<SmoothingPoint2D> candidate, IReadOnlyList<SmoothingPoint2D> raw)
|
||||
{
|
||||
if (candidate == null || raw == null || candidate.Count == 0 || raw.Count < 2) return double.NaN;
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < candidate.Count; index++)
|
||||
{
|
||||
double nearest = double.PositiveInfinity;
|
||||
for (int segment = 1; segment < raw.Count; segment++)
|
||||
nearest = Math.Min(nearest, PointToSegmentDistance(candidate[index], raw[segment - 1], raw[segment]));
|
||||
maximum = Math.Max(maximum, nearest);
|
||||
}
|
||||
return maximum;
|
||||
}
|
||||
|
||||
private static double PointToSegmentDistance(SmoothingPoint2D point, SmoothingPoint2D start, SmoothingPoint2D end)
|
||||
{
|
||||
double dx = end.X - start.X;
|
||||
double dy = end.Y - start.Y;
|
||||
double lengthSquared = dx * dx + dy * dy;
|
||||
if (!NumericGuard.IsPositiveFinite(lengthSquared)) return double.NaN;
|
||||
double projection = ((point.X - start.X) * dx + (point.Y - start.Y) * dy) / lengthSquared;
|
||||
projection = Math.Max(0d, Math.Min(1d, projection));
|
||||
double nearestX = start.X + projection * dx;
|
||||
double nearestY = start.Y + projection * dy;
|
||||
double distanceX = point.X - nearestX;
|
||||
double distanceY = point.Y - nearestY;
|
||||
return Math.Sqrt(distanceX * distanceX + distanceY * distanceY);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateEvaluation Rejected(int candidateIndex, PathSmoothingRegionFailureReason failureReason, string reason)
|
||||
{
|
||||
return new LocalG2CandidateEvaluation(false, failureReason, candidateIndex, null, EmptyPath, EmptySegments,
|
||||
0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, reason);
|
||||
}
|
||||
|
||||
/// <summary>反射脚本使用的窄范围确定性质量门覆盖入口。</summary>
|
||||
public static class TestHooks
|
||||
{
|
||||
public static EvaluationTestSnapshot Execute(string scenario)
|
||||
{
|
||||
LocalG2CandidateEvaluation evaluation;
|
||||
switch (scenario)
|
||||
{
|
||||
case "TooFar":
|
||||
PreparedPath tooFarPath = CreateWavyPath(1d);
|
||||
evaluation = Evaluate(tooFarPath, CreateCandidate(tooFarPath, 0, 1.12d), 0d);
|
||||
break;
|
||||
case "Overshoot":
|
||||
PreparedPath overshootPath = CreateStraightPath(1d);
|
||||
evaluation = Evaluate(overshootPath, CreateCandidate(overshootPath, 0, 1.15d), 0d);
|
||||
break;
|
||||
case "NoOp":
|
||||
PreparedPath noOpPath = CreateWavyPath(1d);
|
||||
evaluation = Evaluate(noOpPath, CreateCandidateFromPath(noOpPath, 0), 0d);
|
||||
break;
|
||||
case "Oscillating":
|
||||
PreparedPath oscillatingPath = CreateOscillationBaseline(1d);
|
||||
evaluation = Evaluate(oscillatingPath, CreateOscillatingCandidate(oscillatingPath), 0d);
|
||||
break;
|
||||
case "LowClearance":
|
||||
PreparedPath lowClearancePath = CreateWavyPath(0.27d);
|
||||
evaluation = Evaluate(lowClearancePath, CreateCandidateFromPath(lowClearancePath, 0), 0.02d, false, true);
|
||||
break;
|
||||
case "Improved":
|
||||
PreparedPath improvedPath = CreateWavyPath(1d);
|
||||
evaluation = Evaluate(improvedPath, CreateCandidate(improvedPath, 0, 1d), 0d);
|
||||
break;
|
||||
case "SmallestDeviation":
|
||||
PreparedPath smallestDeviationPath = CreateWavyPath(1d);
|
||||
evaluation = Evaluate(smallestDeviationPath, CreateCandidate(smallestDeviationPath, 4, 1d), 100d);
|
||||
break;
|
||||
case "Best": evaluation = SelectBest(CreateWavyPath(1d)); break;
|
||||
default: throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
return new EvaluationTestSnapshot(evaluation.Accepted ? "Accepted" : "Rejected", evaluation.FailureReason.ToString(), evaluation.CandidateIndex,
|
||||
evaluation.MinimumBodyClearanceMeters, evaluation.Reason);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateEvaluation Evaluate(PreparedPath path, LocalG2CandidateGeometry candidate, double minimumClearance,
|
||||
bool useNearObstacle = false, bool useEmptyMap = false)
|
||||
{
|
||||
PathSmoothingConfiguration configuration = CreateConfiguration(minimumClearance);
|
||||
PathSmoothingRequest request = new PathSmoothingRequest(null, null,
|
||||
useEmptyMap ? CreateEmptyMap() : CreateMap(useNearObstacle), CreateVehicle(), configuration);
|
||||
return new LocalG2CandidateEvaluator().Evaluate(path, path, CreateRegion(), candidate, request,
|
||||
new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateEvaluation SelectBest(PreparedPath path)
|
||||
{
|
||||
PathSmoothingConfiguration configuration = CreateConfiguration(0d);
|
||||
PathSmoothingRequest request = new PathSmoothingRequest(null, null, CreateMap(false), CreateVehicle(), configuration);
|
||||
var evaluator = new LocalG2CandidateEvaluator();
|
||||
LocalG2CandidateEvaluation smallestDeviation = evaluator.Evaluate(path, path, CreateRegion(),
|
||||
CreateCandidate(path, 4, 1d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
|
||||
LocalG2CandidateEvaluation smootherButFarther = evaluator.Evaluate(path, path, CreateRegion(),
|
||||
CreateCandidate(path, 5, 1.01d), request, new LocalG2OptionsSnapshot(configuration), CancellationToken.None);
|
||||
return LocalG2CandidateEvaluator.SelectBest(new[] { smootherButFarther, smallestDeviation });
|
||||
}
|
||||
|
||||
private static PreparedPath CreateWavyPath(double startX)
|
||||
{
|
||||
return CreatePath(new[]
|
||||
{
|
||||
Point(startX, 1d), Point(startX + 0.10d, 1.04d), Point(startX + 0.20d, 1.08d),
|
||||
Point(startX + 0.30d, 1.05d), Point(startX + 0.40d, 0.98d), Point(startX + 0.50d, 0.92d),
|
||||
Point(startX + 0.60d, 0.98d), Point(startX + 0.70d, 1.05d), Point(startX + 0.80d, 1.08d),
|
||||
Point(startX + 0.90d, 1.04d), Point(startX + 1d, 1d),
|
||||
});
|
||||
}
|
||||
|
||||
private static PreparedPath CreateStraightPath(double startX) => CreatePath(new[] { Point(startX, 1d), Point(startX + 0.5d, 1d), Point(startX + 1d, 1d) });
|
||||
|
||||
private static PreparedPath CreateOscillationBaseline(double startX)
|
||||
{
|
||||
return CreatePath(new[]
|
||||
{
|
||||
Point(startX, 1d), Point(startX + 0.05d, 1.022d), Point(startX + 0.10d, 1d),
|
||||
Point(startX + 2.03d, 1d),
|
||||
});
|
||||
}
|
||||
|
||||
private static PreparedPath CreatePath(IReadOnlyList<SmoothingPoint2D> points)
|
||||
{
|
||||
var normalized = new List<SmoothingPoint2D>(points.Count);
|
||||
double arcLength = 0d;
|
||||
for (int index = 0; index < points.Count; index++)
|
||||
{
|
||||
if (index > 0)
|
||||
{
|
||||
double dx = points[index].X - points[index - 1].X;
|
||||
double dy = points[index].Y - points[index - 1].Y;
|
||||
arcLength += Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
normalized.Add(new SmoothingPoint2D(points[index].X, points[index].Y, arcLength, 0d, 0d, 1d,
|
||||
false, SmoothedPathPointSource.LocalG2Transition));
|
||||
}
|
||||
return new PreparedPath(new[] { new PreparedDirectionSegment(0, TravelDirection.Forward, normalized, false, false) });
|
||||
}
|
||||
|
||||
private static LocalG2CandidateGeometry CreateCandidate(PreparedPath path, int index, double middleY)
|
||||
{
|
||||
IReadOnlyList<SmoothingPoint2D> source = path.Segments[0].Points;
|
||||
SmoothingPoint2D start = source[0];
|
||||
SmoothingPoint2D end = source[source.Count - 1];
|
||||
return new LocalG2CandidateGeometry(index, 0, start.ArcLength, end.ArcLength, 0d, 0d,
|
||||
new[] { Point(start.X, start.Y), Point((start.X + end.X) / 2d, middleY), Point(end.X, end.Y) },
|
||||
0d, 0d, 0d, 0d, true);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateGeometry CreateCandidateFromPath(PreparedPath path, int index)
|
||||
{
|
||||
IReadOnlyList<SmoothingPoint2D> points = path.Segments[0].Points;
|
||||
return new LocalG2CandidateGeometry(index, 0, points[0].ArcLength, points[points.Count - 1].ArcLength, 0d, 0d, points,
|
||||
0d, 0d, 0d, 0d, true);
|
||||
}
|
||||
|
||||
private static LocalG2CandidateGeometry CreateOscillatingCandidate(PreparedPath path)
|
||||
{
|
||||
IReadOnlyList<SmoothingPoint2D> source = path.Segments[0].Points;
|
||||
SmoothingPoint2D start = source[0];
|
||||
SmoothingPoint2D end = source[source.Count - 1];
|
||||
var points = new List<SmoothingPoint2D> { Point(start.X, start.Y) };
|
||||
SmoothingPoint2D straightStart = source[source.Count - 2];
|
||||
points.Add(Point(straightStart.X, straightStart.Y));
|
||||
for (int index = 1; index < 20; index++)
|
||||
{
|
||||
double x = straightStart.X + index * (end.X - straightStart.X) / 20d;
|
||||
points.Add(Point(x, straightStart.Y + (index % 2 == 0 ? 0.004d : -0.004d)));
|
||||
}
|
||||
points.Add(Point(end.X, end.Y));
|
||||
return new LocalG2CandidateGeometry(0, 0, start.ArcLength, end.ArcLength, 0d, 0d, points, 0d, 0d, 0d, 0d, true);
|
||||
}
|
||||
|
||||
private static LocalG2SmoothingRegion CreateRegion()
|
||||
{
|
||||
return new LocalG2SmoothingRegion(0,
|
||||
new[] { new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0d, 0d) },
|
||||
0d, 1d, new[] { new LocalG2WindowVariant(0, 0d, 1d, 0d, 0d) });
|
||||
}
|
||||
|
||||
private static PathSmoothingConfiguration CreateConfiguration(double minimumClearance)
|
||||
{
|
||||
var configuration = new PathSmoothingConfiguration
|
||||
{
|
||||
OutputSpacingMeters = 0.05d,
|
||||
MaximumCollisionCheckStepMeters = 0.05d,
|
||||
MinimumClearanceReserveMeters = minimumClearance,
|
||||
};
|
||||
configuration.LocalG2Quintic.MaximumDeviationMeters = 0.10d;
|
||||
configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.20d;
|
||||
configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio = 0.02d;
|
||||
return configuration;
|
||||
}
|
||||
|
||||
private static VehicleParameters CreateVehicle() => new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.20d, WidthMeters = 0.20d, SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 1000000d, MinimumTurningRadiusMeters = 0.000001d,
|
||||
};
|
||||
|
||||
private static PlanningGridMap CreateMap(bool useNearObstacle)
|
||||
{
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("local-g2-evaluator", 1, true, new IMapObstacle[]
|
||||
{
|
||||
useNearObstacle
|
||||
? new AxisAlignedRectangleObstacle(100f, 150f, 900f, 1100f)
|
||||
: new AxisAlignedRectangleObstacle(3000f, 3100f, 3000f, 3100f),
|
||||
}),
|
||||
},
|
||||
};
|
||||
PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
|
||||
if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试地图创建失败。");
|
||||
return result.Map;
|
||||
}
|
||||
|
||||
private static PlanningGridMap CreateEmptyMap()
|
||||
{
|
||||
var request = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 4000f, 0f, 4000f), ResolutionMm = 20f,
|
||||
ObstacleSources = Array.Empty<IMapObstacleSource>(), AllowExplicitEmptyMap = true,
|
||||
};
|
||||
PlanningMapBuildResult result = new PlanningMapFactory().Create(request);
|
||||
if (!result.Succeeded || result.Map == null) throw new InvalidOperationException("测试空地图创建失败。");
|
||||
return result.Map;
|
||||
}
|
||||
|
||||
private static SmoothingPoint2D Point(double x, double y) =>
|
||||
new SmoothingPoint2D(x, y, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.LocalG2Transition);
|
||||
|
||||
public sealed class EvaluationTestSnapshot
|
||||
{
|
||||
internal EvaluationTestSnapshot(string status, string failureReason, int candidateIndex, double minimumClearanceMeters, string reason)
|
||||
{
|
||||
Status = status;
|
||||
FailureReason = failureReason;
|
||||
CandidateIndex = candidateIndex;
|
||||
MinimumClearanceMeters = minimumClearanceMeters;
|
||||
Reason = reason;
|
||||
}
|
||||
public string Status { get; }
|
||||
public string FailureReason { get; }
|
||||
public int CandidateIndex { get; }
|
||||
public double MinimumClearanceMeters { get; }
|
||||
public string Reason { get; }
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>不可变的局部 G2 候选质量与安全评价结果。</summary>
|
||||
internal sealed class LocalG2CandidateEvaluation
|
||||
{
|
||||
internal LocalG2CandidateEvaluation(bool accepted, PathSmoothingRegionFailureReason failureReason, int candidateIndex,
|
||||
PreparedPath splicedPreparedPath, IReadOnlyList<SmoothedPathPoint> safePath, IReadOnlyList<SmoothedPathSegment> safeSegments,
|
||||
double rawPeakCurvatureDerivativePerSquareMeter, double resultPeakCurvatureDerivativePerSquareMeter,
|
||||
double rawCurvatureVariationCost, double resultCurvatureVariationCost, double maximumDeviationMeters,
|
||||
double minimumBodyClearanceMeters, double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double absolutePathLengthChangeMeters, string reason)
|
||||
{
|
||||
Accepted = accepted;
|
||||
FailureReason = failureReason;
|
||||
CandidateIndex = candidateIndex;
|
||||
SplicedPreparedPath = splicedPreparedPath;
|
||||
SafePath = Copy(safePath);
|
||||
SafeSegments = Copy(safeSegments);
|
||||
RawPeakCurvatureDerivativePerSquareMeter = rawPeakCurvatureDerivativePerSquareMeter;
|
||||
ResultPeakCurvatureDerivativePerSquareMeter = resultPeakCurvatureDerivativePerSquareMeter;
|
||||
RawCurvatureVariationCost = rawCurvatureVariationCost;
|
||||
ResultCurvatureVariationCost = resultCurvatureVariationCost;
|
||||
MaximumDeviationMeters = maximumDeviationMeters;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
AbsolutePathLengthChangeMeters = absolutePathLengthChangeMeters;
|
||||
Reason = reason ?? string.Empty;
|
||||
}
|
||||
|
||||
internal bool Accepted { get; }
|
||||
internal PathSmoothingRegionFailureReason FailureReason { get; }
|
||||
internal int CandidateIndex { get; }
|
||||
internal PreparedPath SplicedPreparedPath { get; }
|
||||
internal IReadOnlyList<SmoothedPathPoint> SafePath { get; }
|
||||
internal IReadOnlyList<SmoothedPathSegment> SafeSegments { get; }
|
||||
internal double RawPeakCurvatureDerivativePerSquareMeter { get; }
|
||||
internal double ResultPeakCurvatureDerivativePerSquareMeter { get; }
|
||||
internal double RawCurvatureVariationCost { get; }
|
||||
internal double ResultCurvatureVariationCost { get; }
|
||||
internal double MaximumDeviationMeters { get; }
|
||||
internal double MinimumBodyClearanceMeters { get; }
|
||||
internal double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
|
||||
internal double AbsolutePathLengthChangeMeters { get; }
|
||||
internal string Reason { get; }
|
||||
|
||||
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null) for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>一个尚未经过安全和质量评价的局部 G2 替换几何。</summary>
|
||||
internal sealed class LocalG2CandidateGeometry
|
||||
{
|
||||
internal LocalG2CandidateGeometry(
|
||||
int candidateIndex,
|
||||
int segmentIndex,
|
||||
double startArcLengthMeters,
|
||||
double endArcLengthMeters,
|
||||
double leftWindowLengthMeters,
|
||||
double rightWindowLengthMeters,
|
||||
IReadOnlyList<SmoothingPoint2D> regionPoints,
|
||||
double startVehicleCurvaturePerMeter,
|
||||
double endVehicleCurvaturePerMeter,
|
||||
double startGeometricCurvaturePerMeter,
|
||||
double endGeometricCurvaturePerMeter,
|
||||
bool internalConnectionsAreG2)
|
||||
{
|
||||
if (candidateIndex < 0 || segmentIndex < 0 || !NumericGuard.IsFinite(startArcLengthMeters) ||
|
||||
!NumericGuard.IsFinite(endArcLengthMeters) || startArcLengthMeters < 0d ||
|
||||
endArcLengthMeters < startArcLengthMeters || !NumericGuard.IsFinite(leftWindowLengthMeters) ||
|
||||
!NumericGuard.IsFinite(rightWindowLengthMeters) || leftWindowLengthMeters < 0d ||
|
||||
rightWindowLengthMeters < 0d || regionPoints == null || regionPoints.Count < 2 ||
|
||||
!NumericGuard.IsFinite(startVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(endVehicleCurvaturePerMeter) ||
|
||||
!NumericGuard.IsFinite(startGeometricCurvaturePerMeter) || !NumericGuard.IsFinite(endGeometricCurvaturePerMeter))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(regionPoints));
|
||||
}
|
||||
|
||||
CandidateIndex = candidateIndex;
|
||||
SegmentIndex = segmentIndex;
|
||||
StartArcLengthMeters = startArcLengthMeters;
|
||||
EndArcLengthMeters = endArcLengthMeters;
|
||||
LeftWindowLengthMeters = leftWindowLengthMeters;
|
||||
RightWindowLengthMeters = rightWindowLengthMeters;
|
||||
RegionPoints = Copy(regionPoints);
|
||||
StartVehicleCurvaturePerMeter = startVehicleCurvaturePerMeter;
|
||||
EndVehicleCurvaturePerMeter = endVehicleCurvaturePerMeter;
|
||||
StartGeometricCurvaturePerMeter = startGeometricCurvaturePerMeter;
|
||||
EndGeometricCurvaturePerMeter = endGeometricCurvaturePerMeter;
|
||||
InternalConnectionsAreG2 = internalConnectionsAreG2;
|
||||
}
|
||||
|
||||
internal int CandidateIndex { get; }
|
||||
internal int SegmentIndex { get; }
|
||||
internal double StartArcLengthMeters { get; }
|
||||
internal double EndArcLengthMeters { get; }
|
||||
internal double LeftWindowLengthMeters { get; }
|
||||
internal double RightWindowLengthMeters { get; }
|
||||
internal IReadOnlyList<SmoothingPoint2D> RegionPoints { get; }
|
||||
|
||||
// 这些边界状态供候选评价和窄范围反射验证使用，不能替代统一几何分析器的最终统计。
|
||||
internal double StartVehicleCurvaturePerMeter { get; }
|
||||
internal double EndVehicleCurvaturePerMeter { get; }
|
||||
internal double StartGeometricCurvaturePerMeter { get; }
|
||||
internal double EndGeometricCurvaturePerMeter { get; }
|
||||
internal bool InternalConnectionsAreG2 { get; }
|
||||
|
||||
private static IReadOnlyList<SmoothingPoint2D> Copy(IReadOnlyList<SmoothingPoint2D> source)
|
||||
{
|
||||
var copy = new List<SmoothingPoint2D>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
if (source[index] == null) throw new ArgumentOutOfRangeException(nameof(source));
|
||||
copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingPoint2D>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>局部 G2 预平滑一次运行使用的已校验不可变选项。</summary>
|
||||
internal sealed class LocalG2OptionsSnapshot
|
||||
{
|
||||
internal LocalG2OptionsSnapshot(PathSmoothingConfiguration configuration)
|
||||
{
|
||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||
LocalG2QuinticOptions source = configuration.LocalG2Quintic;
|
||||
if (source == null) throw new ArgumentOutOfRangeException(nameof(configuration));
|
||||
|
||||
MinimumWindowLengthMeters = source.MinimumWindowLengthMeters;
|
||||
PreferredWindowLengthMeters = source.PreferredWindowLengthMeters;
|
||||
MaximumWindowLengthMeters = source.MaximumWindowLengthMeters;
|
||||
MaximumDeviationMeters = source.MaximumDeviationMeters;
|
||||
AbsoluteCurvatureJumpFloorPerMeter = source.AbsoluteCurvatureJumpFloorPerMeter;
|
||||
CurvatureJumpRatioOfMaximum = source.CurvatureJumpRatioOfMaximum;
|
||||
MinimumPeakGradientImprovementRatio = source.MinimumPeakGradientImprovementRatio;
|
||||
MaximumVariationCostRegressionRatio = source.MaximumVariationCostRegressionRatio;
|
||||
MaximumCandidatesPerRegion = source.MaximumCandidatesPerRegion;
|
||||
|
||||
if (!NumericGuard.IsPositiveFinite(MinimumWindowLengthMeters))
|
||||
throw new ArgumentOutOfRangeException(nameof(MinimumWindowLengthMeters));
|
||||
if (!NumericGuard.IsFinite(PreferredWindowLengthMeters) || PreferredWindowLengthMeters < MinimumWindowLengthMeters)
|
||||
throw new ArgumentOutOfRangeException(nameof(PreferredWindowLengthMeters));
|
||||
if (!NumericGuard.IsFinite(MaximumWindowLengthMeters) || MaximumWindowLengthMeters < PreferredWindowLengthMeters)
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumWindowLengthMeters));
|
||||
if (!NumericGuard.IsPositiveFinite(MaximumDeviationMeters))
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumDeviationMeters));
|
||||
if (!NumericGuard.IsPositiveFinite(AbsoluteCurvatureJumpFloorPerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(AbsoluteCurvatureJumpFloorPerMeter));
|
||||
if (!NumericGuard.IsFinite(CurvatureJumpRatioOfMaximum) || CurvatureJumpRatioOfMaximum <= 0d || CurvatureJumpRatioOfMaximum > 1d)
|
||||
throw new ArgumentOutOfRangeException(nameof(CurvatureJumpRatioOfMaximum));
|
||||
if (!NumericGuard.IsFinite(MinimumPeakGradientImprovementRatio) ||
|
||||
MinimumPeakGradientImprovementRatio <= 0d || MinimumPeakGradientImprovementRatio >= 1d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(MinimumPeakGradientImprovementRatio));
|
||||
}
|
||||
if (!NumericGuard.IsFinite(MaximumVariationCostRegressionRatio) || MaximumVariationCostRegressionRatio < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumVariationCostRegressionRatio));
|
||||
if (MaximumCandidatesPerRegion < 1)
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumCandidatesPerRegion));
|
||||
}
|
||||
|
||||
internal double MinimumWindowLengthMeters { get; }
|
||||
internal double PreferredWindowLengthMeters { get; }
|
||||
internal double MaximumWindowLengthMeters { get; }
|
||||
internal double MaximumDeviationMeters { get; }
|
||||
internal double AbsoluteCurvatureJumpFloorPerMeter { get; }
|
||||
internal double CurvatureJumpRatioOfMaximum { get; }
|
||||
internal double MinimumPeakGradientImprovementRatio { get; }
|
||||
internal double MaximumVariationCostRegressionRatio { get; }
|
||||
internal int MaximumCandidatesPerRegion { get; }
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>只替换一个方向段内局部窗口，并保持其余方向拓扑不变。</summary>
|
||||
internal sealed class LocalG2PathSplicer
|
||||
{
|
||||
internal bool TryReplace(
|
||||
PreparedPath currentPath,
|
||||
LocalG2CandidateGeometry candidate,
|
||||
out PreparedPath replacedPath,
|
||||
out string reason)
|
||||
{
|
||||
replacedPath = null;
|
||||
reason = string.Empty;
|
||||
if (currentPath == null || candidate == null || candidate.SegmentIndex < 0 ||
|
||||
candidate.SegmentIndex >= currentPath.Segments.Count || candidate.RegionPoints == null ||
|
||||
candidate.RegionPoints.Count < 2)
|
||||
{
|
||||
reason = "局部 G2 拼接输入无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
PreparedDirectionSegment source = currentPath.Segments[candidate.SegmentIndex];
|
||||
if (source == null || source.SegmentIndex != candidate.SegmentIndex ||
|
||||
!PathReferenceInterpolator.TryInterpolateByArcLength(source.Points, candidate.StartArcLengthMeters, out SmoothingPoint2D start, out reason) ||
|
||||
!PathReferenceInterpolator.TryInterpolateByArcLength(source.Points, candidate.EndArcLengthMeters, out SmoothingPoint2D end, out reason) ||
|
||||
!SamePosition(candidate.RegionPoints[0], start) || !SamePosition(candidate.RegionPoints[candidate.RegionPoints.Count - 1], end))
|
||||
{
|
||||
if (string.IsNullOrEmpty(reason)) reason = "局部 G2 候选端点不匹配原始窗口。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var combined = new List<SmoothingPoint2D>();
|
||||
for (int index = 0; index < source.Points.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = source.Points[index];
|
||||
if (point.ArcLength < candidate.StartArcLengthMeters) AddWithoutNonGearDuplicates(combined, point);
|
||||
}
|
||||
// Candidate endpoints are only validated within a numerical tolerance. The replacement
|
||||
// itself must use the exact source-window endpoints, including a possible gear marker.
|
||||
AddWithoutNonGearDuplicates(combined, start);
|
||||
for (int index = 1; index < candidate.RegionPoints.Count - 1; index++)
|
||||
AddWithoutNonGearDuplicates(combined, candidate.RegionPoints[index]);
|
||||
AddWithoutNonGearDuplicates(combined, end);
|
||||
for (int index = 0; index < source.Points.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = source.Points[index];
|
||||
if (point.ArcLength > candidate.EndArcLengthMeters) AddWithoutNonGearDuplicates(combined, point);
|
||||
}
|
||||
if (!TryRecalculateArcLengths(combined, out IReadOnlyList<SmoothingPoint2D> localPoints, out reason)) return false;
|
||||
|
||||
var segments = new List<PreparedDirectionSegment>(currentPath.Segments.Count);
|
||||
for (int index = 0; index < currentPath.Segments.Count; index++)
|
||||
{
|
||||
PreparedDirectionSegment segment = currentPath.Segments[index];
|
||||
if (index != candidate.SegmentIndex)
|
||||
{
|
||||
segments.Add(segment);
|
||||
continue;
|
||||
}
|
||||
segments.Add(new PreparedDirectionSegment(segment.SegmentIndex, segment.Direction, localPoints,
|
||||
segment.StartsAtGearSwitch, segment.EndsAtGearSwitch, segment.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
replacedPath = new PreparedPath(segments);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryRecalculateArcLengths(
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
out IReadOnlyList<SmoothingPoint2D> recalculated,
|
||||
out string reason)
|
||||
{
|
||||
recalculated = null;
|
||||
reason = string.Empty;
|
||||
if (points == null || points.Count < 2)
|
||||
{
|
||||
reason = "局部 G2 拼接后方向段没有足够点。";
|
||||
return false;
|
||||
}
|
||||
var output = new List<SmoothingPoint2D>(points.Count);
|
||||
double arcLength = 0d;
|
||||
for (int index = 0; index < points.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = points[index];
|
||||
if (!IsValid(point))
|
||||
{
|
||||
reason = "局部 G2 拼接点包含非法数值。";
|
||||
return false;
|
||||
}
|
||||
if (index > 0)
|
||||
{
|
||||
double distance = Distance(points[index - 1], point);
|
||||
if (!NumericGuard.IsPositiveFinite(distance))
|
||||
{
|
||||
reason = "局部 G2 拼接后存在重复非换向点。";
|
||||
return false;
|
||||
}
|
||||
arcLength += distance;
|
||||
}
|
||||
output.Add(new SmoothingPoint2D(point.X, point.Y, arcLength, point.Heading, point.UnwrappedHeading,
|
||||
point.BodyClearance, point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
recalculated = output;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void AddWithoutNonGearDuplicates(List<SmoothingPoint2D> output, SmoothingPoint2D point)
|
||||
{
|
||||
if (output.Count == 0)
|
||||
{
|
||||
output.Add(point);
|
||||
return;
|
||||
}
|
||||
SmoothingPoint2D previous = output[output.Count - 1];
|
||||
if (!previous.IsGearSwitchPoint && !point.IsGearSwitchPoint && SamePosition(previous, point)) return;
|
||||
output.Add(point);
|
||||
}
|
||||
|
||||
private static bool IsValid(SmoothingPoint2D point) => point != null && NumericGuard.IsFinite(point.X) &&
|
||||
NumericGuard.IsFinite(point.Y) && NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
||||
NumericGuard.IsFinite(point.BodyClearance) && point.BodyClearance >= 0d;
|
||||
private static bool SamePosition(SmoothingPoint2D left, SmoothingPoint2D right) =>
|
||||
left != null && right != null && Distance(left, right) <= 1e-9d;
|
||||
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double dx = right.X - left.X;
|
||||
double dy = right.Y - left.Y;
|
||||
return Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,234 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>Publishes independently validated Local G2 regional replacements through the dedicated service route.</summary>
|
||||
internal sealed class LocalG2PreSmoothingPipeline
|
||||
{
|
||||
private readonly CurvatureTransitionDetector _detector = new CurvatureTransitionDetector();
|
||||
private readonly LocalG2WindowPlanner _windowPlanner = new LocalG2WindowPlanner();
|
||||
private readonly LocalG2CandidateBuilder _builder = new LocalG2CandidateBuilder();
|
||||
private readonly LocalG2CandidateEvaluator _evaluator = new LocalG2CandidateEvaluator();
|
||||
private readonly LocalG2RegionWorkOrder _workOrder = new LocalG2RegionWorkOrder();
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
||||
|
||||
internal PathSmoothingResult Smooth(
|
||||
PathSmoothingRequest request,
|
||||
PreparedPath preparedPath,
|
||||
RawPathBaseline rawBaseline,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
try
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (request == null || preparedPath == null || rawBaseline == null ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(request.Vehicle, out double maximumCurvature))
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, "局部 G2 预平滑输入无效。");
|
||||
}
|
||||
|
||||
var options = new LocalG2OptionsSnapshot(request.Configuration);
|
||||
if (!_detector.TryDetect(request, maximumCurvature, options, out IReadOnlyList<CurvatureTransition> transitions, out string reason) ||
|
||||
!_windowPlanner.TryPlan(preparedPath, transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out reason))
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, reason);
|
||||
}
|
||||
|
||||
IReadOnlyList<LocalG2SmoothingRegion> reportOrder =
|
||||
new ReadOnlyCollection<LocalG2SmoothingRegion>(new List<LocalG2SmoothingRegion>(regions));
|
||||
if (!_workOrder.TryCreate(reportOrder, out IReadOnlyList<LocalG2SmoothingRegion> workRegions, out string orderReason))
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, orderReason);
|
||||
|
||||
PreparedPath current = preparedPath;
|
||||
var reportsByRegion = new Dictionary<LocalG2SmoothingRegion, PathSmoothingRegionReport>();
|
||||
var accepted = new List<AcceptedRegion>();
|
||||
int improvedCount = 0;
|
||||
|
||||
foreach (LocalG2SmoothingRegion region in workRegions)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
IReadOnlyList<LocalG2CandidateGeometry> candidates = _builder.Build(
|
||||
preparedPath.Segments[region.SegmentIndex], region,
|
||||
request.Configuration.OutputSpacingMeters, options, cancellationToken);
|
||||
var evaluations = new List<LocalG2CandidateEvaluation>();
|
||||
for (int candidateIndex = 0; candidateIndex < candidates.Count; candidateIndex++)
|
||||
evaluations.Add(_evaluator.Evaluate(
|
||||
preparedPath, current, region, candidates[candidateIndex], request, options, cancellationToken));
|
||||
|
||||
LocalG2CandidateEvaluation best = LocalG2CandidateEvaluator.SelectBest(evaluations);
|
||||
if (best.Accepted)
|
||||
{
|
||||
accepted.Add(new AcceptedRegion(region, current, best));
|
||||
current = best.SplicedPreparedPath;
|
||||
improvedCount++;
|
||||
reportsByRegion.Add(region, CreateImprovedReport(region, candidates.Count, best));
|
||||
}
|
||||
else
|
||||
{
|
||||
reportsByRegion.Add(region, CreateRetainedReport(region, candidates.Count, best));
|
||||
}
|
||||
}
|
||||
|
||||
if (!TryValidateFinal(current, preparedPath, rawBaseline, request, options, out IReadOnlyList<SmoothedPathPoint> path,
|
||||
out IReadOnlyList<SmoothedPathSegment> segments, out PathQualityMetrics metrics, out reason))
|
||||
{
|
||||
for (int index = accepted.Count - 1; index >= 0; index--)
|
||||
{
|
||||
AcceptedRegion rollback = accepted[index];
|
||||
current = rollback.Before;
|
||||
reportsByRegion[rollback.Region] = CreateRollbackReport(rollback.Region, rollback.Evaluation);
|
||||
improvedCount--;
|
||||
if (TryValidateFinal(current, preparedPath, rawBaseline, request, options, out path, out segments, out metrics, out reason))
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (metrics == null)
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, reason);
|
||||
|
||||
var reports = new List<PathSmoothingRegionReport>(reportOrder.Count);
|
||||
for (int reportIndex = 0; reportIndex < reportOrder.Count; reportIndex++)
|
||||
reports.Add(reportsByRegion[reportOrder[reportIndex]]);
|
||||
|
||||
PathSmoothingStatus status;
|
||||
if (transitions.Count == 0) status = PathSmoothingStatus.NotNeeded;
|
||||
else if (improvedCount == regions.Count) status = PathSmoothingStatus.Complete;
|
||||
else if (improvedCount > 0) status = PathSmoothingStatus.PartialImprovement;
|
||||
else status = PathSmoothingStatus.Unchanged;
|
||||
return PathSmoothingResult.PublishLocalG2(
|
||||
status,
|
||||
path,
|
||||
segments,
|
||||
new PathSmoothingDiagnostics(metrics, stopwatch.Elapsed, 0, 0d, reason ?? string.Empty),
|
||||
reports);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Cancelled, stopwatch, "路径平滑已取消。");
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, exception.Message);
|
||||
}
|
||||
}
|
||||
|
||||
private bool TryValidateFinal(
|
||||
PreparedPath current,
|
||||
PreparedPath rawPath,
|
||||
RawPathBaseline rawBaseline,
|
||||
PathSmoothingRequest request,
|
||||
LocalG2OptionsSnapshot options,
|
||||
out IReadOnlyList<SmoothedPathPoint> path,
|
||||
out IReadOnlyList<SmoothedPathSegment> segments,
|
||||
out PathQualityMetrics metrics,
|
||||
out string reason)
|
||||
{
|
||||
path = null;
|
||||
segments = null;
|
||||
metrics = null;
|
||||
reason = string.Empty;
|
||||
if (!_analyzer.TryAnalyze(current.Segments, request.Configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason) ||
|
||||
!_validator.TryValidate(analysis.Path, analysis.Segments, rawPath, request.Map, request.Vehicle,
|
||||
request.Configuration.MaximumCollisionCheckStepMeters, out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearance, out reason) ||
|
||||
minimumClearance < request.Configuration.MinimumClearanceReserveMeters ||
|
||||
analysis.CurvatureVariationCost > rawBaseline.Metrics.CurvatureVariationCost *
|
||||
(1d + options.MaximumVariationCostRegressionRatio))
|
||||
{
|
||||
if (string.IsNullOrEmpty(reason)) reason = "局部 G2 完整路径复核未通过。";
|
||||
return false;
|
||||
}
|
||||
|
||||
path = safePath;
|
||||
segments = analysis.Segments;
|
||||
metrics = new PathQualityMetrics(
|
||||
true,
|
||||
analysis.PathLengthMeters,
|
||||
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
analysis.RootMeanSquareVehicleCurvaturePerMeter,
|
||||
analysis.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
analysis.CurvatureVariationCost,
|
||||
minimumClearance,
|
||||
0d, 0d, 0d, 0d);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static PathSmoothingRegionReport CreateImprovedReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
int candidateCount,
|
||||
LocalG2CandidateEvaluation evaluation) =>
|
||||
CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.Improved, PathSmoothingRegionFailureReason.None);
|
||||
|
||||
private static PathSmoothingRegionReport CreateRetainedReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
int candidateCount,
|
||||
LocalG2CandidateEvaluation evaluation) =>
|
||||
CreateReport(region, candidateCount, evaluation, PathSmoothingRegionStatus.RetainedOriginal, evaluation.FailureReason);
|
||||
|
||||
private static PathSmoothingRegionReport CreateRollbackReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
LocalG2CandidateEvaluation evaluation) =>
|
||||
CreateReport(region, region.WindowVariants.Count, evaluation, PathSmoothingRegionStatus.RetainedOriginal,
|
||||
PathSmoothingRegionFailureReason.GlobalValidationRollback);
|
||||
|
||||
private static PathSmoothingRegionReport CreateReport(
|
||||
LocalG2SmoothingRegion region,
|
||||
int candidateCount,
|
||||
LocalG2CandidateEvaluation evaluation,
|
||||
PathSmoothingRegionStatus status,
|
||||
PathSmoothingRegionFailureReason failureReason)
|
||||
{
|
||||
LocalG2WindowVariant window = region.WindowVariants[0];
|
||||
var jumps = new List<double>(region.Transitions.Count);
|
||||
for (int index = 0; index < region.Transitions.Count; index++)
|
||||
jumps.Add(region.Transitions[index].RightVehicleCurvaturePerMeter - region.Transitions[index].LeftVehicleCurvaturePerMeter);
|
||||
return new PathSmoothingRegionReport(
|
||||
region.SegmentIndex,
|
||||
window.StartArcLengthMeters,
|
||||
window.EndArcLengthMeters,
|
||||
jumps,
|
||||
window.EndArcLengthMeters - window.StartArcLengthMeters,
|
||||
window.EndArcLengthMeters - window.StartArcLengthMeters,
|
||||
window.LeftWindowLengthMeters,
|
||||
window.RightWindowLengthMeters,
|
||||
candidateCount,
|
||||
evaluation.CandidateIndex,
|
||||
status,
|
||||
failureReason,
|
||||
evaluation.RawPeakCurvatureDerivativePerSquareMeter,
|
||||
evaluation.ResultPeakCurvatureDerivativePerSquareMeter,
|
||||
evaluation.RawCurvatureVariationCost,
|
||||
evaluation.ResultCurvatureVariationCost,
|
||||
evaluation.MaximumDeviationMeters,
|
||||
evaluation.MinimumBodyClearanceMeters,
|
||||
evaluation.MaximumAbsoluteVehicleCurvaturePerMeter);
|
||||
}
|
||||
|
||||
private static PathSmoothingResult Failure(PathSmoothingStatus status, Stopwatch stopwatch, string reason) =>
|
||||
PathSmoothingResult.Failure(status,
|
||||
new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, 0, 0d, reason));
|
||||
|
||||
private sealed class AcceptedRegion
|
||||
{
|
||||
internal AcceptedRegion(LocalG2SmoothingRegion region, PreparedPath before, LocalG2CandidateEvaluation evaluation)
|
||||
{
|
||||
Region = region;
|
||||
Before = before;
|
||||
Evaluation = evaluation;
|
||||
}
|
||||
|
||||
internal LocalG2SmoothingRegion Region { get; }
|
||||
internal PreparedPath Before { get; }
|
||||
internal LocalG2CandidateEvaluation Evaluation { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>把稳定报告顺序转换为不会使后续原始局部弧长错位的工作顺序。</summary>
|
||||
internal sealed class LocalG2RegionWorkOrder
|
||||
{
|
||||
internal bool TryCreate(
|
||||
IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
|
||||
out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
|
||||
out string reason)
|
||||
{
|
||||
workOrder = Empty();
|
||||
reason = string.Empty;
|
||||
if (reportOrder == null)
|
||||
{
|
||||
reason = "局部 G2 区域工作顺序输入无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var indexed = new List<IndexedRegion>(reportOrder.Count);
|
||||
for (int index = 0; index < reportOrder.Count; index++)
|
||||
{
|
||||
LocalG2SmoothingRegion region = reportOrder[index];
|
||||
if (!TryValidate(region, out double firstArc, out reason))
|
||||
return false;
|
||||
indexed.Add(new IndexedRegion(region, index, firstArc));
|
||||
}
|
||||
|
||||
indexed.Sort(Compare);
|
||||
var ordered = new List<LocalG2SmoothingRegion>(indexed.Count);
|
||||
for (int index = 0; index < indexed.Count; index++)
|
||||
ordered.Add(indexed[index].Region);
|
||||
workOrder = new ReadOnlyCollection<LocalG2SmoothingRegion>(ordered);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryValidate(
|
||||
LocalG2SmoothingRegion region,
|
||||
out double firstArc,
|
||||
out string reason)
|
||||
{
|
||||
firstArc = 0d;
|
||||
reason = string.Empty;
|
||||
if (region == null || region.SegmentIndex < 0 ||
|
||||
region.Transitions == null || region.Transitions.Count == 0 ||
|
||||
region.WindowVariants == null || region.WindowVariants.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 工作顺序包含空区域或空窗口集。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double previousArc = -1d;
|
||||
for (int index = 0; index < region.Transitions.Count; index++)
|
||||
{
|
||||
CurvatureTransition transition = region.Transitions[index];
|
||||
if (transition == null ||
|
||||
transition.SegmentIndex != region.SegmentIndex ||
|
||||
!NumericGuard.IsFinite(transition.LocalArcLengthMeters) ||
|
||||
transition.LocalArcLengthMeters < previousArc)
|
||||
{
|
||||
reason = "局部 G2 工作顺序要求区域事件有限、同段且按弧长升序。";
|
||||
return false;
|
||||
}
|
||||
previousArc = transition.LocalArcLengthMeters;
|
||||
}
|
||||
|
||||
firstArc = region.Transitions[0].LocalArcLengthMeters;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static int Compare(IndexedRegion left, IndexedRegion right)
|
||||
{
|
||||
int segment = left.Region.SegmentIndex.CompareTo(right.Region.SegmentIndex);
|
||||
if (segment != 0) return segment;
|
||||
int descendingArc = right.FirstArc.CompareTo(left.FirstArc);
|
||||
return descendingArc != 0
|
||||
? descendingArc
|
||||
: left.ReportIndex.CompareTo(right.ReportIndex);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<LocalG2SmoothingRegion> Empty()
|
||||
{
|
||||
return new ReadOnlyCollection<LocalG2SmoothingRegion>(
|
||||
new List<LocalG2SmoothingRegion>());
|
||||
}
|
||||
|
||||
private sealed class IndexedRegion
|
||||
{
|
||||
internal IndexedRegion(
|
||||
LocalG2SmoothingRegion region,
|
||||
int reportIndex,
|
||||
double firstArc)
|
||||
{
|
||||
Region = region;
|
||||
ReportIndex = reportIndex;
|
||||
FirstArc = firstArc;
|
||||
}
|
||||
|
||||
internal LocalG2SmoothingRegion Region { get; }
|
||||
internal int ReportIndex { get; }
|
||||
internal double FirstArc { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>一个局部 G2 候选可替换的弧长窗口。</summary>
|
||||
internal sealed class LocalG2WindowVariant
|
||||
{
|
||||
internal LocalG2WindowVariant(int candidateIndex, double startArcLengthMeters, double endArcLengthMeters,
|
||||
double leftWindowLengthMeters, double rightWindowLengthMeters)
|
||||
{
|
||||
if (candidateIndex < 0 || startArcLengthMeters < 0d || endArcLengthMeters < startArcLengthMeters ||
|
||||
leftWindowLengthMeters < 0d || rightWindowLengthMeters < 0d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(candidateIndex));
|
||||
}
|
||||
CandidateIndex = candidateIndex;
|
||||
StartArcLengthMeters = startArcLengthMeters;
|
||||
EndArcLengthMeters = endArcLengthMeters;
|
||||
LeftWindowLengthMeters = leftWindowLengthMeters;
|
||||
RightWindowLengthMeters = rightWindowLengthMeters;
|
||||
}
|
||||
|
||||
internal int CandidateIndex { get; }
|
||||
internal double StartArcLengthMeters { get; }
|
||||
internal double EndArcLengthMeters { get; }
|
||||
internal double LeftWindowLengthMeters { get; }
|
||||
internal double RightWindowLengthMeters { get; }
|
||||
}
|
||||
|
||||
/// <summary>至少存在一个联合合法生成窗口变体的一组曲率过渡。</summary>
|
||||
internal sealed class LocalG2SmoothingRegion
|
||||
{
|
||||
internal LocalG2SmoothingRegion(
|
||||
int segmentIndex,
|
||||
IReadOnlyList<CurvatureTransition> transitions,
|
||||
double maximumStartArcLengthMeters,
|
||||
double maximumEndArcLengthMeters,
|
||||
IReadOnlyList<LocalG2WindowVariant> windowVariants)
|
||||
{
|
||||
if (segmentIndex < 0 || transitions == null || transitions.Count == 0 ||
|
||||
!NumericGuard.IsFinite(maximumStartArcLengthMeters) ||
|
||||
!NumericGuard.IsFinite(maximumEndArcLengthMeters) ||
|
||||
maximumStartArcLengthMeters < 0d ||
|
||||
maximumEndArcLengthMeters < maximumStartArcLengthMeters ||
|
||||
windowVariants == null || windowVariants.Count == 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(transitions));
|
||||
}
|
||||
SegmentIndex = segmentIndex;
|
||||
Transitions = Copy(transitions);
|
||||
MaximumStartArcLengthMeters = maximumStartArcLengthMeters;
|
||||
MaximumEndArcLengthMeters = maximumEndArcLengthMeters;
|
||||
WindowVariants = Copy(windowVariants);
|
||||
}
|
||||
|
||||
internal int SegmentIndex { get; }
|
||||
internal IReadOnlyList<CurvatureTransition> Transitions { get; }
|
||||
internal double MaximumStartArcLengthMeters { get; }
|
||||
internal double MaximumEndArcLengthMeters { get; }
|
||||
internal IReadOnlyList<LocalG2WindowVariant> WindowVariants { get; }
|
||||
|
||||
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,378 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>按硬方向边界生成并合并局部 G2 曲率事件的候选窗口。</summary>
|
||||
internal sealed class LocalG2WindowPlanner
|
||||
{
|
||||
private const double MergeToleranceMeters = 1e-9d;
|
||||
|
||||
internal bool TryPlan(
|
||||
PreparedPath originalPath,
|
||||
IReadOnlyList<CurvatureTransition> transitions,
|
||||
LocalG2OptionsSnapshot options,
|
||||
out IReadOnlyList<LocalG2SmoothingRegion> regions,
|
||||
out string reason)
|
||||
{
|
||||
regions = Empty<LocalG2SmoothingRegion>();
|
||||
reason = string.Empty;
|
||||
if (originalPath == null || transitions == null || options == null)
|
||||
{
|
||||
reason = "局部 G2 窗口规划输入无效。";
|
||||
return false;
|
||||
}
|
||||
if (!TryGetSegmentLengths(originalPath, out Dictionary<int, double> segmentLengths, out reason)) return false;
|
||||
|
||||
var ordered = new List<CurvatureTransition>(transitions.Count);
|
||||
for (int index = 0; index < transitions.Count; index++)
|
||||
{
|
||||
CurvatureTransition transition = transitions[index];
|
||||
if (transition == null || !segmentLengths.TryGetValue(transition.SegmentIndex, out double length) ||
|
||||
!NumericGuard.IsFinite(transition.LocalArcLengthMeters) || transition.LocalArcLengthMeters < 0d ||
|
||||
transition.LocalArcLengthMeters > length + MergeToleranceMeters)
|
||||
{
|
||||
reason = "局部 G2 曲率事件不属于有效方向分段。";
|
||||
return false;
|
||||
}
|
||||
ordered.Add(transition);
|
||||
}
|
||||
ordered.Sort(CompareTransitions);
|
||||
|
||||
var planned = new List<LocalG2SmoothingRegion>();
|
||||
int cursor = 0;
|
||||
while (cursor < ordered.Count)
|
||||
{
|
||||
CurvatureTransition first = ordered[cursor];
|
||||
double segmentLength = segmentLengths[first.SegmentIndex];
|
||||
var group = new List<CurvatureTransition> { first };
|
||||
IReadOnlyList<LocalG2WindowVariant> variants =
|
||||
BuildVariants(group, segmentLength, options);
|
||||
if (variants.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 单事件无法生成满足总长度约束的窗口。";
|
||||
return false;
|
||||
}
|
||||
cursor++;
|
||||
|
||||
while (cursor < ordered.Count &&
|
||||
ordered[cursor].SegmentIndex == first.SegmentIndex)
|
||||
{
|
||||
var tentative = new List<CurvatureTransition>(group)
|
||||
{
|
||||
ordered[cursor],
|
||||
};
|
||||
IReadOnlyList<LocalG2WindowVariant> tentativeVariants =
|
||||
BuildVariants(tentative, segmentLength, options);
|
||||
if (tentativeVariants.Count == 0) break;
|
||||
group = tentative;
|
||||
variants = tentativeVariants;
|
||||
cursor++;
|
||||
}
|
||||
|
||||
double minimumStart = double.PositiveInfinity;
|
||||
double maximumEnd = double.NegativeInfinity;
|
||||
for (int variantIndex = 0; variantIndex < variants.Count; variantIndex++)
|
||||
{
|
||||
minimumStart = Math.Min(
|
||||
minimumStart,
|
||||
variants[variantIndex].StartArcLengthMeters);
|
||||
maximumEnd = Math.Max(
|
||||
maximumEnd,
|
||||
variants[variantIndex].EndArcLengthMeters);
|
||||
}
|
||||
|
||||
planned.Add(new LocalG2SmoothingRegion(
|
||||
first.SegmentIndex,
|
||||
group,
|
||||
minimumStart,
|
||||
maximumEnd,
|
||||
variants));
|
||||
}
|
||||
regions = new ReadOnlyCollection<LocalG2SmoothingRegion>(planned);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<LocalG2WindowVariant> BuildVariants(
|
||||
IReadOnlyList<CurvatureTransition> transitions,
|
||||
double segmentLength,
|
||||
LocalG2OptionsSnapshot options)
|
||||
{
|
||||
var variants = new List<LocalG2WindowVariant>();
|
||||
double firstEvent = transitions[0].LocalArcLengthMeters;
|
||||
double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters;
|
||||
double anchor = (firstEvent + lastEvent) / 2d;
|
||||
foreach (double target in BuildTargets(options, segmentLength))
|
||||
{
|
||||
if (variants.Count >= options.MaximumCandidatesPerRegion) break;
|
||||
AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options);
|
||||
AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options);
|
||||
AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options);
|
||||
}
|
||||
return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
|
||||
}
|
||||
|
||||
private static void AddIfLegal(List<LocalG2WindowVariant> variants, double target, double leftRatio,
|
||||
double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift,
|
||||
LocalG2OptionsSnapshot options)
|
||||
{
|
||||
if (variants.Count >= options.MaximumCandidatesPerRegion) return;
|
||||
double left = target * leftRatio;
|
||||
double right = target - left;
|
||||
double availableLeft = anchor;
|
||||
double availableRight = segmentLength - anchor;
|
||||
if (permitBoundaryShift)
|
||||
{
|
||||
left = Math.Min(left, availableLeft);
|
||||
right = Math.Min(right, availableRight);
|
||||
double missing = target - left - right;
|
||||
double addRight = Math.Min(missing, availableRight - right);
|
||||
right += addRight;
|
||||
left += Math.Min(missing - addRight, availableLeft - left);
|
||||
}
|
||||
else if (left > availableLeft + MergeToleranceMeters || right > availableRight + MergeToleranceMeters)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
double start = anchor - left;
|
||||
double end = anchor + right;
|
||||
if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target)
|
||||
return;
|
||||
double actualLength = end - start;
|
||||
if (actualLength + MergeToleranceMeters < options.MinimumWindowLengthMeters ||
|
||||
actualLength > options.MaximumWindowLengthMeters + MergeToleranceMeters)
|
||||
{
|
||||
return;
|
||||
}
|
||||
variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right));
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> BuildTargets(LocalG2OptionsSnapshot options, double segmentLength)
|
||||
{
|
||||
if (segmentLength + MergeToleranceMeters < options.MinimumWindowLengthMeters)
|
||||
return new ReadOnlyCollection<double>(new List<double>());
|
||||
|
||||
double[] requested =
|
||||
{
|
||||
options.PreferredWindowLengthMeters,
|
||||
0.75d * options.PreferredWindowLengthMeters,
|
||||
1.25d * options.PreferredWindowLengthMeters,
|
||||
options.MinimumWindowLengthMeters,
|
||||
options.MaximumWindowLengthMeters,
|
||||
};
|
||||
var targets = new List<double>(requested.Length);
|
||||
for (int index = 0; index < requested.Length; index++)
|
||||
{
|
||||
double target = Math.Min(segmentLength,
|
||||
Math.Max(options.MinimumWindowLengthMeters, Math.Min(options.MaximumWindowLengthMeters, requested[index])));
|
||||
bool duplicate = false;
|
||||
for (int prior = 0; prior < targets.Count; prior++)
|
||||
{
|
||||
if (Math.Abs(targets[prior] - target) <= MergeToleranceMeters)
|
||||
{
|
||||
duplicate = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!duplicate) targets.Add(target);
|
||||
}
|
||||
return targets;
|
||||
}
|
||||
|
||||
private static bool TryGetSegmentLengths(PreparedPath originalPath, out Dictionary<int, double> lengths, out string reason)
|
||||
{
|
||||
lengths = new Dictionary<int, double>();
|
||||
reason = string.Empty;
|
||||
for (int position = 0; position < originalPath.Segments.Count; position++)
|
||||
{
|
||||
PreparedDirectionSegment segment = originalPath.Segments[position];
|
||||
if (segment == null || segment.SegmentIndex != position || segment.Points == null || segment.Points.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 窗口规划的预处理方向分段无效。";
|
||||
return false;
|
||||
}
|
||||
double previousArc = -1d;
|
||||
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
||||
{
|
||||
double arc = segment.Points[pointIndex].ArcLength;
|
||||
if (!NumericGuard.IsFinite(arc) || arc < 0d || arc < previousArc)
|
||||
{
|
||||
reason = "局部 G2 窗口规划要求分段弧长有限且非递减。";
|
||||
return false;
|
||||
}
|
||||
previousArc = arc;
|
||||
}
|
||||
lengths.Add(segment.SegmentIndex, previousArc);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static int CompareTransitions(CurvatureTransition left, CurvatureTransition right)
|
||||
{
|
||||
int segment = left.SegmentIndex.CompareTo(right.SegmentIndex);
|
||||
if (segment != 0) return segment;
|
||||
int arc = left.LocalArcLengthMeters.CompareTo(right.LocalArcLengthMeters);
|
||||
return arc != 0 ? arc : left.LeftCoarsePathIndex.CompareTo(right.LeftCoarsePathIndex);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());
|
||||
|
||||
public static class TestHooks
|
||||
{
|
||||
public static WindowPlanningTestSnapshot Execute(string scenario)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(scenario))
|
||||
throw new ArgumentException("A scenario is required.", nameof(scenario));
|
||||
|
||||
IReadOnlyList<CurvatureTransition> transitions;
|
||||
double segmentLength;
|
||||
switch (scenario)
|
||||
{
|
||||
case "SeparatedByOneMeter":
|
||||
transitions = new[]
|
||||
{
|
||||
Transition(0.2d, 0),
|
||||
Transition(1.2d, 1),
|
||||
};
|
||||
segmentLength = 1.4d;
|
||||
break;
|
||||
case "Mergeable":
|
||||
transitions = new[]
|
||||
{
|
||||
Transition(0.4d, 0),
|
||||
Transition(0.7d, 1),
|
||||
};
|
||||
segmentLength = 1.4d;
|
||||
break;
|
||||
case "ThreeEventPartition":
|
||||
transitions = new[]
|
||||
{
|
||||
Transition(0.2d, 0),
|
||||
Transition(0.6d, 1),
|
||||
Transition(1.2d, 2),
|
||||
};
|
||||
segmentLength = 1.4d;
|
||||
break;
|
||||
case "NearBoundary":
|
||||
transitions = new[] { Transition(0.1d, 0) };
|
||||
segmentLength = 1d;
|
||||
break;
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
|
||||
var planner = new LocalG2WindowPlanner();
|
||||
if (!planner.TryPlan(
|
||||
CreatePreparedPath(segmentLength),
|
||||
transitions,
|
||||
new LocalG2OptionsSnapshot(new PathSmoothingConfiguration()),
|
||||
out IReadOnlyList<LocalG2SmoothingRegion> regions,
|
||||
out string reason))
|
||||
{
|
||||
throw new InvalidOperationException(reason);
|
||||
}
|
||||
|
||||
double maximumLength = 0d;
|
||||
bool exactEnvelope = true;
|
||||
var counts = new List<string>(regions.Count);
|
||||
var signature = new List<string>();
|
||||
for (int regionIndex = 0; regionIndex < regions.Count; regionIndex++)
|
||||
{
|
||||
LocalG2SmoothingRegion region = regions[regionIndex];
|
||||
counts.Add(region.Transitions.Count.ToString());
|
||||
double minimumStart = double.PositiveInfinity;
|
||||
double maximumEnd = double.NegativeInfinity;
|
||||
for (int variantIndex = 0; variantIndex < region.WindowVariants.Count; variantIndex++)
|
||||
{
|
||||
LocalG2WindowVariant variant = region.WindowVariants[variantIndex];
|
||||
maximumLength = Math.Max(
|
||||
maximumLength,
|
||||
variant.EndArcLengthMeters - variant.StartArcLengthMeters);
|
||||
minimumStart = Math.Min(minimumStart, variant.StartArcLengthMeters);
|
||||
maximumEnd = Math.Max(maximumEnd, variant.EndArcLengthMeters);
|
||||
signature.Add(
|
||||
region.SegmentIndex + ":" +
|
||||
variant.CandidateIndex + ":" +
|
||||
variant.StartArcLengthMeters.ToString("R") + ":" +
|
||||
variant.EndArcLengthMeters.ToString("R"));
|
||||
}
|
||||
exactEnvelope &= Math.Abs(region.MaximumStartArcLengthMeters - minimumStart) <= 1e-9d;
|
||||
exactEnvelope &= Math.Abs(region.MaximumEndArcLengthMeters - maximumEnd) <= 1e-9d;
|
||||
}
|
||||
|
||||
LocalG2WindowVariant first = regions[0].WindowVariants[0];
|
||||
return new WindowPlanningTestSnapshot(
|
||||
regions.Count,
|
||||
string.Join(",", counts),
|
||||
maximumLength,
|
||||
exactEnvelope,
|
||||
first.LeftWindowLengthMeters,
|
||||
first.RightWindowLengthMeters,
|
||||
string.Join("|", signature));
|
||||
}
|
||||
|
||||
public sealed class WindowPlanningTestSnapshot
|
||||
{
|
||||
internal WindowPlanningTestSnapshot(
|
||||
int regionCount,
|
||||
string transitionCounts,
|
||||
double maximumWindowLength,
|
||||
bool exactEnvelope,
|
||||
double firstLeftLength,
|
||||
double firstRightLength,
|
||||
string signature)
|
||||
{
|
||||
RegionCount = regionCount;
|
||||
TransitionCounts = transitionCounts;
|
||||
MaximumWindowLength = maximumWindowLength;
|
||||
ExactEnvelope = exactEnvelope;
|
||||
FirstLeftLength = firstLeftLength;
|
||||
FirstRightLength = firstRightLength;
|
||||
Signature = signature;
|
||||
}
|
||||
|
||||
public int RegionCount { get; }
|
||||
public string TransitionCounts { get; }
|
||||
public double MaximumWindowLength { get; }
|
||||
public bool ExactEnvelope { get; }
|
||||
public double FirstLeftLength { get; }
|
||||
public double FirstRightLength { get; }
|
||||
public string Signature { get; }
|
||||
}
|
||||
|
||||
private static CurvatureTransition Transition(double arcLength, int index)
|
||||
{
|
||||
return new CurvatureTransition(
|
||||
0,
|
||||
index,
|
||||
index + 1,
|
||||
arcLength,
|
||||
arcLength,
|
||||
0d,
|
||||
0d,
|
||||
index % 2 == 0 ? 0d : 0.5d,
|
||||
index % 2 == 0 ? 0.5d : 0d);
|
||||
}
|
||||
|
||||
private static PreparedPath CreatePreparedPath(double length)
|
||||
{
|
||||
var points = new[]
|
||||
{
|
||||
new SmoothingPoint2D(
|
||||
0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
new SmoothingPoint2D(
|
||||
length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
return new PreparedPath(new[]
|
||||
{
|
||||
new PreparedDirectionSegment(
|
||||
0, TravelDirection.Forward, points, false, false),
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,124 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>满足两个二维端点二阶边界条件的参数五次 Hermite 曲线。</summary>
|
||||
internal sealed class QuinticHermiteCurve2D
|
||||
{
|
||||
private readonly double _x0;
|
||||
private readonly double _x1;
|
||||
private readonly double _x2;
|
||||
private readonly double _x3;
|
||||
private readonly double _x4;
|
||||
private readonly double _x5;
|
||||
private readonly double _y0;
|
||||
private readonly double _y1;
|
||||
private readonly double _y2;
|
||||
private readonly double _y3;
|
||||
private readonly double _y4;
|
||||
private readonly double _y5;
|
||||
|
||||
private QuinticHermiteCurve2D(
|
||||
double x0, double x1, double x2, double x3, double x4, double x5,
|
||||
double y0, double y1, double y2, double y3, double y4, double y5)
|
||||
{
|
||||
_x0 = x0;
|
||||
_x1 = x1;
|
||||
_x2 = x2;
|
||||
_x3 = x3;
|
||||
_x4 = x4;
|
||||
_x5 = x5;
|
||||
_y0 = y0;
|
||||
_y1 = y1;
|
||||
_y2 = y2;
|
||||
_y3 = y3;
|
||||
_y4 = y4;
|
||||
_y5 = y5;
|
||||
}
|
||||
|
||||
internal static bool TryCreate(
|
||||
double x0, double y0, double dx0, double dy0, double ddx0, double ddy0,
|
||||
double x1, double y1, double dx1, double dy1, double ddx1, double ddy1,
|
||||
out QuinticHermiteCurve2D curve,
|
||||
out string reason)
|
||||
{
|
||||
curve = null;
|
||||
reason = string.Empty;
|
||||
if (!AreFinite(x0, y0, dx0, dy0, ddx0, ddy0, x1, y1, dx1, dy1, ddx1, ddy1))
|
||||
{
|
||||
reason = "五次 Hermite 曲线需要有限的边界条件。";
|
||||
return false;
|
||||
}
|
||||
if (IsZeroVector(dx0, dy0) || IsZeroVector(dx1, dy1))
|
||||
{
|
||||
reason = "五次 Hermite 曲线端点一阶导数不能为零。";
|
||||
return false;
|
||||
}
|
||||
|
||||
SolveCoordinate(x0, dx0, ddx0, x1, dx1, ddx1,
|
||||
out double ax0, out double ax1, out double ax2, out double ax3, out double ax4, out double ax5);
|
||||
SolveCoordinate(y0, dy0, ddy0, y1, dy1, ddy1,
|
||||
out double ay0, out double ay1, out double ay2, out double ay3, out double ay4, out double ay5);
|
||||
if (!AreFinite(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5))
|
||||
{
|
||||
reason = "五次 Hermite 曲线系数溢出。";
|
||||
return false;
|
||||
}
|
||||
|
||||
curve = new QuinticHermiteCurve2D(ax0, ax1, ax2, ax3, ax4, ax5, ay0, ay1, ay2, ay3, ay4, ay5);
|
||||
return true;
|
||||
}
|
||||
|
||||
internal void Evaluate(
|
||||
double u,
|
||||
out double x, out double y,
|
||||
out double dx, out double dy,
|
||||
out double ddx, out double ddy)
|
||||
{
|
||||
if (!IsFinite(u) || u < 0d || u > 1d)
|
||||
throw new ArgumentOutOfRangeException(nameof(u), "The curve parameter must be finite and within [0, 1].");
|
||||
|
||||
x = EvaluateValue(_x0, _x1, _x2, _x3, _x4, _x5, u);
|
||||
y = EvaluateValue(_y0, _y1, _y2, _y3, _y4, _y5, u);
|
||||
dx = EvaluateFirstDerivative(_x1, _x2, _x3, _x4, _x5, u);
|
||||
dy = EvaluateFirstDerivative(_y1, _y2, _y3, _y4, _y5, u);
|
||||
ddx = EvaluateSecondDerivative(_x2, _x3, _x4, _x5, u);
|
||||
ddy = EvaluateSecondDerivative(_y2, _y3, _y4, _y5, u);
|
||||
}
|
||||
|
||||
private static void SolveCoordinate(double p0, double v0, double acceleration0, double p1, double v1, double acceleration1,
|
||||
out double a0, out double a1, out double a2, out double a3, out double a4, out double a5)
|
||||
{
|
||||
a0 = p0;
|
||||
a1 = v0;
|
||||
a2 = acceleration0 / 2d;
|
||||
double c0 = p1 - (a0 + a1 + a2);
|
||||
double c1 = v1 - (a1 + 2d * a2);
|
||||
double c2 = acceleration1 - 2d * a2;
|
||||
a3 = 10d * c0 - 4d * c1 + 0.5d * c2;
|
||||
a4 = -15d * c0 + 7d * c1 - c2;
|
||||
a5 = 6d * c0 - 3d * c1 + 0.5d * c2;
|
||||
}
|
||||
|
||||
private static double EvaluateValue(double a0, double a1, double a2, double a3, double a4, double a5, double u) =>
|
||||
(((((a5 * u + a4) * u + a3) * u + a2) * u + a1) * u) + a0;
|
||||
|
||||
private static double EvaluateFirstDerivative(double a1, double a2, double a3, double a4, double a5, double u) =>
|
||||
((((5d * a5 * u + 4d * a4) * u + 3d * a3) * u + 2d * a2) * u) + a1;
|
||||
|
||||
private static double EvaluateSecondDerivative(double a2, double a3, double a4, double a5, double u) =>
|
||||
(((20d * a5 * u + 12d * a4) * u + 6d * a3) * u) + 2d * a2;
|
||||
|
||||
private static bool IsZeroVector(double x, double y) => x == 0d && y == 0d;
|
||||
|
||||
private static bool AreFinite(params double[] values)
|
||||
{
|
||||
for (int index = 0; index < values.Length; index++)
|
||||
{
|
||||
if (!IsFinite(values[index])) return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
@@ -0,0 +1,145 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>按单一方向段的弧长线性插值并保留精确锚点的确定性重采样器。</summary>
|
||||
public sealed class ArcLengthResampler
|
||||
{
|
||||
private const double Tolerance = 1e-10d;
|
||||
|
||||
/// <summary>以目标间距重采样一个方向段；段末锚点始终原样保留。</summary>
|
||||
public bool TryResample(
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
double spacingMeters,
|
||||
out IReadOnlyList<SmoothingPoint2D> resampled,
|
||||
out string reason)
|
||||
{
|
||||
resampled = EmptyPoints();
|
||||
reason = string.Empty;
|
||||
if (points == null || points.Count == 0 || !NumericGuard.IsPositiveFinite(spacingMeters))
|
||||
{
|
||||
reason = "重采样点集或采样间距无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 0; index < points.Count; index++)
|
||||
{
|
||||
if (!IsValidPoint(points[index]))
|
||||
{
|
||||
reason = "重采样输入包含非法数值。";
|
||||
return false;
|
||||
}
|
||||
if (index == 0) continue;
|
||||
|
||||
SmoothingPoint2D previous = points[index - 1];
|
||||
SmoothingPoint2D current = points[index];
|
||||
if (current.ArcLength <= previous.ArcLength + Tolerance)
|
||||
{
|
||||
reason = "同一方向段的弧长必须严格增加。";
|
||||
return false;
|
||||
}
|
||||
double distance = Distance(previous, current);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= Tolerance)
|
||||
{
|
||||
reason = "同一方向段中不允许重复位姿或数值溢出的距离。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (points.Count == 1)
|
||||
{
|
||||
resampled = CopyReadOnly(points);
|
||||
return true;
|
||||
}
|
||||
|
||||
var output = new List<SmoothingPoint2D> { points[0] };
|
||||
double firstArc = points[0].ArcLength;
|
||||
double finalArc = points[points.Count - 1].ArcLength;
|
||||
int rightIndex = 1;
|
||||
for (double targetArc = firstArc + spacingMeters;
|
||||
targetArc < finalArc - Tolerance;
|
||||
targetArc += spacingMeters)
|
||||
{
|
||||
while (rightIndex < points.Count - 1 && points[rightIndex].ArcLength < targetArc)
|
||||
rightIndex++;
|
||||
|
||||
SmoothingPoint2D left = points[rightIndex - 1];
|
||||
SmoothingPoint2D right = points[rightIndex];
|
||||
double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
|
||||
double unwrappedHeading = left.UnwrappedHeading +
|
||||
ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
|
||||
output.Add(new SmoothingPoint2D(
|
||||
left.X + ratio * (right.X - left.X),
|
||||
left.Y + ratio * (right.Y - left.Y),
|
||||
targetArc,
|
||||
AngleMath.NormalizeRadians(unwrappedHeading),
|
||||
unwrappedHeading,
|
||||
Math.Min(left.BodyClearance, right.BodyClearance),
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
|
||||
// Appending the original object, rather than interpolating at the final arc, preserves the exact endpoint.
|
||||
output.Add(points[points.Count - 1]);
|
||||
resampled = new ReadOnlyCollection<SmoothingPoint2D>(output);
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>重采样一个完整方向段并保留其换向拓扑标记。</summary>
|
||||
public bool TryResample(
|
||||
PreparedDirectionSegment segment,
|
||||
double spacingMeters,
|
||||
out PreparedDirectionSegment resampled,
|
||||
out string reason)
|
||||
{
|
||||
resampled = null;
|
||||
reason = string.Empty;
|
||||
if (segment == null)
|
||||
{
|
||||
reason = "待重采样方向段为空。";
|
||||
return false;
|
||||
}
|
||||
if (!TryResample(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> points, out reason))
|
||||
return false;
|
||||
|
||||
resampled = new PreparedDirectionSegment(
|
||||
segment.SegmentIndex,
|
||||
segment.Direction,
|
||||
points,
|
||||
segment.StartsAtGearSwitch,
|
||||
segment.EndsAtGearSwitch,
|
||||
segment.StartVehicleCurvaturePerMeter);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(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 Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double x = right.X - left.X;
|
||||
double y = right.Y - left.Y;
|
||||
return Math.Sqrt(x * x + y * y);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingPoint2D> EmptyPoints()
|
||||
{
|
||||
return new ReadOnlyCollection<SmoothingPoint2D>(new List<SmoothingPoint2D>());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>同一几何分析器产生的路径、方向段和未验证质量统计。</summary>
|
||||
public sealed class PathGeometryAnalysis
|
||||
{
|
||||
internal PathGeometryAnalysis(
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters)
|
||||
{
|
||||
Path = CopyReadOnly(path);
|
||||
Segments = CopyReadOnly(segments);
|
||||
PathLengthMeters = pathLengthMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter = maximumAbsoluteVehicleCurvatureDerivativePerSquareMeter;
|
||||
RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
|
||||
TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
|
||||
CurvatureVariationEnergy = curvatureVariationEnergy;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
}
|
||||
|
||||
/// <summary>完成几何重计算的不可变路径。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>完整覆盖 <see cref="Path"/> 的不可变方向段。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>路径总长度，单位 m。</summary>
|
||||
public double PathLengthMeters { get; }
|
||||
|
||||
/// <summary>车辆曲率绝对值峰值，单位 1/m。</summary>
|
||||
public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>车辆曲率导数绝对值峰值，单位 1/m²。</summary>
|
||||
public double MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter { get; }
|
||||
|
||||
/// <summary>车辆曲率均方根，单位 1/m。</summary>
|
||||
public double RootMeanSquareVehicleCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>不跨换向点累计的绝对曲率变化，单位 1/m。</summary>
|
||||
public double TotalAbsoluteCurvatureVariationPerMeter { get; }
|
||||
|
||||
/// <summary>不跨换向点累计的曲率变化代价。</summary>
|
||||
public double CurvatureVariationEnergy { get; }
|
||||
|
||||
/// <summary>曲率变化代价的面向用户名称；保留 <see cref="CurvatureVariationEnergy"/> 以兼容既有调用方。</summary>
|
||||
public double CurvatureVariationCost => CurvatureVariationEnergy;
|
||||
|
||||
/// <summary>输入点携带的最小保守净空，单位 m。</summary>
|
||||
public double MinimumBodyClearanceMeters { get; }
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,490 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>在每个单独方向段内统一重采样、恢复航向并计算几何曲率。</summary>
|
||||
public sealed class PathGeometryAnalyzer
|
||||
{
|
||||
private const double MinimumDistanceMeters = 1e-10d;
|
||||
private const double BoundaryToleranceMeters = 1e-8d;
|
||||
|
||||
/// <summary>
|
||||
/// 对候选方向段进行确定性几何分析。换向点两侧永不参与同一次差分。
|
||||
/// </summary>
|
||||
public bool TryAnalyze(
|
||||
IReadOnlyList<PreparedDirectionSegment> candidateSegments,
|
||||
double spacingMeters,
|
||||
out PathGeometryAnalysis analysis,
|
||||
out string reason)
|
||||
{
|
||||
analysis = null;
|
||||
reason = string.Empty;
|
||||
if (candidateSegments == null || candidateSegments.Count == 0 ||
|
||||
!NumericGuard.IsPositiveFinite(spacingMeters))
|
||||
{
|
||||
reason = "候选方向段或输出采样间距无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var outputPath = new List<SmoothedPathPoint>();
|
||||
var outputSegments = new List<SmoothedPathSegment>();
|
||||
double cumulativeArcLength = 0d;
|
||||
double previousOutputHeading = 0d;
|
||||
double previousOutputUnwrappedHeading = 0d;
|
||||
bool hasPreviousOutputHeading = false;
|
||||
double maximumAbsoluteVehicleCurvature = 0d;
|
||||
double maximumAbsoluteVehicleCurvatureDerivative = 0d;
|
||||
double curvatureSquareSum = 0d;
|
||||
int curvatureSampleCount = 0;
|
||||
double totalCurvatureVariation = 0d;
|
||||
double curvatureVariationEnergy = 0d;
|
||||
double minimumClearance = double.PositiveInfinity;
|
||||
|
||||
for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
|
||||
{
|
||||
PreparedDirectionSegment segment = candidateSegments[segmentIndex];
|
||||
if (!IsValidSegment(segment, segmentIndex, out reason)) return false;
|
||||
if (!TryValidateBoundary(candidateSegments, segmentIndex, out reason)) return false;
|
||||
|
||||
if (!TryResampleByGeometry(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> samples, out reason))
|
||||
return false;
|
||||
|
||||
if (!TryAnalyzeSegment(
|
||||
segment,
|
||||
samples,
|
||||
ref cumulativeArcLength,
|
||||
ref previousOutputHeading,
|
||||
ref previousOutputUnwrappedHeading,
|
||||
ref hasPreviousOutputHeading,
|
||||
outputPath,
|
||||
out double segmentMaximumCurvature,
|
||||
out double segmentMaximumCurvatureDerivative,
|
||||
out double segmentCurvatureSquareSum,
|
||||
out int segmentCurvatureSampleCount,
|
||||
out double segmentVariation,
|
||||
out double segmentVariationEnergy,
|
||||
out double segmentMinimumClearance,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
|
||||
maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
segmentMaximumCurvatureDerivative);
|
||||
curvatureSquareSum += segmentCurvatureSquareSum;
|
||||
curvatureSampleCount += segmentCurvatureSampleCount;
|
||||
totalCurvatureVariation += segmentVariation;
|
||||
curvatureVariationEnergy += segmentVariationEnergy;
|
||||
minimumClearance = Math.Min(minimumClearance, segmentMinimumClearance);
|
||||
int endIndex = outputPath.Count - 1;
|
||||
int startIndex = endIndex - samples.Count + 1;
|
||||
outputSegments.Add(new SmoothedPathSegment(
|
||||
segment.SegmentIndex,
|
||||
segment.Direction,
|
||||
startIndex,
|
||||
endIndex,
|
||||
segment.StartsAtGearSwitch,
|
||||
segment.EndsAtGearSwitch));
|
||||
}
|
||||
|
||||
double rmsCurvature = curvatureSampleCount == 0 ? 0d : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
|
||||
analysis = new PathGeometryAnalysis(
|
||||
outputPath,
|
||||
outputSegments,
|
||||
cumulativeArcLength,
|
||||
maximumAbsoluteVehicleCurvature,
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
rmsCurvature,
|
||||
totalCurvatureVariation,
|
||||
curvatureVariationEnergy,
|
||||
minimumClearance);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAnalyzeSegment(
|
||||
PreparedDirectionSegment segment,
|
||||
IReadOnlyList<SmoothingPoint2D> samples,
|
||||
ref double cumulativeArcLength,
|
||||
ref double previousOutputHeading,
|
||||
ref double previousOutputUnwrappedHeading,
|
||||
ref bool hasPreviousOutputHeading,
|
||||
List<SmoothedPathPoint> output,
|
||||
out double maximumAbsoluteVehicleCurvature,
|
||||
out double maximumAbsoluteVehicleCurvatureDerivative,
|
||||
out double curvatureSquareSum,
|
||||
out int curvatureSampleCount,
|
||||
out double totalCurvatureVariation,
|
||||
out double curvatureVariationEnergy,
|
||||
out double minimumClearance,
|
||||
out string reason)
|
||||
{
|
||||
maximumAbsoluteVehicleCurvature = 0d;
|
||||
maximumAbsoluteVehicleCurvatureDerivative = 0d;
|
||||
curvatureSquareSum = 0d;
|
||||
curvatureSampleCount = 0;
|
||||
totalCurvatureVariation = 0d;
|
||||
curvatureVariationEnergy = 0d;
|
||||
minimumClearance = double.PositiveInfinity;
|
||||
reason = string.Empty;
|
||||
int count = samples.Count;
|
||||
var localArcLengths = new double[count];
|
||||
var headings = new double[count];
|
||||
var unwrappedHeadings = new double[count];
|
||||
var geometricCurvatures = new double[count];
|
||||
var vehicleCurvatures = new double[count];
|
||||
var curvatureDerivatives = new double[count];
|
||||
|
||||
for (int index = 1; index < count; index++)
|
||||
{
|
||||
double distance = Distance(samples[index - 1], samples[index]);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
|
||||
{
|
||||
reason = "同一方向段中包含重复或退化的路径点。";
|
||||
return false;
|
||||
}
|
||||
localArcLengths[index] = localArcLengths[index - 1] + distance;
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
double travelHeading;
|
||||
if (index == 0 || index == count - 1)
|
||||
{
|
||||
// Coarse-path endpoints encode the vehicle pose at the exact integration anchor.
|
||||
// A chord across a finite integration step is not that pose's heading.
|
||||
travelHeading = segment.Direction == TravelDirection.Forward
|
||||
? samples[index].Heading
|
||||
: samples[index].Heading - Math.PI;
|
||||
}
|
||||
else
|
||||
{
|
||||
travelHeading = Math.Atan2(samples[index + 1].Y - samples[index - 1].Y,
|
||||
samples[index + 1].X - samples[index - 1].X);
|
||||
}
|
||||
|
||||
double heading = AngleMath.NormalizeRadians(
|
||||
segment.Direction == TravelDirection.Forward ? travelHeading : travelHeading + Math.PI);
|
||||
if (!NumericGuard.IsFinite(heading))
|
||||
{
|
||||
reason = "候选路径航向无法归一化。";
|
||||
return false;
|
||||
}
|
||||
|
||||
headings[index] = heading;
|
||||
if (index == 0 && !hasPreviousOutputHeading)
|
||||
{
|
||||
unwrappedHeadings[index] = heading;
|
||||
}
|
||||
else if (index == 0)
|
||||
{
|
||||
unwrappedHeadings[index] = previousOutputUnwrappedHeading +
|
||||
AngleMath.ShortestSignedDifference(previousOutputHeading, heading);
|
||||
}
|
||||
else
|
||||
{
|
||||
unwrappedHeadings[index] = unwrappedHeadings[index - 1] +
|
||||
AngleMath.ShortestSignedDifference(headings[index - 1], heading);
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
if (count == 1)
|
||||
{
|
||||
geometricCurvatures[index] = 0d;
|
||||
}
|
||||
else
|
||||
{
|
||||
int leftIndex = index == 0 ? 0 : index - 1;
|
||||
int rightIndex = index == count - 1 ? count - 1 : index + 1;
|
||||
if (!TryEstimateGeometricCurvature(
|
||||
samples,
|
||||
unwrappedHeadings,
|
||||
leftIndex,
|
||||
rightIndex,
|
||||
out geometricCurvatures[index],
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsFinite(geometricCurvatures[index]))
|
||||
{
|
||||
reason = "候选路径曲率计算产生了非法数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
vehicleCurvatures[index] = directionSign * geometricCurvatures[index];
|
||||
}
|
||||
|
||||
if (segment.StartVehicleCurvaturePerMeter.HasValue)
|
||||
{
|
||||
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
vehicleCurvatures[0] = segment.StartVehicleCurvaturePerMeter.Value;
|
||||
geometricCurvatures[0] = directionSign * vehicleCurvatures[0];
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
if (count == 1)
|
||||
{
|
||||
curvatureDerivatives[index] = 0d;
|
||||
}
|
||||
else if (index == 0)
|
||||
{
|
||||
curvatureDerivatives[index] =
|
||||
(vehicleCurvatures[1] - vehicleCurvatures[0]) /
|
||||
(localArcLengths[1] - localArcLengths[0]);
|
||||
}
|
||||
else if (index == count - 1)
|
||||
{
|
||||
curvatureDerivatives[index] =
|
||||
(vehicleCurvatures[index] - vehicleCurvatures[index - 1]) /
|
||||
(localArcLengths[index] - localArcLengths[index - 1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
curvatureDerivatives[index] =
|
||||
(vehicleCurvatures[index + 1] - vehicleCurvatures[index - 1]) /
|
||||
(localArcLengths[index + 1] - localArcLengths[index - 1]);
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsFinite(curvatureDerivatives[index]))
|
||||
{
|
||||
reason = "候选路径曲率导数计算产生了非法数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
SmoothingPoint2D sample = samples[index];
|
||||
double vehicleCurvature = vehicleCurvatures[index];
|
||||
double arcLength = cumulativeArcLength + localArcLengths[index];
|
||||
bool isGearSwitch = index == 0 && segment.StartsAtGearSwitch;
|
||||
SmoothedPathPointSource source = isGearSwitch ? SmoothedPathPointSource.GearSwitch : sample.Source;
|
||||
output.Add(new SmoothedPathPoint(
|
||||
sample.X,
|
||||
sample.Y,
|
||||
headings[index],
|
||||
unwrappedHeadings[index],
|
||||
arcLength,
|
||||
segment.Direction,
|
||||
geometricCurvatures[index],
|
||||
vehicleCurvature,
|
||||
curvatureDerivatives[index],
|
||||
sample.BodyClearance,
|
||||
isGearSwitch,
|
||||
source));
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(vehicleCurvature));
|
||||
maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
Math.Abs(curvatureDerivatives[index]));
|
||||
curvatureSquareSum += vehicleCurvature * vehicleCurvature;
|
||||
curvatureSampleCount++;
|
||||
minimumClearance = Math.Min(minimumClearance, sample.BodyClearance);
|
||||
if (index > 0)
|
||||
{
|
||||
double deltaCurvature = geometricCurvatures[index] - geometricCurvatures[index - 1];
|
||||
double deltaArc = localArcLengths[index] - localArcLengths[index - 1];
|
||||
totalCurvatureVariation += Math.Abs(deltaCurvature);
|
||||
curvatureVariationEnergy += (deltaCurvature / deltaArc) * (deltaCurvature / deltaArc) * deltaArc;
|
||||
}
|
||||
}
|
||||
|
||||
cumulativeArcLength += localArcLengths[count - 1];
|
||||
previousOutputHeading = headings[count - 1];
|
||||
previousOutputUnwrappedHeading = unwrappedHeadings[count - 1];
|
||||
hasPreviousOutputHeading = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryEstimateGeometricCurvature(
|
||||
IReadOnlyList<SmoothingPoint2D> samples,
|
||||
IReadOnlyList<double> unwrappedHeadings,
|
||||
int leftIndex,
|
||||
int rightIndex,
|
||||
out double curvature,
|
||||
out string reason)
|
||||
{
|
||||
curvature = 0d;
|
||||
reason = string.Empty;
|
||||
double chordLength = Distance(samples[leftIndex], samples[rightIndex]);
|
||||
if (!NumericGuard.IsFinite(chordLength) || chordLength <= 0d)
|
||||
{
|
||||
reason = "候选路径曲率估计弦长无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double deltaHeading = unwrappedHeadings[rightIndex] - unwrappedHeadings[leftIndex];
|
||||
if (!NumericGuard.IsFinite(deltaHeading))
|
||||
{
|
||||
reason = "候选路径曲率估计航向差无效。";
|
||||
return false;
|
||||
}
|
||||
if (Math.Abs(deltaHeading) >= Math.PI)
|
||||
{
|
||||
reason = "候选路径曲率估计航向差存在π歧义。";
|
||||
return false;
|
||||
}
|
||||
|
||||
curvature = 2d * Math.Sin(deltaHeading / 2d) / chordLength;
|
||||
if (!NumericGuard.IsFinite(curvature))
|
||||
{
|
||||
reason = "候选路径曲率计算产生了非法数值。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryResampleByGeometry(
|
||||
IReadOnlyList<SmoothingPoint2D> input,
|
||||
double spacingMeters,
|
||||
out IReadOnlyList<SmoothingPoint2D> samples,
|
||||
out string reason)
|
||||
{
|
||||
samples = null;
|
||||
reason = string.Empty;
|
||||
var normalized = new List<SmoothingPoint2D>(input.Count);
|
||||
double localArcLength = 0d;
|
||||
for (int index = 0; index < input.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = input[index];
|
||||
if (!IsValidPoint(point))
|
||||
{
|
||||
reason = "候选路径点包含非法数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (index > 0)
|
||||
{
|
||||
double distance = Distance(input[index - 1], point);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
|
||||
{
|
||||
reason = "同一方向段中包含重复或退化的路径点。";
|
||||
return false;
|
||||
}
|
||||
localArcLength += distance;
|
||||
}
|
||||
|
||||
normalized.Add(new SmoothingPoint2D(
|
||||
point.X, point.Y, localArcLength, point.Heading, point.UnwrappedHeading,
|
||||
point.BodyClearance, point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
|
||||
if (normalized.Count == 1)
|
||||
{
|
||||
samples = normalized;
|
||||
return true;
|
||||
}
|
||||
|
||||
var result = new List<SmoothingPoint2D> { normalized[0] };
|
||||
double finalArc = normalized[normalized.Count - 1].ArcLength;
|
||||
int rightIndex = 1;
|
||||
for (double targetArc = spacingMeters; targetArc < finalArc - MinimumDistanceMeters; targetArc += spacingMeters)
|
||||
{
|
||||
while (rightIndex < normalized.Count - 1 && normalized[rightIndex].ArcLength < targetArc)
|
||||
rightIndex++;
|
||||
SmoothingPoint2D left = normalized[rightIndex - 1];
|
||||
SmoothingPoint2D right = normalized[rightIndex];
|
||||
double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
|
||||
double unwrappedHeading = left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
|
||||
result.Add(new SmoothingPoint2D(
|
||||
left.X + ratio * (right.X - left.X),
|
||||
left.Y + ratio * (right.Y - left.Y),
|
||||
targetArc,
|
||||
AngleMath.NormalizeRadians(unwrappedHeading),
|
||||
unwrappedHeading,
|
||||
Math.Min(left.BodyClearance, right.BodyClearance),
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
|
||||
result.Add(normalized[normalized.Count - 1]);
|
||||
samples = result;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidSegment(PreparedDirectionSegment segment, int expectedIndex, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (segment == null || segment.SegmentIndex != expectedIndex ||
|
||||
(segment.Direction != TravelDirection.Forward && segment.Direction != TravelDirection.Reverse) ||
|
||||
segment.Points == null || segment.Points.Count == 0)
|
||||
{
|
||||
reason = "候选方向段索引、方向或点集无效。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryValidateBoundary(
|
||||
IReadOnlyList<PreparedDirectionSegment> segments,
|
||||
int segmentIndex,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
PreparedDirectionSegment current = segments[segmentIndex];
|
||||
if (segmentIndex == segments.Count - 1 && current.EndsAtGearSwitch)
|
||||
{
|
||||
reason = "末个方向段不得声明不存在的后续换向点。";
|
||||
return false;
|
||||
}
|
||||
if (segmentIndex == 0)
|
||||
{
|
||||
if (current.StartsAtGearSwitch)
|
||||
{
|
||||
reason = "首个方向段不得从换向点开始。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
PreparedDirectionSegment previous = segments[segmentIndex - 1];
|
||||
if (previous == null || !previous.EndsAtGearSwitch || !current.StartsAtGearSwitch ||
|
||||
previous.Direction == current.Direction)
|
||||
{
|
||||
reason = "方向段边界必须是前后成对且方向相反的换向点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
SmoothingPoint2D previousEnd = previous.Points[previous.Points.Count - 1];
|
||||
SmoothingPoint2D currentStart = current.Points[0];
|
||||
if (!currentStart.IsGearSwitchPoint ||
|
||||
Math.Abs(previousEnd.X - currentStart.X) > BoundaryToleranceMeters ||
|
||||
Math.Abs(previousEnd.Y - currentStart.Y) > BoundaryToleranceMeters ||
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(previousEnd.Heading, currentStart.Heading)) > BoundaryToleranceMeters)
|
||||
{
|
||||
reason = "换向点两侧必须保留同一位姿和航向。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(SmoothingPoint2D point)
|
||||
{
|
||||
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
||||
NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
||||
NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.BodyClearance) &&
|
||||
point.BodyClearance >= 0d;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
+102
@@ -0,0 +1,102 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
+201
@@ -0,0 +1,201 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>校验粗路径契约、保护换向拓扑并产生统一间距的平滑输入。</summary>
|
||||
public sealed class PathSmoothingPreprocessor
|
||||
{
|
||||
private const double Tolerance = 1e-8d;
|
||||
private readonly ArcLengthResampler _resampler;
|
||||
|
||||
/// <summary>创建使用默认确定性重采样器的预处理器。</summary>
|
||||
public PathSmoothingPreprocessor()
|
||||
: this(new ArcLengthResampler())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定重采样器的预处理器。</summary>
|
||||
public PathSmoothingPreprocessor(ArcLengthResampler resampler)
|
||||
{
|
||||
_resampler = resampler ?? throw new ArgumentNullException(nameof(resampler));
|
||||
}
|
||||
|
||||
/// <summary>将一条粗路径请求校验、按方向拆分并按配置间距重采样。</summary>
|
||||
public bool TryPrepare(PathSmoothingRequest request, out PreparedPath preparedPath, out string reason)
|
||||
{
|
||||
preparedPath = null;
|
||||
reason = string.Empty;
|
||||
if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
|
||||
request.CoarsePath == null || request.Segments == null || request.CoarsePath.Count == 0 || request.Segments.Count == 0)
|
||||
{
|
||||
reason = "平滑请求缺少粗路径、方向分段、地图、车辆或配置。";
|
||||
return false;
|
||||
}
|
||||
|
||||
PathSmoothingConfiguration configuration = request.Configuration;
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters))
|
||||
{
|
||||
reason = "平滑输出采样间距无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!ValidatePathPoints(request.CoarsePath, out reason) ||
|
||||
!ValidateSegments(request.CoarsePath, request.Segments, out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var preparedSegments = new List<PreparedDirectionSegment>(request.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < request.Segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment sourceSegment = request.Segments[segmentIndex];
|
||||
double segmentStartArcLength = request.CoarsePath[sourceSegment.StartIndex].ArcLength;
|
||||
var segmentPoints = new List<SmoothingPoint2D>(sourceSegment.EndIndex - sourceSegment.StartIndex + 1);
|
||||
for (int pointIndex = sourceSegment.StartIndex; pointIndex <= sourceSegment.EndIndex; pointIndex++)
|
||||
{
|
||||
CoarsePathPoint point = request.CoarsePath[pointIndex];
|
||||
bool isGearSwitch = point.IsGearSwitchPoint;
|
||||
segmentPoints.Add(new SmoothingPoint2D(
|
||||
point.X,
|
||||
point.Y,
|
||||
point.ArcLength - segmentStartArcLength,
|
||||
point.Heading,
|
||||
point.UnwrappedHeading,
|
||||
point.BodyClearance,
|
||||
isGearSwitch,
|
||||
isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
|
||||
}
|
||||
|
||||
var unresampled = new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
segmentPoints,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch,
|
||||
request.CoarsePath[sourceSegment.StartIndex].VehicleCurvature);
|
||||
if (!_resampler.TryResample(unresampled, configuration.OutputSpacingMeters, out PreparedDirectionSegment resampled, out reason))
|
||||
return false;
|
||||
preparedSegments.Add(new PreparedDirectionSegment(
|
||||
resampled.SegmentIndex,
|
||||
resampled.Direction,
|
||||
resampled.Points,
|
||||
resampled.StartsAtGearSwitch,
|
||||
resampled.EndsAtGearSwitch,
|
||||
unresampled.StartVehicleCurvaturePerMeter));
|
||||
}
|
||||
|
||||
preparedPath = new PreparedPath(preparedSegments);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidatePathPoints(IReadOnlyList<CoarsePathPoint> path, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
CoarsePathPoint first = path[0];
|
||||
if (!IsValidPoint(first) || first.IsGearSwitchPoint || Math.Abs(first.ArcLength) > Tolerance)
|
||||
{
|
||||
reason = "粗路径首点无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 1; index < path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint previous = path[index - 1];
|
||||
CoarsePathPoint current = path[index];
|
||||
if (!IsValidPoint(current) || current.ArcLength + Tolerance < previous.ArcLength)
|
||||
{
|
||||
reason = "粗路径包含非法数值或非递增弧长。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double expectedHeadingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
||||
if (!NumericGuard.IsFinite(expectedHeadingDelta) ||
|
||||
Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedHeadingDelta) > Tolerance)
|
||||
{
|
||||
reason = "粗路径展开航向不连续。";
|
||||
return false;
|
||||
}
|
||||
|
||||
bool duplicatePoseAndArc = Math.Abs(current.X - previous.X) <= Tolerance &&
|
||||
Math.Abs(current.Y - previous.Y) <= Tolerance &&
|
||||
Math.Abs(current.ArcLength - previous.ArcLength) <= Tolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance;
|
||||
if (duplicatePoseAndArc)
|
||||
{
|
||||
if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
|
||||
{
|
||||
reason = "粗路径包含非法的重复点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
|
||||
{
|
||||
reason = "粗路径普通点必须有正弧长增量且不得标记为换向点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateSegments(
|
||||
IReadOnlyList<CoarsePathPoint> path,
|
||||
IReadOnlyList<PathSegment> segments,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
int expectedStartIndex = 0;
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment segment = segments[segmentIndex];
|
||||
if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
|
||||
segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
|
||||
segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
|
||||
{
|
||||
reason = "粗路径方向分段索引或起始换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
|
||||
{
|
||||
if (path[pointIndex].Direction != segment.Direction)
|
||||
{
|
||||
reason = "粗路径方向分段包含不同方向的点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool hasNextSegment = segmentIndex + 1 < segments.Count;
|
||||
bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
|
||||
path[segment.EndIndex + 1].IsGearSwitchPoint;
|
||||
if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
|
||||
{
|
||||
reason = "粗路径方向分段末尾换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
expectedStartIndex = segment.EndIndex + 1;
|
||||
}
|
||||
|
||||
if (expectedStartIndex != path.Count)
|
||||
{
|
||||
reason = "粗路径方向分段未完整覆盖全部点。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(CoarsePathPoint point)
|
||||
{
|
||||
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
||||
NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
||||
NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
|
||||
NumericGuard.IsFinite(point.VehicleCurvature) && NumericGuard.IsFinite(point.BodyClearance) &&
|
||||
point.BodyClearance >= 0d &&
|
||||
(point.Direction == TravelDirection.Forward || point.Direction == TravelDirection.Reverse) &&
|
||||
Enum.IsDefined(typeof(CoarsePathPointSource), point.Source) &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>已校验、已按单一行驶方向分割并重采样的路径段。</summary>
|
||||
public sealed class PreparedDirectionSegment
|
||||
{
|
||||
/// <summary>创建不可变方向段。</summary>
|
||||
public PreparedDirectionSegment(
|
||||
int segmentIndex,
|
||||
TravelDirection direction,
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
bool startsAtGearSwitch,
|
||||
bool endsAtGearSwitch)
|
||||
: this(segmentIndex, direction, points, startsAtGearSwitch, endsAtGearSwitch, null)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建带有真实起始车辆曲率边界状态的不可变方向段。</summary>
|
||||
public PreparedDirectionSegment(
|
||||
int segmentIndex,
|
||||
TravelDirection direction,
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
bool startsAtGearSwitch,
|
||||
bool endsAtGearSwitch,
|
||||
double? startVehicleCurvaturePerMeter)
|
||||
{
|
||||
if (segmentIndex < 0) throw new ArgumentOutOfRangeException(nameof(segmentIndex));
|
||||
if (points == null || points.Count == 0) throw new ArgumentException("A prepared segment requires points.", nameof(points));
|
||||
if (startVehicleCurvaturePerMeter.HasValue && !IsFinite(startVehicleCurvaturePerMeter.Value))
|
||||
throw new ArgumentOutOfRangeException(nameof(startVehicleCurvaturePerMeter));
|
||||
|
||||
SegmentIndex = segmentIndex;
|
||||
Direction = direction;
|
||||
Points = CopyReadOnly(points);
|
||||
StartsAtGearSwitch = startsAtGearSwitch;
|
||||
EndsAtGearSwitch = endsAtGearSwitch;
|
||||
StartVehicleCurvaturePerMeter = startVehicleCurvaturePerMeter;
|
||||
}
|
||||
|
||||
/// <summary>从零开始的分段序号；在 <see cref="PreparedPath.Segments"/> 中必须与其位置一致。</summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>该段的唯一行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>不包含相邻段点的本段不可变采样点。</summary>
|
||||
public IReadOnlyList<SmoothingPoint2D> Points { get; }
|
||||
|
||||
/// <summary>本段首点是否为换向后保留的新方向点。</summary>
|
||||
public bool StartsAtGearSwitch { get; }
|
||||
|
||||
/// <summary>本段末点之后是否紧邻换向点。</summary>
|
||||
public bool EndsAtGearSwitch { get; }
|
||||
|
||||
/// <summary>原始车辆在本段物理起点的曲率边界状态，单位 1/m。</summary>
|
||||
public double? StartVehicleCurvaturePerMeter { get; }
|
||||
|
||||
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>已校验并按方向拆分的粗路径输入快照。</summary>
|
||||
public sealed class PreparedPath
|
||||
{
|
||||
/// <summary>创建不可变预处理路径。</summary>
|
||||
public PreparedPath(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("A prepared path requires direction segments.", nameof(segments));
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
if (segments[segmentIndex] == null)
|
||||
throw new ArgumentException("A prepared path cannot contain null direction segments.", nameof(segments));
|
||||
}
|
||||
|
||||
Segments = CopyReadOnly(segments);
|
||||
Points = Flatten(Segments);
|
||||
}
|
||||
|
||||
/// <summary>按原始前进/倒车拓扑排列的方向段。</summary>
|
||||
public IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
||||
|
||||
/// <summary>将所有方向段顺序拼接后的点；换向重复点保留两次。</summary>
|
||||
public IReadOnlyList<SmoothingPoint2D> Points { get; }
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingPoint2D> Flatten(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
var points = new List<SmoothingPoint2D>();
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
PreparedDirectionSegment segment = segments[segmentIndex];
|
||||
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
||||
points.Add(segment.Points[pointIndex]);
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingPoint2D>(points);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,264 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>通过候选路径同一几何分析器和验证器构建安全、可公平比较的原始基线。</summary>
|
||||
internal static class RawPathBaselineBuilder
|
||||
{
|
||||
internal static bool TryCreate(
|
||||
PathSmoothingRequest request,
|
||||
PreparedPath preparedPath,
|
||||
PathGeometryAnalyzer analyzer,
|
||||
double outputSpacingMeters,
|
||||
SmoothedPathValidator validator,
|
||||
double maximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline baseline,
|
||||
out string reason)
|
||||
{
|
||||
baseline = null;
|
||||
reason = string.Empty;
|
||||
if (request == null || preparedPath == null || analyzer == null || validator == null)
|
||||
{
|
||||
reason = "原始粗路径基线缺少请求、预处理路径、几何分析器或安全验证器。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!analyzer.TryAnalyze(preparedPath.Segments, outputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
|
||||
return false;
|
||||
|
||||
if (validator.TryValidate(
|
||||
analysis.Path,
|
||||
analysis.Segments,
|
||||
preparedPath,
|
||||
request.Map,
|
||||
request.Vehicle,
|
||||
maximumCollisionCheckStepMeters,
|
||||
out IReadOnlyList<SmoothedPathPoint> safePath,
|
||||
out double minimumClearanceMeters,
|
||||
out reason))
|
||||
{
|
||||
baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
|
||||
return true;
|
||||
}
|
||||
|
||||
if (reason != "平滑路径包含非法数值或超限车辆曲率。" ||
|
||||
!HasTrustedVehicleCurvaturesWithinLimit(request) ||
|
||||
!TryCreateTrustedRawAnalysis(request, out analysis, out reason) ||
|
||||
!validator.TryValidate(
|
||||
analysis.Path,
|
||||
analysis.Segments,
|
||||
preparedPath,
|
||||
request.Map,
|
||||
request.Vehicle,
|
||||
maximumCollisionCheckStepMeters,
|
||||
out safePath,
|
||||
out minimumClearanceMeters,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool HasTrustedVehicleCurvaturesWithinLimit(PathSmoothingRequest request)
|
||||
{
|
||||
if (request?.CoarsePath == null || request.CoarsePath.Count == 0 ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(
|
||||
request.Vehicle,
|
||||
out double maximumCurvaturePerMeter))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 0; index < request.CoarsePath.Count; index++)
|
||||
{
|
||||
CoarsePathPoint point = request.CoarsePath[index];
|
||||
if (point == null || !IsFinite(point.VehicleCurvature) ||
|
||||
Math.Abs(point.VehicleCurvature) > maximumCurvaturePerMeter)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateTrustedRawAnalysis(
|
||||
PathSmoothingRequest request,
|
||||
out PathGeometryAnalysis analysis,
|
||||
out string reason)
|
||||
{
|
||||
analysis = null;
|
||||
reason = string.Empty;
|
||||
if (request.CoarsePath == null || request.Segments == null ||
|
||||
request.CoarsePath.Count == 0 || request.Segments.Count == 0)
|
||||
{
|
||||
reason = "原始粗路径基线缺少可信路径或方向分段。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var path = new List<SmoothedPathPoint>(request.CoarsePath.Count);
|
||||
var segments = new List<SmoothedPathSegment>(request.Segments.Count);
|
||||
double maximumAbsoluteVehicleCurvature = 0d;
|
||||
double maximumAbsoluteVehicleCurvatureDerivative = 0d;
|
||||
double curvatureSquareSum = 0d;
|
||||
int curvatureSampleCount = 0;
|
||||
double totalCurvatureVariation = 0d;
|
||||
double curvatureVariationEnergy = 0d;
|
||||
double minimumClearance = double.PositiveInfinity;
|
||||
|
||||
for (int segmentIndex = 0; segmentIndex < request.Segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment segment = request.Segments[segmentIndex];
|
||||
if (segment == null || segment.SegmentIndex != segmentIndex ||
|
||||
segment.StartIndex != path.Count || segment.EndIndex < segment.StartIndex ||
|
||||
segment.EndIndex >= request.CoarsePath.Count)
|
||||
{
|
||||
reason = "原始粗路径基线方向分段无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
|
||||
{
|
||||
CoarsePathPoint point = request.CoarsePath[pointIndex];
|
||||
if (point == null || point.Direction != segment.Direction ||
|
||||
!IsFinite(point.X) || !IsFinite(point.Y) || !IsFinite(point.Heading) ||
|
||||
!IsFinite(point.UnwrappedHeading) || !IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!IsFinite(point.VehicleCurvature) || !IsFinite(point.BodyClearance) || point.BodyClearance < 0d)
|
||||
{
|
||||
reason = "原始粗路径基线包含非法可信点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double derivative = EstimateVehicleCurvatureDerivative(request.CoarsePath, segment, pointIndex, out bool validDerivative);
|
||||
if (!validDerivative)
|
||||
{
|
||||
reason = "原始粗路径基线曲率导数弧长无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
double geometricCurvature = directionSign * point.VehicleCurvature;
|
||||
SmoothedPathPointSource source = point.IsGearSwitchPoint
|
||||
? SmoothedPathPointSource.GearSwitch
|
||||
: SmoothedPathPointSource.CoarsePathFallback;
|
||||
path.Add(new SmoothedPathPoint(
|
||||
point.X,
|
||||
point.Y,
|
||||
point.Heading,
|
||||
point.UnwrappedHeading,
|
||||
point.ArcLength,
|
||||
point.Direction,
|
||||
geometricCurvature,
|
||||
point.VehicleCurvature,
|
||||
derivative,
|
||||
point.BodyClearance,
|
||||
point.IsGearSwitchPoint,
|
||||
source));
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(point.VehicleCurvature));
|
||||
maximumAbsoluteVehicleCurvatureDerivative = Math.Max(
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
Math.Abs(derivative));
|
||||
curvatureSquareSum += point.VehicleCurvature * point.VehicleCurvature;
|
||||
curvatureSampleCount++;
|
||||
minimumClearance = Math.Min(minimumClearance, point.BodyClearance);
|
||||
if (pointIndex > segment.StartIndex)
|
||||
{
|
||||
CoarsePathPoint previous = request.CoarsePath[pointIndex - 1];
|
||||
double deltaArc = point.ArcLength - previous.ArcLength;
|
||||
double deltaCurvature = geometricCurvature -
|
||||
(directionSign * previous.VehicleCurvature);
|
||||
totalCurvatureVariation += Math.Abs(deltaCurvature);
|
||||
curvatureVariationEnergy +=
|
||||
(deltaCurvature / deltaArc) * (deltaCurvature / deltaArc) * deltaArc;
|
||||
}
|
||||
}
|
||||
|
||||
segments.Add(new SmoothedPathSegment(
|
||||
segment.SegmentIndex,
|
||||
segment.Direction,
|
||||
segment.StartIndex,
|
||||
segment.EndIndex,
|
||||
segment.StartsAtGearSwitch,
|
||||
segment.EndsAtGearSwitch));
|
||||
}
|
||||
|
||||
double rmsCurvature = curvatureSampleCount == 0 ? 0d : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
|
||||
analysis = new PathGeometryAnalysis(
|
||||
path,
|
||||
segments,
|
||||
request.CoarsePath[request.CoarsePath.Count - 1].ArcLength,
|
||||
maximumAbsoluteVehicleCurvature,
|
||||
maximumAbsoluteVehicleCurvatureDerivative,
|
||||
rmsCurvature,
|
||||
totalCurvatureVariation,
|
||||
curvatureVariationEnergy,
|
||||
minimumClearance);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double EstimateVehicleCurvatureDerivative(
|
||||
IReadOnlyList<CoarsePathPoint> path,
|
||||
PathSegment segment,
|
||||
int pointIndex,
|
||||
out bool valid)
|
||||
{
|
||||
valid = true;
|
||||
if (segment.StartIndex == segment.EndIndex) return 0d;
|
||||
|
||||
int leftIndex = pointIndex == segment.StartIndex ? pointIndex : pointIndex - 1;
|
||||
int rightIndex = pointIndex == segment.EndIndex ? pointIndex : pointIndex + 1;
|
||||
double deltaArc = path[rightIndex].ArcLength - path[leftIndex].ArcLength;
|
||||
if (!IsFinite(deltaArc) || deltaArc <= 0d)
|
||||
{
|
||||
valid = false;
|
||||
return 0d;
|
||||
}
|
||||
return (path[rightIndex].VehicleCurvature - path[leftIndex].VehicleCurvature) / deltaArc;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value) => !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
|
||||
private static PathQualityMetrics CreateMetrics(
|
||||
PathGeometryAnalysis analysis,
|
||||
double minimumClearanceMeters)
|
||||
{
|
||||
return new PathQualityMetrics(
|
||||
true,
|
||||
analysis.PathLengthMeters,
|
||||
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
analysis.RootMeanSquareVehicleCurvaturePerMeter,
|
||||
analysis.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
analysis.CurvatureVariationEnergy,
|
||||
minimumClearanceMeters,
|
||||
0d,
|
||||
0d,
|
||||
0d,
|
||||
0d);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>已通过完整车体复核的原始粗路径及其比较指标。</summary>
|
||||
internal sealed class RawPathBaseline
|
||||
{
|
||||
internal RawPathBaseline(
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
PathQualityMetrics metrics)
|
||||
{
|
||||
Path = path;
|
||||
Segments = segments;
|
||||
Metrics = metrics;
|
||||
}
|
||||
|
||||
internal IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
internal PathQualityMetrics Metrics { get; }
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>供平滑算法处理的二维路径采样点；所有长度单位均为 m，航向单位为 rad。</summary>
|
||||
public sealed class SmoothingPoint2D
|
||||
{
|
||||
/// <summary>创建不可变二维路径点。</summary>
|
||||
public SmoothingPoint2D(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double arcLengthMeters,
|
||||
double headingRadians,
|
||||
double unwrappedHeadingRadians,
|
||||
double bodyClearanceMeters,
|
||||
bool isGearSwitchPoint,
|
||||
SmoothedPathPointSource source)
|
||||
{
|
||||
X = xMeters;
|
||||
Y = yMeters;
|
||||
ArcLength = arcLengthMeters;
|
||||
Heading = headingRadians;
|
||||
UnwrappedHeading = unwrappedHeadingRadians;
|
||||
BodyClearance = bodyClearanceMeters;
|
||||
IsGearSwitchPoint = isGearSwitchPoint;
|
||||
Source = source;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 坐标，单位 m。</summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>世界 Y 坐标，单位 m。</summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>本方向段中的累计弧长，单位 m。</summary>
|
||||
public double ArcLength { get; }
|
||||
|
||||
/// <summary>归一化的车辆航向，单位 rad。</summary>
|
||||
public double Heading { get; }
|
||||
|
||||
/// <summary>连续展开的车辆航向，单位 rad。</summary>
|
||||
public double UnwrappedHeading { get; }
|
||||
|
||||
/// <summary>输入路径携带的保守净空，单位 m。</summary>
|
||||
public double BodyClearance { get; }
|
||||
|
||||
/// <summary>该点是否为新方向段开始处的换向点。</summary>
|
||||
public bool IsGearSwitchPoint { get; }
|
||||
|
||||
/// <summary>该点在平滑流程中的来源。</summary>
|
||||
public SmoothedPathPointSource Source { get; }
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
# Path smoothing comparison
|
||||
|
||||
This module compares the clamped cubic B-spline, local cubic Bézier, and piecewise-quintic smoothers against the raw Hybrid A* coarse path. It is an offline developer analysis tool; it does not alter coarse-path search acceptance.
|
||||
|
||||
## Units and coordinates
|
||||
|
||||
All path positions, lengths, clearances, and collision-check spacing use **meters**. Heading uses **radians** and vehicle curvature uses `1/m`. Map construction retains its existing millimeter input contract; `PlanningGridMap` provides the meter-coordinate world queries used by smoothing. A smoothing result preserves the coarse path's first and last pose, rather than the original requested goal pose, so its endpoint error is inherited from the coarse planner's accepted goal tolerance.
|
||||
|
||||
## Facade usage
|
||||
|
||||
Only smooth a successful coarse result. A minimal formal call flow is:
|
||||
|
||||
```csharp
|
||||
if (coarseResult.PlanningResult.Status != PlanningStatus.Success)
|
||||
return;
|
||||
var smoothing = new PathSmoothingService().Smooth(
|
||||
new PathSmoothingRequest(
|
||||
coarseResult.PlanningResult.Path,
|
||||
coarseResult.PlanningResult.Segments,
|
||||
coarseResult.MapResult.Map,
|
||||
job.Vehicle,
|
||||
smoothingConfiguration),
|
||||
cancellationToken);
|
||||
if (smoothing.Status == PathSmoothingStatus.Success ||
|
||||
smoothing.Status == PathSmoothingStatus.FallbackToCoarsePath)
|
||||
ConsumeSpatialReference(smoothing.Path, smoothing.Segments);
|
||||
```
|
||||
|
||||
## Status handling and fallback
|
||||
|
||||
`Success` supplies a validated smoothed path. `FallbackToCoarsePath` is an explicit, safe degraded result and may be consumed by the same downstream spatial-reference interface. `Infeasible`, `InvalidInput`, `Failed`, and `Cancelled` must not be treated as a path. Comparison reports keep an infeasible candidate's markers for diagnosis but never select it as a recommendation.
|
||||
|
||||
## Fixture freshness
|
||||
|
||||
The eight fast fixtures are snapshots of successful coarse paths. Their configuration fingerprint is checked before use. After deliberately changing a scenario or planning configuration, regenerate them with `generate_path_smoothing_fixtures.ps1 -Overwrite`, then run `verify_path_smoothing_fixtures.ps1`. Fixture-only comparison never runs Hybrid A*.
|
||||
|
||||
## IEEE colors, fonts, and Windows PNG
|
||||
|
||||
SVG and PNG use the shared IEEE-style colors, status-bearing legends, coordinate ticks, and units. Every raw or smoothed trajectory is rendered as its complete set of discrete samples: reports intentionally draw no line segment between adjacent samples. The Windows PNG renderer requires the exact `SimSun` and `Times New Roman` font families and writes a 600 dpi raster image. If either font is unavailable, export returns `FontUnavailable`; it does not substitute a different font. PNG rendering relies on Windows GDI+, while SVG and CSV remain available without it. SVG uses text-family references, so portable publication requires checking the target font installation or converting text to paths in an external publishing tool.
|
||||
|
||||
## SQP boundary
|
||||
|
||||
This module is geometric smoothing and full-body validation, not sequential quadratic programming (SQP) trajectory optimization. It has no time parameterization, velocity, acceleration, steering-rate, or dynamic-obstacle constraints. Feed only its validated spatial reference into any later SQP or time-parameterization stage.
|
||||
|
||||
## Output files
|
||||
|
||||
`run_path_smoothing_comparison.ps1` writes developer reports only below `ClumsyPilot/obj/path_smoothing_reports`. Each scenario directory contains one `comparison.csv` and both SVG and 600 dpi PNG versions of these six focused figures:
|
||||
|
||||
1. `01-coarse-path-overview` — the Hybrid A* coarse-path planning view with map, start, and goal.
|
||||
2. `02-all-paths-comparison` — raw and all three smoother point clouds only, without map or endpoint decorations.
|
||||
3. `03-cubic-bspline-overview` — faded coarse reference and cubic B-spline result with map context.
|
||||
4. `04-local-cubic-bezier-overview` — faded coarse reference and local cubic Bézier result with map context.
|
||||
5. `05-piecewise-quintic-overview` — faded coarse reference and piecewise-quintic result with map context.
|
||||
6. `06-curvature-comparison` — all available curvature samples with `s (m)` and `κ (m⁻¹)` axes.
|
||||
|
||||
Overhead figures preserve equal X/Y scale and use trajectory-focused bounds; their coordinate ticks are in metres. Inspect the SVG/PNG visually, retain CSV for numerical review, and use an external PDF/EPS publishing step if the final venue requires those formats.
|
||||
+19451
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,161 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>开发人员离线导出八个快速夹具和四个 Hybrid A* 端到端场景的比较报告。</summary>
|
||||
public sealed class PathSmoothingComparisonDemo
|
||||
{
|
||||
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
|
||||
private readonly PathSmoothingComparisonService _comparisonService = new PathSmoothingComparisonService();
|
||||
private readonly SmoothingFigureModelBuilder _figureBuilder = new SmoothingFigureModelBuilder();
|
||||
private readonly SmoothingReportExporter _reportExporter = new SmoothingReportExporter();
|
||||
|
||||
/// <summary>导出八个已验证夹具；不运行 Hybrid A*。</summary>
|
||||
public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportFixtureReports(
|
||||
string fixturePath,
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
|
||||
IReadOnlyList<PathSmoothingComparisonRequest> requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath);
|
||||
var results = new List<PathSmoothingComparisonScenarioResult>(requests.Count);
|
||||
for (int index = 0; index < requests.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
results.Add(Export(fixtures[index].Id, requests[index], outputDirectory, cancellationToken));
|
||||
}
|
||||
return results.AsReadOnly();
|
||||
}
|
||||
|
||||
/// <summary>运行四个固定 Hybrid A* 场景，并仅为成功粗路径导出平滑比较报告。</summary>
|
||||
public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportEndToEndReports(
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
CoarsePathTestScenario[] scenarios =
|
||||
{
|
||||
CoarsePathTestScenario.ExplicitEmpty,
|
||||
CoarsePathTestScenario.RectangleDetour,
|
||||
CoarsePathTestScenario.ManualAndTwoLeg,
|
||||
CoarsePathTestScenario.ReverseGearSwitch,
|
||||
};
|
||||
var planner = new CoarsePathPlanningService();
|
||||
var results = new List<PathSmoothingComparisonScenarioResult>(scenarios.Length);
|
||||
for (int index = 0; index < scenarios.Length; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenarios[index]);
|
||||
CoarsePathPlanningJobResult coarse = planner.Plan(job, cancellationToken);
|
||||
if (coarse.PlanningResult.Status != PlanningStatus.Success)
|
||||
{
|
||||
results.Add(PathSmoothingComparisonScenarioResult.CoarsePathFailure(
|
||||
scenarios[index].ToString(), coarse.PlanningResult.Status, coarse.PlanningResult.Diagnostics.TerminationReason));
|
||||
continue;
|
||||
}
|
||||
results.Add(Export(
|
||||
scenarios[index].ToString(),
|
||||
SmoothingScenarioFactory.CreateEndToEndRequest(job, coarse),
|
||||
outputDirectory,
|
||||
cancellationToken));
|
||||
}
|
||||
return results.AsReadOnly();
|
||||
}
|
||||
|
||||
/// <summary>供业务调用示例使用：仅在已成功的粗路径上调用平滑服务。</summary>
|
||||
public PathSmoothingResult SmoothSuccessfulCoarsePath(
|
||||
CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult coarseResult,
|
||||
PathSmoothingConfiguration configuration,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (job == null || coarseResult == null || coarseResult.PlanningResult.Status != PlanningStatus.Success)
|
||||
throw new ArgumentException("只有成功的粗路径能够进入平滑流程。", nameof(coarseResult));
|
||||
return _smoothingService.Smooth(new PathSmoothingRequest(
|
||||
coarseResult.PlanningResult.Path,
|
||||
coarseResult.PlanningResult.Segments,
|
||||
coarseResult.MapResult.Map,
|
||||
job.Vehicle,
|
||||
configuration), cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>业务调用方只可消费成功平滑或显式回退的路径。</summary>
|
||||
public static bool IsConsumable(PathSmoothingStatus status)
|
||||
{
|
||||
return status == PathSmoothingStatus.Success || status == PathSmoothingStatus.FallbackToCoarsePath;
|
||||
}
|
||||
|
||||
private PathSmoothingComparisonScenarioResult Export(
|
||||
string scenarioId,
|
||||
PathSmoothingComparisonRequest request,
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (request == null || request.SmoothingRequest == null || request.SmoothingRequest.CoarsePath.Count == 0)
|
||||
throw new ArgumentException("比较请求必须包含粗路径。", nameof(request));
|
||||
PathSmoothingComparisonResult comparison = _comparisonService.Compare(request, cancellationToken);
|
||||
CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0];
|
||||
CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1];
|
||||
SmoothingFigureModel model = _figureBuilder.Build(
|
||||
comparison,
|
||||
request.SmoothingRequest.Map,
|
||||
new Pose2D(first.X, first.Y, first.Heading),
|
||||
new Pose2D(last.X, last.Y, last.Heading),
|
||||
scenarioId,
|
||||
scenarioId);
|
||||
SmoothingReportExportResult report = _reportExporter.Export(new SmoothingReportExportRequest
|
||||
{
|
||||
Model = model,
|
||||
OutputDirectory = System.IO.Path.Combine(outputDirectory, scenarioId),
|
||||
FileStem = "comparison",
|
||||
});
|
||||
return PathSmoothingComparisonScenarioResult.ComparisonComplete(scenarioId, comparison, report);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>批处理为一个场景发布的粗路径状态、平滑比较和报告结果。</summary>
|
||||
public sealed class PathSmoothingComparisonScenarioResult
|
||||
{
|
||||
private PathSmoothingComparisonScenarioResult(
|
||||
string scenarioId,
|
||||
PlanningStatus coarsePathStatus,
|
||||
string diagnostic,
|
||||
PathSmoothingComparisonResult comparison,
|
||||
SmoothingReportExportResult report)
|
||||
{
|
||||
ScenarioId = scenarioId ?? string.Empty;
|
||||
CoarsePathStatus = coarsePathStatus;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
Comparison = comparison;
|
||||
Report = report;
|
||||
}
|
||||
|
||||
public string ScenarioId { get; }
|
||||
public PlanningStatus CoarsePathStatus { get; }
|
||||
public string Diagnostic { get; }
|
||||
public PathSmoothingComparisonResult Comparison { get; }
|
||||
public SmoothingReportExportResult Report { get; }
|
||||
|
||||
internal static PathSmoothingComparisonScenarioResult CoarsePathFailure(
|
||||
string scenarioId,
|
||||
PlanningStatus status,
|
||||
string diagnostic)
|
||||
{
|
||||
return new PathSmoothingComparisonScenarioResult(scenarioId, status, diagnostic, null, null);
|
||||
}
|
||||
|
||||
internal static PathSmoothingComparisonScenarioResult ComparisonComplete(
|
||||
string scenarioId,
|
||||
PathSmoothingComparisonResult comparison,
|
||||
SmoothingReportExportResult report)
|
||||
{
|
||||
return new PathSmoothingComparisonScenarioResult(scenarioId, PlanningStatus.Success, string.Empty, comparison, report);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,260 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>显式开发入口：从已成功的 Hybrid A* 粗路径结果生成八个稳定的快速比较夹具。</summary>
|
||||
public static class SmoothingFixtureGenerator
|
||||
{
|
||||
// 仅用于开发时固化真实规划输出；不会改变业务场景或运行时服务的预算。
|
||||
private static readonly TimeSpan FixtureGenerationSearchTimeout = TimeSpan.FromSeconds(120d);
|
||||
|
||||
/// <summary>生成夹具 JSON；目标已存在且未明确允许覆盖时拒绝写入。</summary>
|
||||
public static void Generate(string outputPath, bool overwrite)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(outputPath)) throw new ArgumentException("输出路径不能为空。", nameof(outputPath));
|
||||
if (File.Exists(outputPath) && !overwrite)
|
||||
throw new IOException("夹具目标已存在；必须显式允许覆盖。");
|
||||
|
||||
SmoothingFixtureDocument document = CreateDocument();
|
||||
Directory.CreateDirectory(Path.GetDirectoryName(Path.GetFullPath(outputPath)));
|
||||
var settings = new JsonSerializerSettings { Culture = CultureInfo.InvariantCulture, Formatting = Formatting.Indented };
|
||||
File.WriteAllText(outputPath, JsonConvert.SerializeObject(document, settings), new System.Text.UTF8Encoding(false));
|
||||
}
|
||||
|
||||
private static SmoothingFixtureDocument CreateDocument()
|
||||
{
|
||||
var document = new SmoothingFixtureDocument { SchemaVersion = 1 };
|
||||
var planner = new CoarsePathPlanningService();
|
||||
IReadOnlyList<FixturePlan> plans = CreatePlans();
|
||||
for (int index = 0; index < plans.Count; index++)
|
||||
{
|
||||
FixturePlan plan = plans[index];
|
||||
CoarsePathPlanningJob job = plan.CreateJob();
|
||||
job.Configuration.SearchTimeout = FixtureGenerationSearchTimeout;
|
||||
CoarsePathPlanningJobResult result = planner.Plan(job, CancellationToken.None);
|
||||
document.Scenarios.Add(CreateRecord(plan.Id, job, result));
|
||||
}
|
||||
|
||||
for (int index = 0; index < document.Scenarios.Count; index++)
|
||||
document.Scenarios[index].ConfigurationFingerprint = SmoothingScenarioFixtureLoader.ComputeFingerprint(document.Scenarios[index]);
|
||||
return document;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<FixturePlan> CreatePlans()
|
||||
{
|
||||
return new List<FixturePlan>
|
||||
{
|
||||
new FixturePlan("straight", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ExplicitEmpty)),
|
||||
new FixturePlan("single-turn", () => CoarsePathScenarioFactory.CreateManualGoalDemo(1000d, 1000d, 0d, 3000d, 2000d, 45d)),
|
||||
new FixturePlan("s-bend", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ManualAndTwoLeg, 1000d, 1000d, 45d)),
|
||||
new FixturePlan("large-heading-change", () => CoarsePathScenarioFactory.CreateManualGoalDemo(1000d, 1000d, 0d, 3000d, 3000d, 90d)),
|
||||
new FixturePlan("rectangle-detour", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.RectangleDetour)),
|
||||
new FixturePlan("multi-obstacle-detour", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ManualAndTwoLeg)),
|
||||
new FixturePlan("narrow-corridor", CreateNarrowCorridor),
|
||||
new FixturePlan("forward-reverse-switch", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ReverseGearSwitch)),
|
||||
};
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateNarrowCorridor()
|
||||
{
|
||||
return CreateFixedObstacleJob("narrow-corridor", new IMapObstacle[]
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(1500f, 4500f, 500f, 1200f),
|
||||
new AxisAlignedRectangleObstacle(1500f, 4500f, 2800f, 3500f),
|
||||
});
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateFixedObstacleJob(string id, IReadOnlyList<IMapObstacle> obstacles)
|
||||
{
|
||||
var job = new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
AllowExplicitEmptyMap = false,
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("fixture-" + id, 1L, true, obstacles),
|
||||
},
|
||||
},
|
||||
Start = new Pose2D(1d, 2d, 0d),
|
||||
Goal = new Pose2D(5d, 2d, 0d),
|
||||
Vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.80d,
|
||||
WidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
},
|
||||
Configuration = new HybridAStarConfiguration(),
|
||||
GoalDirection = GoalDirectionConstraint.Forward,
|
||||
};
|
||||
job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
|
||||
return job;
|
||||
}
|
||||
|
||||
private static SmoothingFixtureRecord CreateRecord(string id, CoarsePathPlanningJob job, CoarsePathPlanningJobResult result)
|
||||
{
|
||||
if (job == null || result == null || result.MapResult == null || !result.MapResult.Succeeded ||
|
||||
result.MapResult.Map == null || result.PlanningResult == null || result.PlanningResult.Status != PlanningStatus.Success)
|
||||
{
|
||||
string status = result == null || result.PlanningResult == null ? "no-result" : result.PlanningResult.Status.ToString();
|
||||
string reason = result == null || result.PlanningResult == null || result.PlanningResult.Diagnostics == null
|
||||
? string.Empty
|
||||
: result.PlanningResult.Diagnostics.TerminationReason;
|
||||
throw new InvalidOperationException("夹具场景未产生成功的粗路径：" + id + "；状态=" + status + "；原因=" + reason);
|
||||
}
|
||||
|
||||
PlanningGridMap map = result.MapResult.Map;
|
||||
IReadOnlyList<CoarsePathPoint> smoothingPath = SmoothingScenarioFactory.CopyWithFiniteClearance(result.PlanningResult.Path, map);
|
||||
var record = new SmoothingFixtureRecord
|
||||
{
|
||||
Id = id,
|
||||
FixtureVersion = 1,
|
||||
Map = CreateMapRecord(map, result.MapResult.SourceResults),
|
||||
Vehicle = CreateVehicleRecord(job.Vehicle),
|
||||
PlanningConfiguration = CreatePlanningConfigurationRecord(job.Configuration),
|
||||
};
|
||||
|
||||
for (int index = 0; index < smoothingPath.Count; index++)
|
||||
record.Path.Add(CreatePathPointRecord(smoothingPath[index]));
|
||||
for (int index = 0; index < result.PlanningResult.Segments.Count; index++)
|
||||
record.Segments.Add(CreateSegmentRecord(result.PlanningResult.Segments[index]));
|
||||
return record;
|
||||
}
|
||||
|
||||
private static SmoothingFixtureMap CreateMapRecord(PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
var record = new SmoothingFixtureMap
|
||||
{
|
||||
XMinMm = map.Bounds.XMin,
|
||||
XMaxMm = map.Bounds.XMax,
|
||||
YMinMm = map.Bounds.YMin,
|
||||
YMaxMm = map.Bounds.YMax,
|
||||
ResolutionMm = map.ResolutionMm,
|
||||
};
|
||||
for (int sourceIndex = 0; sourceIndex < sourceResults.Count; sourceIndex++)
|
||||
{
|
||||
IReadOnlyList<IMapObstacle> obstacles = sourceResults[sourceIndex].Obstacles;
|
||||
for (int obstacleIndex = 0; obstacleIndex < obstacles.Count; obstacleIndex++)
|
||||
record.Obstacles.Add(CreateObstacleRecord(obstacles[obstacleIndex]));
|
||||
}
|
||||
return record;
|
||||
}
|
||||
|
||||
private static SmoothingFixtureObstacle CreateObstacleRecord(IMapObstacle obstacle)
|
||||
{
|
||||
if (obstacle is CircleObstacle circle)
|
||||
{
|
||||
return new SmoothingFixtureObstacle
|
||||
{
|
||||
Kind = "circle",
|
||||
CenterXMm = circle.CenterX,
|
||||
CenterYMm = circle.CenterY,
|
||||
RadiusMm = circle.RadiusMm,
|
||||
};
|
||||
}
|
||||
if (obstacle is AxisAlignedRectangleObstacle rectangle)
|
||||
{
|
||||
return new SmoothingFixtureObstacle
|
||||
{
|
||||
Kind = "axis-aligned-rectangle",
|
||||
XMinMm = rectangle.XMin,
|
||||
XMaxMm = rectangle.XMax,
|
||||
YMinMm = rectangle.YMin,
|
||||
YMaxMm = rectangle.YMax,
|
||||
};
|
||||
}
|
||||
throw new InvalidOperationException("夹具仅支持圆形和轴对齐矩形障碍物。" + obstacle?.GetType().FullName);
|
||||
}
|
||||
|
||||
private static SmoothingFixtureVehicle CreateVehicleRecord(VehicleParameters vehicle)
|
||||
{
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
throw new InvalidOperationException("夹具车辆缺少有效最大曲率限制。");
|
||||
return new SmoothingFixtureVehicle
|
||||
{
|
||||
LengthMeters = vehicle.LengthMeters,
|
||||
WidthMeters = vehicle.WidthMeters,
|
||||
SafetyMarginMeters = vehicle.SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = maximumCurvaturePerMeter,
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothingFixturePlanningConfiguration CreatePlanningConfigurationRecord(HybridAStarConfiguration configuration)
|
||||
{
|
||||
return new SmoothingFixturePlanningConfiguration
|
||||
{
|
||||
PrimitiveLengthMeters = configuration.PrimitiveLengthMeters,
|
||||
IntegrationStepMeters = configuration.IntegrationStepMeters,
|
||||
MaximumCollisionCheckStepMeters = configuration.MaximumCollisionCheckStepMeters,
|
||||
HeadingResolutionRadians = configuration.HeadingResolutionRadians,
|
||||
CurvatureLevelCount = configuration.CurvatureLevelCount,
|
||||
GoalPositionToleranceMeters = configuration.GoalPositionToleranceMeters,
|
||||
GoalHeadingToleranceRadians = configuration.GoalHeadingToleranceRadians,
|
||||
MaximumExpandedNodes = configuration.MaximumExpandedNodes,
|
||||
SearchTimeoutSeconds = configuration.SearchTimeout.TotalSeconds,
|
||||
HeuristicWeight = configuration.HeuristicWeight,
|
||||
ReverseCostMultiplier = configuration.ReverseCostMultiplier,
|
||||
GearSwitchPenaltyMeters = configuration.GearSwitchPenaltyMeters,
|
||||
CurvatureMagnitudeWeight = configuration.CurvatureMagnitudeWeight,
|
||||
CurvatureChangePenaltyMetersPerLevel = configuration.CurvatureChangePenaltyMetersPerLevel,
|
||||
ClearanceCostWeight = configuration.ClearanceCostWeight,
|
||||
ClearanceCostDistanceMeters = configuration.ClearanceCostDistanceMeters,
|
||||
AllowReverse = configuration.AllowReverse,
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothingFixturePathPoint CreatePathPointRecord(CoarsePathPoint point)
|
||||
{
|
||||
return new SmoothingFixturePathPoint
|
||||
{
|
||||
XMeters = point.X,
|
||||
YMeters = point.Y,
|
||||
HeadingRadians = point.Heading,
|
||||
UnwrappedHeadingRadians = point.UnwrappedHeading,
|
||||
ArcLengthMeters = point.ArcLength,
|
||||
Direction = point.Direction,
|
||||
VehicleCurvaturePerMeter = point.VehicleCurvature,
|
||||
BodyClearanceMeters = point.BodyClearance,
|
||||
IsGearSwitchPoint = point.IsGearSwitchPoint,
|
||||
Source = point.Source,
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothingFixtureSegment CreateSegmentRecord(PathSegment segment)
|
||||
{
|
||||
return new SmoothingFixtureSegment
|
||||
{
|
||||
SegmentIndex = segment.SegmentIndex,
|
||||
Direction = segment.Direction,
|
||||
StartIndex = segment.StartIndex,
|
||||
EndIndex = segment.EndIndex,
|
||||
StartsAtGearSwitch = segment.StartsAtGearSwitch,
|
||||
EndsAtGearSwitch = segment.EndsAtGearSwitch,
|
||||
};
|
||||
}
|
||||
|
||||
private sealed class FixturePlan
|
||||
{
|
||||
public FixturePlan(string id, Func<CoarsePathPlanningJob> createJob)
|
||||
{
|
||||
Id = id;
|
||||
CreateJob = createJob;
|
||||
}
|
||||
|
||||
public string Id { get; }
|
||||
public Func<CoarsePathPlanningJob> CreateJob { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>将快速夹具或现有粗路径业务结果转换为独立平滑比较请求。</summary>
|
||||
public static class SmoothingScenarioFactory
|
||||
{
|
||||
/// <summary>不运行 Hybrid A*，从已验证的 JSON 夹具创建八个快速比较请求。</summary>
|
||||
public static IReadOnlyList<PathSmoothingComparisonRequest> CreateFixtureRequests(string fixturePath)
|
||||
{
|
||||
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
|
||||
var requests = new List<PathSmoothingComparisonRequest>(fixtures.Count);
|
||||
for (int index = 0; index < fixtures.Count; index++)
|
||||
{
|
||||
SmoothingScenarioFixture fixture = fixtures[index];
|
||||
requests.Add(new PathSmoothingComparisonRequest(
|
||||
new PathSmoothingRequest(
|
||||
fixture.Path,
|
||||
fixture.Segments,
|
||||
SmoothingScenarioFixtureLoader.BuildMap(fixture),
|
||||
SmoothingScenarioFixtureLoader.BuildVehicle(fixture),
|
||||
new PathSmoothingConfiguration())));
|
||||
}
|
||||
return requests.AsReadOnly();
|
||||
}
|
||||
|
||||
/// <summary>把现有 <see cref="CoarsePathScenarioFactory"/> 已规划成功的结果转换为比较请求。</summary>
|
||||
public static PathSmoothingComparisonRequest CreateEndToEndRequest(
|
||||
CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult result)
|
||||
{
|
||||
if (job == null || result == null || result.MapResult == null || result.MapResult.Map == null ||
|
||||
result.PlanningResult == null || result.PlanningResult.Status != PlanningStatus.Success)
|
||||
throw new ArgumentException("只能从包含成功地图和粗路径的业务结果创建平滑比较请求。", nameof(result));
|
||||
|
||||
return new PathSmoothingComparisonRequest(new PathSmoothingRequest(
|
||||
CopyWithFiniteClearance(result.PlanningResult.Path, result.MapResult.Map),
|
||||
result.PlanningResult.Segments,
|
||||
result.MapResult.Map,
|
||||
job.Vehicle,
|
||||
new PathSmoothingConfiguration()));
|
||||
}
|
||||
|
||||
internal static IReadOnlyList<CoarsePathPoint> CopyWithFiniteClearance(
|
||||
IReadOnlyList<CoarsePathPoint> source,
|
||||
MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap map)
|
||||
{
|
||||
double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
|
||||
double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
|
||||
double finiteEmptyMapClearance = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
|
||||
var copy = new List<CoarsePathPoint>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
CoarsePathPoint point = source[index];
|
||||
double clearance = double.IsPositiveInfinity(point.BodyClearance)
|
||||
? finiteEmptyMapClearance
|
||||
: point.BodyClearance;
|
||||
copy.Add(new CoarsePathPoint(
|
||||
point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
|
||||
point.VehicleCurvature, clearance, point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
return copy.AsReadOnly();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,156 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>一个可复现的快速粗路径夹具及其地图、车辆和方向段快照。</summary>
|
||||
public sealed class SmoothingScenarioFixture
|
||||
{
|
||||
internal SmoothingScenarioFixture(SmoothingFixtureRecord record, bool fingerprintCurrent)
|
||||
{
|
||||
Record = record;
|
||||
Id = record.Id;
|
||||
FixtureVersion = record.FixtureVersion;
|
||||
ConfigurationFingerprint = record.ConfigurationFingerprint;
|
||||
IsConfigurationFingerprintCurrent = fingerprintCurrent;
|
||||
Path = ToPath(record.Path);
|
||||
Segments = ToSegments(record.Segments);
|
||||
}
|
||||
|
||||
/// <summary>稳定英文场景标识。</summary>
|
||||
public string Id { get; }
|
||||
/// <summary>夹具自身的正版本号。</summary>
|
||||
public int FixtureVersion { get; }
|
||||
/// <summary>由夹具输入生成的稳定 SHA-256 指纹。</summary>
|
||||
public string ConfigurationFingerprint { get; }
|
||||
/// <summary>存储指纹是否与当前夹具内容一致。</summary>
|
||||
public bool IsConfigurationFingerprintCurrent { get; }
|
||||
/// <summary>快速比较使用的不可变粗路径点。</summary>
|
||||
public IReadOnlyList<CoarsePathPoint> Path { get; }
|
||||
/// <summary>覆盖粗路径的不可变方向段。</summary>
|
||||
public IReadOnlyList<PathSegment> Segments { get; }
|
||||
|
||||
internal SmoothingFixtureRecord Record { get; }
|
||||
|
||||
private static IReadOnlyList<CoarsePathPoint> ToPath(IReadOnlyList<SmoothingFixturePathPoint> source)
|
||||
{
|
||||
var result = new List<CoarsePathPoint>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
SmoothingFixturePathPoint point = source[index];
|
||||
result.Add(new CoarsePathPoint(
|
||||
point.XMeters, point.YMeters, point.HeadingRadians, point.UnwrappedHeadingRadians,
|
||||
point.ArcLengthMeters, point.Direction, point.VehicleCurvaturePerMeter,
|
||||
point.BodyClearanceMeters, point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
return new ReadOnlyCollection<CoarsePathPoint>(result);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> ToSegments(IReadOnlyList<SmoothingFixtureSegment> source)
|
||||
{
|
||||
var result = new List<PathSegment>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
SmoothingFixtureSegment segment = source[index];
|
||||
result.Add(new PathSegment(segment.SegmentIndex, segment.Direction, segment.StartIndex, segment.EndIndex,
|
||||
segment.StartsAtGearSwitch, segment.EndsAtGearSwitch));
|
||||
}
|
||||
return new ReadOnlyCollection<PathSegment>(result);
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureDocument
|
||||
{
|
||||
[JsonProperty("schemaVersion")] public int SchemaVersion { get; set; }
|
||||
[JsonProperty("scenarios")] public List<SmoothingFixtureRecord> Scenarios { get; set; } = new List<SmoothingFixtureRecord>();
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureRecord
|
||||
{
|
||||
[JsonProperty("id")] public string Id { get; set; }
|
||||
[JsonProperty("fixtureVersion")] public int FixtureVersion { get; set; }
|
||||
[JsonProperty("configurationFingerprint")] public string ConfigurationFingerprint { get; set; }
|
||||
[JsonProperty("map")] public SmoothingFixtureMap Map { get; set; }
|
||||
[JsonProperty("vehicle")] public SmoothingFixtureVehicle Vehicle { get; set; }
|
||||
[JsonProperty("planningConfiguration")] public SmoothingFixturePlanningConfiguration PlanningConfiguration { get; set; }
|
||||
[JsonProperty("path")] public List<SmoothingFixturePathPoint> Path { get; set; } = new List<SmoothingFixturePathPoint>();
|
||||
[JsonProperty("segments")] public List<SmoothingFixtureSegment> Segments { get; set; } = new List<SmoothingFixtureSegment>();
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureMap
|
||||
{
|
||||
[JsonProperty("xMinMm")] public float XMinMm { get; set; }
|
||||
[JsonProperty("xMaxMm")] public float XMaxMm { get; set; }
|
||||
[JsonProperty("yMinMm")] public float YMinMm { get; set; }
|
||||
[JsonProperty("yMaxMm")] public float YMaxMm { get; set; }
|
||||
[JsonProperty("resolutionMm")] public float ResolutionMm { get; set; }
|
||||
[JsonProperty("obstacles")] public List<SmoothingFixtureObstacle> Obstacles { get; set; } = new List<SmoothingFixtureObstacle>();
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureObstacle
|
||||
{
|
||||
[JsonProperty("kind")] public string Kind { get; set; }
|
||||
[JsonProperty("xMinMm")] public float XMinMm { get; set; }
|
||||
[JsonProperty("xMaxMm")] public float XMaxMm { get; set; }
|
||||
[JsonProperty("yMinMm")] public float YMinMm { get; set; }
|
||||
[JsonProperty("yMaxMm")] public float YMaxMm { get; set; }
|
||||
[JsonProperty("centerXMm")] public float CenterXMm { get; set; }
|
||||
[JsonProperty("centerYMm")] public float CenterYMm { get; set; }
|
||||
[JsonProperty("radiusMm")] public float RadiusMm { get; set; }
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureVehicle
|
||||
{
|
||||
[JsonProperty("lengthMeters")] public double LengthMeters { get; set; }
|
||||
[JsonProperty("widthMeters")] public double WidthMeters { get; set; }
|
||||
[JsonProperty("safetyMarginMeters")] public double SafetyMarginMeters { get; set; }
|
||||
[JsonProperty("maximumCurvaturePerMeter")] public double MaximumCurvaturePerMeter { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>产生夹具粗路径时使用的 Hybrid A* 配置快照；仅用于溯源和指纹验证，不参与快速夹具加载时的规划。</summary>
|
||||
internal sealed class SmoothingFixturePlanningConfiguration
|
||||
{
|
||||
[JsonProperty("primitiveLengthMeters")] public double PrimitiveLengthMeters { get; set; }
|
||||
[JsonProperty("integrationStepMeters")] public double IntegrationStepMeters { get; set; }
|
||||
[JsonProperty("maximumCollisionCheckStepMeters")] public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
[JsonProperty("headingResolutionRadians")] public double HeadingResolutionRadians { get; set; }
|
||||
[JsonProperty("curvatureLevelCount")] public int CurvatureLevelCount { get; set; }
|
||||
[JsonProperty("goalPositionToleranceMeters")] public double GoalPositionToleranceMeters { get; set; }
|
||||
[JsonProperty("goalHeadingToleranceRadians")] public double GoalHeadingToleranceRadians { get; set; }
|
||||
[JsonProperty("maximumExpandedNodes")] public int MaximumExpandedNodes { get; set; }
|
||||
[JsonProperty("searchTimeoutSeconds")] public double SearchTimeoutSeconds { get; set; }
|
||||
[JsonProperty("heuristicWeight")] public double HeuristicWeight { get; set; }
|
||||
[JsonProperty("reverseCostMultiplier")] public double ReverseCostMultiplier { get; set; }
|
||||
[JsonProperty("gearSwitchPenaltyMeters")] public double GearSwitchPenaltyMeters { get; set; }
|
||||
[JsonProperty("curvatureMagnitudeWeight")] public double CurvatureMagnitudeWeight { get; set; }
|
||||
[JsonProperty("curvatureChangePenaltyMetersPerLevel")] public double CurvatureChangePenaltyMetersPerLevel { get; set; }
|
||||
[JsonProperty("clearanceCostWeight")] public double ClearanceCostWeight { get; set; }
|
||||
[JsonProperty("clearanceCostDistanceMeters")] public double ClearanceCostDistanceMeters { get; set; }
|
||||
[JsonProperty("allowReverse")] public bool AllowReverse { get; set; }
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixturePathPoint
|
||||
{
|
||||
[JsonProperty("xMeters")] public double XMeters { get; set; }
|
||||
[JsonProperty("yMeters")] public double YMeters { get; set; }
|
||||
[JsonProperty("headingRadians")] public double HeadingRadians { get; set; }
|
||||
[JsonProperty("unwrappedHeadingRadians")] public double UnwrappedHeadingRadians { get; set; }
|
||||
[JsonProperty("arcLengthMeters")] public double ArcLengthMeters { get; set; }
|
||||
[JsonProperty("direction")] public TravelDirection Direction { get; set; }
|
||||
[JsonProperty("vehicleCurvaturePerMeter")] public double VehicleCurvaturePerMeter { get; set; }
|
||||
[JsonProperty("bodyClearanceMeters")] public double BodyClearanceMeters { get; set; }
|
||||
[JsonProperty("isGearSwitchPoint")] public bool IsGearSwitchPoint { get; set; }
|
||||
[JsonProperty("source")] public CoarsePathPointSource Source { get; set; }
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureSegment
|
||||
{
|
||||
[JsonProperty("segmentIndex")] public int SegmentIndex { get; set; }
|
||||
[JsonProperty("direction")] public TravelDirection Direction { get; set; }
|
||||
[JsonProperty("startIndex")] public int StartIndex { get; set; }
|
||||
[JsonProperty("endIndex")] public int EndIndex { get; set; }
|
||||
[JsonProperty("startsAtGearSwitch")] public bool StartsAtGearSwitch { get; set; }
|
||||
[JsonProperty("endsAtGearSwitch")] public bool EndsAtGearSwitch { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,258 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Security.Cryptography;
|
||||
using System.Text;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>加载并验证版本化 JSON 快速夹具；此类不运行 Hybrid A*。</summary>
|
||||
public static class SmoothingScenarioFixtureLoader
|
||||
{
|
||||
/// <summary>读取、校验并冻结指定的快速夹具文件。</summary>
|
||||
public static IReadOnlyList<SmoothingScenarioFixture> LoadAndVerify(string fixturePath)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(fixturePath)) throw new ArgumentException("夹具路径不能为空。", nameof(fixturePath));
|
||||
if (!File.Exists(fixturePath)) throw new FileNotFoundException("找不到路径平滑夹具文件。", fixturePath);
|
||||
|
||||
SmoothingFixtureDocument document = JsonConvert.DeserializeObject<SmoothingFixtureDocument>(
|
||||
File.ReadAllText(fixturePath, Encoding.UTF8));
|
||||
if (document == null || document.SchemaVersion != 1 || document.Scenarios == null)
|
||||
throw new InvalidDataException("路径平滑夹具架构版本无效。");
|
||||
|
||||
var fixtures = new List<SmoothingScenarioFixture>(document.Scenarios.Count);
|
||||
var ids = new HashSet<string>(StringComparer.Ordinal);
|
||||
for (int index = 0; index < document.Scenarios.Count; index++)
|
||||
{
|
||||
SmoothingFixtureRecord record = document.Scenarios[index];
|
||||
Validate(record, ids);
|
||||
string fingerprint = ComputeFingerprint(record);
|
||||
if (!string.Equals(record.ConfigurationFingerprint, fingerprint, StringComparison.Ordinal))
|
||||
throw new InvalidDataException("路径平滑夹具指纹已过期：" + record.Id + "。");
|
||||
fixtures.Add(new SmoothingScenarioFixture(record, true));
|
||||
}
|
||||
return fixtures.AsReadOnly();
|
||||
}
|
||||
|
||||
internal static string ComputeFingerprint(SmoothingFixtureRecord record)
|
||||
{
|
||||
string material = BuildFingerprintMaterial(record);
|
||||
using (SHA256 sha256 = SHA256.Create())
|
||||
{
|
||||
byte[] hash = sha256.ComputeHash(Encoding.UTF8.GetBytes(material));
|
||||
var builder = new StringBuilder(hash.Length * 2 + 7);
|
||||
builder.Append("sha256:");
|
||||
for (int index = 0; index < hash.Length; index++) builder.Append(hash[index].ToString("x2", CultureInfo.InvariantCulture));
|
||||
return builder.ToString();
|
||||
}
|
||||
}
|
||||
|
||||
internal static PlanningGridMap BuildMap(SmoothingScenarioFixture fixture)
|
||||
{
|
||||
SmoothingFixtureRecord record = fixture.Record;
|
||||
var obstacles = new List<IMapObstacle>(record.Map.Obstacles.Count);
|
||||
for (int index = 0; index < record.Map.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFixtureObstacle obstacle = record.Map.Obstacles[index];
|
||||
if (string.Equals(obstacle.Kind, "circle", StringComparison.Ordinal))
|
||||
obstacles.Add(new CircleObstacle(obstacle.CenterXMm, obstacle.CenterYMm, obstacle.RadiusMm));
|
||||
else if (string.Equals(obstacle.Kind, "axis-aligned-rectangle", StringComparison.Ordinal))
|
||||
obstacles.Add(new AxisAlignedRectangleObstacle(obstacle.XMinMm, obstacle.XMaxMm, obstacle.YMinMm, obstacle.YMaxMm));
|
||||
else
|
||||
throw new InvalidDataException("夹具包含未知障碍物类型。");
|
||||
}
|
||||
|
||||
var mapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(record.Map.XMinMm, record.Map.XMaxMm, record.Map.YMinMm, record.Map.YMaxMm),
|
||||
ResolutionMm = record.Map.ResolutionMm,
|
||||
AllowExplicitEmptyMap = obstacles.Count == 0,
|
||||
ObstacleSources = obstacles.Count == 0
|
||||
? Array.Empty<IMapObstacleSource>()
|
||||
: new IMapObstacleSource[] { new ManualObstacleSource("fixture-" + record.Id, record.FixtureVersion, true, obstacles) },
|
||||
};
|
||||
PlanningMapBuildResult build = new PlanningMapFactory().Create(mapRequest);
|
||||
if (!build.Succeeded || build.Map == null) throw new InvalidDataException("夹具地图无法重建：" + record.Id + "。");
|
||||
return build.Map;
|
||||
}
|
||||
|
||||
internal static VehicleParameters BuildVehicle(SmoothingScenarioFixture fixture)
|
||||
{
|
||||
SmoothingFixtureVehicle vehicle = fixture.Record.Vehicle;
|
||||
return new VehicleParameters
|
||||
{
|
||||
LengthMeters = vehicle.LengthMeters,
|
||||
WidthMeters = vehicle.WidthMeters,
|
||||
SafetyMarginMeters = vehicle.SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = vehicle.MaximumCurvaturePerMeter,
|
||||
};
|
||||
}
|
||||
|
||||
private static void Validate(SmoothingFixtureRecord record, ISet<string> ids)
|
||||
{
|
||||
if (record == null || string.IsNullOrWhiteSpace(record.Id) || record.FixtureVersion <= 0 ||
|
||||
record.Map == null || record.Vehicle == null || record.PlanningConfiguration == null || record.Path == null || record.Path.Count < 2 ||
|
||||
record.Segments == null || record.Segments.Count == 0 || !ids.Add(record.Id))
|
||||
throw new InvalidDataException("路径平滑夹具缺少必需字段、版本或唯一 ID。");
|
||||
|
||||
ValidateMap(record);
|
||||
ValidateVehicle(record);
|
||||
ValidatePlanningConfiguration(record);
|
||||
ValidatePathContract(record);
|
||||
}
|
||||
|
||||
private static void ValidateMap(SmoothingFixtureRecord record)
|
||||
{
|
||||
SmoothingFixtureMap map = record.Map;
|
||||
if (!IsFinite(map.XMinMm) || !IsFinite(map.XMaxMm) || !IsFinite(map.YMinMm) || !IsFinite(map.YMaxMm) ||
|
||||
!IsFinite(map.ResolutionMm) || map.XMaxMm <= map.XMinMm || map.YMaxMm <= map.YMinMm || map.ResolutionMm <= 0f ||
|
||||
map.Obstacles == null)
|
||||
{
|
||||
throw new InvalidDataException("Fixture map contract is invalid: " + record.Id + ".");
|
||||
}
|
||||
|
||||
for (int index = 0; index < map.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFixtureObstacle obstacle = map.Obstacles[index];
|
||||
bool circle = obstacle != null && string.Equals(obstacle.Kind, "circle", StringComparison.Ordinal) &&
|
||||
IsFinite(obstacle.CenterXMm) && IsFinite(obstacle.CenterYMm) && IsFinite(obstacle.RadiusMm) && obstacle.RadiusMm >= 0f;
|
||||
bool rectangle = obstacle != null && string.Equals(obstacle.Kind, "axis-aligned-rectangle", StringComparison.Ordinal) &&
|
||||
IsFinite(obstacle.XMinMm) && IsFinite(obstacle.XMaxMm) && IsFinite(obstacle.YMinMm) && IsFinite(obstacle.YMaxMm) &&
|
||||
obstacle.XMaxMm >= obstacle.XMinMm && obstacle.YMaxMm >= obstacle.YMinMm;
|
||||
if (!circle && !rectangle)
|
||||
throw new InvalidDataException("Fixture map contract is invalid: " + record.Id + ".");
|
||||
}
|
||||
}
|
||||
|
||||
private static void ValidateVehicle(SmoothingFixtureRecord record)
|
||||
{
|
||||
SmoothingFixtureVehicle vehicle = record.Vehicle;
|
||||
if (!IsFinite(vehicle.LengthMeters) || !IsFinite(vehicle.WidthMeters) || !IsFinite(vehicle.SafetyMarginMeters) ||
|
||||
!IsFinite(vehicle.MaximumCurvaturePerMeter) || vehicle.LengthMeters <= 0d || vehicle.WidthMeters <= 0d ||
|
||||
vehicle.SafetyMarginMeters < 0d || vehicle.MaximumCurvaturePerMeter <= 0d)
|
||||
{
|
||||
throw new InvalidDataException("Fixture vehicle contract is invalid: " + record.Id + ".");
|
||||
}
|
||||
}
|
||||
|
||||
private static void ValidatePlanningConfiguration(SmoothingFixtureRecord record)
|
||||
{
|
||||
SmoothingFixturePlanningConfiguration configuration = record.PlanningConfiguration;
|
||||
if (!IsPositiveFinite(configuration.PrimitiveLengthMeters) || !IsPositiveFinite(configuration.IntegrationStepMeters) ||
|
||||
!IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) || !IsPositiveFinite(configuration.HeadingResolutionRadians) ||
|
||||
configuration.CurvatureLevelCount < 2 || !IsPositiveFinite(configuration.GoalPositionToleranceMeters) ||
|
||||
!IsPositiveFinite(configuration.GoalHeadingToleranceRadians) || configuration.MaximumExpandedNodes <= 0 ||
|
||||
!IsPositiveFinite(configuration.SearchTimeoutSeconds) || !IsFinite(configuration.HeuristicWeight) ||
|
||||
!IsFinite(configuration.ReverseCostMultiplier) || !IsFinite(configuration.GearSwitchPenaltyMeters) ||
|
||||
!IsFinite(configuration.CurvatureMagnitudeWeight) || !IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) ||
|
||||
!IsFinite(configuration.ClearanceCostWeight) || !IsPositiveFinite(configuration.ClearanceCostDistanceMeters))
|
||||
{
|
||||
throw new InvalidDataException("Fixture planning configuration contract is invalid: " + record.Id + ".");
|
||||
}
|
||||
}
|
||||
|
||||
private static void ValidatePathContract(SmoothingFixtureRecord record)
|
||||
{
|
||||
var fixture = new SmoothingScenarioFixture(record, true);
|
||||
try
|
||||
{
|
||||
var request = new PathSmoothingRequest(
|
||||
fixture.Path,
|
||||
fixture.Segments,
|
||||
BuildMap(fixture),
|
||||
BuildVehicle(fixture),
|
||||
new PathSmoothingConfiguration());
|
||||
if (!new PathSmoothingPreprocessor().TryPrepare(request, out _, out string reason))
|
||||
throw new InvalidDataException("Fixture path contract is invalid: " + record.Id + "; " + reason);
|
||||
}
|
||||
catch (InvalidDataException)
|
||||
{
|
||||
throw;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
throw new InvalidDataException("Fixture path contract is invalid: " + record.Id + "; " + exception.Message, exception);
|
||||
}
|
||||
}
|
||||
|
||||
private static bool IsFinite(float value)
|
||||
{
|
||||
return !float.IsNaN(value) && !float.IsInfinity(value);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0d;
|
||||
}
|
||||
|
||||
private static string BuildFingerprintMaterial(SmoothingFixtureRecord record)
|
||||
{
|
||||
var builder = new StringBuilder();
|
||||
Append(builder, record.Id); Append(builder, record.FixtureVersion);
|
||||
Append(builder, record.Map.XMinMm); Append(builder, record.Map.XMaxMm); Append(builder, record.Map.YMinMm);
|
||||
Append(builder, record.Map.YMaxMm); Append(builder, record.Map.ResolutionMm);
|
||||
for (int index = 0; index < record.Map.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFixtureObstacle obstacle = record.Map.Obstacles[index];
|
||||
Append(builder, obstacle.Kind); Append(builder, obstacle.XMinMm); Append(builder, obstacle.XMaxMm);
|
||||
Append(builder, obstacle.YMinMm); Append(builder, obstacle.YMaxMm); Append(builder, obstacle.CenterXMm);
|
||||
Append(builder, obstacle.CenterYMm); Append(builder, obstacle.RadiusMm);
|
||||
}
|
||||
Append(builder, record.Vehicle.LengthMeters); Append(builder, record.Vehicle.WidthMeters);
|
||||
Append(builder, record.Vehicle.SafetyMarginMeters); Append(builder, record.Vehicle.MaximumCurvaturePerMeter);
|
||||
SmoothingFixturePlanningConfiguration configuration = record.PlanningConfiguration;
|
||||
Append(builder, configuration.PrimitiveLengthMeters); Append(builder, configuration.IntegrationStepMeters);
|
||||
Append(builder, configuration.MaximumCollisionCheckStepMeters); Append(builder, configuration.HeadingResolutionRadians);
|
||||
Append(builder, configuration.CurvatureLevelCount); Append(builder, configuration.GoalPositionToleranceMeters);
|
||||
Append(builder, configuration.GoalHeadingToleranceRadians); Append(builder, configuration.MaximumExpandedNodes);
|
||||
Append(builder, configuration.SearchTimeoutSeconds); Append(builder, configuration.HeuristicWeight);
|
||||
Append(builder, configuration.ReverseCostMultiplier); Append(builder, configuration.GearSwitchPenaltyMeters);
|
||||
Append(builder, configuration.CurvatureMagnitudeWeight); Append(builder, configuration.CurvatureChangePenaltyMetersPerLevel);
|
||||
Append(builder, configuration.ClearanceCostWeight); Append(builder, configuration.ClearanceCostDistanceMeters);
|
||||
Append(builder, configuration.AllowReverse ? 1 : 0);
|
||||
for (int index = 0; index < record.Path.Count; index++)
|
||||
{
|
||||
SmoothingFixturePathPoint point = record.Path[index];
|
||||
Append(builder, point.XMeters); Append(builder, point.YMeters); Append(builder, point.HeadingRadians);
|
||||
Append(builder, point.UnwrappedHeadingRadians); Append(builder, point.ArcLengthMeters); Append(builder, (int)point.Direction);
|
||||
Append(builder, point.VehicleCurvaturePerMeter); Append(builder, point.BodyClearanceMeters);
|
||||
Append(builder, point.IsGearSwitchPoint ? 1 : 0); Append(builder, (int)point.Source);
|
||||
}
|
||||
for (int index = 0; index < record.Segments.Count; index++)
|
||||
{
|
||||
SmoothingFixtureSegment segment = record.Segments[index];
|
||||
Append(builder, segment.SegmentIndex); Append(builder, (int)segment.Direction); Append(builder, segment.StartIndex);
|
||||
Append(builder, segment.EndIndex); Append(builder, segment.StartsAtGearSwitch ? 1 : 0); Append(builder, segment.EndsAtGearSwitch ? 1 : 0);
|
||||
}
|
||||
return builder.ToString();
|
||||
}
|
||||
|
||||
private static void Append(StringBuilder builder, string value) { builder.Append(value ?? string.Empty).Append('|'); }
|
||||
private static void Append(StringBuilder builder, int value) { builder.Append(value.ToString(CultureInfo.InvariantCulture)).Append('|'); }
|
||||
private static void Append(StringBuilder builder, float value)
|
||||
{
|
||||
AppendBits(builder, BitConverter.GetBytes(value));
|
||||
}
|
||||
|
||||
private static void Append(StringBuilder builder, double value)
|
||||
{
|
||||
AppendBits(builder, BitConverter.GetBytes(value));
|
||||
}
|
||||
|
||||
private static void AppendBits(StringBuilder builder, byte[] bytes)
|
||||
{
|
||||
for (int index = bytes.Length - 1; index >= 0; index--)
|
||||
builder.Append(bytes[index].ToString("x2", CultureInfo.InvariantCulture));
|
||||
builder.Append('|');
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,245 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
/// <summary>独立复核平滑候选的端点拓扑、车辆曲率和连续车体安全性。</summary>
|
||||
public sealed class SmoothedPathValidator
|
||||
{
|
||||
private const double Tolerance = 1e-6d;
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
/// <summary>创建使用默认连续车体碰撞检查器的平滑路径验证器。</summary>
|
||||
public SmoothedPathValidator()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定连续车体碰撞检查器的平滑路径验证器。</summary>
|
||||
public SmoothedPathValidator(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 复核候选平滑路径。每个候选方向段必须保持原始段的端点和换向拓扑；
|
||||
/// 输出中的净空均由本次实际车体检查重新计算，绝不沿用候选声明值。
|
||||
/// </summary>
|
||||
public bool TryValidate(
|
||||
IReadOnlyList<SmoothedPathPoint> candidatePath,
|
||||
IReadOnlyList<SmoothedPathSegment> candidateSegments,
|
||||
PreparedPath originalPath,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
double maximumCollisionCheckStepMeters,
|
||||
out IReadOnlyList<SmoothedPathPoint> pathWithClearance,
|
||||
out double minimumClearanceMeters,
|
||||
out string reason)
|
||||
{
|
||||
pathWithClearance = EmptyPath();
|
||||
minimumClearanceMeters = 0d;
|
||||
reason = string.Empty;
|
||||
if (candidatePath == null || candidateSegments == null || originalPath == null || map == null || vehicle == null ||
|
||||
candidatePath.Count == 0 || candidateSegments.Count == 0 || !NumericGuard.IsPositiveFinite(maximumCollisionCheckStepMeters))
|
||||
{
|
||||
reason = "平滑候选、原始路径、地图、车辆或碰撞步长无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvatureMeters))
|
||||
{
|
||||
reason = "车辆曲率约束无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryValidateSegmentTopology(candidatePath, candidateSegments, originalPath, out reason)) return false;
|
||||
if (Math.Abs(candidatePath[0].ArcLength) > Tolerance)
|
||||
{
|
||||
reason = "平滑路径首点的全局弧长必须为零。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var checkedClearances = new double[candidatePath.Count];
|
||||
double minimumClearance = double.PositiveInfinity;
|
||||
for (int index = 0; index < candidatePath.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint current = candidatePath[index];
|
||||
if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvatureMeters + Tolerance)
|
||||
{
|
||||
reason = "平滑路径包含非法数值或超限车辆曲率。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var currentPose = new Pose2D(current.X, current.Y, current.Heading);
|
||||
if (!_collisionChecker.IsPoseCollisionFree(currentPose, map, vehicle, 0d, out double poseClearance))
|
||||
{
|
||||
reason = "平滑路径点未通过完整车体碰撞或边界复核。";
|
||||
return false;
|
||||
}
|
||||
|
||||
checkedClearances[index] = poseClearance;
|
||||
minimumClearance = Math.Min(minimumClearance, poseClearance);
|
||||
if (index == 0) continue;
|
||||
|
||||
SmoothedPathPoint previous = candidatePath[index - 1];
|
||||
if (!IsUnwrappedHeadingContinuous(previous, current))
|
||||
{
|
||||
reason = "平滑路径展开航向不连续。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (IsDuplicatePoseAndArcLength(previous, current))
|
||||
{
|
||||
if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
|
||||
{
|
||||
reason = "相邻重复点不是合法换向对。";
|
||||
return false;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
|
||||
{
|
||||
reason = "非换向点必须保持正弧长增量，换向点必须保留重复位姿。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
|
||||
if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, map, vehicle,
|
||||
maximumCollisionCheckStepMeters, out double sweptClearance))
|
||||
{
|
||||
reason = "平滑路径相邻点之间的完整车体扫掠碰撞复核失败。";
|
||||
return false;
|
||||
}
|
||||
|
||||
checkedClearances[index - 1] = Math.Min(checkedClearances[index - 1], sweptClearance);
|
||||
checkedClearances[index] = Math.Min(checkedClearances[index], sweptClearance);
|
||||
minimumClearance = Math.Min(minimumClearance, sweptClearance);
|
||||
}
|
||||
|
||||
var output = new List<SmoothedPathPoint>(candidatePath.Count);
|
||||
for (int index = 0; index < candidatePath.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint point = candidatePath[index];
|
||||
output.Add(new SmoothedPathPoint(
|
||||
point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
|
||||
point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
|
||||
checkedClearances[index], point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
|
||||
pathWithClearance = new ReadOnlyCollection<SmoothedPathPoint>(output);
|
||||
minimumClearanceMeters = minimumClearance;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryValidateSegmentTopology(
|
||||
IReadOnlyList<SmoothedPathPoint> candidatePath,
|
||||
IReadOnlyList<SmoothedPathSegment> candidateSegments,
|
||||
PreparedPath originalPath,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (originalPath.Segments == null || originalPath.Segments.Count != candidateSegments.Count)
|
||||
{
|
||||
reason = "平滑路径方向段数量必须保持原始换向拓扑。";
|
||||
return false;
|
||||
}
|
||||
|
||||
int expectedStartIndex = 0;
|
||||
for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
|
||||
{
|
||||
SmoothedPathSegment candidateSegment = candidateSegments[segmentIndex];
|
||||
PreparedDirectionSegment originalSegment = originalPath.Segments[segmentIndex];
|
||||
if (candidateSegment == null || originalSegment == null || candidateSegment.SegmentIndex != segmentIndex ||
|
||||
candidateSegment.StartIndex != expectedStartIndex || candidateSegment.StartIndex < 0 ||
|
||||
candidateSegment.EndIndex < candidateSegment.StartIndex || candidateSegment.EndIndex >= candidatePath.Count ||
|
||||
candidateSegment.Direction != originalSegment.Direction ||
|
||||
candidateSegment.StartsAtGearSwitch != originalSegment.StartsAtGearSwitch ||
|
||||
candidateSegment.EndsAtGearSwitch != originalSegment.EndsAtGearSwitch ||
|
||||
originalSegment.Points == null || originalSegment.Points.Count == 0)
|
||||
{
|
||||
reason = "平滑路径方向段索引、方向或换向拓扑无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
SmoothedPathPoint candidateStart = candidatePath[candidateSegment.StartIndex];
|
||||
SmoothedPathPoint candidateEnd = candidatePath[candidateSegment.EndIndex];
|
||||
SmoothingPoint2D originalStart = originalSegment.Points[0];
|
||||
SmoothingPoint2D originalEnd = originalSegment.Points[originalSegment.Points.Count - 1];
|
||||
if (!SamePose(candidateStart, originalStart) || !SamePose(candidateEnd, originalEnd) ||
|
||||
candidateStart.Direction != originalSegment.Direction || candidateEnd.Direction != originalSegment.Direction ||
|
||||
candidateStart.IsGearSwitchPoint != originalStart.IsGearSwitchPoint ||
|
||||
candidateEnd.IsGearSwitchPoint != originalEnd.IsGearSwitchPoint)
|
||||
{
|
||||
reason = "平滑路径改变了原始方向段端点或换向点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int pointIndex = candidateSegment.StartIndex; pointIndex <= candidateSegment.EndIndex; pointIndex++)
|
||||
{
|
||||
if (candidatePath[pointIndex] == null || candidatePath[pointIndex].Direction != originalSegment.Direction)
|
||||
{
|
||||
reason = "平滑路径方向段包含与段方向不一致的点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
expectedStartIndex = candidateSegment.EndIndex + 1;
|
||||
}
|
||||
|
||||
if (expectedStartIndex != candidatePath.Count)
|
||||
{
|
||||
reason = "平滑路径方向段未完整覆盖候选点。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(SmoothedPathPoint point)
|
||||
{
|
||||
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
||||
NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
||||
NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
|
||||
NumericGuard.IsFinite(point.GeometricCurvature) && NumericGuard.IsFinite(point.VehicleCurvature) &&
|
||||
NumericGuard.IsFinite(point.VehicleCurvatureDerivative) &&
|
||||
IsDirection(point.Direction) && Enum.IsDefined(typeof(SmoothedPathPointSource), point.Source) &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
|
||||
}
|
||||
|
||||
private static bool SamePose(SmoothedPathPoint candidate, SmoothingPoint2D original)
|
||||
{
|
||||
return candidate != null && original != null && Math.Abs(candidate.X - original.X) <= Tolerance &&
|
||||
Math.Abs(candidate.Y - original.Y) <= Tolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(candidate.Heading, original.Heading)) <= Tolerance;
|
||||
}
|
||||
|
||||
private static bool IsUnwrappedHeadingContinuous(SmoothedPathPoint previous, SmoothedPathPoint current)
|
||||
{
|
||||
double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
||||
return NumericGuard.IsFinite(expectedDelta) &&
|
||||
Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= Tolerance;
|
||||
}
|
||||
|
||||
private static bool IsDuplicatePoseAndArcLength(SmoothedPathPoint previous, SmoothedPathPoint current)
|
||||
{
|
||||
return Math.Abs(previous.X - current.X) <= Tolerance && Math.Abs(previous.Y - current.Y) <= Tolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance &&
|
||||
Math.Abs(previous.ArcLength - current.ArcLength) <= Tolerance;
|
||||
}
|
||||
|
||||
private static bool IsDirection(TravelDirection direction)
|
||||
{
|
||||
return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothedPathPoint> EmptyPath()
|
||||
{
|
||||
return new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>路径平滑对比报告使用的固定 IEEE 风格物理尺寸和色卡。</summary>
|
||||
public static class IeeeFigureStyle
|
||||
{
|
||||
/// <summary>双栏宽度，单位 pt。</summary>
|
||||
public const double FigureWidthPoints = 7.16d * 72d;
|
||||
|
||||
/// <summary>图形高度，单位 pt。</summary>
|
||||
public const double FigureHeightPoints = 5.20d * 72d;
|
||||
|
||||
/// <summary>原始粗路径颜色。</summary>
|
||||
public const string RawColor = "#4D4D4D";
|
||||
|
||||
/// <summary>三次 B 样条颜色。</summary>
|
||||
public const string BSplineColor = "#0072B2";
|
||||
|
||||
/// <summary>局部三次 Bézier 颜色。</summary>
|
||||
public const string BezierColor = "#D55E00";
|
||||
|
||||
/// <summary>分段五次路径颜色。</summary>
|
||||
public const string QuinticColor = "#009E73";
|
||||
|
||||
/// <summary>曲率限制颜色。</summary>
|
||||
public const string LimitColor = "#CC79A7";
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
using System;
|
||||
using System.Globalization;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>把共享图形模型的指标表写为带 BOM 的 UTF-8 CSV。</summary>
|
||||
public sealed class SmoothingCsvWriter
|
||||
{
|
||||
private const string Header = "ScenarioId,Method,Status,PathLengthMeters,MaximumAbsoluteVehicleCurvaturePerMeter,RootMeanSquareVehicleCurvaturePerMeter,TotalAbsoluteCurvatureVariationPerMeter,CurvatureVariationEnergy,MinimumBodyClearanceMeters,MedianElapsedMilliseconds,TimingSampleCount,IsDeterministic,RetryCount,AcceptedStrength";
|
||||
|
||||
public byte[] Write(SmoothingFigureModel model)
|
||||
{
|
||||
if (model == null) throw new ArgumentNullException(nameof(model));
|
||||
var text = new StringBuilder(Header).Append("\r\n");
|
||||
for (int index = 0; index < model.MetricRows.Count; index++)
|
||||
{
|
||||
SmoothingFigureMetricRow row = model.MetricRows[index];
|
||||
PathQualityMetrics metrics = row.Metrics;
|
||||
text.Append(Field(model.ScenarioId)).Append(',').Append(Field(row.Method)).Append(',').Append(Field(row.Status.ToString())).Append(',')
|
||||
.Append(Number(metrics.PathLengthMeters)).Append(',').Append(Number(metrics.MaximumAbsoluteVehicleCurvaturePerMeter)).Append(',')
|
||||
.Append(Number(metrics.RootMeanSquareVehicleCurvaturePerMeter)).Append(',').Append(Number(metrics.TotalAbsoluteCurvatureVariationPerMeter)).Append(',')
|
||||
.Append(Number(metrics.CurvatureVariationEnergy)).Append(',').Append(Number(metrics.MinimumBodyClearanceMeters)).Append(',')
|
||||
.Append(Number(row.Timing == null ? 0d : row.Timing.MedianElapsedMilliseconds)).Append(',')
|
||||
.Append(row.Timing == null ? 0 : row.Timing.MeasuredElapsedMilliseconds.Count).Append(',')
|
||||
.Append(row.Timing != null && row.Timing.IsDeterministic ? "true" : "false").Append(',').Append(row.RetryCount).Append(',')
|
||||
.Append(Number(row.AcceptedStrength)).Append("\r\n");
|
||||
}
|
||||
byte[] body = new UTF8Encoding(false).GetBytes(text.ToString());
|
||||
byte[] preamble = new UTF8Encoding(true).GetPreamble();
|
||||
var output = new byte[preamble.Length + body.Length];
|
||||
Buffer.BlockCopy(preamble, 0, output, 0, preamble.Length);
|
||||
Buffer.BlockCopy(body, 0, output, preamble.Length, body.Length);
|
||||
return output;
|
||||
}
|
||||
|
||||
private static string Number(double value) { return value.ToString("0.#################", CultureInfo.InvariantCulture); }
|
||||
private static string Field(string value)
|
||||
{
|
||||
string text = value ?? string.Empty;
|
||||
return text.IndexOfAny(new[] { ',', '\"', '\r', '\n' }) < 0 ? text : "\"" + text.Replace("\"", "\"\"") + "\"";
|
||||
}
|
||||
}
|
||||
+109
@@ -0,0 +1,109 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>单张 SVG/PNG 共享的不可变绘图视图。</summary>
|
||||
public sealed class SmoothingFigureDefinition
|
||||
{
|
||||
internal SmoothingFigureDefinition(
|
||||
SmoothingFigureKind kind,
|
||||
string fileStem,
|
||||
string title,
|
||||
SmoothingFigureModel model,
|
||||
bool showsMapContext,
|
||||
IReadOnlyList<SmoothingFigureSeriesView> series,
|
||||
double worldXMinMeters,
|
||||
double worldXMaxMeters,
|
||||
double worldYMinMeters,
|
||||
double worldYMaxMeters,
|
||||
IReadOnlyList<double> xTicks,
|
||||
IReadOnlyList<double> yTicks,
|
||||
double curvatureArcLengthMaximumMeters,
|
||||
double curvatureMinimumPerMeter,
|
||||
double curvatureMaximumPerMeter,
|
||||
IReadOnlyList<double> curvatureArcLengthTicks,
|
||||
IReadOnlyList<double> curvatureTicks)
|
||||
{
|
||||
Kind = kind;
|
||||
FileStem = fileStem ?? string.Empty;
|
||||
Title = title ?? string.Empty;
|
||||
Model = model ?? throw new ArgumentNullException(nameof(model));
|
||||
ShowsMapContext = showsMapContext;
|
||||
Series = Copy(series);
|
||||
WorldXMinMeters = worldXMinMeters;
|
||||
WorldXMaxMeters = worldXMaxMeters;
|
||||
WorldYMinMeters = worldYMinMeters;
|
||||
WorldYMaxMeters = worldYMaxMeters;
|
||||
XTicks = Copy(xTicks);
|
||||
YTicks = Copy(yTicks);
|
||||
CurvatureArcLengthMaximumMeters = curvatureArcLengthMaximumMeters;
|
||||
CurvatureMinimumPerMeter = curvatureMinimumPerMeter;
|
||||
CurvatureMaximumPerMeter = curvatureMaximumPerMeter;
|
||||
CurvatureArcLengthTicks = Copy(curvatureArcLengthTicks);
|
||||
CurvatureTicks = Copy(curvatureTicks);
|
||||
}
|
||||
|
||||
public SmoothingFigureKind Kind { get; }
|
||||
public string FileStem { get; }
|
||||
public string Title { get; }
|
||||
public SmoothingFigureModel Model { get; }
|
||||
public bool ShowsMapContext { get; }
|
||||
public bool IsCurvatureFigure => Kind == SmoothingFigureKind.CurvatureComparison;
|
||||
public double FigureWidthPoints => Model.FigureWidthPoints;
|
||||
public double FigureHeightPoints => Model.FigureHeightPoints;
|
||||
public double PlotXPoints => 68d;
|
||||
public double PlotYPoints => 44d;
|
||||
public double PlotWidthPoints => 400d;
|
||||
public double PlotHeightPoints => 245d;
|
||||
public double LegendYPoints => 340d;
|
||||
public IReadOnlyList<SmoothingFigureSeriesView> Series { get; }
|
||||
public IReadOnlyList<SmoothingFigureLegendEntry> LegendEntries => BuildLegend(Series);
|
||||
public double WorldXMinMeters { get; }
|
||||
public double WorldXMaxMeters { get; }
|
||||
public double WorldYMinMeters { get; }
|
||||
public double WorldYMaxMeters { get; }
|
||||
public double WorldScalePointsPerMeter => Math.Min(PlotWidthPoints / (WorldXMaxMeters - WorldXMinMeters), PlotHeightPoints / (WorldYMaxMeters - WorldYMinMeters));
|
||||
public IReadOnlyList<double> XTicks { get; }
|
||||
public IReadOnlyList<double> YTicks { get; }
|
||||
public double CurvatureArcLengthMaximumMeters { get; }
|
||||
public double CurvatureMinimumPerMeter { get; }
|
||||
public double CurvatureMaximumPerMeter { get; }
|
||||
public IReadOnlyList<double> CurvatureArcLengthTicks { get; }
|
||||
public IReadOnlyList<double> CurvatureTicks { get; }
|
||||
|
||||
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null) for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureLegendEntry> BuildLegend(IReadOnlyList<SmoothingFigureSeriesView> views)
|
||||
{
|
||||
var entries = new List<SmoothingFigureLegendEntry>(views == null ? 0 : views.Count);
|
||||
if (views != null)
|
||||
{
|
||||
for (int index = 0; index < views.Count; index++)
|
||||
{
|
||||
SmoothingFigureSeries series = views[index].Series;
|
||||
entries.Add(new SmoothingFigureLegendEntry(series.Label + " (" + series.Status + ")", series.Color, string.Empty));
|
||||
}
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingFigureLegendEntry>(entries);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>某条路径在特定图中的显示透明度。</summary>
|
||||
public sealed class SmoothingFigureSeriesView
|
||||
{
|
||||
internal SmoothingFigureSeriesView(SmoothingFigureSeries series, double opacity)
|
||||
{
|
||||
Series = series ?? throw new ArgumentNullException(nameof(series));
|
||||
Opacity = opacity < 0d ? 0d : (opacity > 1d ? 1d : opacity);
|
||||
}
|
||||
|
||||
public SmoothingFigureSeries Series { get; }
|
||||
public double Opacity { get; }
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>每个场景稳定发布的六种报告图。</summary>
|
||||
public enum SmoothingFigureKind
|
||||
{
|
||||
CoarsePathOverview,
|
||||
AllPathsComparison,
|
||||
CubicBSplineOverview,
|
||||
LocalCubicBezierOverview,
|
||||
PiecewiseQuinticOverview,
|
||||
CurvatureComparison,
|
||||
}
|
||||
@@ -0,0 +1,185 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>SVG 和 PNG 共享的不可变路径平滑报告图形模型。</summary>
|
||||
public sealed class SmoothingFigureModel
|
||||
{
|
||||
internal SmoothingFigureModel(
|
||||
string scenarioId,
|
||||
string scenarioLabel,
|
||||
double worldXMinMeters,
|
||||
double worldXMaxMeters,
|
||||
double worldYMinMeters,
|
||||
double worldYMaxMeters,
|
||||
double pathPanelX,
|
||||
double pathPanelY,
|
||||
double pathPanelWidth,
|
||||
double pathPanelHeight,
|
||||
double curvaturePanelX,
|
||||
double curvaturePanelY,
|
||||
double curvaturePanelWidth,
|
||||
double curvaturePanelHeight,
|
||||
double metricsPanelX,
|
||||
double metricsPanelY,
|
||||
double metricsPanelWidth,
|
||||
double metricsPanelHeight,
|
||||
IReadOnlyList<SmoothingFigureObstacle> obstacles,
|
||||
IReadOnlyList<SmoothingFigureSeries> series,
|
||||
IReadOnlyList<SmoothingFigureMetricRow> metricRows,
|
||||
SmoothingFigurePoint start,
|
||||
SmoothingFigurePoint goal)
|
||||
{
|
||||
ScenarioId = scenarioId ?? string.Empty;
|
||||
ScenarioLabel = scenarioLabel ?? string.Empty;
|
||||
WorldXMinMeters = worldXMinMeters;
|
||||
WorldXMaxMeters = worldXMaxMeters;
|
||||
WorldYMinMeters = worldYMinMeters;
|
||||
WorldYMaxMeters = worldYMaxMeters;
|
||||
PathPanelX = pathPanelX;
|
||||
PathPanelY = pathPanelY;
|
||||
PathPanelWidth = pathPanelWidth;
|
||||
PathPanelHeight = pathPanelHeight;
|
||||
CurvaturePanelX = curvaturePanelX;
|
||||
CurvaturePanelY = curvaturePanelY;
|
||||
CurvaturePanelWidth = curvaturePanelWidth;
|
||||
CurvaturePanelHeight = curvaturePanelHeight;
|
||||
MetricsPanelX = metricsPanelX;
|
||||
MetricsPanelY = metricsPanelY;
|
||||
MetricsPanelWidth = metricsPanelWidth;
|
||||
MetricsPanelHeight = metricsPanelHeight;
|
||||
Obstacles = Copy(obstacles);
|
||||
Series = Copy(series);
|
||||
MetricRows = Copy(metricRows);
|
||||
Start = start ?? throw new ArgumentNullException(nameof(start));
|
||||
Goal = goal ?? throw new ArgumentNullException(nameof(goal));
|
||||
}
|
||||
|
||||
public string ScenarioId { get; }
|
||||
public string ScenarioLabel { get; }
|
||||
public double FigureWidthPoints => IeeeFigureStyle.FigureWidthPoints;
|
||||
public double FigureHeightPoints => IeeeFigureStyle.FigureHeightPoints;
|
||||
public string PathPanelLabel => "(a)";
|
||||
public string CurvaturePanelLabel => "(b)";
|
||||
public string MetricsPanelLabel => "(c)";
|
||||
public double WorldXMinMeters { get; }
|
||||
public double WorldXMaxMeters { get; }
|
||||
public double WorldYMinMeters { get; }
|
||||
public double WorldYMaxMeters { get; }
|
||||
public double PathPanelX { get; }
|
||||
public double PathPanelY { get; }
|
||||
public double PathPanelWidth { get; }
|
||||
public double PathPanelHeight { get; }
|
||||
public double CurvaturePanelX { get; }
|
||||
public double CurvaturePanelY { get; }
|
||||
public double CurvaturePanelWidth { get; }
|
||||
public double CurvaturePanelHeight { get; }
|
||||
public double MetricsPanelX { get; }
|
||||
public double MetricsPanelY { get; }
|
||||
public double MetricsPanelWidth { get; }
|
||||
public double MetricsPanelHeight { get; }
|
||||
public double PathScaleX { get; internal set; }
|
||||
public double PathScaleY { get; internal set; }
|
||||
public IReadOnlyList<SmoothingFigureObstacle> Obstacles { get; }
|
||||
public IReadOnlyList<SmoothingFigureSeries> Series { get; }
|
||||
public IReadOnlyList<SmoothingFigureLegendEntry> LegendEntries => BuildLegend(Series);
|
||||
public IReadOnlyList<SmoothingFigureMetricRow> MetricRows { get; }
|
||||
public SmoothingFigurePoint Start { get; }
|
||||
public SmoothingFigurePoint Goal { get; }
|
||||
|
||||
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null) for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureLegendEntry> BuildLegend(IReadOnlyList<SmoothingFigureSeries> series)
|
||||
{
|
||||
var legend = new List<SmoothingFigureLegendEntry>(series == null ? 0 : series.Count);
|
||||
if (series != null)
|
||||
{
|
||||
for (int index = 0; index < series.Count; index++)
|
||||
legend.Add(new SmoothingFigureLegendEntry(series[index].Label, series[index].Color, series[index].DashArray));
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingFigureLegendEntry>(legend);
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigurePoint
|
||||
{
|
||||
public SmoothingFigurePoint(double xMeters, double yMeters, double arcLengthMeters, double vehicleCurvaturePerMeter)
|
||||
{
|
||||
X = xMeters; Y = yMeters; ArcLength = arcLengthMeters; VehicleCurvature = vehicleCurvaturePerMeter;
|
||||
}
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double ArcLength { get; }
|
||||
public double VehicleCurvature { get; }
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigureObstacle
|
||||
{
|
||||
public SmoothingFigureObstacle(double xMeters, double yMeters, double widthMeters, double heightMeters)
|
||||
{
|
||||
X = xMeters; Y = yMeters; Width = widthMeters; Height = heightMeters;
|
||||
}
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double Width { get; }
|
||||
public double Height { get; }
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigureSeries
|
||||
{
|
||||
internal SmoothingFigureSeries(SmoothingMethod? method, string key, string label, PathSmoothingStatus status, string color, string dashArray,
|
||||
bool isRawPathBaseline, IReadOnlyList<SmoothingFigurePoint> points, IReadOnlyList<SmoothingFigurePoint> violationMarkers)
|
||||
{
|
||||
Method = method; Key = key ?? string.Empty; Label = label ?? string.Empty; Status = status; Color = color ?? string.Empty;
|
||||
DashArray = dashArray ?? string.Empty; IsRawPathBaseline = isRawPathBaseline; Points = Copy(points); ViolationMarkers = Copy(violationMarkers);
|
||||
}
|
||||
public SmoothingMethod? Method { get; }
|
||||
public string Key { get; }
|
||||
public string Label { get; }
|
||||
public PathSmoothingStatus Status { get; }
|
||||
public string Color { get; }
|
||||
public string DashArray { get; }
|
||||
public bool IsRawPathBaseline { get; }
|
||||
public bool IsCurveVisible => Points.Count > 0;
|
||||
public IReadOnlyList<SmoothingFigurePoint> Points { get; }
|
||||
public IReadOnlyList<SmoothingFigurePoint> ViolationMarkers { get; }
|
||||
private static IReadOnlyList<T> Copy<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null) for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigureLegendEntry
|
||||
{
|
||||
internal SmoothingFigureLegendEntry(string label, string color, string dashArray) { Label = label ?? string.Empty; Color = color ?? string.Empty; DashArray = dashArray ?? string.Empty; }
|
||||
public string Label { get; }
|
||||
public string Color { get; }
|
||||
public string DashArray { get; }
|
||||
}
|
||||
|
||||
public sealed class SmoothingFigureMetricRow
|
||||
{
|
||||
internal SmoothingFigureMetricRow(string method, string label, PathSmoothingStatus status, PathQualityMetrics metrics,
|
||||
SmoothingTimingSummary timing, int retryCount, double acceptedStrength)
|
||||
{
|
||||
Method = method ?? string.Empty; Label = label ?? string.Empty; Status = status; Metrics = metrics ?? new PathQualityMetrics();
|
||||
Timing = timing; RetryCount = retryCount < 0 ? 0 : retryCount; AcceptedStrength = acceptedStrength;
|
||||
}
|
||||
public string Method { get; }
|
||||
public string Label { get; }
|
||||
public PathSmoothingStatus Status { get; }
|
||||
public PathQualityMetrics Metrics { get; }
|
||||
public SmoothingTimingSummary Timing { get; }
|
||||
public int RetryCount { get; }
|
||||
public double AcceptedStrength { get; }
|
||||
}
|
||||
+170
@@ -0,0 +1,170 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>将不可变的比较结果、地图和端点转换为共享报告图形模型。</summary>
|
||||
public sealed class SmoothingFigureModelBuilder
|
||||
{
|
||||
private const double MarginPoints = 18d;
|
||||
private const double PanelGapPoints = 12d;
|
||||
|
||||
public SmoothingFigureModel Build(
|
||||
PathSmoothingComparisonResult comparison,
|
||||
PlanningGridMap map,
|
||||
Pose2D start,
|
||||
Pose2D goal,
|
||||
string scenarioId,
|
||||
string scenarioLabel)
|
||||
{
|
||||
if (comparison == null) throw new ArgumentNullException(nameof(comparison));
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
if (start == null) throw new ArgumentNullException(nameof(start));
|
||||
if (goal == null) throw new ArgumentNullException(nameof(goal));
|
||||
|
||||
double worldXMin = map.Bounds.XMin / 1000d;
|
||||
double worldXMax = map.Bounds.XMax / 1000d;
|
||||
double worldYMin = map.Bounds.YMin / 1000d;
|
||||
double worldYMax = map.Bounds.YMax / 1000d;
|
||||
double innerWidth = IeeeFigureStyle.FigureWidthPoints - 2d * MarginPoints;
|
||||
double innerHeight = IeeeFigureStyle.FigureHeightPoints - 2d * MarginPoints;
|
||||
double pathWidth = innerWidth * 0.60d;
|
||||
double rightWidth = innerWidth - pathWidth - PanelGapPoints;
|
||||
double rightHeight = (innerHeight - PanelGapPoints) / 2d;
|
||||
|
||||
var model = new SmoothingFigureModel(
|
||||
scenarioId, scenarioLabel, worldXMin, worldXMax, worldYMin, worldYMax,
|
||||
MarginPoints, MarginPoints, pathWidth, innerHeight,
|
||||
MarginPoints + pathWidth + PanelGapPoints, MarginPoints, rightWidth, rightHeight,
|
||||
MarginPoints + pathWidth + PanelGapPoints, MarginPoints + rightHeight + PanelGapPoints, rightWidth, rightHeight,
|
||||
BuildObstacles(map), BuildSeries(comparison, map), BuildMetricRows(comparison),
|
||||
new SmoothingFigurePoint(start.X, start.Y, 0d, 0d), new SmoothingFigurePoint(goal.X, goal.Y, 0d, 0d));
|
||||
|
||||
double worldWidth = worldXMax - worldXMin;
|
||||
double worldHeight = worldYMax - worldYMin;
|
||||
if (worldWidth <= 0d || worldHeight <= 0d) throw new ArgumentOutOfRangeException(nameof(map), "地图边界必须为非退化有限范围。");
|
||||
double scale = Math.Min(pathWidth / worldWidth, innerHeight / worldHeight);
|
||||
model.PathScaleX = scale;
|
||||
model.PathScaleY = scale;
|
||||
return model;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureObstacle> BuildObstacles(PlanningGridMap map)
|
||||
{
|
||||
var runs = new List<ObstacleRun>();
|
||||
double resolution = map.ResolutionMeters;
|
||||
double xMin = map.Bounds.XMin / 1000d;
|
||||
double yMin = map.Bounds.YMin / 1000d;
|
||||
for (int row = 0; row < map.Rows; row++)
|
||||
{
|
||||
int runStart = -1;
|
||||
for (int column = 0; column <= map.Cols; column++)
|
||||
{
|
||||
bool occupied = column < map.Cols && map.IsOccupied(row, column);
|
||||
if (occupied && runStart < 0) { runStart = column; continue; }
|
||||
if (!occupied && runStart >= 0)
|
||||
{
|
||||
AddOrExtendRun(runs, xMin + runStart * resolution, yMin + row * resolution,
|
||||
(column - runStart) * resolution, resolution);
|
||||
runStart = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
var obstacles = new List<SmoothingFigureObstacle>(runs.Count);
|
||||
for (int index = 0; index < runs.Count; index++)
|
||||
{
|
||||
ObstacleRun run = runs[index];
|
||||
obstacles.Add(new SmoothingFigureObstacle(run.X, run.Y, run.Width, run.Height));
|
||||
}
|
||||
return obstacles;
|
||||
}
|
||||
|
||||
private static void AddOrExtendRun(IList<ObstacleRun> runs, double x, double y, double width, double height)
|
||||
{
|
||||
for (int index = runs.Count - 1; index >= 0; index--)
|
||||
{
|
||||
ObstacleRun candidate = runs[index];
|
||||
if (NearlyEqual(candidate.X, x) && NearlyEqual(candidate.Width, width) && NearlyEqual(candidate.Y + candidate.Height, y))
|
||||
{
|
||||
candidate.Height += height;
|
||||
return;
|
||||
}
|
||||
}
|
||||
runs.Add(new ObstacleRun(x, y, width, height));
|
||||
}
|
||||
|
||||
private static bool NearlyEqual(double first, double second) { return Math.Abs(first - second) < 1e-9d; }
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureSeries> BuildSeries(PathSmoothingComparisonResult comparison, PlanningGridMap map)
|
||||
{
|
||||
var series = new List<SmoothingFigureSeries>(4)
|
||||
{
|
||||
CreateSeries(comparison.RawPathBaseline, null, "raw", "原始粗路径", IeeeFigureStyle.RawColor, string.Empty, true, map)
|
||||
};
|
||||
series.Add(CreateSeries(Find(comparison, SmoothingMethod.CubicBSpline), SmoothingMethod.CubicBSpline, "bspline", "三次 B 样条", IeeeFigureStyle.BSplineColor, string.Empty, false, map));
|
||||
series.Add(CreateSeries(Find(comparison, SmoothingMethod.LocalCubicBezier), SmoothingMethod.LocalCubicBezier, "bezier", "局部三次 Bézier", IeeeFigureStyle.BezierColor, string.Empty, false, map));
|
||||
series.Add(CreateSeries(Find(comparison, SmoothingMethod.PiecewiseQuintic), SmoothingMethod.PiecewiseQuintic, "quintic", "分段五次", IeeeFigureStyle.QuinticColor, string.Empty, false, map));
|
||||
return series;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingFigureMetricRow> BuildMetricRows(PathSmoothingComparisonResult comparison)
|
||||
{
|
||||
return new List<SmoothingFigureMetricRow>
|
||||
{
|
||||
CreateRow(comparison.RawPathBaseline, "RawPath", "原始粗路径"),
|
||||
CreateRow(Find(comparison, SmoothingMethod.CubicBSpline), "CubicBSpline", "三次 B 样条"),
|
||||
CreateRow(Find(comparison, SmoothingMethod.LocalCubicBezier), "LocalCubicBezier", "局部三次 Bézier"),
|
||||
CreateRow(Find(comparison, SmoothingMethod.PiecewiseQuintic), "PiecewiseQuintic", "分段五次")
|
||||
};
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonEntry Find(PathSmoothingComparisonResult comparison, SmoothingMethod method)
|
||||
{
|
||||
for (int index = 0; index < comparison.Entries.Count; index++)
|
||||
{
|
||||
PathSmoothingComparisonEntry entry = comparison.Entries[index];
|
||||
if (entry.Method == method) return entry;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private static SmoothingFigureMetricRow CreateRow(PathSmoothingComparisonEntry entry, string method, string label)
|
||||
{
|
||||
return entry == null
|
||||
? new SmoothingFigureMetricRow(method, label, PathSmoothingStatus.Failed, new PathQualityMetrics(), null, 0, 0d)
|
||||
: new SmoothingFigureMetricRow(method, label, entry.Status, entry.Metrics, entry.Timing, entry.RetryCount, entry.AcceptedStrength);
|
||||
}
|
||||
|
||||
private static SmoothingFigureSeries CreateSeries(PathSmoothingComparisonEntry entry, SmoothingMethod? method, string key, string label,
|
||||
string color, string dashArray, bool isRawPathBaseline, PlanningGridMap map)
|
||||
{
|
||||
var points = new List<SmoothingFigurePoint>();
|
||||
var violations = new List<SmoothingFigurePoint>();
|
||||
PathSmoothingStatus status = entry == null ? PathSmoothingStatus.Failed : entry.Status;
|
||||
if (entry != null)
|
||||
{
|
||||
for (int index = 0; index < entry.Path.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint point = entry.Path[index];
|
||||
var figurePoint = new SmoothingFigurePoint(point.X, point.Y, point.ArcLength, point.VehicleCurvature);
|
||||
points.Add(figurePoint);
|
||||
if (status == PathSmoothingStatus.Infeasible && map.IsOccupiedWorld(point.X, point.Y)) violations.Add(figurePoint);
|
||||
}
|
||||
}
|
||||
if (status == PathSmoothingStatus.Infeasible && points.Count > 0 && violations.Count == 0)
|
||||
violations.Add(points[points.Count / 2]);
|
||||
return new SmoothingFigureSeries(method, key, label, status, color, dashArray, isRawPathBaseline, points, violations);
|
||||
}
|
||||
|
||||
private sealed class ObstacleRun
|
||||
{
|
||||
public ObstacleRun(double x, double y, double width, double height) { X = x; Y = y; Width = width; Height = height; }
|
||||
public double X { get; }
|
||||
public double Y { get; }
|
||||
public double Width { get; }
|
||||
public double Height { get; set; }
|
||||
}
|
||||
}
|
||||
+171
@@ -0,0 +1,171 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>从一份比较模型构建六张含义固定、相机聚焦的报告图。</summary>
|
||||
public sealed class SmoothingFigureSetBuilder
|
||||
{
|
||||
private const double MinimumExtentMeters = 0.25d;
|
||||
private const double PaddingFraction = 0.10d;
|
||||
|
||||
public SmoothingFigureSet Build(SmoothingFigureModel model)
|
||||
{
|
||||
if (model == null) throw new ArgumentNullException(nameof(model));
|
||||
SmoothingFigureSeries raw = Find(model, "raw");
|
||||
SmoothingFigureSeries bSpline = Find(model, "bspline");
|
||||
SmoothingFigureSeries bezier = Find(model, "bezier");
|
||||
SmoothingFigureSeries quintic = Find(model, "quintic");
|
||||
var figures = new List<SmoothingFigureDefinition>
|
||||
{
|
||||
BuildOverhead(SmoothingFigureKind.CoarsePathOverview, "01-coarse-path-overview", "Hybrid A* 原始粗路径", model, true, View(raw, 1d)),
|
||||
BuildOverhead(SmoothingFigureKind.AllPathsComparison, "02-all-paths-comparison", "路径平滑结果对比", model, false, View(raw, 1d), View(bSpline, 1d), View(bezier, 1d), View(quintic, 1d)),
|
||||
BuildOverhead(SmoothingFigureKind.CubicBSplineOverview, "03-cubic-bspline-overview", "三次 B 样条平滑", model, true, View(raw, 0.28d), View(bSpline, 1d)),
|
||||
BuildOverhead(SmoothingFigureKind.LocalCubicBezierOverview, "04-local-cubic-bezier-overview", "局部三次 Bézier 平滑", model, true, View(raw, 0.28d), View(bezier, 1d)),
|
||||
BuildOverhead(SmoothingFigureKind.PiecewiseQuinticOverview, "05-piecewise-quintic-overview", "分段五次平滑", model, true, View(raw, 0.28d), View(quintic, 1d)),
|
||||
BuildCurvature(model, View(raw, 1d), View(bSpline, 1d), View(bezier, 1d), View(quintic, 1d)),
|
||||
};
|
||||
return new SmoothingFigureSet(figures);
|
||||
}
|
||||
|
||||
private static SmoothingFigureDefinition BuildOverhead(SmoothingFigureKind kind, string stem, string title, SmoothingFigureModel model, bool mapContext, params SmoothingFigureSeriesView[] series)
|
||||
{
|
||||
Bounds bounds = CalculateWorldBounds(model, mapContext, series);
|
||||
return new SmoothingFigureDefinition(kind, stem, title, model, mapContext, series,
|
||||
bounds.XMin, bounds.XMax, bounds.YMin, bounds.YMax,
|
||||
BuildTicks(bounds.XMin, bounds.XMax), BuildTicks(bounds.YMin, bounds.YMax),
|
||||
1d, -1d, 1d, Array.Empty<double>(), Array.Empty<double>());
|
||||
}
|
||||
|
||||
private static SmoothingFigureDefinition BuildCurvature(SmoothingFigureModel model, params SmoothingFigureSeriesView[] series)
|
||||
{
|
||||
double arcMaximum = 0d;
|
||||
double curvatureMinimum = 0d;
|
||||
double curvatureMaximum = 0d;
|
||||
for (int viewIndex = 0; viewIndex < series.Length; viewIndex++)
|
||||
{
|
||||
IReadOnlyList<SmoothingFigurePoint> points = series[viewIndex].Series.Points;
|
||||
for (int pointIndex = 0; pointIndex < points.Count; pointIndex++)
|
||||
{
|
||||
SmoothingFigurePoint point = points[pointIndex];
|
||||
if (point.ArcLength > arcMaximum) arcMaximum = point.ArcLength;
|
||||
if (point.VehicleCurvature < curvatureMinimum) curvatureMinimum = point.VehicleCurvature;
|
||||
if (point.VehicleCurvature > curvatureMaximum) curvatureMaximum = point.VehicleCurvature;
|
||||
}
|
||||
}
|
||||
arcMaximum = ExpandMaximum(arcMaximum, MinimumExtentMeters);
|
||||
ExpandRange(ref curvatureMinimum, ref curvatureMaximum, MinimumExtentMeters);
|
||||
return new SmoothingFigureDefinition(SmoothingFigureKind.CurvatureComparison, "06-curvature-comparison", "车辆曲率对比", model, false, series,
|
||||
0d, 1d, 0d, 1d, Array.Empty<double>(), Array.Empty<double>(),
|
||||
arcMaximum, curvatureMinimum, curvatureMaximum, BuildTicks(0d, arcMaximum), BuildTicks(curvatureMinimum, curvatureMaximum));
|
||||
}
|
||||
|
||||
private static Bounds CalculateWorldBounds(SmoothingFigureModel model, bool includesEndpoints, IReadOnlyList<SmoothingFigureSeriesView> series)
|
||||
{
|
||||
bool hasPoint = false;
|
||||
double xMin = 0d, xMax = 0d, yMin = 0d, yMax = 0d;
|
||||
for (int viewIndex = 0; viewIndex < series.Count; viewIndex++)
|
||||
{
|
||||
IReadOnlyList<SmoothingFigurePoint> points = series[viewIndex].Series.Points;
|
||||
for (int pointIndex = 0; pointIndex < points.Count; pointIndex++) Include(points[pointIndex].X, points[pointIndex].Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
|
||||
}
|
||||
if (includesEndpoints)
|
||||
{
|
||||
Include(model.Start.X, model.Start.Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
|
||||
Include(model.Goal.X, model.Goal.Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
|
||||
}
|
||||
if (!hasPoint)
|
||||
{
|
||||
xMin = model.WorldXMinMeters; xMax = model.WorldXMaxMeters;
|
||||
yMin = model.WorldYMinMeters; yMax = model.WorldYMaxMeters;
|
||||
}
|
||||
ExpandRange(ref xMin, ref xMax, MinimumExtentMeters);
|
||||
ExpandRange(ref yMin, ref yMax, MinimumExtentMeters);
|
||||
double xPadding = (xMax - xMin) * PaddingFraction;
|
||||
double yPadding = (yMax - yMin) * PaddingFraction;
|
||||
xMin -= xPadding; xMax += xPadding; yMin -= yPadding; yMax += yPadding;
|
||||
double desiredAspect = 400d / 245d;
|
||||
double width = xMax - xMin;
|
||||
double height = yMax - yMin;
|
||||
if (width / height < desiredAspect)
|
||||
{
|
||||
double halfWidth = height * desiredAspect / 2d;
|
||||
double center = (xMin + xMax) / 2d;
|
||||
xMin = center - halfWidth; xMax = center + halfWidth;
|
||||
}
|
||||
else
|
||||
{
|
||||
double halfHeight = width / desiredAspect / 2d;
|
||||
double center = (yMin + yMax) / 2d;
|
||||
yMin = center - halfHeight; yMax = center + halfHeight;
|
||||
}
|
||||
return new Bounds(xMin, xMax, yMin, yMax);
|
||||
}
|
||||
|
||||
private static void Include(double x, double y, ref bool hasPoint, ref double xMin, ref double xMax, ref double yMin, ref double yMax)
|
||||
{
|
||||
if (!hasPoint) { hasPoint = true; xMin = xMax = x; yMin = yMax = y; return; }
|
||||
if (x < xMin) xMin = x;
|
||||
if (x > xMax) xMax = x;
|
||||
if (y < yMin) yMin = y;
|
||||
if (y > yMax) yMax = y;
|
||||
}
|
||||
|
||||
private static void ExpandRange(ref double minimum, ref double maximum, double minimumExtent)
|
||||
{
|
||||
double extent = maximum - minimum;
|
||||
if (extent >= minimumExtent) return;
|
||||
double center = (minimum + maximum) / 2d;
|
||||
minimum = center - minimumExtent / 2d;
|
||||
maximum = center + minimumExtent / 2d;
|
||||
}
|
||||
|
||||
private static double ExpandMaximum(double value, double minimum) { return value < minimum ? minimum : value * (1d + PaddingFraction); }
|
||||
|
||||
private static IReadOnlyList<double> BuildTicks(double minimum, double maximum)
|
||||
{
|
||||
double span = maximum - minimum;
|
||||
if (span <= 0d) return new[] { minimum, maximum };
|
||||
double roughStep = span / 5d;
|
||||
double magnitude = Math.Pow(10d, Math.Floor(Math.Log10(roughStep)));
|
||||
double normalized = roughStep / magnitude;
|
||||
double nice = normalized <= 1d ? 1d : (normalized <= 2d ? 2d : (normalized <= 5d ? 5d : 10d));
|
||||
double step = nice * magnitude;
|
||||
var ticks = new List<double>();
|
||||
double first = Math.Ceiling(minimum / step) * step;
|
||||
for (double value = first; value <= maximum + step * 0.001d; value += step) ticks.Add(value);
|
||||
if (ticks.Count < 2) { ticks.Clear(); ticks.Add(minimum); ticks.Add(maximum); }
|
||||
return new ReadOnlyCollection<double>(ticks);
|
||||
}
|
||||
|
||||
private static SmoothingFigureSeries Find(SmoothingFigureModel model, string key)
|
||||
{
|
||||
for (int index = 0; index < model.Series.Count; index++) if (model.Series[index].Key == key) return model.Series[index];
|
||||
throw new InvalidOperationException("比较图形模型缺少路径序列: " + key);
|
||||
}
|
||||
|
||||
private static SmoothingFigureSeriesView View(SmoothingFigureSeries series, double opacity) { return new SmoothingFigureSeriesView(series, opacity); }
|
||||
|
||||
private readonly struct Bounds
|
||||
{
|
||||
public Bounds(double xMin, double xMax, double yMin, double yMax) { XMin = xMin; XMax = xMax; YMin = yMin; YMax = yMax; }
|
||||
public double XMin { get; }
|
||||
public double XMax { get; }
|
||||
public double YMin { get; }
|
||||
public double YMax { get; }
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>六张图的稳定顺序。</summary>
|
||||
public sealed class SmoothingFigureSet
|
||||
{
|
||||
internal SmoothingFigureSet(IReadOnlyList<SmoothingFigureDefinition> figures)
|
||||
{
|
||||
var copy = new List<SmoothingFigureDefinition>(figures == null ? 0 : figures.Count);
|
||||
if (figures != null) for (int index = 0; index < figures.Count; index++) copy.Add(figures[index]);
|
||||
Figures = new ReadOnlyCollection<SmoothingFigureDefinition>(copy);
|
||||
}
|
||||
|
||||
public IReadOnlyList<SmoothingFigureDefinition> Figures { get; }
|
||||
}
|
||||
@@ -0,0 +1,140 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Drawing.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>按准确族名解析报告所需字体，并提供混合中英文文本的共同基线度量。</summary>
|
||||
public sealed class SmoothingFontResolver
|
||||
{
|
||||
public bool TryResolve(
|
||||
string chineseFamilyName,
|
||||
string latinFamilyName,
|
||||
out SmoothingFontResolution resolution,
|
||||
out string reason)
|
||||
{
|
||||
resolution = null;
|
||||
reason = string.Empty;
|
||||
if (string.IsNullOrWhiteSpace(chineseFamilyName) || string.IsNullOrWhiteSpace(latinFamilyName))
|
||||
{
|
||||
reason = "中文和拉丁字体族名均不能为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var collection = new InstalledFontCollection();
|
||||
FontFamily chinese = FindExact(collection.Families, chineseFamilyName);
|
||||
FontFamily latin = FindExact(collection.Families, latinFamilyName);
|
||||
if (chinese == null || latin == null)
|
||||
{
|
||||
collection.Dispose();
|
||||
reason = "未安装报告所需的精确字体族：" + (chinese == null ? chineseFamilyName : latinFamilyName) + "。";
|
||||
return false;
|
||||
}
|
||||
|
||||
resolution = new SmoothingFontResolution(collection, chinese, latin, chineseFamilyName, latinFamilyName);
|
||||
return true;
|
||||
}
|
||||
|
||||
public RectangleF MeasureMixedText(SmoothingFontResolution resolution, string text, float points)
|
||||
{
|
||||
if (resolution == null) throw new ArgumentNullException(nameof(resolution));
|
||||
if (string.IsNullOrEmpty(text) || points <= 0f) return RectangleF.Empty;
|
||||
using (var bitmap = new Bitmap(1, 1))
|
||||
using (Graphics graphics = Graphics.FromImage(bitmap))
|
||||
using (var format = (StringFormat)StringFormat.GenericTypographic.Clone())
|
||||
{
|
||||
format.FormatFlags |= StringFormatFlags.MeasureTrailingSpaces;
|
||||
float width = 0f, height = 0f;
|
||||
foreach (TextRun run in SplitRuns(text))
|
||||
{
|
||||
using (Font font = resolution.CreateFont(run.IsChinese ? resolution.ChineseFamily : resolution.LatinFamily, points))
|
||||
{
|
||||
SizeF size = graphics.MeasureString(run.Text, font, PointF.Empty, format);
|
||||
width += size.Width;
|
||||
height = Math.Max(height, size.Height);
|
||||
}
|
||||
}
|
||||
return new RectangleF(0f, 0f, width, height);
|
||||
}
|
||||
}
|
||||
|
||||
internal static IReadOnlyList<TextRun> SplitRuns(string text)
|
||||
{
|
||||
var runs = new List<TextRun>();
|
||||
if (string.IsNullOrEmpty(text)) return runs;
|
||||
int start = 0;
|
||||
bool isChinese = IsChinese(text[0]);
|
||||
for (int index = 1; index < text.Length; index++)
|
||||
{
|
||||
bool currentIsChinese = IsChinese(text[index]);
|
||||
if (currentIsChinese == isChinese) continue;
|
||||
runs.Add(new TextRun(text.Substring(start, index - start), isChinese));
|
||||
start = index;
|
||||
isChinese = currentIsChinese;
|
||||
}
|
||||
runs.Add(new TextRun(text.Substring(start), isChinese));
|
||||
return runs;
|
||||
}
|
||||
|
||||
private static FontFamily FindExact(IReadOnlyList<FontFamily> families, string name)
|
||||
{
|
||||
for (int index = 0; index < families.Count; index++)
|
||||
{
|
||||
FontFamily family = families[index];
|
||||
if (string.Equals(family.Name, name, StringComparison.Ordinal) ||
|
||||
string.Equals(family.GetName(1033), name, StringComparison.Ordinal)) return family;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
private static bool IsChinese(char value)
|
||||
{
|
||||
return (value >= 0x3400 && value <= 0x4dbf) || (value >= 0x4e00 && value <= 0x9fff) ||
|
||||
(value >= 0xf900 && value <= 0xfaff) || value == 0x3002 || value == 0xff0c || value == 0xff1a;
|
||||
}
|
||||
|
||||
internal sealed class TextRun
|
||||
{
|
||||
public TextRun(string text, bool isChinese) { Text = text; IsChinese = isChinese; }
|
||||
public string Text { get; }
|
||||
public bool IsChinese { get; }
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>一次报告导出所使用的精确字体族；释放时同时释放字体集合。</summary>
|
||||
public sealed class SmoothingFontResolution : IDisposable
|
||||
{
|
||||
private readonly InstalledFontCollection _collection;
|
||||
private readonly string _chineseFamilyName;
|
||||
private readonly string _latinFamilyName;
|
||||
|
||||
internal SmoothingFontResolution(
|
||||
InstalledFontCollection collection,
|
||||
FontFamily chineseFamily,
|
||||
FontFamily latinFamily,
|
||||
string chineseFamilyName,
|
||||
string latinFamilyName)
|
||||
{
|
||||
_collection = collection ?? throw new ArgumentNullException(nameof(collection));
|
||||
ChineseFamily = chineseFamily ?? throw new ArgumentNullException(nameof(chineseFamily));
|
||||
LatinFamily = latinFamily ?? throw new ArgumentNullException(nameof(latinFamily));
|
||||
_chineseFamilyName = chineseFamilyName ?? throw new ArgumentNullException(nameof(chineseFamilyName));
|
||||
_latinFamilyName = latinFamilyName ?? throw new ArgumentNullException(nameof(latinFamilyName));
|
||||
}
|
||||
|
||||
public string ChineseFamilyName => _chineseFamilyName;
|
||||
public string LatinFamilyName => _latinFamilyName;
|
||||
internal FontFamily ChineseFamily { get; }
|
||||
internal FontFamily LatinFamily { get; }
|
||||
|
||||
internal Font CreateFont(FontFamily family, float points)
|
||||
{
|
||||
return new Font(family, points, FontStyle.Regular, GraphicsUnit.Point);
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
_collection.Dispose();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,287 @@
|
||||
using System;
|
||||
using System.Drawing;
|
||||
using System.Drawing.Drawing2D;
|
||||
using System.Drawing.Imaging;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Runtime.InteropServices;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>仅 Windows 运行时使用 GDI+ 将每张共享图形定义渲染为 600 dpi PNG；轨迹仅绘制离散点。</summary>
|
||||
public sealed class SmoothingPngRenderer
|
||||
{
|
||||
public const int WidthPixels = 4296;
|
||||
public const int HeightPixels = 3120;
|
||||
public const uint PixelsPerMeter = 23622u;
|
||||
private const float PixelsPerPoint = 600f / 72f;
|
||||
|
||||
public byte[] Render(SmoothingFigureModel model, SmoothingFontResolution fonts)
|
||||
{
|
||||
if (model == null) throw new ArgumentNullException(nameof(model));
|
||||
return Render(new SmoothingFigureSetBuilder().Build(model).Figures[1], fonts);
|
||||
}
|
||||
|
||||
public byte[] Render(SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
if (figure == null) throw new ArgumentNullException(nameof(figure));
|
||||
if (fonts == null) throw new ArgumentNullException(nameof(fonts));
|
||||
using (var bitmap = new Bitmap(WidthPixels, HeightPixels, PixelFormat.Format32bppArgb))
|
||||
{
|
||||
bitmap.SetResolution(600f, 600f);
|
||||
using (Graphics graphics = Graphics.FromImage(bitmap))
|
||||
{
|
||||
graphics.Clear(Color.White);
|
||||
graphics.SmoothingMode = SmoothingMode.AntiAlias;
|
||||
graphics.InterpolationMode = InterpolationMode.HighQualityBicubic;
|
||||
graphics.PixelOffsetMode = PixelOffsetMode.HighQuality;
|
||||
DrawFigure(graphics, figure, fonts);
|
||||
}
|
||||
return Encode(bitmap);
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawFigure(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
DrawMixedText(graphics, fonts, figure.Title + " — " + figure.Model.ScenarioLabel, PointX(figure.PlotXPoints), PointY(12d), 12f, Color.Black);
|
||||
if (figure.IsCurvatureFigure) DrawCurvatureAxes(graphics, figure, fonts); else DrawOverheadAxes(graphics, figure, fonts);
|
||||
GraphicsState state = graphics.Save();
|
||||
graphics.SetClip(new RectangleF(PointX(figure.PlotXPoints), PointY(figure.PlotYPoints), PointX(figure.PlotWidthPoints), PointY(figure.PlotHeightPoints)));
|
||||
if (!figure.IsCurvatureFigure && figure.ShowsMapContext) DrawObstacles(graphics, figure);
|
||||
if (figure.IsCurvatureFigure) DrawCurvaturePoints(graphics, figure); else DrawPathPoints(graphics, figure);
|
||||
if (!figure.IsCurvatureFigure && figure.ShowsMapContext) DrawStartGoal(graphics, figure);
|
||||
graphics.Restore(state);
|
||||
DrawLegend(graphics, figure, fonts);
|
||||
}
|
||||
|
||||
private static void DrawOverheadAxes(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
DrawPlotFrame(graphics, figure);
|
||||
using (var grid = new Pen(Color.FromArgb(230, 230, 230), 0.5f * PixelsPerPoint))
|
||||
{
|
||||
for (int index = 0; index < figure.XTicks.Count; index++)
|
||||
{
|
||||
float x = WorldX(figure, figure.XTicks[index]);
|
||||
graphics.DrawLine(grid, x, PointY(figure.PlotYPoints), x, PointY(figure.PlotYPoints + figure.PlotHeightPoints));
|
||||
DrawMixedText(graphics, fonts, Number(figure.XTicks[index]), x - 10f * PixelsPerPoint, PointY(figure.PlotYPoints + figure.PlotHeightPoints + 5d), 8f, Color.Black);
|
||||
}
|
||||
for (int index = 0; index < figure.YTicks.Count; index++)
|
||||
{
|
||||
float y = WorldY(figure, figure.YTicks[index]);
|
||||
graphics.DrawLine(grid, PointX(figure.PlotXPoints), y, PointX(figure.PlotXPoints + figure.PlotWidthPoints), y);
|
||||
DrawMixedText(graphics, fonts, Number(figure.YTicks[index]), PointX(figure.PlotXPoints - 34d), y - 5f * PixelsPerPoint, 8f, Color.Black);
|
||||
}
|
||||
}
|
||||
DrawMixedText(graphics, fonts, "X (m)", PointX(figure.PlotXPoints + figure.PlotWidthPoints / 2d - 11d), PointY(figure.PlotYPoints + figure.PlotHeightPoints + 20d), 10f, Color.Black);
|
||||
DrawVerticalText(graphics, fonts, "Y (m)", PointX(12d), PointY(figure.PlotYPoints + figure.PlotHeightPoints / 2d + 17d), 10f, Color.Black);
|
||||
}
|
||||
|
||||
private static void DrawCurvatureAxes(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
DrawPlotFrame(graphics, figure);
|
||||
using (var grid = new Pen(Color.FromArgb(230, 230, 230), 0.5f * PixelsPerPoint))
|
||||
{
|
||||
for (int index = 0; index < figure.CurvatureArcLengthTicks.Count; index++)
|
||||
{
|
||||
float x = CurvatureX(figure, figure.CurvatureArcLengthTicks[index]);
|
||||
graphics.DrawLine(grid, x, PointY(figure.PlotYPoints), x, PointY(figure.PlotYPoints + figure.PlotHeightPoints));
|
||||
DrawMixedText(graphics, fonts, Number(figure.CurvatureArcLengthTicks[index]), x - 10f * PixelsPerPoint, PointY(figure.PlotYPoints + figure.PlotHeightPoints + 5d), 8f, Color.Black);
|
||||
}
|
||||
for (int index = 0; index < figure.CurvatureTicks.Count; index++)
|
||||
{
|
||||
float y = CurvatureY(figure, figure.CurvatureTicks[index]);
|
||||
graphics.DrawLine(grid, PointX(figure.PlotXPoints), y, PointX(figure.PlotXPoints + figure.PlotWidthPoints), y);
|
||||
DrawMixedText(graphics, fonts, Number(figure.CurvatureTicks[index]), PointX(figure.PlotXPoints - 34d), y - 5f * PixelsPerPoint, 8f, Color.Black);
|
||||
}
|
||||
}
|
||||
if (figure.CurvatureMinimumPerMeter < 0d && figure.CurvatureMaximumPerMeter > 0d)
|
||||
{
|
||||
float y = CurvatureY(figure, 0d);
|
||||
using (var zero = new Pen(Color.FromArgb(77, 77, 77), 0.65f * PixelsPerPoint))
|
||||
graphics.DrawLine(zero, PointX(figure.PlotXPoints), y, PointX(figure.PlotXPoints + figure.PlotWidthPoints), y);
|
||||
}
|
||||
DrawMixedText(graphics, fonts, "s (m)", PointX(figure.PlotXPoints + figure.PlotWidthPoints / 2d - 10d), PointY(figure.PlotYPoints + figure.PlotHeightPoints + 20d), 10f, Color.Black);
|
||||
DrawVerticalText(graphics, fonts, "κ (m⁻¹)", PointX(12d), PointY(figure.PlotYPoints + figure.PlotHeightPoints / 2d + 22d), 10f, Color.Black);
|
||||
}
|
||||
|
||||
private static void DrawPlotFrame(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
using (var border = new Pen(Color.Black, 0.75f * PixelsPerPoint))
|
||||
graphics.DrawRectangle(border, PointX(figure.PlotXPoints), PointY(figure.PlotYPoints), PointX(figure.PlotWidthPoints), PointY(figure.PlotHeightPoints));
|
||||
}
|
||||
|
||||
private static void DrawObstacles(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
using (var fill = new SolidBrush(Color.FromArgb(217, 217, 217)))
|
||||
using (var outline = new Pen(Color.FromArgb(128, 128, 128), 0.35f * PixelsPerPoint))
|
||||
{
|
||||
for (int index = 0; index < figure.Model.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFigureObstacle obstacle = figure.Model.Obstacles[index];
|
||||
float x = WorldX(figure, obstacle.X);
|
||||
float y = WorldY(figure, obstacle.Y + obstacle.Height);
|
||||
float width = (float)(obstacle.Width * figure.WorldScalePointsPerMeter * PixelsPerPoint);
|
||||
float height = (float)(obstacle.Height * figure.WorldScalePointsPerMeter * PixelsPerPoint);
|
||||
graphics.FillRectangle(fill, x, y, width, height);
|
||||
graphics.DrawRectangle(outline, x, y, width, height);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawPathPoints(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++)
|
||||
{
|
||||
SmoothingFigureSeriesView view = figure.Series[viewIndex];
|
||||
DrawPoints(graphics, view, point => new PointF(WorldX(figure, point.X), WorldY(figure, point.Y)));
|
||||
DrawViolations(graphics, figure, view);
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawCurvaturePoints(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++)
|
||||
{
|
||||
SmoothingFigureSeriesView view = figure.Series[viewIndex];
|
||||
DrawPoints(graphics, view, point => new PointF(CurvatureX(figure, point.ArcLength), CurvatureY(figure, point.VehicleCurvature)));
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawPoints(Graphics graphics, SmoothingFigureSeriesView view, Func<SmoothingFigurePoint, PointF> transform)
|
||||
{
|
||||
if (!view.Series.IsCurveVisible) return;
|
||||
using (var fill = new SolidBrush(WithOpacity(ColorFromHex(view.Series.Color), view.Opacity)))
|
||||
{
|
||||
float radius = 1.35f * PixelsPerPoint;
|
||||
for (int index = 0; index < view.Series.Points.Count; index++)
|
||||
{
|
||||
PointF point = transform(view.Series.Points[index]);
|
||||
graphics.FillEllipse(fill, point.X - radius, point.Y - radius, radius * 2f, radius * 2f);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawViolations(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFigureSeriesView view)
|
||||
{
|
||||
if (view.Series.ViolationMarkers.Count == 0) return;
|
||||
using (var marker = new Pen(ColorFromHex(view.Series.Color), 1.1f * PixelsPerPoint))
|
||||
{
|
||||
float radius = 3f * PixelsPerPoint;
|
||||
for (int index = 0; index < view.Series.ViolationMarkers.Count; index++)
|
||||
{
|
||||
SmoothingFigurePoint point = view.Series.ViolationMarkers[index];
|
||||
float x = WorldX(figure, point.X), y = WorldY(figure, point.Y);
|
||||
graphics.DrawLine(marker, x - radius, y - radius, x + radius, y + radius);
|
||||
graphics.DrawLine(marker, x - radius, y + radius, x + radius, y - radius);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawStartGoal(Graphics graphics, SmoothingFigureDefinition figure)
|
||||
{
|
||||
float startX = WorldX(figure, figure.Model.Start.X), startY = WorldY(figure, figure.Model.Start.Y), radius = 3.2f * PixelsPerPoint;
|
||||
using (var startBrush = new SolidBrush(Color.FromArgb(240, 228, 66)))
|
||||
using (var border = new Pen(Color.Black, 0.8f * PixelsPerPoint))
|
||||
using (var goalBrush = new SolidBrush(ColorFromHex(IeeeFigureStyle.LimitColor)))
|
||||
{
|
||||
graphics.FillEllipse(startBrush, startX - radius, startY - radius, radius * 2f, radius * 2f);
|
||||
graphics.DrawEllipse(border, startX - radius, startY - radius, radius * 2f, radius * 2f);
|
||||
float goalX = WorldX(figure, figure.Model.Goal.X), goalY = WorldY(figure, figure.Model.Goal.Y), diamond = 4f * PixelsPerPoint;
|
||||
PointF[] points = { new PointF(goalX, goalY - diamond), new PointF(goalX + diamond, goalY), new PointF(goalX, goalY + diamond), new PointF(goalX - diamond, goalY) };
|
||||
graphics.FillPolygon(goalBrush, points);
|
||||
graphics.DrawPolygon(border, points);
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawLegend(Graphics graphics, SmoothingFigureDefinition figure, SmoothingFontResolution fonts)
|
||||
{
|
||||
const double columnWidth = 198d;
|
||||
for (int index = 0; index < figure.LegendEntries.Count; index++)
|
||||
{
|
||||
SmoothingFigureLegendEntry entry = figure.LegendEntries[index];
|
||||
int column = index % 2, row = index / 2;
|
||||
float x = PointX(figure.PlotXPoints + column * columnWidth);
|
||||
float y = PointY(figure.LegendYPoints + row * 15d - 3d);
|
||||
using (var fill = new SolidBrush(ColorFromHex(entry.Color))) graphics.FillEllipse(fill, x, y - 2.2f * PixelsPerPoint, 4.4f * PixelsPerPoint, 4.4f * PixelsPerPoint);
|
||||
DrawMixedText(graphics, fonts, entry.Label, x + 10f * PixelsPerPoint, y - 5f * PixelsPerPoint, 8.5f, Color.Black);
|
||||
}
|
||||
}
|
||||
|
||||
private static void DrawVerticalText(Graphics graphics, SmoothingFontResolution fonts, string text, float x, float y, float points, Color color)
|
||||
{
|
||||
GraphicsState state = graphics.Save();
|
||||
graphics.TranslateTransform(x, y);
|
||||
graphics.RotateTransform(-90f);
|
||||
DrawMixedText(graphics, fonts, text, 0f, 0f, points, color);
|
||||
graphics.Restore(state);
|
||||
}
|
||||
|
||||
private static void DrawMixedText(Graphics graphics, SmoothingFontResolution fonts, string text, float x, float top, float points, Color color)
|
||||
{
|
||||
var runs = SmoothingFontResolver.SplitRuns(text);
|
||||
float emPixels = points * PixelsPerPoint, maxAscent = 0f;
|
||||
for (int index = 0; index < runs.Count; index++)
|
||||
{
|
||||
FontFamily family = runs[index].IsChinese ? fonts.ChineseFamily : fonts.LatinFamily;
|
||||
maxAscent = Math.Max(maxAscent, family.GetCellAscent(FontStyle.Regular) * emPixels / family.GetEmHeight(FontStyle.Regular));
|
||||
}
|
||||
float baseline = top + maxAscent;
|
||||
using (var brush = new SolidBrush(color))
|
||||
using (var format = (StringFormat)StringFormat.GenericTypographic.Clone())
|
||||
{
|
||||
format.FormatFlags |= StringFormatFlags.MeasureTrailingSpaces;
|
||||
for (int index = 0; index < runs.Count; index++)
|
||||
{
|
||||
FontFamily family = runs[index].IsChinese ? fonts.ChineseFamily : fonts.LatinFamily;
|
||||
using (Font font = fonts.CreateFont(family, points))
|
||||
{
|
||||
float runTop = baseline - family.GetCellAscent(FontStyle.Regular) * emPixels / family.GetEmHeight(FontStyle.Regular);
|
||||
graphics.DrawString(runs[index].Text, font, brush, x, runTop, format);
|
||||
x += graphics.MeasureString(runs[index].Text, font, PointF.Empty, format).Width;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static byte[] Encode(Bitmap bitmap)
|
||||
{
|
||||
Rectangle rectangle = new Rectangle(0, 0, bitmap.Width, bitmap.Height);
|
||||
BitmapData data = bitmap.LockBits(rectangle, ImageLockMode.ReadOnly, PixelFormat.Format32bppArgb);
|
||||
try
|
||||
{
|
||||
int sourceLength = checked(data.Stride * bitmap.Height);
|
||||
var source = new byte[sourceLength];
|
||||
Marshal.Copy(data.Scan0, source, 0, source.Length);
|
||||
var rgba = new byte[checked(bitmap.Width * bitmap.Height * 4)];
|
||||
for (int y = 0; y < bitmap.Height; y++)
|
||||
{
|
||||
int sourceRow = y * data.Stride, outputRow = y * bitmap.Width * 4;
|
||||
for (int x = 0; x < bitmap.Width; x++)
|
||||
{
|
||||
int sourceOffset = sourceRow + x * 4, outputOffset = outputRow + x * 4;
|
||||
rgba[outputOffset] = source[sourceOffset + 2];
|
||||
rgba[outputOffset + 1] = source[sourceOffset + 1];
|
||||
rgba[outputOffset + 2] = source[sourceOffset];
|
||||
rgba[outputOffset + 3] = source[sourceOffset + 3];
|
||||
}
|
||||
}
|
||||
using (var output = new MemoryStream())
|
||||
{
|
||||
ValidatedPngWriter.Write(rgba, bitmap.Width, bitmap.Height, output, PixelsPerMeter);
|
||||
return output.ToArray();
|
||||
}
|
||||
}
|
||||
finally { bitmap.UnlockBits(data); }
|
||||
}
|
||||
|
||||
private static Color ColorFromHex(string value) { return ColorTranslator.FromHtml(value); }
|
||||
private static Color WithOpacity(Color color, double opacity) { return Color.FromArgb((int)Math.Round(255d * opacity), color.R, color.G, color.B); }
|
||||
private static float PointX(double points) { return (float)(points * PixelsPerPoint); }
|
||||
private static float PointY(double points) { return (float)(points * PixelsPerPoint); }
|
||||
private static float WorldX(SmoothingFigureDefinition figure, double x) { return PointX(figure.PlotXPoints + (x - figure.WorldXMinMeters) * figure.WorldScalePointsPerMeter); }
|
||||
private static float WorldY(SmoothingFigureDefinition figure, double y) { return PointY(figure.PlotYPoints + figure.PlotHeightPoints - (y - figure.WorldYMinMeters) * figure.WorldScalePointsPerMeter); }
|
||||
private static float CurvatureX(SmoothingFigureDefinition figure, double arcLength) { return PointX(figure.PlotXPoints + arcLength / figure.CurvatureArcLengthMaximumMeters * figure.PlotWidthPoints); }
|
||||
private static float CurvatureY(SmoothingFigureDefinition figure, double curvature) { return PointY(figure.PlotYPoints + figure.PlotHeightPoints - (curvature - figure.CurvatureMinimumPerMeter) / (figure.CurvatureMaximumPerMeter - figure.CurvatureMinimumPerMeter) * figure.PlotHeightPoints); }
|
||||
private static string Number(double value) { return value.ToString("0.###", CultureInfo.InvariantCulture); }
|
||||
}
|
||||
+11
@@ -0,0 +1,11 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>一次报告导出的共享图形模型、目标目录与精确字体要求。</summary>
|
||||
public sealed class SmoothingReportExportRequest
|
||||
{
|
||||
public SmoothingFigureModel Model { get; set; }
|
||||
public string OutputDirectory { get; set; }
|
||||
public string FileStem { get; set; }
|
||||
public string ChineseFontFamilyName { get; set; } = "SimSun";
|
||||
public string LatinFontFamilyName { get; set; } = "Times New Roman";
|
||||
}
|
||||
+49
@@ -0,0 +1,49 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>报告导出结果的稳定状态。</summary>
|
||||
public enum SmoothingReportExportStatus
|
||||
{
|
||||
Success,
|
||||
InvalidInput,
|
||||
FontUnavailable,
|
||||
Failed,
|
||||
}
|
||||
|
||||
/// <summary>六张 SVG、六张 PNG 与一个 CSV 的发布结果；失败时不返回部分输出路径。</summary>
|
||||
public sealed class SmoothingReportExportResult
|
||||
{
|
||||
internal SmoothingReportExportResult(SmoothingReportExportStatus status, string reason, IReadOnlyList<string> svgPaths, IReadOnlyList<string> pngPaths, string csvPath)
|
||||
{
|
||||
Status = status;
|
||||
Reason = reason ?? string.Empty;
|
||||
SvgPaths = Copy(svgPaths);
|
||||
PngPaths = Copy(pngPaths);
|
||||
CsvPath = csvPath ?? string.Empty;
|
||||
}
|
||||
|
||||
public SmoothingReportExportStatus Status { get; }
|
||||
public string Reason { get; }
|
||||
public IReadOnlyList<string> SvgPaths { get; }
|
||||
public IReadOnlyList<string> PngPaths { get; }
|
||||
public string CsvPath { get; }
|
||||
|
||||
internal static SmoothingReportExportResult Success(IReadOnlyList<string> svgPaths, IReadOnlyList<string> pngPaths, string csvPath)
|
||||
{
|
||||
return new SmoothingReportExportResult(SmoothingReportExportStatus.Success, string.Empty, svgPaths, pngPaths, csvPath);
|
||||
}
|
||||
|
||||
internal static SmoothingReportExportResult Failure(SmoothingReportExportStatus status, string reason)
|
||||
{
|
||||
return new SmoothingReportExportResult(status, reason, new string[0], new string[0], string.Empty);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<string> Copy(IReadOnlyList<string> source)
|
||||
{
|
||||
var copy = new List<string>(source == null ? 0 : source.Count);
|
||||
if (source != null) for (int index = 0; index < source.Count; index++) copy.Add(source[index] ?? string.Empty);
|
||||
return new ReadOnlyCollection<string>(copy);
|
||||
}
|
||||
}
|
||||
+130
@@ -0,0 +1,130 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>以同级临时文件生成六张 SVG、六张 PNG 和 CSV 后成组发布。</summary>
|
||||
public sealed class SmoothingReportExporter
|
||||
{
|
||||
private readonly SmoothingFontResolver _fontResolver = new SmoothingFontResolver();
|
||||
private readonly SmoothingFigureSetBuilder _figureSetBuilder = new SmoothingFigureSetBuilder();
|
||||
private readonly SmoothingSvgRenderer _svgRenderer = new SmoothingSvgRenderer();
|
||||
private readonly SmoothingPngRenderer _pngRenderer = new SmoothingPngRenderer();
|
||||
private readonly SmoothingCsvWriter _csvWriter = new SmoothingCsvWriter();
|
||||
|
||||
public SmoothingReportExportResult Export(SmoothingReportExportRequest request)
|
||||
{
|
||||
if (request == null || request.Model == null || string.IsNullOrWhiteSpace(request.OutputDirectory) || !IsFileStem(request.FileStem))
|
||||
return SmoothingReportExportResult.Failure(SmoothingReportExportStatus.InvalidInput, "报告模型、输出目录或文件名无效。");
|
||||
if (!_fontResolver.TryResolve(request.ChineseFontFamilyName, request.LatinFontFamilyName, out SmoothingFontResolution availableFonts, out string fontReason))
|
||||
return SmoothingReportExportResult.Failure(SmoothingReportExportStatus.FontUnavailable, fontReason);
|
||||
availableFonts.Dispose();
|
||||
|
||||
var pending = new List<PendingFile>();
|
||||
var svgPaths = new List<string>();
|
||||
var pngPaths = new List<string>();
|
||||
string csvPath = Path.Combine(request.OutputDirectory, request.FileStem + ".csv");
|
||||
try
|
||||
{
|
||||
SmoothingFigureSet figures = _figureSetBuilder.Build(request.Model);
|
||||
for (int index = 0; index < figures.Figures.Count; index++)
|
||||
{
|
||||
SmoothingFigureDefinition figure = figures.Figures[index];
|
||||
string svgPath = Path.Combine(request.OutputDirectory, figure.FileStem + ".svg");
|
||||
string pngPath = Path.Combine(request.OutputDirectory, figure.FileStem + ".png");
|
||||
svgPaths.Add(svgPath);
|
||||
pngPaths.Add(pngPath);
|
||||
pending.Add(new PendingFile(svgPath, Encoding.UTF8.GetBytes(_svgRenderer.Render(figure))));
|
||||
if (!_fontResolver.TryResolve(request.ChineseFontFamilyName, request.LatinFontFamilyName, out SmoothingFontResolution renderFonts, out fontReason))
|
||||
throw new InvalidOperationException(fontReason);
|
||||
byte[] png;
|
||||
using (renderFonts) png = _pngRenderer.Render(figure, renderFonts);
|
||||
pending.Add(new PendingFile(pngPath, png));
|
||||
}
|
||||
pending.Add(new PendingFile(csvPath, _csvWriter.Write(request.Model)));
|
||||
Directory.CreateDirectory(request.OutputDirectory);
|
||||
for (int index = 0; index < pending.Count; index++) File.WriteAllBytes(pending[index].TemporaryPath, pending[index].Content);
|
||||
PublishAll(pending);
|
||||
DeleteIfExists(Path.Combine(request.OutputDirectory, "comparison.svg"));
|
||||
DeleteIfExists(Path.Combine(request.OutputDirectory, "comparison.png"));
|
||||
return SmoothingReportExportResult.Success(svgPaths, pngPaths, csvPath);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
RestorePublishedFiles(pending);
|
||||
return SmoothingReportExportResult.Failure(SmoothingReportExportStatus.Failed, exception.Message);
|
||||
}
|
||||
finally
|
||||
{
|
||||
for (int index = 0; index < pending.Count; index++)
|
||||
{
|
||||
DeleteIfExists(pending[index].TemporaryPath);
|
||||
DeleteIfExists(pending[index].BackupPath);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void PublishAll(IReadOnlyList<PendingFile> files)
|
||||
{
|
||||
string transaction = Guid.NewGuid().ToString("N");
|
||||
for (int index = 0; index < files.Count; index++)
|
||||
{
|
||||
PendingFile file = files[index];
|
||||
file.ExistedBeforePublish = File.Exists(file.FinalPath);
|
||||
file.BackupPath = file.ExistedBeforePublish ? file.FinalPath + ".backup-" + transaction : string.Empty;
|
||||
if (file.ExistedBeforePublish) File.Replace(file.TemporaryPath, file.FinalPath, file.BackupPath);
|
||||
else File.Move(file.TemporaryPath, file.FinalPath);
|
||||
file.Published = true;
|
||||
}
|
||||
for (int index = 0; index < files.Count; index++) DeleteIfExists(files[index].BackupPath);
|
||||
}
|
||||
|
||||
private static void RestorePublishedFiles(IReadOnlyList<PendingFile> files)
|
||||
{
|
||||
for (int index = files.Count - 1; index >= 0; index--)
|
||||
{
|
||||
PendingFile file = files[index];
|
||||
if (!file.Published) continue;
|
||||
try
|
||||
{
|
||||
if (file.ExistedBeforePublish && File.Exists(file.BackupPath))
|
||||
{
|
||||
if (File.Exists(file.FinalPath)) File.Replace(file.BackupPath, file.FinalPath, null);
|
||||
else File.Move(file.BackupPath, file.FinalPath);
|
||||
}
|
||||
else if (!file.ExistedBeforePublish) DeleteIfExists(file.FinalPath);
|
||||
}
|
||||
catch { }
|
||||
}
|
||||
}
|
||||
|
||||
private static bool IsFileStem(string value)
|
||||
{
|
||||
return !string.IsNullOrWhiteSpace(value) && value.IndexOfAny(Path.GetInvalidFileNameChars()) < 0 && value.IndexOf(Path.DirectorySeparatorChar) < 0 && value.IndexOf(Path.AltDirectorySeparatorChar) < 0;
|
||||
}
|
||||
|
||||
private static void DeleteIfExists(string path)
|
||||
{
|
||||
if (!string.IsNullOrEmpty(path) && File.Exists(path)) File.Delete(path);
|
||||
}
|
||||
|
||||
private sealed class PendingFile
|
||||
{
|
||||
public PendingFile(string finalPath, byte[] content)
|
||||
{
|
||||
FinalPath = finalPath;
|
||||
Content = content;
|
||||
TemporaryPath = finalPath + ".tmp";
|
||||
BackupPath = string.Empty;
|
||||
}
|
||||
|
||||
public string FinalPath { get; }
|
||||
public byte[] Content { get; }
|
||||
public string TemporaryPath { get; }
|
||||
public string BackupPath { get; set; }
|
||||
public bool ExistedBeforePublish { get; set; }
|
||||
public bool Published { get; set; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,189 @@
|
||||
using System;
|
||||
using System.Globalization;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
/// <summary>把单张共享图形定义渲染为 UTF-8 XML 可编辑 SVG；轨迹仅由离散点组成。</summary>
|
||||
public sealed class SmoothingSvgRenderer
|
||||
{
|
||||
public string Render(SmoothingFigureModel model)
|
||||
{
|
||||
if (model == null) throw new ArgumentNullException(nameof(model));
|
||||
return Render(new SmoothingFigureSetBuilder().Build(model).Figures[1]);
|
||||
}
|
||||
|
||||
public string Render(SmoothingFigureDefinition figure)
|
||||
{
|
||||
if (figure == null) throw new ArgumentNullException(nameof(figure));
|
||||
var svg = new StringBuilder();
|
||||
svg.Append("<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n<svg xmlns=\"http://www.w3.org/2000/svg\" width=\"")
|
||||
.Append(Number(figure.FigureWidthPoints)).Append("pt\" height=\"").Append(Number(figure.FigureHeightPoints))
|
||||
.Append("pt\" viewBox=\"0 0 ").Append(Number(figure.FigureWidthPoints)).Append(' ').Append(Number(figure.FigureHeightPoints)).Append("\">\n")
|
||||
.Append("<rect width=\"100%\" height=\"100%\" fill=\"#FFFFFF\"/>\n")
|
||||
.Append("<clipPath id=\"plot-clip\"><rect x=\"").Append(Number(figure.PlotXPoints)).Append("\" y=\"").Append(Number(figure.PlotYPoints))
|
||||
.Append("\" width=\"").Append(Number(figure.PlotWidthPoints)).Append("\" height=\"").Append(Number(figure.PlotHeightPoints)).Append("\"/></clipPath>\n");
|
||||
AppendTitle(svg, figure);
|
||||
if (figure.IsCurvatureFigure) AppendCurvatureAxes(svg, figure); else AppendOverheadAxes(svg, figure);
|
||||
svg.Append("<g clip-path=\"url(#plot-clip)\">\n");
|
||||
if (!figure.IsCurvatureFigure && figure.ShowsMapContext) AppendObstacles(svg, figure);
|
||||
if (figure.IsCurvatureFigure) AppendCurvaturePoints(svg, figure); else AppendPathPoints(svg, figure);
|
||||
if (!figure.IsCurvatureFigure && figure.ShowsMapContext) AppendStartGoal(svg, figure);
|
||||
svg.Append("</g>\n");
|
||||
AppendLegend(svg, figure);
|
||||
svg.Append("</svg>");
|
||||
return svg.ToString();
|
||||
}
|
||||
|
||||
private static void AppendTitle(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints)).Append("\" y=\"23\" font-size=\"12\"><tspan font-family=\"SimSun\">")
|
||||
.Append(Escape(figure.Title)).Append("</tspan><tspan font-family=\"Times New Roman\"> — ").Append(Escape(figure.Model.ScenarioLabel)).Append("</tspan></text>\n");
|
||||
}
|
||||
|
||||
private static void AppendOverheadAxes(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
AppendPlotFrame(svg, figure);
|
||||
for (int index = 0; index < figure.XTicks.Count; index++)
|
||||
{
|
||||
double x = WorldX(figure, figure.XTicks[index]);
|
||||
AppendGridLine(svg, x, figure.PlotYPoints, x, figure.PlotYPoints + figure.PlotHeightPoints);
|
||||
svg.Append("<text x=\"").Append(Number(x)).Append("\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints + 15d))
|
||||
.Append("\" text-anchor=\"middle\" font-family=\"Times New Roman\" font-size=\"8\">").Append(Number(figure.XTicks[index])).Append("</text>\n");
|
||||
}
|
||||
for (int index = 0; index < figure.YTicks.Count; index++)
|
||||
{
|
||||
double y = WorldY(figure, figure.YTicks[index]);
|
||||
AppendGridLine(svg, figure.PlotXPoints, y, figure.PlotXPoints + figure.PlotWidthPoints, y);
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints - 8d)).Append("\" y=\"").Append(Number(y + 3d))
|
||||
.Append("\" text-anchor=\"end\" font-family=\"Times New Roman\" font-size=\"8\">").Append(Number(figure.YTicks[index])).Append("</text>\n");
|
||||
}
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints + figure.PlotWidthPoints / 2d)).Append("\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints + 31d))
|
||||
.Append("\" text-anchor=\"middle\" font-family=\"Times New Roman\" font-size=\"10\">X (m)</text>\n")
|
||||
.Append("<text x=\"19\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints / 2d)).Append("\" text-anchor=\"middle\" transform=\"rotate(-90 19 ")
|
||||
.Append(Number(figure.PlotYPoints + figure.PlotHeightPoints / 2d)).Append(")\" font-family=\"Times New Roman\" font-size=\"10\">Y (m)</text>\n");
|
||||
}
|
||||
|
||||
private static void AppendCurvatureAxes(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
AppendPlotFrame(svg, figure);
|
||||
for (int index = 0; index < figure.CurvatureArcLengthTicks.Count; index++)
|
||||
{
|
||||
double x = CurvatureX(figure, figure.CurvatureArcLengthTicks[index]);
|
||||
AppendGridLine(svg, x, figure.PlotYPoints, x, figure.PlotYPoints + figure.PlotHeightPoints);
|
||||
svg.Append("<text x=\"").Append(Number(x)).Append("\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints + 15d))
|
||||
.Append("\" text-anchor=\"middle\" font-family=\"Times New Roman\" font-size=\"8\">").Append(Number(figure.CurvatureArcLengthTicks[index])).Append("</text>\n");
|
||||
}
|
||||
for (int index = 0; index < figure.CurvatureTicks.Count; index++)
|
||||
{
|
||||
double y = CurvatureY(figure, figure.CurvatureTicks[index]);
|
||||
AppendGridLine(svg, figure.PlotXPoints, y, figure.PlotXPoints + figure.PlotWidthPoints, y);
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints - 8d)).Append("\" y=\"").Append(Number(y + 3d))
|
||||
.Append("\" text-anchor=\"end\" font-family=\"Times New Roman\" font-size=\"8\">").Append(Number(figure.CurvatureTicks[index])).Append("</text>\n");
|
||||
}
|
||||
if (figure.CurvatureMinimumPerMeter < 0d && figure.CurvatureMaximumPerMeter > 0d)
|
||||
{
|
||||
double zero = CurvatureY(figure, 0d);
|
||||
svg.Append("<line x1=\"").Append(Number(figure.PlotXPoints)).Append("\" y1=\"").Append(Number(zero)).Append("\" x2=\"")
|
||||
.Append(Number(figure.PlotXPoints + figure.PlotWidthPoints)).Append("\" y2=\"").Append(Number(zero)).Append("\" stroke=\"#4D4D4D\" stroke-width=\"0.65\"/>\n");
|
||||
}
|
||||
svg.Append("<text x=\"").Append(Number(figure.PlotXPoints + figure.PlotWidthPoints / 2d)).Append("\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints + 31d))
|
||||
.Append("\" text-anchor=\"middle\" font-family=\"Times New Roman\" font-size=\"10\">s (m)</text>\n")
|
||||
.Append("<text x=\"19\" y=\"").Append(Number(figure.PlotYPoints + figure.PlotHeightPoints / 2d)).Append("\" text-anchor=\"middle\" transform=\"rotate(-90 19 ")
|
||||
.Append(Number(figure.PlotYPoints + figure.PlotHeightPoints / 2d)).Append(")\" font-family=\"Times New Roman\" font-size=\"10\">κ (m⁻¹)</text>\n");
|
||||
}
|
||||
|
||||
private static void AppendPlotFrame(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
svg.Append("<rect x=\"").Append(Number(figure.PlotXPoints)).Append("\" y=\"").Append(Number(figure.PlotYPoints)).Append("\" width=\"")
|
||||
.Append(Number(figure.PlotWidthPoints)).Append("\" height=\"").Append(Number(figure.PlotHeightPoints)).Append("\" fill=\"#FFFFFF\" stroke=\"#000000\" stroke-width=\"0.75\"/>\n");
|
||||
}
|
||||
|
||||
private static void AppendGridLine(StringBuilder svg, double x1, double y1, double x2, double y2)
|
||||
{
|
||||
svg.Append("<line x1=\"").Append(Number(x1)).Append("\" y1=\"").Append(Number(y1)).Append("\" x2=\"").Append(Number(x2)).Append("\" y2=\"")
|
||||
.Append(Number(y2)).Append("\" stroke=\"#E6E6E6\" stroke-width=\"0.5\"/>\n");
|
||||
}
|
||||
|
||||
private static void AppendObstacles(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int index = 0; index < figure.Model.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFigureObstacle obstacle = figure.Model.Obstacles[index];
|
||||
svg.Append("<rect class=\"obstacle\" x=\"").Append(Number(WorldX(figure, obstacle.X))).Append("\" y=\"")
|
||||
.Append(Number(WorldY(figure, obstacle.Y + obstacle.Height))).Append("\" width=\"").Append(Number(obstacle.Width * figure.WorldScalePointsPerMeter))
|
||||
.Append("\" height=\"").Append(Number(obstacle.Height * figure.WorldScalePointsPerMeter)).Append("\" fill=\"#D9D9D9\" stroke=\"#808080\" stroke-width=\"0.35\"/>\n");
|
||||
}
|
||||
}
|
||||
|
||||
private static void AppendPathPoints(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++)
|
||||
{
|
||||
SmoothingFigureSeriesView view = figure.Series[viewIndex];
|
||||
AppendPoints(svg, view, point => WorldX(figure, point.X), point => WorldY(figure, point.Y));
|
||||
for (int markerIndex = 0; markerIndex < view.Series.ViolationMarkers.Count; markerIndex++) AppendCross(svg, figure, view.Series.ViolationMarkers[markerIndex], view.Series.Color);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AppendCurvaturePoints(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
for (int viewIndex = 0; viewIndex < figure.Series.Count; viewIndex++)
|
||||
{
|
||||
SmoothingFigureSeriesView view = figure.Series[viewIndex];
|
||||
AppendPoints(svg, view, point => CurvatureX(figure, point.ArcLength), point => CurvatureY(figure, point.VehicleCurvature));
|
||||
}
|
||||
}
|
||||
|
||||
private static void AppendPoints(StringBuilder svg, SmoothingFigureSeriesView view, Func<SmoothingFigurePoint, double> x, Func<SmoothingFigurePoint, double> y)
|
||||
{
|
||||
if (!view.Series.IsCurveVisible) return;
|
||||
svg.Append("<g class=\"trajectory-series\" data-series=\"").Append(Escape(view.Series.Key)).Append("\" fill=\"").Append(view.Series.Color).Append("\" opacity=\"").Append(Number(view.Opacity)).Append("\">\n");
|
||||
for (int index = 0; index < view.Series.Points.Count; index++)
|
||||
{
|
||||
SmoothingFigurePoint point = view.Series.Points[index];
|
||||
svg.Append("<circle class=\"trajectory-point\" cx=\"").Append(Number(x(point))).Append("\" cy=\"").Append(Number(y(point))).Append("\" r=\"1.35\"/>\n");
|
||||
}
|
||||
svg.Append("</g>\n");
|
||||
}
|
||||
|
||||
private static void AppendCross(StringBuilder svg, SmoothingFigureDefinition figure, SmoothingFigurePoint point, string color)
|
||||
{
|
||||
double x = WorldX(figure, point.X), y = WorldY(figure, point.Y), radius = 3d;
|
||||
svg.Append("<g class=\"violation-cross\" stroke=\"").Append(color).Append("\" stroke-width=\"1.1\"><line x1=\"").Append(Number(x - radius)).Append("\" y1=\"")
|
||||
.Append(Number(y - radius)).Append("\" x2=\"").Append(Number(x + radius)).Append("\" y2=\"").Append(Number(y + radius)).Append("\"/><line x1=\"")
|
||||
.Append(Number(x - radius)).Append("\" y1=\"").Append(Number(y + radius)).Append("\" x2=\"").Append(Number(x + radius)).Append("\" y2=\"").Append(Number(y - radius)).Append("\"/></g>\n");
|
||||
}
|
||||
|
||||
private static void AppendStartGoal(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
svg.Append("<circle class=\"start-marker\" cx=\"").Append(Number(WorldX(figure, figure.Model.Start.X))).Append("\" cy=\"").Append(Number(WorldY(figure, figure.Model.Start.Y)))
|
||||
.Append("\" r=\"3.2\" fill=\"#F0E442\" stroke=\"#000000\" stroke-width=\"0.8\"/>\n");
|
||||
double x = WorldX(figure, figure.Model.Goal.X), y = WorldY(figure, figure.Model.Goal.Y), radius = 4d;
|
||||
svg.Append("<polygon class=\"goal-marker\" points=\"").Append(Number(x)).Append(',').Append(Number(y - radius)).Append(' ').Append(Number(x + radius)).Append(',').Append(Number(y)).Append(' ')
|
||||
.Append(Number(x)).Append(',').Append(Number(y + radius)).Append(' ').Append(Number(x - radius)).Append(',').Append(Number(y)).Append("\" fill=\"#CC79A7\" stroke=\"#000000\" stroke-width=\"0.8\"/>\n");
|
||||
}
|
||||
|
||||
private static void AppendLegend(StringBuilder svg, SmoothingFigureDefinition figure)
|
||||
{
|
||||
double columnWidth = 198d;
|
||||
for (int index = 0; index < figure.LegendEntries.Count; index++)
|
||||
{
|
||||
SmoothingFigureLegendEntry entry = figure.LegendEntries[index];
|
||||
int column = index % 2;
|
||||
int row = index / 2;
|
||||
double x = figure.PlotXPoints + column * columnWidth;
|
||||
double y = figure.LegendYPoints + row * 15d;
|
||||
svg.Append("<circle class=\"legend-point\" cx=\"").Append(Number(x + 3d)).Append("\" cy=\"").Append(Number(y - 3d)).Append("\" r=\"2.2\" fill=\"")
|
||||
.Append(entry.Color).Append("\"/>\n<text x=\"").Append(Number(x + 10d)).Append("\" y=\"").Append(Number(y)).Append("\" font-size=\"8.5\"><tspan font-family=\"SimSun\">")
|
||||
.Append(Escape(entry.Label)).Append("</tspan></text>\n");
|
||||
}
|
||||
}
|
||||
|
||||
private static double WorldX(SmoothingFigureDefinition figure, double x) { return figure.PlotXPoints + (x - figure.WorldXMinMeters) * figure.WorldScalePointsPerMeter; }
|
||||
private static double WorldY(SmoothingFigureDefinition figure, double y) { return figure.PlotYPoints + figure.PlotHeightPoints - (y - figure.WorldYMinMeters) * figure.WorldScalePointsPerMeter; }
|
||||
private static double CurvatureX(SmoothingFigureDefinition figure, double arcLength) { return figure.PlotXPoints + arcLength / figure.CurvatureArcLengthMaximumMeters * figure.PlotWidthPoints; }
|
||||
private static double CurvatureY(SmoothingFigureDefinition figure, double curvature) { return figure.PlotYPoints + figure.PlotHeightPoints - (curvature - figure.CurvatureMinimumPerMeter) / (figure.CurvatureMaximumPerMeter - figure.CurvatureMinimumPerMeter) * figure.PlotHeightPoints; }
|
||||
private static string Number(double value) { return value.ToString("0.###", CultureInfo.InvariantCulture); }
|
||||
private static string Escape(string value) { return (value ?? string.Empty).Replace("&", "&").Replace("<", "<").Replace(">", ">").Replace("\"", """).Replace("'", "'"); }
|
||||
}
|
||||
Reference in New Issue
Block a user