fix: enforce b-spline displacement constraints
This commit is contained in:
梁薄云
@@ -15,6 +15,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
||||
private const double StraightToleranceMeters = 1e-9d;
|
||||
private const double EndpointTangentScale = 1d / 3d;
|
||||
private const double EndpointProbeParameter = 1e-6d;
|
||||
private const double MinimumTangentHandleLengthMeters = 1e-10d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
|
||||
@@ -88,11 +89,9 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
||||
return true;
|
||||
}
|
||||
|
||||
Point2D[] controls = CreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
|
||||
cancellationToken);
|
||||
if (controls == null)
|
||||
if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
|
||||
cancellationToken, out Point2D[] controls, out reason))
|
||||
{
|
||||
reason = "B 样条控制点构造产生非法数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -100,45 +99,73 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
||||
var sampled = new List<SmoothingPoint2D>();
|
||||
int spanCount = controls.Length - Degree;
|
||||
int uniformIntervals = spanCount * SamplesPerSpan;
|
||||
AddSample(0d, anchors, controls, knots, sampled, cancellationToken);
|
||||
AddSample(EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken);
|
||||
if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason))
|
||||
return false;
|
||||
if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
||||
sampled, cancellationToken, out reason))
|
||||
return false;
|
||||
for (int index = 1; index < uniformIntervals; index++)
|
||||
{
|
||||
AddSample((double)index / uniformIntervals, anchors, controls, knots, sampled, cancellationToken);
|
||||
if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters,
|
||||
sampled, cancellationToken, out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
AddSample(1d - EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken);
|
||||
AddSample(1d, anchors, controls, knots, sampled, cancellationToken);
|
||||
if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
|
||||
sampled, cancellationToken, out reason))
|
||||
return false;
|
||||
if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason))
|
||||
return false;
|
||||
|
||||
result = sampled;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static Point2D[] CreateControls(
|
||||
private static bool TryCreateControls(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
TravelDirection direction,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
CancellationToken cancellationToken)
|
||||
CancellationToken cancellationToken,
|
||||
out Point2D[] controls,
|
||||
out string reason)
|
||||
{
|
||||
var controls = new Point2D[anchors.Count];
|
||||
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);
|
||||
Point2D startProposed = new Point2D(
|
||||
anchors[0].X + startHandleLength * Math.Cos(startTravelHeading),
|
||||
anchors[0].Y + startHandleLength * Math.Sin(startTravelHeading));
|
||||
controls[1] = ClampDisplacement(anchors[1], startProposed, GetAllowedRadius(anchors[1], reserveMeters));
|
||||
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);
|
||||
Point2D endProposed = new Point2D(
|
||||
anchors[finalIndex].X - endHandleLength * Math.Cos(endTravelHeading),
|
||||
anchors[finalIndex].Y - endHandleLength * Math.Sin(endTravelHeading));
|
||||
controls[finalIndex - 1] = ClampDisplacement(
|
||||
anchors[finalIndex - 1], endProposed, GetAllowedRadius(anchors[finalIndex - 1], reserveMeters));
|
||||
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++)
|
||||
{
|
||||
@@ -158,22 +185,34 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
||||
for (int index = 0; index < controls.Length; index++)
|
||||
{
|
||||
if (!NumericGuard.IsFinite(controls[index].X) || !NumericGuard.IsFinite(controls[index].Y))
|
||||
return null;
|
||||
{
|
||||
reason = "B 样条控制点构造产生非法数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return controls;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static void AddSample(
|
||||
private static bool TryAddSample(
|
||||
double parameter,
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
Point2D[] controls,
|
||||
double[] knots,
|
||||
double reserveMeters,
|
||||
List<SmoothingPoint2D> output,
|
||||
CancellationToken cancellationToken)
|
||||
CancellationToken cancellationToken,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Point2D evaluated = Evaluate(controls, knots, parameter);
|
||||
SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter);
|
||||
double displacement = Distance(evaluated, reference);
|
||||
if (!NumericGuard.IsFinite(displacement) || displacement > GetAllowedRadius(reference, reserveMeters))
|
||||
{
|
||||
reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
|
||||
return false;
|
||||
}
|
||||
bool endpoint = parameter == 0d || parameter == 1d;
|
||||
output.Add(new SmoothingPoint2D(
|
||||
evaluated.X,
|
||||
@@ -184,6 +223,38 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
||||
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)
|
||||
@@ -303,6 +374,13 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
|
||||
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)
|
||||
|
||||
@@ -77,9 +77,13 @@ function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
|
||||
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters))
|
||||
}
|
||||
|
||||
function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
|
||||
$candidate = $smoothMethod.Invoke($smoother, @(
|
||||
function Invoke-Candidate([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
|
||||
return $smoothMethod.Invoke($smoother, @(
|
||||
(New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None))
|
||||
}
|
||||
|
||||
function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
|
||||
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength
|
||||
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.'
|
||||
return @(Get-PropertyValue $candidate 'Segments')
|
||||
}
|
||||
@@ -198,6 +202,26 @@ foreach ($point in $cornerPoints) {
|
||||
Assert-True ((Get-DistanceToPolyline $point $cornerSource) -le ($allowedRadius + 0.000000001)) 'Every B-spline displacement must stay inside the per-anchor clearance reserve radius.'
|
||||
}
|
||||
|
||||
# A tight reserve may not publish an evaluated B-spline that leaves its 0.005 m movement radius.
|
||||
$tightReserveCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.075
|
||||
|
||||
# A tight endpoint circle that cannot meet the heading=0.2 rad tangent ray must fail, not silently rotate the handle.
|
||||
$misalignedHeadingSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.2 0.08),
|
||||
(New-Point 0.05 0.0 0.05 0.0 0.08),
|
||||
(New-Point 0.10 0.0 0.10 0.0 0.08),
|
||||
(New-Point 0.10 0.05 0.15 ([Math]::PI / 2.0) 0.08),
|
||||
(New-Point 0.10 0.10 0.20 ([Math]::PI / 2.0) 0.08))
|
||||
$misalignedHeadingCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $misalignedHeadingSource)) 0.075
|
||||
$requiredFailures = New-Object System.Collections.Generic.List[string]
|
||||
if (Get-PropertyValue $tightReserveCandidate 'Succeeded') {
|
||||
[void]$requiredFailures.Add('A tight reserve published an evaluated candidate outside its permitted movement radius.')
|
||||
}
|
||||
if (Get-PropertyValue $misalignedHeadingCandidate 'Succeeded') {
|
||||
[void]$requiredFailures.Add('A tight reserve silently accepted an endpoint handle that cannot follow the supplied travel tangent.')
|
||||
}
|
||||
Assert-Equal 0 $requiredFailures.Count ([string]::Join(' ', $requiredFailures))
|
||||
|
||||
# Adjacent direction segments retain their duplicated switch pose and independent topology; no fit may cross the switch.
|
||||
$reverseSource = @(
|
||||
(New-Point 0.10 0.10 0.0 ([Math]::PI / 2.0) 0.08 $true),
|
||||
|
||||
Reference in New Issue
Block a user