feat: add piecewise quintic path smoother
This commit is contained in:
梁薄云
@@ -0,0 +1,532 @@
|
||||
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;
|
||||
|
||||
/// <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));
|
||||
}
|
||||
|
||||
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 targetArcLength = startArcLength + knotSpacingMeters;
|
||||
while (targetArcLength < endArcLength)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!NumericGuard.IsFinite(targetArcLength))
|
||||
{
|
||||
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);
|
||||
targetArcLength += knotSpacingMeters;
|
||||
}
|
||||
|
||||
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 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,313 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Get-PropertyValue($Instance, [string]$Name) {
|
||||
$property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
|
||||
Assert-True ($null -ne $property) ("Missing property: " + $Name)
|
||||
return $property.GetValue($Instance)
|
||||
}
|
||||
|
||||
function New-Point(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
[double]$Heading,
|
||||
[double]$BodyClearance = 1.0,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($pointType, @(
|
||||
$X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $IsGearSwitch, $anchor))
|
||||
}
|
||||
|
||||
function New-DirectionSegment(
|
||||
[int]$Index,
|
||||
$Direction,
|
||||
[object[]]$Points,
|
||||
[bool]$StartsAtGearSwitch = $false,
|
||||
[bool]$EndsAtGearSwitch = $false) {
|
||||
$typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
|
||||
for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
|
||||
$typedPoints.SetValue($Points[$pointIndex], $pointIndex)
|
||||
}
|
||||
return [Activator]::CreateInstance($segmentType, @(
|
||||
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
|
||||
}
|
||||
|
||||
function New-EmptyMap {
|
||||
$request = [Activator]::CreateInstance($mapRequestType)
|
||||
$request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map
|
||||
Assert-True ($null -ne $map) 'Quintic test must create an explicit empty planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-AlgorithmInput(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters,
|
||||
[double]$KnotSpacingMeters = 1.0,
|
||||
[double]$MinimumKnotSpacingMeters = 0.10) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
$preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$typedSegments))
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.20
|
||||
$vehicle.WidthMeters = [double]0.20
|
||||
$vehicle.SafetyMarginMeters = [double]0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
||||
$vehicle.MinimumTurningRadiusMeters = [double]0.01
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$configuration.PiecewiseQuintic.KnotSpacingMeters = $KnotSpacingMeters
|
||||
$configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = $MinimumKnotSpacingMeters
|
||||
$options = $optionsConstructor.Invoke(@($configuration))
|
||||
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
|
||||
}
|
||||
|
||||
function Invoke-Candidate(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.0,
|
||||
[double]$KnotSpacingMeters = 1.0,
|
||||
[double]$MinimumKnotSpacingMeters = 0.10) {
|
||||
return $smoothMethod.Invoke($smoother, @(
|
||||
(New-AlgorithmInput $Segments $ReserveMeters $KnotSpacingMeters $MinimumKnotSpacingMeters),
|
||||
[double]1.0, [Threading.CancellationToken]::None))
|
||||
}
|
||||
|
||||
function Invoke-Smoothing(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.0,
|
||||
[double]$KnotSpacingMeters = 1.0,
|
||||
[double]$MinimumKnotSpacingMeters = 0.10) {
|
||||
$candidate = Invoke-Candidate $Segments $ReserveMeters $KnotSpacingMeters $MinimumKnotSpacingMeters
|
||||
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Quintic smoothing must produce a candidate for the deterministic fixture.'
|
||||
return @(Get-PropertyValue $candidate 'Segments')
|
||||
}
|
||||
|
||||
function Get-PointDistance($Left, $Right) {
|
||||
$deltaX = $Left.X - $Right.X
|
||||
$deltaY = $Left.Y - $Right.Y
|
||||
return [Math]::Sqrt($deltaX * $deltaX + $deltaY * $deltaY)
|
||||
}
|
||||
|
||||
function Get-Reference([object[]]$Source, [double]$ArcLength) {
|
||||
$typedPoints = [Array]::CreateInstance($pointType, $Source.Count)
|
||||
for ($index = 0; $index -lt $Source.Count; $index++) { $typedPoints.SetValue($Source[$index], $index) }
|
||||
$arguments = [object[]]@($typedPoints, $ArcLength, $null, $null)
|
||||
Assert-True $interpolateMethod.Invoke($null, $arguments) 'Quintic test must resolve every sampled local-arc reference.'
|
||||
return $arguments[2]
|
||||
}
|
||||
|
||||
function Get-PointAtArcLength([object[]]$Points, [double]$ArcLength) {
|
||||
foreach ($point in $Points) {
|
||||
if ([Math]::Abs($point.ArcLength - $ArcLength) -lt 0.000000000001) { return $point }
|
||||
}
|
||||
throw "Missing quintic sample at arc length $ArcLength"
|
||||
}
|
||||
|
||||
function Get-EndpointDerivative([object[]]$Samples, [double]$StepMeters, [bool]$AtStart) {
|
||||
$firstCoefficients = @((-137.0 / 60.0), 5.0, -5.0, (10.0 / 3.0), (-5.0 / 4.0), (1.0 / 5.0))
|
||||
$x = 0.0
|
||||
$y = 0.0
|
||||
for ($index = 0; $index -lt 6; $index++) {
|
||||
$sampleIndex = if ($AtStart) { $index } else { 5 - $index }
|
||||
$sign = if ($AtStart) { 1.0 } else { -1.0 }
|
||||
$x += $firstCoefficients[$index] * $Samples[$sampleIndex].X
|
||||
$y += $firstCoefficients[$index] * $Samples[$sampleIndex].Y
|
||||
}
|
||||
return [PSCustomObject]@{ X = $sign * $x / $StepMeters; Y = $sign * $y / $StepMeters }
|
||||
}
|
||||
|
||||
function Get-EndpointSecondDerivative([object[]]$Samples, [double]$StepMeters, [bool]$AtStart) {
|
||||
$coefficients = @((15.0 / 4.0), (-77.0 / 6.0), (107.0 / 6.0), -13.0, (61.0 / 12.0), (-5.0 / 6.0))
|
||||
$x = 0.0
|
||||
$y = 0.0
|
||||
for ($index = 0; $index -lt 6; $index++) {
|
||||
$sampleIndex = if ($AtStart) { $index } else { 5 - $index }
|
||||
$x += $coefficients[$index] * $Samples[$sampleIndex].X
|
||||
$y += $coefficients[$index] * $Samples[$sampleIndex].Y
|
||||
}
|
||||
return [PSCustomObject]@{ X = $x / ($StepMeters * $StepMeters); Y = $y / ($StepMeters * $StepMeters) }
|
||||
}
|
||||
|
||||
function Get-IntervalSamples([object[]]$Points, [double]$StartArcLength, [double]$EndArcLength, [bool]$FromStart) {
|
||||
$intervalLength = $EndArcLength - $StartArcLength
|
||||
$result = @()
|
||||
for ($index = 0; $index -lt 6; $index++) {
|
||||
$parameter = if ($FromStart) { $index / 8.0 } else { (3.0 + $index) / 8.0 }
|
||||
$result += Get-PointAtArcLength $Points ($StartArcLength + $parameter * $intervalLength)
|
||||
}
|
||||
return $result
|
||||
}
|
||||
|
||||
function Get-QuinticPositionFromInteriorSamples(
|
||||
[object[]]$Points,
|
||||
[double]$StartArcLength,
|
||||
[double]$EndArcLength,
|
||||
[double[]]$Parameters,
|
||||
[double]$TargetParameter) {
|
||||
$intervalLength = $EndArcLength - $StartArcLength
|
||||
$x = 0.0
|
||||
$y = 0.0
|
||||
for ($index = 0; $index -lt $Parameters.Count; $index++) {
|
||||
$weight = 1.0
|
||||
for ($otherIndex = 0; $otherIndex -lt $Parameters.Count; $otherIndex++) {
|
||||
if ($index -ne $otherIndex) {
|
||||
$weight *= ($TargetParameter - $Parameters[$otherIndex]) / ($Parameters[$index] - $Parameters[$otherIndex])
|
||||
}
|
||||
}
|
||||
$sample = Get-PointAtArcLength $Points ($StartArcLength + $Parameters[$index] * $intervalLength)
|
||||
$x += $weight * $sample.X
|
||||
$y += $weight * $sample.Y
|
||||
}
|
||||
return [PSCustomObject]@{ X = $x; Y = $y }
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
|
||||
$smootherType = Get-RequiredType ($algorithms + 'PiecewiseQuinticSmoother')
|
||||
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
||||
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
||||
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
||||
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
||||
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
||||
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
||||
|
||||
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
|
||||
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry immutable quintic options and clearance reserve.'
|
||||
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
||||
Assert-True ($null -ne $optionsConstructor) 'Quintic tests must create immutable options snapshots.'
|
||||
$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic')
|
||||
Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose local-arc interpolation.'
|
||||
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
||||
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
||||
Assert-True ($null -ne $smoothMethod) 'PiecewiseQuinticSmoother must implement the internal smoother contract.'
|
||||
Assert-Equal 'PiecewiseQuintic' $smoother.Method.ToString() 'Quintic smoother must identify its public smoothing method.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
|
||||
# The 1.0 m local-arc knot spacing creates shared knots at s=1 and s=2.
|
||||
# The generated 1/8 samples allow exact one-sided quintic derivative reconstruction.
|
||||
$continuitySource = @(
|
||||
(New-Point 0.00 0.00 0.00 0.00),
|
||||
(New-Point 0.50 0.00 0.50 0.00),
|
||||
(New-Point 1.00 0.00 1.00 0.00),
|
||||
(New-Point 1.00 0.50 1.50 ([Math]::PI / 2.0)),
|
||||
(New-Point 1.00 1.00 2.00 ([Math]::PI / 2.0)),
|
||||
(New-Point 1.30 1.30 2.40 ([Math]::PI / 4.0)))
|
||||
$continuityOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)))[0].Points
|
||||
foreach ($sharedArcLength in @(1.0, 2.0)) {
|
||||
$leftSamples = Get-IntervalSamples $continuityOutput ($sharedArcLength - 1.0) $sharedArcLength $false
|
||||
$rightEnd = if ($sharedArcLength -eq 2.0) { 2.4 } else { $sharedArcLength + 1.0 }
|
||||
$rightSamples = Get-IntervalSamples $continuityOutput $sharedArcLength $rightEnd $true
|
||||
$leftPosition = Get-QuinticPositionFromInteriorSamples $continuityOutput ($sharedArcLength - 1.0) $sharedArcLength `
|
||||
@((2.0 / 8.0), (3.0 / 8.0), (4.0 / 8.0), (5.0 / 8.0), (6.0 / 8.0), (7.0 / 8.0)) 1.0
|
||||
$rightPosition = Get-QuinticPositionFromInteriorSamples $continuityOutput $sharedArcLength $rightEnd `
|
||||
@((1.0 / 8.0), (2.0 / 8.0), (3.0 / 8.0), (4.0 / 8.0), (5.0 / 8.0), (6.0 / 8.0)) 0.0
|
||||
$leftFirst = Get-EndpointDerivative $leftSamples (1.0 / 8.0) $false
|
||||
$rightFirst = Get-EndpointDerivative $rightSamples (($rightEnd - $sharedArcLength) / 8.0) $true
|
||||
$leftSecond = Get-EndpointSecondDerivative $leftSamples (1.0 / 8.0) $false
|
||||
$rightSecond = Get-EndpointSecondDerivative $rightSamples (($rightEnd - $sharedArcLength) / 8.0) $true
|
||||
Assert-Near $leftPosition.X $rightPosition.X 0.000001 'Shared knot X position must match from both quintic intervals.'
|
||||
Assert-Near $leftPosition.Y $rightPosition.Y 0.000001 'Shared knot Y position must match from both quintic intervals.'
|
||||
Assert-Near $leftFirst.X $rightFirst.X 0.000001 'Shared knot X first derivative must be C1.'
|
||||
Assert-Near $leftFirst.Y $rightFirst.Y 0.000001 'Shared knot Y first derivative must be C1.'
|
||||
Assert-Near $leftSecond.X $rightSecond.X 0.000001 'Shared knot X second derivative must be C2.'
|
||||
Assert-Near $leftSecond.Y $rightSecond.Y 0.000001 'Shared knot Y second derivative must be C2.'
|
||||
}
|
||||
|
||||
$firstOutput = $continuityOutput[0]
|
||||
$lastOutput = $continuityOutput[$continuityOutput.Count - 1]
|
||||
Assert-Near $continuitySource[0].X $firstOutput.X 0.0 'Quintic start X must remain exact.'
|
||||
Assert-Near $continuitySource[0].Y $firstOutput.Y 0.0 'Quintic start Y must remain exact.'
|
||||
Assert-Near $continuitySource[$continuitySource.Count - 1].X $lastOutput.X 0.0 'Quintic end X must remain exact.'
|
||||
Assert-Near $continuitySource[$continuitySource.Count - 1].Y $lastOutput.Y 0.0 'Quintic end Y must remain exact.'
|
||||
foreach ($point in $continuityOutput) {
|
||||
$reference = Get-Reference $continuitySource $point.ArcLength
|
||||
Assert-True ((Get-PointDistance $point $reference) -le ($reference.BodyClearance + 0.000000000001)) 'Every quintic sample must remain inside its local reference movement bound.'
|
||||
}
|
||||
|
||||
# Separate prepared direction segments must retain their exact duplicated switch pose and topology.
|
||||
$reverseSource = @(
|
||||
(New-Point 1.30 1.30 0.00 ([Math]::PI / 4.0) 1.0 $true),
|
||||
(New-Point 1.30 0.80 0.50 ([Math]::PI / 2.0)),
|
||||
(New-Point 1.30 0.30 1.00 ([Math]::PI / 2.0)))
|
||||
$switchOutput = @(Invoke-Smoothing @(
|
||||
(New-DirectionSegment 0 $forward $continuitySource $false $true),
|
||||
(New-DirectionSegment 1 $reverse $reverseSource $true $false)))
|
||||
Assert-Equal 2 $switchOutput.Count 'Quintic smoothing must retain separate direction segments.'
|
||||
Assert-True $switchOutput[0].EndsAtGearSwitch 'Forward quintic segment must retain its gear-switch boundary flag.'
|
||||
Assert-True $switchOutput[1].StartsAtGearSwitch 'Reverse quintic segment must retain its gear-switch boundary flag.'
|
||||
$leftSwitch = $switchOutput[0].Points[$switchOutput[0].Points.Count - 1]
|
||||
$rightSwitch = $switchOutput[1].Points[0]
|
||||
Assert-Near $leftSwitch.X $rightSwitch.X 0.0 'Quintic smoothing must preserve switch X exactly.'
|
||||
Assert-Near $leftSwitch.Y $rightSwitch.Y 0.0 'Quintic smoothing must preserve switch Y exactly.'
|
||||
|
||||
# Spacing selects local-arc knots, while a valid minimum spacing does not change that selection.
|
||||
$oneMeterOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 1.0 0.10)[0].Points
|
||||
$halfMeterOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 0.50 0.10)[0].Points
|
||||
$largeMinimumOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 1.0 0.30)[0].Points
|
||||
Assert-True ($halfMeterOutput.Count -gt $oneMeterOutput.Count) 'Custom knot spacing must create additional local-arc knot intervals.'
|
||||
Assert-Equal $oneMeterOutput.Count $largeMinimumOutput.Count 'Valid minimum knot spacing must not change knot selection.'
|
||||
for ($index = 0; $index -lt $oneMeterOutput.Count; $index++) {
|
||||
Assert-Near $oneMeterOutput[$index].X $largeMinimumOutput[$index].X 0.000000000001 'Minimum knot spacing must not change valid quintic X geometry.'
|
||||
Assert-Near $oneMeterOutput[$index].Y $largeMinimumOutput[$index].Y 0.000000000001 'Minimum knot spacing must not change valid quintic Y geometry.'
|
||||
}
|
||||
|
||||
$shortSource = @(
|
||||
(New-Point 0.00 0.00 0.00 0.00),
|
||||
(New-Point 0.05 0.00 0.05 0.00))
|
||||
$shortCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $shortSource)) 0.0 1.0 0.10
|
||||
Assert-Equal 'Failed' (Get-PropertyValue $shortCandidate 'Status').ToString() 'A segment shorter than the configured minimum knot spacing must fail terminally.'
|
||||
Assert-True (-not (Get-PropertyValue $shortCandidate 'Succeeded')) 'A degenerate short quintic segment must not be executable.'
|
||||
Assert-Equal 0 (Get-PropertyValue $shortCandidate 'Segments').Count 'A terminal quintic degeneracy must publish no geometry.'
|
||||
|
||||
# Local-arc reference mapping, rather than sample index/global distance, must reject this unsafe nonuniform path.
|
||||
$nonuniformUnsafeSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0 0.50),
|
||||
(New-Point 1.0 0.0 4.0 0.0 0.50),
|
||||
(New-Point 2.0 0.0 5.0 0.0 0.50),
|
||||
(New-Point 3.0 0.0 6.0 0.0 0.50),
|
||||
(New-Point 3.0 1.0 9.0 ([Math]::PI / 2.0) 0.50),
|
||||
(New-Point 3.0 2.0 20.0 ([Math]::PI / 2.0) 0.50))
|
||||
$nonuniformUnsafeCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $nonuniformUnsafeSource)) 0.0 5.0 0.10
|
||||
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $nonuniformUnsafeCandidate 'Status').ToString() 'Unsafe nonuniform local-arc quintic movement must be retryable.'
|
||||
Assert-True (-not (Get-PropertyValue $nonuniformUnsafeCandidate 'Succeeded')) 'Unsafe nonuniform quintic geometry must not be executable.'
|
||||
Assert-Equal 0 (Get-PropertyValue $nonuniformUnsafeCandidate 'Segments').Count 'Retryable quintic infeasibility must publish no geometry.'
|
||||
|
||||
Write-Output 'Path smoothing piecewise quintic checks passed.'
|
||||
Reference in New Issue
Block a user