feat: add cubic b-spline path smoother
This commit is contained in:
梁薄云
@@ -0,0 +1,322 @@
|
||||
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 EndpointTangentScale = 1d / 3d;
|
||||
private const double EndpointProbeParameter = 1e-6d;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
|
||||
|
||||
/// <inheritdoc />
|
||||
public SmoothingCandidate Smooth(
|
||||
SmoothingAlgorithmInput input,
|
||||
double effectiveStrength,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (input == null || input.OriginalPath == 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,
|
||||
cancellationToken, out IReadOnlyList<SmoothingPoint2D> points, out string reason))
|
||||
{
|
||||
return 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 strength,
|
||||
double reserveMeters,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothingPoint2D> result,
|
||||
out string reason)
|
||||
{
|
||||
result = null;
|
||||
reason = string.Empty;
|
||||
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;
|
||||
}
|
||||
|
||||
Point2D[] controls = CreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
|
||||
cancellationToken);
|
||||
if (controls == null)
|
||||
{
|
||||
reason = "B 样条控制点构造产生非法数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double[] knots = CreateClampedKnots(controls.Length);
|
||||
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);
|
||||
for (int index = 1; index < uniformIntervals; index++)
|
||||
{
|
||||
AddSample((double)index / uniformIntervals, anchors, controls, knots, sampled, cancellationToken);
|
||||
}
|
||||
AddSample(1d - EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken);
|
||||
AddSample(1d, anchors, controls, knots, sampled, cancellationToken);
|
||||
|
||||
result = sampled;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static Point2D[] CreateControls(
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
TravelDirection direction,
|
||||
double strength,
|
||||
double reserveMeters,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
var 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));
|
||||
|
||||
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));
|
||||
|
||||
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))
|
||||
return null;
|
||||
}
|
||||
return controls;
|
||||
}
|
||||
|
||||
private static void AddSample(
|
||||
double parameter,
|
||||
IReadOnlyList<SmoothingPoint2D> anchors,
|
||||
Point2D[] controls,
|
||||
double[] knots,
|
||||
List<SmoothingPoint2D> output,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Point2D evaluated = Evaluate(controls, knots, parameter);
|
||||
SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter);
|
||||
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));
|
||||
}
|
||||
|
||||
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 SmoothingPoint2D InterpolateAnchor(IReadOnlyList<SmoothingPoint2D> anchors, double parameter)
|
||||
{
|
||||
if (parameter <= 0d) return anchors[0];
|
||||
if (parameter >= 1d) return anchors[anchors.Count - 1];
|
||||
|
||||
double scaled = parameter * (anchors.Count - 1);
|
||||
int leftIndex = (int)Math.Floor(scaled);
|
||||
double ratio = scaled - leftIndex;
|
||||
SmoothingPoint2D left = anchors[leftIndex];
|
||||
SmoothingPoint2D right = anchors[leftIndex + 1];
|
||||
return new SmoothingPoint2D(
|
||||
left.X + ratio * (right.X - left.X),
|
||||
left.Y + ratio * (right.Y - left.Y),
|
||||
left.ArcLength + ratio * (right.ArcLength - left.ArcLength),
|
||||
left.Heading + ratio * (right.Heading - left.Heading),
|
||||
left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
|
||||
left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated);
|
||||
}
|
||||
|
||||
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 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -12,12 +12,14 @@ internal sealed class SmoothingAlgorithmInput
|
||||
PreparedPath originalPath,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
double maximumCollisionCheckStepMeters)
|
||||
double maximumCollisionCheckStepMeters,
|
||||
double minimumClearanceReserveMeters)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
/// <summary>已校验并按方向分段的原始路径。</summary>
|
||||
@@ -31,4 +33,7 @@ internal sealed class SmoothingAlgorithmInput
|
||||
|
||||
/// <summary>连续车体碰撞检查的最大步长，单位 m。</summary>
|
||||
internal double MaximumCollisionCheckStepMeters { get; }
|
||||
|
||||
/// <summary>候选几何必须从原始保守净空中预留的最小安全余量，单位 m。</summary>
|
||||
internal double MinimumClearanceReserveMeters { get; }
|
||||
}
|
||||
|
||||
@@ -277,7 +277,7 @@ internal sealed class SmoothingAlgorithmRunner
|
||||
MaximumCurvaturePerMeter = 100d,
|
||||
MinimumTurningRadiusMeters = 0.01d,
|
||||
};
|
||||
return new SmoothingAlgorithmInput(new PreparedPath(originalSegments), mapResult.Map, vehicle, 0.05d);
|
||||
return new SmoothingAlgorithmInput(new PreparedPath(originalSegments), mapResult.Map, vehicle, 0.05d, 0.02d);
|
||||
}
|
||||
|
||||
private static SmoothingCandidate CreateAcceptedCandidate()
|
||||
|
||||
@@ -0,0 +1,217 @@
|
||||
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,
|
||||
[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-spline test must create an explicit empty planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
|
||||
$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
|
||||
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, @(
|
||||
(New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None))
|
||||
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline 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-DistanceToSegment($Point, $Left, $Right) {
|
||||
$deltaX = $Right.X - $Left.X
|
||||
$deltaY = $Right.Y - $Left.Y
|
||||
$lengthSquared = $deltaX * $deltaX + $deltaY * $deltaY
|
||||
if ($lengthSquared -le 0.0) { return Get-PointDistance $Point $Left }
|
||||
$projection = (($Point.X - $Left.X) * $deltaX + ($Point.Y - $Left.Y) * $deltaY) / $lengthSquared
|
||||
$projection = [Math]::Max(0.0, [Math]::Min(1.0, $projection))
|
||||
$closest = New-Object PSObject -Property @{
|
||||
X = $Left.X + $projection * $deltaX
|
||||
Y = $Left.Y + $projection * $deltaY
|
||||
}
|
||||
return Get-PointDistance $Point $closest
|
||||
}
|
||||
|
||||
function Get-DistanceToPolyline($Point, [object[]]$SourcePoints) {
|
||||
$minimum = [double]::PositiveInfinity
|
||||
for ($index = 1; $index -lt $SourcePoints.Count; $index++) {
|
||||
$minimum = [Math]::Min($minimum, (Get-DistanceToSegment $Point $SourcePoints[$index - 1] $SourcePoints[$index]))
|
||||
}
|
||||
return $minimum
|
||||
}
|
||||
|
||||
function Get-TravelAngle($Left, $Right) {
|
||||
return [Math]::Atan2($Right.Y - $Left.Y, $Right.X - $Left.X)
|
||||
}
|
||||
|
||||
function Get-AngleDifference([double]$Left, [double]$Right) {
|
||||
$difference = $Left - $Right
|
||||
while ($difference -gt [Math]::PI) { $difference -= 2.0 * [Math]::PI }
|
||||
while ($difference -lt -[Math]::PI) { $difference += 2.0 * [Math]::PI }
|
||||
return [Math]::Abs($difference)
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
|
||||
$smootherType = Get-RequiredType ($algorithms + 'CubicBSplineSmoother')
|
||||
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
||||
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
||||
$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]), $null)
|
||||
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry the minimum clearance reserve for per-anchor movement limits.'
|
||||
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
||||
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
||||
Assert-True ($null -ne $smoothMethod) 'CubicBSplineSmoother must implement the internal smoother contract.'
|
||||
Assert-Equal 'CubicBSpline' $smoother.Method.ToString() 'B-spline smoother must identify its public smoothing method.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
|
||||
# Straight samples are returned exactly, so a straight is never distorted or densified.
|
||||
$straightSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0 0.03),
|
||||
(New-Point 1.0 0.0 1.0 0.0 0.03),
|
||||
(New-Point 2.0 0.0 2.0 0.0 0.03),
|
||||
(New-Point 3.0 0.0 3.0 0.0 0.03))
|
||||
$straightResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)) 0.02
|
||||
Assert-Equal 1 $straightResult.Count 'A single direction segment must produce exactly one candidate segment.'
|
||||
Assert-Equal $straightSource.Count $straightResult[0].Points.Count 'A straight must retain its original samples.'
|
||||
for ($index = 0; $index -lt $straightSource.Count; $index++) {
|
||||
Assert-Near $straightSource[$index].X $straightResult[0].Points[$index].X 0.0 'Straight X coordinates must remain exact.'
|
||||
Assert-Near $straightSource[$index].Y $straightResult[0].Points[$index].Y 0.0 'Straight Y coordinates must remain exact.'
|
||||
}
|
||||
|
||||
# A five-anchor corner uses the preprocessor's 0.05 m sampling scale. It must retain exact endpoint poses,
|
||||
# follow endpoint travel tangents, turn continuously, and stay within the per-anchor clearance reserve radius.
|
||||
$cornerSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0 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))
|
||||
$cornerResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02
|
||||
$cornerPoints = @($cornerResult[0].Points)
|
||||
Assert-True ($cornerPoints.Count -gt $cornerSource.Count) 'A non-straight B-spline candidate must provide sampled curve geometry.'
|
||||
$cornerStart = $cornerPoints[0]
|
||||
$cornerEnd = $cornerPoints[$cornerPoints.Count - 1]
|
||||
Assert-Near $cornerSource[0].X $cornerStart.X 0.0 'B-spline start X must be exact.'
|
||||
Assert-Near $cornerSource[0].Y $cornerStart.Y 0.0 'B-spline start Y must be exact.'
|
||||
Assert-Near $cornerSource[$cornerSource.Count - 1].X $cornerEnd.X 0.0 'B-spline end X must be exact.'
|
||||
Assert-Near $cornerSource[$cornerSource.Count - 1].Y $cornerEnd.Y 0.0 'B-spline end Y must be exact.'
|
||||
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[0] $cornerPoints[1]) 0.0) 0.02 'B-spline start travel tangent must follow the supplied forward heading.'
|
||||
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[$cornerPoints.Count - 2] $cornerPoints[$cornerPoints.Count - 1]) ([Math]::PI / 2.0)) 0.02 'B-spline end travel tangent must follow the supplied forward heading.'
|
||||
for ($index = 2; $index -lt $cornerPoints.Count; $index++) {
|
||||
$previousAngle = Get-TravelAngle $cornerPoints[$index - 2] $cornerPoints[$index - 1]
|
||||
$currentAngle = Get-TravelAngle $cornerPoints[$index - 1] $cornerPoints[$index]
|
||||
Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.'
|
||||
}
|
||||
|
||||
# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance.
|
||||
$allowedRadius = 0.06
|
||||
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.'
|
||||
}
|
||||
|
||||
# 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),
|
||||
(New-Point 0.10 0.05 0.05 ([Math]::PI / 2.0) 0.08),
|
||||
(New-Point 0.10 0.0 0.10 ([Math]::PI / 2.0) 0.08))
|
||||
$switchResult = Invoke-Smoothing @(
|
||||
(New-DirectionSegment 0 $forward $cornerSource $false $true),
|
||||
(New-DirectionSegment 1 $reverse $reverseSource $true $false)) 0.02
|
||||
Assert-Equal 2 $switchResult.Count 'B-spline smoothing must preserve each direction segment boundary.'
|
||||
Assert-True $switchResult[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
|
||||
Assert-True $switchResult[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
|
||||
$switchLeft = $switchResult[0].Points[$switchResult[0].Points.Count - 1]
|
||||
$switchRight = $switchResult[1].Points[0]
|
||||
Assert-Near $switchLeft.X $switchRight.X 0.0 'B-spline smoothing must retain the duplicated gear-switch X pose.'
|
||||
Assert-Near $switchLeft.Y $switchRight.Y 0.0 'B-spline smoothing must retain the duplicated gear-switch Y pose.'
|
||||
|
||||
Write-Output 'Path smoothing cubic B-spline checks passed.'
|
||||
Reference in New Issue
Block a user