feat: add local cubic bezier smoother
This commit is contained in:
梁薄云
@@ -0,0 +1,395 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
|
||||
/// <summary>在方向段内以局部三次 Bézier 连接替换明显转角。</summary>
|
||||
internal sealed class LocalCubicBezierSmoother : IPathSmoother
|
||||
{
|
||||
private const double WindowToleranceMeters = 1e-9d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.LocalCubicBezier;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null || input.OriginalPath == null || input.Options == null ||
|
||||
!NumericGuard.IsPositiveFinite(effectiveStrength) ||
|
||||
!NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
|
||||
input.MinimumClearanceReserveMeters < 0d)
|
||||
{
|
||||
return SmoothingCandidate.Failed("Bézier 输入、强度或净空预留无效。");
|
||||
}
|
||||
|
||||
var candidateSegments = new List<PreparedDirectionSegment>(input.OriginalPath.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < input.OriginalPath.Segments.Count; segmentIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
|
||||
if (!TrySmoothSegment(
|
||||
sourceSegment,
|
||||
input.Options.BezierCornerHeadingThresholdRadians,
|
||||
input.Options.BezierMaximumWindowLengthMeters,
|
||||
input.Options.BezierHandleLengthRatio,
|
||||
effectiveStrength,
|
||||
input.MinimumClearanceReserveMeters,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> points,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status))
|
||||
{
|
||||
return status == SmoothingCandidateStatus.RetryableInfeasible
|
||||
? SmoothingCandidate.RetryableInfeasible(reason)
|
||||
: SmoothingCandidate.Failed(reason);
|
||||
}
|
||||
|
||||
candidateSegments.Add(new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
points,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch));
|
||||
}
|
||||
|
||||
return SmoothingCandidate.Success(candidateSegments);
|
||||
}
|
||||
|
||||
private static bool TrySmoothSegment(
|
||||
PreparedDirectionSegment sourceSegment,
|
||||
double cornerThresholdRadians,
|
||||
double maximumWindowLengthMeters,
|
||||
double handleLengthRatio,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> result,
|
||||
out string reason,
|
||||
out SmoothingCandidateStatus status)
|
||||
{
|
||||
result = null;
|
||||
reason = string.Empty;
|
||||
status = SmoothingCandidateStatus.Failed;
|
||||
if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
|
||||
{
|
||||
reason = "Bézier 方向段为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
IReadOnlyList<SmoothingPoint2D> anchors = sourceSegment.Points;
|
||||
if (!ValidateAnchors(anchors, cancellationToken, out reason)) return false;
|
||||
if (anchors.Count < 3)
|
||||
{
|
||||
result = anchors;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (!TryCreateMergedWindows(
|
||||
anchors,
|
||||
cornerThresholdRadians,
|
||||
maximumWindowLengthMeters,
|
||||
cancellationToken,
|
||||
out List<Window> windows,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (windows.Count == 0)
|
||||
{
|
||||
result = anchors;
|
||||
return true;
|
||||
}
|
||||
|
||||
var output = new List<SmoothingPoint2D>(anchors.Count);
|
||||
int anchorIndex = 0;
|
||||
for (int windowIndex = 0; windowIndex < windows.Count; windowIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Window window = windows[windowIndex];
|
||||
while (anchorIndex <= window.StartIndex)
|
||||
{
|
||||
output.Add(anchors[anchorIndex]);
|
||||
anchorIndex++;
|
||||
}
|
||||
|
||||
if (!TryAppendWindowInterior(
|
||||
anchors,
|
||||
window,
|
||||
handleLengthRatio,
|
||||
strength,
|
||||
reserveMeters,
|
||||
output,
|
||||
cancellationToken,
|
||||
out reason,
|
||||
out status))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
output.Add(anchors[window.EndIndex]);
|
||||
anchorIndex = window.EndIndex + 1;
|
||||
}
|
||||
|
||||
while (anchorIndex < anchors.Count)
|
||||
{
|
||||
output.Add(anchors[anchorIndex]);
|
||||
anchorIndex++;
|
||||
}
|
||||
|
||||
result = output;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateAnchors(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
CancellationToken cancellationToken,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int index = 0; index < anchors.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
SmoothingPoint2D point = anchors[index];
|
||||
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||
!NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.UnwrappedHeading) ||
|
||||
!NumericGuard.IsFinite(point.BodyClearance) || point.BodyClearance < 0d ||
|
||||
(index > 0 && point.ArcLength <= anchors[index - 1].ArcLength))
|
||||
{
|
||||
reason = "Bézier 方向段包含非有限、非递增弧长或无效净空的锚点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryCreateMergedWindows(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
double cornerThresholdRadians,
|
||||
double maximumWindowLengthMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out List<Window> windows,
|
||||
out string reason)
|
||||
{
|
||||
windows = new List<Window>();
|
||||
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;
|
||||
|
||||
var proposed = new Window(startIndex, endIndex);
|
||||
if (windows.Count == 0 || proposed.StartIndex > windows[windows.Count - 1].EndIndex + 1)
|
||||
{
|
||||
windows.Add(proposed);
|
||||
}
|
||||
else
|
||||
{
|
||||
Window previous = windows[windows.Count - 1];
|
||||
windows[windows.Count - 1] = new Window(
|
||||
previous.StartIndex,
|
||||
Math.Max(previous.EndIndex, proposed.EndIndex));
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
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 handleLength = arcLength * handleLengthRatio * strength;
|
||||
if (!NumericGuard.IsPositiveFinite(arcLength) || !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 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,278 @@
|
||||
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 Assert-PointBitwiseEqual($Expected, $Actual, [string]$Message) {
|
||||
foreach ($name in @('X', 'Y', 'ArcLength', 'Heading', 'UnwrappedHeading', 'BodyClearance')) {
|
||||
$expectedBits = [BitConverter]::DoubleToInt64Bits([double]$Expected.$name)
|
||||
$actualBits = [BitConverter]::DoubleToInt64Bits([double]$Actual.$name)
|
||||
Assert-Equal $expectedBits $actualBits ($Message + ' ' + $name)
|
||||
}
|
||||
Assert-Equal $Expected.IsGearSwitchPoint $Actual.IsGearSwitchPoint ($Message + ' IsGearSwitchPoint')
|
||||
Assert-Equal $Expected.Source.ToString() $Actual.Source.ToString() ($Message + ' Source')
|
||||
}
|
||||
|
||||
function New-Point(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
[double]$Heading,
|
||||
[double]$BodyClearance = 0.10,
|
||||
[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) 'Bézier test must create an explicit empty planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-AlgorithmInput(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.02,
|
||||
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
||||
[double]$MaximumWindowLengthMeters = 0.60,
|
||||
[double]$HandleLengthRatio = (1.0 / 3.0)) {
|
||||
$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.LocalCubicBezier.CornerHeadingThresholdRadians = $CornerThresholdRadians
|
||||
$configuration.LocalCubicBezier.MaximumWindowLengthMeters = $MaximumWindowLengthMeters
|
||||
$configuration.LocalCubicBezier.HandleLengthRatio = $HandleLengthRatio
|
||||
$options = $optionsConstructor.Invoke(@($configuration))
|
||||
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
|
||||
}
|
||||
|
||||
function Invoke-Candidate(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.02,
|
||||
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
||||
[double]$MaximumWindowLengthMeters = 0.60,
|
||||
[double]$HandleLengthRatio = (1.0 / 3.0),
|
||||
[double]$Strength = 1.0) {
|
||||
return $smoothMethod.Invoke($smoother, @(
|
||||
(New-AlgorithmInput $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio),
|
||||
$Strength,
|
||||
[Threading.CancellationToken]::None))
|
||||
}
|
||||
|
||||
function Invoke-Smoothing(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.02,
|
||||
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
||||
[double]$MaximumWindowLengthMeters = 0.60,
|
||||
[double]$HandleLengthRatio = (1.0 / 3.0),
|
||||
[double]$Strength = 1.0) {
|
||||
$candidate = Invoke-Candidate $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio $Strength
|
||||
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Bézier smoothing must produce a candidate for the deterministic fixture.'
|
||||
return @(Get-PropertyValue $candidate 'Segments')
|
||||
}
|
||||
|
||||
function Get-InterpolatedRunCount($Points) {
|
||||
$runCount = 0
|
||||
$inRun = $false
|
||||
foreach ($point in $Points) {
|
||||
$interpolated = $point.Source.ToString() -eq 'Interpolated'
|
||||
if ($interpolated -and -not $inRun) { $runCount++ }
|
||||
$inRun = $interpolated
|
||||
}
|
||||
return $runCount
|
||||
}
|
||||
|
||||
function Get-PointAtArcLength($Points, [double]$ArcLength) {
|
||||
foreach ($point in $Points) {
|
||||
if ([BitConverter]::DoubleToInt64Bits([double]$point.ArcLength) -eq
|
||||
[BitConverter]::DoubleToInt64Bits($ArcLength)) {
|
||||
return $point
|
||||
}
|
||||
}
|
||||
throw "No output point found at local arc length $ArcLength."
|
||||
}
|
||||
|
||||
function Get-FirstInterpolatedPoint($Points) {
|
||||
foreach ($point in $Points) {
|
||||
if ($point.Source.ToString() -eq 'Interpolated') { return $point }
|
||||
}
|
||||
throw 'Expected an interpolated Bézier point.'
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
|
||||
$smootherType = Get-RequiredType ($algorithms + 'LocalCubicBezierSmoother')
|
||||
$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')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$candidateStatusType = Get-RequiredType ($algorithms + 'SmoothingCandidateStatus')
|
||||
$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) 'Bézier tests must construct algorithm input with immutable option values.'
|
||||
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
||||
Assert-True ($null -ne $optionsConstructor) 'Bézier tests must create immutable option snapshots.'
|
||||
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
||||
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
||||
Assert-True ($null -ne $smoothMethod) 'LocalCubicBezierSmoother must implement the internal smoother contract.'
|
||||
Assert-Equal 'LocalCubicBezier' $smoother.Method.ToString() 'Bézier smoother must identify its public smoothing method.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
|
||||
# A straight path must not create a local Bézier window or alter any sample.
|
||||
$straightSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.3 0.0 0.3 0.0))
|
||||
$straightOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)))[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $straightOutput) 'A straight line must create no Bézier replacement window.'
|
||||
Assert-Equal $straightSource.Count $straightOutput.Count 'A straight line must retain its original sample count.'
|
||||
for ($index = 0; $index -lt $straightSource.Count; $index++) {
|
||||
Assert-PointBitwiseEqual $straightSource[$index] $straightOutput[$index] 'Straight samples must remain bitwise unchanged.'
|
||||
}
|
||||
|
||||
# One corner is one local replacement: only the corner sample is evaluated while the window endpoints stay fixed.
|
||||
$cornerSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.2 0.2 0.4 ([Math]::PI / 2.0)))
|
||||
$cornerOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)))[0].Points
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $cornerOutput) 'One corner must produce exactly one contiguous Bézier replacement.'
|
||||
Assert-Equal $cornerSource.Count $cornerOutput.Count 'One local replacement must preserve the segment sampling topology.'
|
||||
Assert-True (($cornerOutput[2].X -ne $cornerSource[2].X) -or ($cornerOutput[2].Y -ne $cornerSource[2].Y)) 'The corner sample must be replaced by cubic Bézier geometry.'
|
||||
Assert-PointBitwiseEqual $cornerSource[1] $cornerOutput[1] 'Bézier entry anchor must remain fixed.'
|
||||
Assert-PointBitwiseEqual $cornerSource[3] $cornerOutput[3] 'Bézier exit anchor must remain fixed.'
|
||||
|
||||
# Adjacent corner windows touch/overlap and must become one merged cubic replacement, not two sequential fits.
|
||||
$overlappingSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.3 0.1 0.4 0.0),
|
||||
(New-Point 0.4 0.1 0.5 0.0))
|
||||
$overlappingOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)))[0].Points
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $overlappingOutput) 'Touching local corner windows must merge into exactly one Bézier replacement.'
|
||||
Assert-PointBitwiseEqual $overlappingSource[1] $overlappingOutput[1] 'Merged Bézier entry anchor must remain fixed.'
|
||||
Assert-PointBitwiseEqual $overlappingSource[4] $overlappingOutput[4] 'Merged Bézier exit anchor must remain fixed.'
|
||||
Assert-True (($overlappingOutput[2].X -ne $overlappingSource[2].X) -or ($overlappingOutput[2].Y -ne $overlappingSource[2].Y)) 'Merged window must replace the first interior corner sample.'
|
||||
Assert-True (($overlappingOutput[3].X -ne $overlappingSource[3].X) -or ($overlappingOutput[3].Y -ne $overlappingSource[3].Y)) 'Merged window must replace the second interior corner sample.'
|
||||
|
||||
# Samples outside a local window must remain bitwise unchanged rather than be globally re-fit.
|
||||
$isolatedSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.3 0.0 0.3 0.0),
|
||||
(New-Point 0.3 0.1 0.4 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.3 0.2 0.5 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.3 0.3 0.6 ([Math]::PI / 2.0)))
|
||||
$isolatedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $isolatedSource)))[0].Points
|
||||
foreach ($index in @(0, 1, 2, 5, 6)) {
|
||||
Assert-PointBitwiseEqual $isolatedSource[$index] (Get-PointAtArcLength $isolatedOutput $isolatedSource[$index].ArcLength) 'Samples outside a Bézier window must remain bitwise unchanged.'
|
||||
}
|
||||
|
||||
# Direction segments stay independent; segment endpoints and the gear-switch anchor are fixed.
|
||||
$reverseSource = @(
|
||||
(New-Point 0.2 0.2 0.0 ([Math]::PI / 2.0) 0.10 $true),
|
||||
(New-Point 0.2 0.1 0.1 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.2 0.0 0.2 ([Math]::PI / 2.0)))
|
||||
$switchOutput = @(Invoke-Smoothing @(
|
||||
(New-DirectionSegment 0 $forward $cornerSource $false $true),
|
||||
(New-DirectionSegment 1 $reverse $reverseSource $true $false)))
|
||||
Assert-Equal 2 $switchOutput.Count 'Bézier smoothing must retain separate forward and reverse direction segments.'
|
||||
Assert-True $switchOutput[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
|
||||
Assert-True $switchOutput[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
|
||||
Assert-PointBitwiseEqual $cornerSource[0] $switchOutput[0].Points[0] 'Segment start endpoint must remain fixed.'
|
||||
Assert-PointBitwiseEqual $cornerSource[$cornerSource.Count - 1] $switchOutput[0].Points[$switchOutput[0].Points.Count - 1] 'Segment end endpoint must remain fixed.'
|
||||
Assert-PointBitwiseEqual $reverseSource[0] $switchOutput[1].Points[0] 'Gear-switch point must remain fixed.'
|
||||
|
||||
# The immutable options each change only their own local behavior.
|
||||
$thresholdOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.70)[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $thresholdOutput) 'A non-default heading threshold above the corner angle must suppress only corner detection.'
|
||||
$windowOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.15)[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $windowOutput) 'A non-default maximum window shorter than the local connection must suppress only that window.'
|
||||
$shortHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.10)[0].Points
|
||||
$longHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.60)[0].Points
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $shortHandleOutput) 'Changing handle ratio must not change detected window topology.'
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $longHandleOutput) 'Changing handle ratio must not change detected window topology.'
|
||||
$shortHandlePoint = Get-FirstInterpolatedPoint $shortHandleOutput
|
||||
$longHandlePoint = Get-FirstInterpolatedPoint $longHandleOutput
|
||||
Assert-True (($shortHandlePoint.X -ne $longHandlePoint.X) -or ($shortHandlePoint.Y -ne $longHandlePoint.Y)) 'A non-default handle ratio must change only the local Bézier geometry.'
|
||||
|
||||
# Parameter-matched local arc-length reference comparison must reject excess displacement as retryable and publish no geometry.
|
||||
$infeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.095
|
||||
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $infeasible 'Status').ToString() 'Exceeded local arc-length displacement must be retryable, not terminal.'
|
||||
Assert-True (-not (Get-PropertyValue $infeasible 'Succeeded')) 'An infeasible Bézier curve must not be executable.'
|
||||
Assert-Equal 0 (Get-PropertyValue $infeasible 'Segments').Count 'A retryable Bézier infeasibility must publish no executable geometry.'
|
||||
|
||||
Write-Output 'Path smoothing local cubic Bézier checks passed.'
|
||||
Reference in New Issue
Block a user