First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: correct local bezier window constraints

Browse Source
This commit is contained in:
梁薄云
2026-07-29 13:33:45 +08:00
parent c893abe7d2
commit 591a10cc3d
2 changed files with 87 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs
+46 -15
View File
@@ -178,6 +178,7 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother
out string reason)
{
windows = new List<Window>();
var candidates = new List<Window>();
reason = string.Empty;
for (int cornerIndex = 1; cornerIndex < anchors.Count - 1; cornerIndex++)
{
@@ -210,22 +211,43 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother
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));
}
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,
@@ -249,10 +271,12 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother
}
double arcLength = p3.ArcLength - p0.ArcLength;
double handleLength = arcLength * handleLengthRatio * strength;
if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(handleLength))
double chordLength = Distance(p0, p3);
double handleLength = chordLength * handleLengthRatio * strength;
if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(chordLength) ||
!NumericGuard.IsPositiveFinite(handleLength))
{
reason = "Bézier 窗口弧长或控制柄长度无效。";
reason = "Bézier 窗口弧长、端点弦长或控制柄长度无效。";
return false;
}
@@ -367,6 +391,13 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother
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)
ClumsyPilot/tests/verify_path_smoothing_bezier.ps1
+41
View File
@@ -211,6 +211,23 @@ Assert-Equal $cornerSource.Count $cornerOutput.Count 'One local replacement must
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.'
$cornerChordLength = [Math]::Sqrt(
[Math]::Pow($cornerSource[3].X - $cornerSource[1].X, 2.0) +
[Math]::Pow($cornerSource[3].Y - $cornerSource[1].Y, 2.0))
$cornerHandleLength = $cornerChordLength / 3.0
$expectedCornerX =
0.125 * $cornerSource[1].X +
0.375 * ($cornerSource[1].X + $cornerHandleLength) +
0.375 * $cornerSource[3].X +
0.125 * $cornerSource[3].X
$expectedCornerY =
0.125 * $cornerSource[1].Y +
0.375 * $cornerSource[1].Y +
0.375 * ($cornerSource[3].Y - $cornerHandleLength) +
0.125 * $cornerSource[3].Y
$cornerInterpolated = Get-FirstInterpolatedPoint $cornerOutput
Assert-Near $expectedCornerX $cornerInterpolated.X 0.000000000001 'Bézier control handles must use the local endpoint chord length for X geometry.'
Assert-Near $expectedCornerY $cornerInterpolated.Y 0.000000000001 'Bézier control handles must use the local endpoint chord length for Y geometry.'
# Adjacent corner windows touch/overlap and must become one merged cubic replacement, not two sequential fits.
$overlappingSource = @(
@@ -227,6 +244,15 @@ Assert-PointBitwiseEqual $overlappingSource[4] $overlappingOutput[4] 'Merged Bé
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.'
# Safe bounded policy: decline an entire connected set when its merged interval exceeds the cap.
# The two candidate windows below are each 0.20 m, but their merged 0.30 m interval must not
# produce one over-length curve or be split into new unrequested joins.
$overCapMergedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)) 0.02 ([Math]::PI / 18.0) 0.20)[0].Points
Assert-Equal 0 (Get-InterpolatedRunCount $overCapMergedOutput) 'An oversized connected Bézier window set must be declined instead of emitting an over-cap replacement.'
for ($index = 0; $index -lt $overlappingSource.Count; $index++) {
Assert-PointBitwiseEqual $overlappingSource[$index] $overCapMergedOutput[$index] 'Declining an oversized connected set must preserve its anchors.'
}
# 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),
@@ -275,4 +301,19 @@ Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $infeasible 'Status').ToSt
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.'
# This nonuniform, offset window evaluates its only interior point at t=0.25 and local s=6.
# A wrong global/index mapping would instead compare to s=5 and accept the 0.50 m clearance;
# the required local-arc reference at s=6 must reject the roughly 0.65 m displacement.
$nonuniformOffsetSource = @(
(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))
$nonuniformOffsetInfeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $nonuniformOffsetSource)) 0.0 ([Math]::PI / 18.0) 5.0
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $nonuniformOffsetInfeasible 'Status').ToString() 'A nonuniform offset window must use local arc-length mapping for retryable clearance rejection.'
Assert-True (-not (Get-PropertyValue $nonuniformOffsetInfeasible 'Succeeded')) 'The nonuniform local-arc infeasibility must not be executable.'
Assert-Equal 0 (Get-PropertyValue $nonuniformOffsetInfeasible 'Segments').Count 'The nonuniform local-arc infeasibility must publish no geometry.'
Write-Output 'Path smoothing local cubic Bézier checks passed.'