feat: add path smoothing geometry foundation
This commit is contained in:
梁薄云
@@ -0,0 +1,144 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>按单一方向段的弧长线性插值并保留精确锚点的确定性重采样器。</summary>
|
||||
public sealed class ArcLengthResampler
|
||||
{
|
||||
private const double Tolerance = 1e-10d;
|
||||
|
||||
/// <summary>以目标间距重采样一个方向段；段末锚点始终原样保留。</summary>
|
||||
public bool TryResample(
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
double spacingMeters,
|
||||
out IReadOnlyList<SmoothingPoint2D> resampled,
|
||||
out string reason)
|
||||
{
|
||||
resampled = EmptyPoints();
|
||||
reason = string.Empty;
|
||||
if (points == null || points.Count == 0 || !NumericGuard.IsPositiveFinite(spacingMeters))
|
||||
{
|
||||
reason = "重采样点集或采样间距无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 0; index < points.Count; index++)
|
||||
{
|
||||
if (!IsValidPoint(points[index]))
|
||||
{
|
||||
reason = "重采样输入包含非法数值。";
|
||||
return false;
|
||||
}
|
||||
if (index == 0) continue;
|
||||
|
||||
SmoothingPoint2D previous = points[index - 1];
|
||||
SmoothingPoint2D current = points[index];
|
||||
if (current.ArcLength <= previous.ArcLength + Tolerance)
|
||||
{
|
||||
reason = "同一方向段的弧长必须严格增加。";
|
||||
return false;
|
||||
}
|
||||
double distance = Distance(previous, current);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= Tolerance)
|
||||
{
|
||||
reason = "同一方向段中不允许重复位姿或数值溢出的距离。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
if (points.Count == 1)
|
||||
{
|
||||
resampled = CopyReadOnly(points);
|
||||
return true;
|
||||
}
|
||||
|
||||
var output = new List<SmoothingPoint2D> { points[0] };
|
||||
double firstArc = points[0].ArcLength;
|
||||
double finalArc = points[points.Count - 1].ArcLength;
|
||||
int rightIndex = 1;
|
||||
for (double targetArc = firstArc + spacingMeters;
|
||||
targetArc < finalArc - Tolerance;
|
||||
targetArc += spacingMeters)
|
||||
{
|
||||
while (rightIndex < points.Count - 1 && points[rightIndex].ArcLength < targetArc)
|
||||
rightIndex++;
|
||||
|
||||
SmoothingPoint2D left = points[rightIndex - 1];
|
||||
SmoothingPoint2D right = points[rightIndex];
|
||||
double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
|
||||
double unwrappedHeading = left.UnwrappedHeading +
|
||||
ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
|
||||
output.Add(new SmoothingPoint2D(
|
||||
left.X + ratio * (right.X - left.X),
|
||||
left.Y + ratio * (right.Y - left.Y),
|
||||
targetArc,
|
||||
AngleMath.NormalizeRadians(unwrappedHeading),
|
||||
unwrappedHeading,
|
||||
Math.Min(left.BodyClearance, right.BodyClearance),
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
|
||||
// Appending the original object, rather than interpolating at the final arc, preserves the exact endpoint.
|
||||
output.Add(points[points.Count - 1]);
|
||||
resampled = new ReadOnlyCollection<SmoothingPoint2D>(output);
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>重采样一个完整方向段并保留其换向拓扑标记。</summary>
|
||||
public bool TryResample(
|
||||
PreparedDirectionSegment segment,
|
||||
double spacingMeters,
|
||||
out PreparedDirectionSegment resampled,
|
||||
out string reason)
|
||||
{
|
||||
resampled = null;
|
||||
reason = string.Empty;
|
||||
if (segment == null)
|
||||
{
|
||||
reason = "待重采样方向段为空。";
|
||||
return false;
|
||||
}
|
||||
if (!TryResample(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> points, out reason))
|
||||
return false;
|
||||
|
||||
resampled = new PreparedDirectionSegment(
|
||||
segment.SegmentIndex,
|
||||
segment.Direction,
|
||||
points,
|
||||
segment.StartsAtGearSwitch,
|
||||
segment.EndsAtGearSwitch);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(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 Distance(SmoothingPoint2D left, SmoothingPoint2D right)
|
||||
{
|
||||
double x = right.X - left.X;
|
||||
double y = right.Y - left.Y;
|
||||
return Math.Sqrt(x * x + y * y);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingPoint2D> EmptyPoints()
|
||||
{
|
||||
return new ReadOnlyCollection<SmoothingPoint2D>(new List<SmoothingPoint2D>());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>同一几何分析器产生的路径、方向段和未验证质量统计。</summary>
|
||||
public sealed class PathGeometryAnalysis
|
||||
{
|
||||
internal PathGeometryAnalysis(
|
||||
IReadOnlyList<SmoothedPathPoint> path,
|
||||
IReadOnlyList<SmoothedPathSegment> segments,
|
||||
double pathLengthMeters,
|
||||
double maximumAbsoluteVehicleCurvaturePerMeter,
|
||||
double rootMeanSquareVehicleCurvaturePerMeter,
|
||||
double totalAbsoluteCurvatureVariationPerMeter,
|
||||
double curvatureVariationEnergy,
|
||||
double minimumBodyClearanceMeters)
|
||||
{
|
||||
Path = CopyReadOnly(path);
|
||||
Segments = CopyReadOnly(segments);
|
||||
PathLengthMeters = pathLengthMeters;
|
||||
MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
|
||||
RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
|
||||
TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
|
||||
CurvatureVariationEnergy = curvatureVariationEnergy;
|
||||
MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
|
||||
}
|
||||
|
||||
/// <summary>完成几何重计算的不可变路径。</summary>
|
||||
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
||||
|
||||
/// <summary>完整覆盖 <see cref="Path"/> 的不可变方向段。</summary>
|
||||
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
||||
|
||||
/// <summary>路径总长度，单位 m。</summary>
|
||||
public double PathLengthMeters { get; }
|
||||
|
||||
/// <summary>车辆曲率绝对值峰值，单位 1/m。</summary>
|
||||
public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>车辆曲率均方根，单位 1/m。</summary>
|
||||
public double RootMeanSquareVehicleCurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>不跨换向点累计的绝对曲率变化，单位 1/m。</summary>
|
||||
public double TotalAbsoluteCurvatureVariationPerMeter { get; }
|
||||
|
||||
/// <summary>不跨换向点累计的曲率变化能量。</summary>
|
||||
public double CurvatureVariationEnergy { get; }
|
||||
|
||||
/// <summary>输入点携带的最小保守净空，单位 m。</summary>
|
||||
public double MinimumBodyClearanceMeters { get; }
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source == null ? 0 : source.Count);
|
||||
if (source != null)
|
||||
{
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
}
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,401 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>在每个单独方向段内统一重采样、恢复航向并计算几何曲率。</summary>
|
||||
public sealed class PathGeometryAnalyzer
|
||||
{
|
||||
private const double MinimumDistanceMeters = 1e-10d;
|
||||
private const double BoundaryToleranceMeters = 1e-8d;
|
||||
|
||||
/// <summary>
|
||||
/// 对候选方向段进行确定性几何分析。换向点两侧永不参与同一次差分。
|
||||
/// </summary>
|
||||
public bool TryAnalyze(
|
||||
IReadOnlyList<PreparedDirectionSegment> candidateSegments,
|
||||
double spacingMeters,
|
||||
out PathGeometryAnalysis analysis,
|
||||
out string reason)
|
||||
{
|
||||
analysis = null;
|
||||
reason = string.Empty;
|
||||
if (candidateSegments == null || candidateSegments.Count == 0 ||
|
||||
!NumericGuard.IsPositiveFinite(spacingMeters))
|
||||
{
|
||||
reason = "候选方向段或输出采样间距无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var outputPath = new List<SmoothedPathPoint>();
|
||||
var outputSegments = new List<SmoothedPathSegment>();
|
||||
double cumulativeArcLength = 0d;
|
||||
double previousOutputHeading = 0d;
|
||||
double previousOutputUnwrappedHeading = 0d;
|
||||
bool hasPreviousOutputHeading = false;
|
||||
double maximumAbsoluteVehicleCurvature = 0d;
|
||||
double curvatureSquareSum = 0d;
|
||||
int curvatureSampleCount = 0;
|
||||
double totalCurvatureVariation = 0d;
|
||||
double curvatureVariationEnergy = 0d;
|
||||
double minimumClearance = double.PositiveInfinity;
|
||||
|
||||
for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
|
||||
{
|
||||
PreparedDirectionSegment segment = candidateSegments[segmentIndex];
|
||||
if (!IsValidSegment(segment, segmentIndex, out reason)) return false;
|
||||
if (!TryValidateBoundary(candidateSegments, segmentIndex, out reason)) return false;
|
||||
|
||||
if (!TryResampleByGeometry(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> samples, out reason))
|
||||
return false;
|
||||
|
||||
if (!TryAnalyzeSegment(
|
||||
segment,
|
||||
samples,
|
||||
ref cumulativeArcLength,
|
||||
ref previousOutputHeading,
|
||||
ref previousOutputUnwrappedHeading,
|
||||
ref hasPreviousOutputHeading,
|
||||
outputPath,
|
||||
out double segmentMaximumCurvature,
|
||||
out double segmentCurvatureSquareSum,
|
||||
out int segmentCurvatureSampleCount,
|
||||
out double segmentVariation,
|
||||
out double segmentVariationEnergy,
|
||||
out double segmentMinimumClearance,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, segmentMaximumCurvature);
|
||||
curvatureSquareSum += segmentCurvatureSquareSum;
|
||||
curvatureSampleCount += segmentCurvatureSampleCount;
|
||||
totalCurvatureVariation += segmentVariation;
|
||||
curvatureVariationEnergy += segmentVariationEnergy;
|
||||
minimumClearance = Math.Min(minimumClearance, segmentMinimumClearance);
|
||||
int endIndex = outputPath.Count - 1;
|
||||
int startIndex = endIndex - samples.Count + 1;
|
||||
outputSegments.Add(new SmoothedPathSegment(
|
||||
segment.SegmentIndex,
|
||||
segment.Direction,
|
||||
startIndex,
|
||||
endIndex,
|
||||
segment.StartsAtGearSwitch,
|
||||
segment.EndsAtGearSwitch));
|
||||
}
|
||||
|
||||
double rmsCurvature = curvatureSampleCount == 0 ? 0d : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
|
||||
analysis = new PathGeometryAnalysis(
|
||||
outputPath,
|
||||
outputSegments,
|
||||
cumulativeArcLength,
|
||||
maximumAbsoluteVehicleCurvature,
|
||||
rmsCurvature,
|
||||
totalCurvatureVariation,
|
||||
curvatureVariationEnergy,
|
||||
minimumClearance);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryAnalyzeSegment(
|
||||
PreparedDirectionSegment segment,
|
||||
IReadOnlyList<SmoothingPoint2D> samples,
|
||||
ref double cumulativeArcLength,
|
||||
ref double previousOutputHeading,
|
||||
ref double previousOutputUnwrappedHeading,
|
||||
ref bool hasPreviousOutputHeading,
|
||||
List<SmoothedPathPoint> output,
|
||||
out double maximumAbsoluteVehicleCurvature,
|
||||
out double curvatureSquareSum,
|
||||
out int curvatureSampleCount,
|
||||
out double totalCurvatureVariation,
|
||||
out double curvatureVariationEnergy,
|
||||
out double minimumClearance,
|
||||
out string reason)
|
||||
{
|
||||
maximumAbsoluteVehicleCurvature = 0d;
|
||||
curvatureSquareSum = 0d;
|
||||
curvatureSampleCount = 0;
|
||||
totalCurvatureVariation = 0d;
|
||||
curvatureVariationEnergy = 0d;
|
||||
minimumClearance = double.PositiveInfinity;
|
||||
reason = string.Empty;
|
||||
int count = samples.Count;
|
||||
var localArcLengths = new double[count];
|
||||
var headings = new double[count];
|
||||
var unwrappedHeadings = new double[count];
|
||||
var geometricCurvatures = new double[count];
|
||||
|
||||
for (int index = 1; index < count; index++)
|
||||
{
|
||||
double distance = Distance(samples[index - 1], samples[index]);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
|
||||
{
|
||||
reason = "同一方向段中包含重复或退化的路径点。";
|
||||
return false;
|
||||
}
|
||||
localArcLengths[index] = localArcLengths[index - 1] + distance;
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
double travelHeading;
|
||||
if (count == 1)
|
||||
{
|
||||
travelHeading = segment.Direction == TravelDirection.Forward
|
||||
? samples[index].Heading
|
||||
: samples[index].Heading - Math.PI;
|
||||
}
|
||||
else if (index == 0)
|
||||
{
|
||||
travelHeading = Math.Atan2(samples[1].Y - samples[0].Y, samples[1].X - samples[0].X);
|
||||
}
|
||||
else if (index == count - 1)
|
||||
{
|
||||
travelHeading = Math.Atan2(samples[index].Y - samples[index - 1].Y,
|
||||
samples[index].X - samples[index - 1].X);
|
||||
}
|
||||
else
|
||||
{
|
||||
travelHeading = Math.Atan2(samples[index + 1].Y - samples[index - 1].Y,
|
||||
samples[index + 1].X - samples[index - 1].X);
|
||||
}
|
||||
|
||||
double heading = AngleMath.NormalizeRadians(
|
||||
segment.Direction == TravelDirection.Forward ? travelHeading : travelHeading + Math.PI);
|
||||
if (!NumericGuard.IsFinite(heading))
|
||||
{
|
||||
reason = "候选路径航向无法归一化。";
|
||||
return false;
|
||||
}
|
||||
|
||||
headings[index] = heading;
|
||||
if (index == 0 && !hasPreviousOutputHeading)
|
||||
{
|
||||
unwrappedHeadings[index] = heading;
|
||||
}
|
||||
else if (index == 0)
|
||||
{
|
||||
unwrappedHeadings[index] = previousOutputUnwrappedHeading +
|
||||
AngleMath.ShortestSignedDifference(previousOutputHeading, heading);
|
||||
}
|
||||
else
|
||||
{
|
||||
unwrappedHeadings[index] = unwrappedHeadings[index - 1] +
|
||||
AngleMath.ShortestSignedDifference(headings[index - 1], heading);
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
if (count == 1)
|
||||
{
|
||||
geometricCurvatures[index] = 0d;
|
||||
}
|
||||
else if (index == 0)
|
||||
{
|
||||
geometricCurvatures[index] = (unwrappedHeadings[1] - unwrappedHeadings[0]) /
|
||||
(localArcLengths[1] - localArcLengths[0]);
|
||||
}
|
||||
else if (index == count - 1)
|
||||
{
|
||||
geometricCurvatures[index] = (unwrappedHeadings[index] - unwrappedHeadings[index - 1]) /
|
||||
(localArcLengths[index] - localArcLengths[index - 1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
geometricCurvatures[index] = (unwrappedHeadings[index + 1] - unwrappedHeadings[index - 1]) /
|
||||
(localArcLengths[index + 1] - localArcLengths[index - 1]);
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsFinite(geometricCurvatures[index]))
|
||||
{
|
||||
reason = "候选路径曲率计算产生了非法数值。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
for (int index = 0; index < count; index++)
|
||||
{
|
||||
SmoothingPoint2D sample = samples[index];
|
||||
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
|
||||
double vehicleCurvature = directionSign * geometricCurvatures[index];
|
||||
double arcLength = cumulativeArcLength + localArcLengths[index];
|
||||
bool isGearSwitch = index == 0 && segment.StartsAtGearSwitch;
|
||||
SmoothedPathPointSource source = isGearSwitch ? SmoothedPathPointSource.GearSwitch : sample.Source;
|
||||
output.Add(new SmoothedPathPoint(
|
||||
sample.X,
|
||||
sample.Y,
|
||||
headings[index],
|
||||
unwrappedHeadings[index],
|
||||
arcLength,
|
||||
segment.Direction,
|
||||
geometricCurvatures[index],
|
||||
vehicleCurvature,
|
||||
sample.BodyClearance,
|
||||
isGearSwitch,
|
||||
source));
|
||||
|
||||
maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(vehicleCurvature));
|
||||
curvatureSquareSum += vehicleCurvature * vehicleCurvature;
|
||||
curvatureSampleCount++;
|
||||
minimumClearance = Math.Min(minimumClearance, sample.BodyClearance);
|
||||
if (index > 0)
|
||||
{
|
||||
double deltaCurvature = geometricCurvatures[index] - geometricCurvatures[index - 1];
|
||||
double deltaArc = localArcLengths[index] - localArcLengths[index - 1];
|
||||
totalCurvatureVariation += Math.Abs(deltaCurvature);
|
||||
curvatureVariationEnergy += (deltaCurvature / deltaArc) * (deltaCurvature / deltaArc) * deltaArc;
|
||||
}
|
||||
}
|
||||
|
||||
cumulativeArcLength += localArcLengths[count - 1];
|
||||
previousOutputHeading = headings[count - 1];
|
||||
previousOutputUnwrappedHeading = unwrappedHeadings[count - 1];
|
||||
hasPreviousOutputHeading = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryResampleByGeometry(
|
||||
IReadOnlyList<SmoothingPoint2D> input,
|
||||
double spacingMeters,
|
||||
out IReadOnlyList<SmoothingPoint2D> samples,
|
||||
out string reason)
|
||||
{
|
||||
samples = null;
|
||||
reason = string.Empty;
|
||||
var normalized = new List<SmoothingPoint2D>(input.Count);
|
||||
double localArcLength = 0d;
|
||||
for (int index = 0; index < input.Count; index++)
|
||||
{
|
||||
SmoothingPoint2D point = input[index];
|
||||
if (!IsValidPoint(point))
|
||||
{
|
||||
reason = "候选路径点包含非法数值。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (index > 0)
|
||||
{
|
||||
double distance = Distance(input[index - 1], point);
|
||||
if (!NumericGuard.IsFinite(distance) || distance <= MinimumDistanceMeters)
|
||||
{
|
||||
reason = "同一方向段中包含重复或退化的路径点。";
|
||||
return false;
|
||||
}
|
||||
localArcLength += distance;
|
||||
}
|
||||
|
||||
normalized.Add(new SmoothingPoint2D(
|
||||
point.X, point.Y, localArcLength, point.Heading, point.UnwrappedHeading,
|
||||
point.BodyClearance, point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
|
||||
if (normalized.Count == 1)
|
||||
{
|
||||
samples = normalized;
|
||||
return true;
|
||||
}
|
||||
|
||||
var result = new List<SmoothingPoint2D> { normalized[0] };
|
||||
double finalArc = normalized[normalized.Count - 1].ArcLength;
|
||||
int rightIndex = 1;
|
||||
for (double targetArc = spacingMeters; targetArc < finalArc - MinimumDistanceMeters; targetArc += spacingMeters)
|
||||
{
|
||||
while (rightIndex < normalized.Count - 1 && normalized[rightIndex].ArcLength < targetArc)
|
||||
rightIndex++;
|
||||
SmoothingPoint2D left = normalized[rightIndex - 1];
|
||||
SmoothingPoint2D right = normalized[rightIndex];
|
||||
double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
|
||||
double unwrappedHeading = left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
|
||||
result.Add(new SmoothingPoint2D(
|
||||
left.X + ratio * (right.X - left.X),
|
||||
left.Y + ratio * (right.Y - left.Y),
|
||||
targetArc,
|
||||
AngleMath.NormalizeRadians(unwrappedHeading),
|
||||
unwrappedHeading,
|
||||
Math.Min(left.BodyClearance, right.BodyClearance),
|
||||
false,
|
||||
SmoothedPathPointSource.Interpolated));
|
||||
}
|
||||
|
||||
result.Add(normalized[normalized.Count - 1]);
|
||||
samples = result;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidSegment(PreparedDirectionSegment segment, int expectedIndex, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (segment == null || segment.SegmentIndex != expectedIndex ||
|
||||
(segment.Direction != TravelDirection.Forward && segment.Direction != TravelDirection.Reverse) ||
|
||||
segment.Points == null || segment.Points.Count == 0)
|
||||
{
|
||||
reason = "候选方向段索引、方向或点集无效。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryValidateBoundary(
|
||||
IReadOnlyList<PreparedDirectionSegment> segments,
|
||||
int segmentIndex,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
PreparedDirectionSegment current = segments[segmentIndex];
|
||||
if (segmentIndex == segments.Count - 1 && current.EndsAtGearSwitch)
|
||||
{
|
||||
reason = "末个方向段不得声明不存在的后续换向点。";
|
||||
return false;
|
||||
}
|
||||
if (segmentIndex == 0)
|
||||
{
|
||||
if (current.StartsAtGearSwitch)
|
||||
{
|
||||
reason = "首个方向段不得从换向点开始。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
PreparedDirectionSegment previous = segments[segmentIndex - 1];
|
||||
if (previous == null || !previous.EndsAtGearSwitch || !current.StartsAtGearSwitch ||
|
||||
previous.Direction == current.Direction)
|
||||
{
|
||||
reason = "方向段边界必须是前后成对且方向相反的换向点。";
|
||||
return false;
|
||||
}
|
||||
|
||||
SmoothingPoint2D previousEnd = previous.Points[previous.Points.Count - 1];
|
||||
SmoothingPoint2D currentStart = current.Points[0];
|
||||
if (!currentStart.IsGearSwitchPoint ||
|
||||
Math.Abs(previousEnd.X - currentStart.X) > BoundaryToleranceMeters ||
|
||||
Math.Abs(previousEnd.Y - currentStart.Y) > BoundaryToleranceMeters ||
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(previousEnd.Heading, currentStart.Heading)) > BoundaryToleranceMeters)
|
||||
{
|
||||
reason = "换向点两侧必须保留同一位姿和航向。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(SmoothingPoint2D point)
|
||||
{
|
||||
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
||||
NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
||||
NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.BodyClearance) &&
|
||||
point.BodyClearance >= 0d;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,194 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>校验粗路径契约、保护换向拓扑并产生统一间距的平滑输入。</summary>
|
||||
public sealed class PathSmoothingPreprocessor
|
||||
{
|
||||
private const double Tolerance = 1e-8d;
|
||||
private readonly ArcLengthResampler _resampler;
|
||||
|
||||
/// <summary>创建使用默认确定性重采样器的预处理器。</summary>
|
||||
public PathSmoothingPreprocessor()
|
||||
: this(new ArcLengthResampler())
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>创建使用指定重采样器的预处理器。</summary>
|
||||
public PathSmoothingPreprocessor(ArcLengthResampler resampler)
|
||||
{
|
||||
_resampler = resampler ?? throw new ArgumentNullException(nameof(resampler));
|
||||
}
|
||||
|
||||
/// <summary>将一条粗路径请求校验、按方向拆分并按配置间距重采样。</summary>
|
||||
public bool TryPrepare(PathSmoothingRequest request, out PreparedPath preparedPath, out string reason)
|
||||
{
|
||||
preparedPath = null;
|
||||
reason = string.Empty;
|
||||
if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
|
||||
request.CoarsePath == null || request.Segments == null || request.CoarsePath.Count == 0 || request.Segments.Count == 0)
|
||||
{
|
||||
reason = "平滑请求缺少粗路径、方向分段、地图、车辆或配置。";
|
||||
return false;
|
||||
}
|
||||
|
||||
PathSmoothingConfiguration configuration = request.Configuration;
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters))
|
||||
{
|
||||
reason = "平滑输出采样间距无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!ValidatePathPoints(request.CoarsePath, out reason) ||
|
||||
!ValidateSegments(request.CoarsePath, request.Segments, out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var preparedSegments = new List<PreparedDirectionSegment>(request.Segments.Count);
|
||||
for (int segmentIndex = 0; segmentIndex < request.Segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment sourceSegment = request.Segments[segmentIndex];
|
||||
double segmentStartArcLength = request.CoarsePath[sourceSegment.StartIndex].ArcLength;
|
||||
var segmentPoints = new List<SmoothingPoint2D>(sourceSegment.EndIndex - sourceSegment.StartIndex + 1);
|
||||
for (int pointIndex = sourceSegment.StartIndex; pointIndex <= sourceSegment.EndIndex; pointIndex++)
|
||||
{
|
||||
CoarsePathPoint point = request.CoarsePath[pointIndex];
|
||||
bool isGearSwitch = point.IsGearSwitchPoint;
|
||||
segmentPoints.Add(new SmoothingPoint2D(
|
||||
point.X,
|
||||
point.Y,
|
||||
point.ArcLength - segmentStartArcLength,
|
||||
point.Heading,
|
||||
point.UnwrappedHeading,
|
||||
point.BodyClearance,
|
||||
isGearSwitch,
|
||||
isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
|
||||
}
|
||||
|
||||
var unresampled = new PreparedDirectionSegment(
|
||||
sourceSegment.SegmentIndex,
|
||||
sourceSegment.Direction,
|
||||
segmentPoints,
|
||||
sourceSegment.StartsAtGearSwitch,
|
||||
sourceSegment.EndsAtGearSwitch);
|
||||
if (!_resampler.TryResample(unresampled, configuration.OutputSpacingMeters, out PreparedDirectionSegment resampled, out reason))
|
||||
return false;
|
||||
preparedSegments.Add(resampled);
|
||||
}
|
||||
|
||||
preparedPath = new PreparedPath(preparedSegments);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidatePathPoints(IReadOnlyList<CoarsePathPoint> path, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
CoarsePathPoint first = path[0];
|
||||
if (!IsValidPoint(first) || first.IsGearSwitchPoint || Math.Abs(first.ArcLength) > Tolerance)
|
||||
{
|
||||
reason = "粗路径首点无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int index = 1; index < path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint previous = path[index - 1];
|
||||
CoarsePathPoint current = path[index];
|
||||
if (!IsValidPoint(current) || current.ArcLength + Tolerance < previous.ArcLength)
|
||||
{
|
||||
reason = "粗路径包含非法数值或非递增弧长。";
|
||||
return false;
|
||||
}
|
||||
|
||||
double expectedHeadingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
|
||||
if (!NumericGuard.IsFinite(expectedHeadingDelta) ||
|
||||
Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedHeadingDelta) > Tolerance)
|
||||
{
|
||||
reason = "粗路径展开航向不连续。";
|
||||
return false;
|
||||
}
|
||||
|
||||
bool duplicatePoseAndArc = Math.Abs(current.X - previous.X) <= Tolerance &&
|
||||
Math.Abs(current.Y - previous.Y) <= Tolerance &&
|
||||
Math.Abs(current.ArcLength - previous.ArcLength) <= Tolerance &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance;
|
||||
if (duplicatePoseAndArc)
|
||||
{
|
||||
if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
|
||||
{
|
||||
reason = "粗路径包含非法的重复点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
|
||||
{
|
||||
reason = "粗路径普通点必须有正弧长增量且不得标记为换向点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateSegments(
|
||||
IReadOnlyList<CoarsePathPoint> path,
|
||||
IReadOnlyList<PathSegment> segments,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
int expectedStartIndex = 0;
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
PathSegment segment = segments[segmentIndex];
|
||||
if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
|
||||
segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
|
||||
segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
|
||||
{
|
||||
reason = "粗路径方向分段索引或起始换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
|
||||
{
|
||||
if (path[pointIndex].Direction != segment.Direction)
|
||||
{
|
||||
reason = "粗路径方向分段包含不同方向的点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool hasNextSegment = segmentIndex + 1 < segments.Count;
|
||||
bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
|
||||
path[segment.EndIndex + 1].IsGearSwitchPoint;
|
||||
if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
|
||||
{
|
||||
reason = "粗路径方向分段末尾换向标记无效。";
|
||||
return false;
|
||||
}
|
||||
expectedStartIndex = segment.EndIndex + 1;
|
||||
}
|
||||
|
||||
if (expectedStartIndex != path.Count)
|
||||
{
|
||||
reason = "粗路径方向分段未完整覆盖全部点。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidPoint(CoarsePathPoint point)
|
||||
{
|
||||
return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
|
||||
NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
|
||||
NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
|
||||
NumericGuard.IsFinite(point.VehicleCurvature) && NumericGuard.IsFinite(point.BodyClearance) &&
|
||||
point.BodyClearance >= 0d &&
|
||||
(point.Direction == TravelDirection.Forward || point.Direction == TravelDirection.Reverse) &&
|
||||
Enum.IsDefined(typeof(CoarsePathPointSource), point.Source) &&
|
||||
Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>已校验、已按单一行驶方向分割并重采样的路径段。</summary>
|
||||
public sealed class PreparedDirectionSegment
|
||||
{
|
||||
/// <summary>创建不可变方向段。</summary>
|
||||
public PreparedDirectionSegment(
|
||||
int segmentIndex,
|
||||
TravelDirection direction,
|
||||
IReadOnlyList<SmoothingPoint2D> points,
|
||||
bool startsAtGearSwitch,
|
||||
bool endsAtGearSwitch)
|
||||
{
|
||||
if (segmentIndex < 0) throw new ArgumentOutOfRangeException(nameof(segmentIndex));
|
||||
if (points == null || points.Count == 0) throw new ArgumentException("A prepared segment requires points.", nameof(points));
|
||||
|
||||
SegmentIndex = segmentIndex;
|
||||
Direction = direction;
|
||||
Points = CopyReadOnly(points);
|
||||
StartsAtGearSwitch = startsAtGearSwitch;
|
||||
EndsAtGearSwitch = endsAtGearSwitch;
|
||||
}
|
||||
|
||||
/// <summary>从零开始的分段序号；在 <see cref="PreparedPath.Segments"/> 中必须与其位置一致。</summary>
|
||||
public int SegmentIndex { get; }
|
||||
|
||||
/// <summary>该段的唯一行驶方向。</summary>
|
||||
public TravelDirection Direction { get; }
|
||||
|
||||
/// <summary>不包含相邻段点的本段不可变采样点。</summary>
|
||||
public IReadOnlyList<SmoothingPoint2D> Points { get; }
|
||||
|
||||
/// <summary>本段首点是否为换向后保留的新方向点。</summary>
|
||||
public bool StartsAtGearSwitch { get; }
|
||||
|
||||
/// <summary>本段末点之后是否紧邻换向点。</summary>
|
||||
public bool EndsAtGearSwitch { get; }
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,49 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>已校验并按方向拆分的粗路径输入快照。</summary>
|
||||
public sealed class PreparedPath
|
||||
{
|
||||
/// <summary>创建不可变预处理路径。</summary>
|
||||
public PreparedPath(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
if (segments == null || segments.Count == 0)
|
||||
throw new ArgumentException("A prepared path requires direction segments.", nameof(segments));
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
if (segments[segmentIndex] == null)
|
||||
throw new ArgumentException("A prepared path cannot contain null direction segments.", nameof(segments));
|
||||
}
|
||||
|
||||
Segments = CopyReadOnly(segments);
|
||||
Points = Flatten(Segments);
|
||||
}
|
||||
|
||||
/// <summary>按原始前进/倒车拓扑排列的方向段。</summary>
|
||||
public IReadOnlyList<PreparedDirectionSegment> Segments { get; }
|
||||
|
||||
/// <summary>将所有方向段顺序拼接后的点；换向重复点保留两次。</summary>
|
||||
public IReadOnlyList<SmoothingPoint2D> Points { get; }
|
||||
|
||||
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
||||
{
|
||||
var copy = new List<T>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
|
||||
return new ReadOnlyCollection<T>(copy);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothingPoint2D> Flatten(IReadOnlyList<PreparedDirectionSegment> segments)
|
||||
{
|
||||
var points = new List<SmoothingPoint2D>();
|
||||
for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
|
||||
{
|
||||
PreparedDirectionSegment segment = segments[segmentIndex];
|
||||
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
||||
points.Add(segment.Points[pointIndex]);
|
||||
}
|
||||
return new ReadOnlyCollection<SmoothingPoint2D>(points);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
|
||||
/// <summary>供平滑算法处理的二维路径采样点；所有长度单位均为 m，航向单位为 rad。</summary>
|
||||
public sealed class SmoothingPoint2D
|
||||
{
|
||||
/// <summary>创建不可变二维路径点。</summary>
|
||||
public SmoothingPoint2D(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double arcLengthMeters,
|
||||
double headingRadians,
|
||||
double unwrappedHeadingRadians,
|
||||
double bodyClearanceMeters,
|
||||
bool isGearSwitchPoint,
|
||||
SmoothedPathPointSource source)
|
||||
{
|
||||
X = xMeters;
|
||||
Y = yMeters;
|
||||
ArcLength = arcLengthMeters;
|
||||
Heading = headingRadians;
|
||||
UnwrappedHeading = unwrappedHeadingRadians;
|
||||
BodyClearance = bodyClearanceMeters;
|
||||
IsGearSwitchPoint = isGearSwitchPoint;
|
||||
Source = source;
|
||||
}
|
||||
|
||||
/// <summary>世界 X 坐标，单位 m。</summary>
|
||||
public double X { get; }
|
||||
|
||||
/// <summary>世界 Y 坐标，单位 m。</summary>
|
||||
public double Y { get; }
|
||||
|
||||
/// <summary>本方向段中的累计弧长，单位 m。</summary>
|
||||
public double ArcLength { get; }
|
||||
|
||||
/// <summary>归一化的车辆航向，单位 rad。</summary>
|
||||
public double Heading { get; }
|
||||
|
||||
/// <summary>连续展开的车辆航向，单位 rad。</summary>
|
||||
public double UnwrappedHeading { get; }
|
||||
|
||||
/// <summary>输入路径携带的保守净空，单位 m。</summary>
|
||||
public double BodyClearance { get; }
|
||||
|
||||
/// <summary>该点是否为新方向段开始处的换向点。</summary>
|
||||
public bool IsGearSwitchPoint { get; }
|
||||
|
||||
/// <summary>该点在平滑流程中的来源。</summary>
|
||||
public SmoothedPathPointSource Source { get; }
|
||||
}
|
||||
@@ -0,0 +1,377 @@
|
||||
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 Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
||||
try {
|
||||
& $Action
|
||||
}
|
||||
catch {
|
||||
return
|
||||
}
|
||||
throw $Message
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function New-GeometryPoint(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
[double]$Heading,
|
||||
[double]$UnwrappedHeading,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($pointType, @(
|
||||
$X, $Y, $ArcLength, $Heading, $UnwrappedHeading,
|
||||
[double]1.0, $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-CoarsePoint(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
$Direction,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($coarsePointType, @(
|
||||
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
|
||||
[double]0.0, [double]1.0, $IsGearSwitch, $coarseAnchor))
|
||||
}
|
||||
|
||||
function Invoke-Analysis([object[]]$Segments, [double]$Spacing = 0.05) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
|
||||
$arguments = [object[]]@($typedSegments, $Spacing, $null, $null)
|
||||
$accepted = $analyzeMethod.Invoke($analyzer, $arguments)
|
||||
$description = [string]::Join(',', @($Segments | ForEach-Object {
|
||||
"index=$($_.SegmentIndex);direction=$($_.Direction);points=$($_.Points.Count)"
|
||||
}))
|
||||
Assert-True $accepted ("Geometry analysis must accept the analytic candidate. Reason=" + $arguments[3] + '; Segments=' + $description)
|
||||
Assert-True ($null -ne $arguments[2]) 'Successful geometry analysis must return PathGeometryAnalysis.'
|
||||
return $arguments[2]
|
||||
}
|
||||
|
||||
function Assert-AnalysisRejected([object[]]$Segments, [string]$Message) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($segmentIndex = 0; $segmentIndex -lt $Segments.Count; $segmentIndex++) {
|
||||
$typedSegments.SetValue($Segments[$segmentIndex], $segmentIndex)
|
||||
}
|
||||
|
||||
$arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
|
||||
$accepted = $analyzeMethod.Invoke($analyzer, $arguments)
|
||||
Assert-True (-not $accepted) ($Message + '; Reason=' + $arguments[3])
|
||||
}
|
||||
|
||||
function Invoke-RejectedAnalysis([object[]]$Segments, [string]$Message) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
|
||||
$arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
|
||||
$accepted = $analyzeMethod.Invoke($analyzer, $arguments)
|
||||
Assert-False $accepted ($Message + '; Reason=' + $arguments[3])
|
||||
}
|
||||
|
||||
function New-CoarsePathPoint(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
$Direction,
|
||||
[bool]$IsGearSwitch = $false,
|
||||
[string]$SourceName = 'MotionPrimitive') {
|
||||
return [Activator]::CreateInstance($coarsePointType, @(
|
||||
$X, $Y, [double]0.0, [double]0.0, $ArcLength,
|
||||
$Direction, [double]0.0, [double]1.0, $IsGearSwitch,
|
||||
[Enum]::Parse($coarsePointSourceType, $SourceName)))
|
||||
}
|
||||
|
||||
function New-EmptyGeometryMap {
|
||||
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
||||
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$mapRequest.ResolutionMm = [single]50
|
||||
$mapRequest.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
|
||||
Assert-True ($null -ne $map) 'Geometry test must create an explicit empty planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
|
||||
$analyzerType = Get-RequiredType ($processing + 'PathGeometryAnalyzer')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
||||
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||
$analysisType = Get-RequiredType ($processing + 'PathGeometryAnalysis')
|
||||
$preprocessorType = Get-RequiredType ($processing + 'PathSmoothingPreprocessor')
|
||||
$resamplerType = Get-RequiredType ($processing + 'ArcLengthResampler')
|
||||
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
||||
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
||||
$coarseSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
||||
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
||||
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
||||
$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
||||
$smoothingRequestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$smoothingConfigurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
||||
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
||||
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
||||
|
||||
Assert-True ($null -ne $preparedPathType) 'PreparedPath must be discoverable for smoothing algorithms.'
|
||||
Assert-True ($null -ne $preprocessorType) 'PathSmoothingPreprocessor must be discoverable for request preparation.'
|
||||
Assert-True ($null -ne $resamplerType) 'ArcLengthResampler must be discoverable for deterministic resampling.'
|
||||
|
||||
$analyzer = [Activator]::CreateInstance($analyzerType)
|
||||
$analyzeMethod = $analyzerType.GetMethod('TryAnalyze')
|
||||
Assert-True ($null -ne $analyzeMethod) 'PathGeometryAnalyzer must expose TryAnalyze.'
|
||||
Assert-Equal 4 $analyzeMethod.GetParameters().Length 'TryAnalyze must accept segments, spacing, analysis, and reason.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
$coarseAnchor = [Enum]::Parse($coarseSourceType, 'Start')
|
||||
|
||||
# Forward straight: resampling is exactly 0.05 m, preserves the exact endpoint, and has zero curvature.
|
||||
$straight = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
|
||||
$straightAnalysis = Invoke-Analysis @($straight)
|
||||
Assert-Equal 21 $straightAnalysis.Path.Count 'A one-metre straight must produce twenty 0.05 m intervals plus the initial point.'
|
||||
for ($index = 1; $index -lt $straightAnalysis.Path.Count; $index++) {
|
||||
$left = $straightAnalysis.Path[$index - 1]
|
||||
$right = $straightAnalysis.Path[$index]
|
||||
$distance = [Math]::Sqrt(($right.X - $left.X) * ($right.X - $left.X) + ($right.Y - $left.Y) * ($right.Y - $left.Y))
|
||||
Assert-Near 0.05 $distance 0.000000001 'Straight resampling intervals must be exactly 0.05 m.'
|
||||
Assert-Near 0.0 $right.GeometricCurvature 0.000000001 'A forward straight must have zero geometric curvature.'
|
||||
Assert-Near 0.0 $right.VehicleCurvature 0.000000001 'A forward straight must have zero vehicle curvature.'
|
||||
}
|
||||
$straightEnd = $straightAnalysis.Path[$straightAnalysis.Path.Count - 1]
|
||||
Assert-Near 1.0 $straightEnd.X 0.0 'Resampling must retain the exact final X coordinate.'
|
||||
Assert-Near 0.0 $straightEnd.Y 0.0 'Resampling must retain the exact final Y coordinate.'
|
||||
|
||||
# A forward R=2 quarter circle has positive +0.5 1/m vehicle curvature.
|
||||
$forwardArcPoints = New-Object System.Collections.Generic.List[object]
|
||||
for ($index = 0; $index -le 32; $index++) {
|
||||
$theta = ([Math]::PI / 2.0) * $index / 32.0
|
||||
$x = 2.0 * [Math]::Sin($theta)
|
||||
$y = 2.0 * (1.0 - [Math]::Cos($theta))
|
||||
$arcLength = 2.0 * $theta
|
||||
[void]$forwardArcPoints.Add((New-GeometryPoint $x $y $arcLength $theta $theta))
|
||||
}
|
||||
$forwardArc = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray()
|
||||
$forwardArcAnalysis = Invoke-Analysis @($forwardArc)
|
||||
$forwardArcMidpoint = $forwardArcAnalysis.Path[[int]($forwardArcAnalysis.Path.Count / 2)]
|
||||
Assert-Near 0.5 $forwardArcMidpoint.GeometricCurvature 0.01 'An R=2 quarter circle must have geometric curvature +0.5 1/m.'
|
||||
Assert-Near 0.5 $forwardArcMidpoint.VehicleCurvature 0.01 'A forward R=2 quarter circle must have vehicle curvature +0.5 1/m.'
|
||||
|
||||
# The same spatial R=2 circle in reverse retains geometric curvature but negates vehicle curvature.
|
||||
$reverseArc = New-DirectionSegment 0 $reverse $forwardArcPoints.ToArray()
|
||||
$reverseArcAnalysis = Invoke-Analysis @($reverseArc)
|
||||
$reverseArcMidpoint = $reverseArcAnalysis.Path[[int]($reverseArcAnalysis.Path.Count / 2)]
|
||||
Assert-Near 0.5 $reverseArcMidpoint.GeometricCurvature 0.01 'Reverse travel must not change geometric curvature.'
|
||||
Assert-Near -0.5 $reverseArcMidpoint.VehicleCurvature 0.01 'A reverse R=2 quarter circle must have vehicle curvature -0.5 1/m.'
|
||||
|
||||
# Gear-switch poses are intentionally duplicated: they keep equal arc length and never enter a derivative denominator.
|
||||
$forwardBeforeSwitch = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true)) $false $true
|
||||
$reverseAfterSwitch = New-DirectionSegment 1 $reverse @(
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true),
|
||||
(New-GeometryPoint 0.0 0.0 2.0 0.0 0.0)) $true $false
|
||||
$switchAnalysis = Invoke-Analysis @($forwardBeforeSwitch, $reverseAfterSwitch)
|
||||
$firstSegment = $switchAnalysis.Segments[0]
|
||||
$secondSegment = $switchAnalysis.Segments[1]
|
||||
$switchLeft = $switchAnalysis.Path[$firstSegment.EndIndex]
|
||||
$switchRight = $switchAnalysis.Path[$secondSegment.StartIndex]
|
||||
Assert-Near $switchLeft.X $switchRight.X 0.0 'Gear-switch endpoints must retain duplicate X coordinates.'
|
||||
Assert-Near $switchLeft.Y $switchRight.Y 0.0 'Gear-switch endpoints must retain duplicate Y coordinates.'
|
||||
Assert-Near $switchLeft.ArcLength $switchRight.ArcLength 0.0 'Gear-switch endpoints must retain duplicate arc length.'
|
||||
Assert-Equal 'Forward' $switchLeft.Direction.ToString() 'The first gear-switch pose must retain its forward segment direction.'
|
||||
Assert-Equal 'Reverse' $switchRight.Direction.ToString() 'The second gear-switch pose must retain its reverse segment direction.'
|
||||
Assert-True (-not [double]::IsNaN($switchLeft.GeometricCurvature)) 'No derivative may cross the gear-switch duplicate point.'
|
||||
Assert-True (-not [double]::IsNaN($switchRight.GeometricCurvature)) 'No reverse derivative may cross the gear-switch duplicate point.'
|
||||
|
||||
# Curvature at a curved segment's end and the following straight reverse segment's start must remain independently differentiated.
|
||||
$forwardArcToSwitch = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray() $false $true
|
||||
$reverseStraightAfterArc = New-DirectionSegment 1 $reverse @(
|
||||
(New-GeometryPoint 2.0 2.0 0.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0) $true),
|
||||
(New-GeometryPoint 2.0 1.0 1.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0))) $true $false
|
||||
$curveSwitchAnalysis = Invoke-Analysis @($forwardArcToSwitch, $reverseStraightAfterArc)
|
||||
$reverseStraightStart = $curveSwitchAnalysis.Path[$curveSwitchAnalysis.Segments[1].StartIndex]
|
||||
Assert-Near 0.0 $reverseStraightStart.GeometricCurvature 0.000000001 'A gear-switch must not use the preceding curve to differentiate a reverse straight segment.'
|
||||
|
||||
# A boundary that claims a gear switch must be a duplicated pose with opposite direction; discontinuities are rejected.
|
||||
$invalidSwitch = New-DirectionSegment 1 $reverse @(
|
||||
(New-GeometryPoint 1.2 0.0 1.0 0.0 0.0 $true),
|
||||
(New-GeometryPoint 0.2 0.0 2.0 0.0 0.0)) $true $false
|
||||
Assert-AnalysisRejected @($forwardBeforeSwitch, $invalidSwitch) 'A discontinuous gear-switch boundary must be rejected.'
|
||||
$trailingGearSwitch = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0)) $false $true
|
||||
Assert-AnalysisRejected @($trailingGearSwitch) 'The final direction segment must not advertise a non-existent trailing gear switch.'
|
||||
|
||||
# The public preprocessor receives a raw coarse path and resets every prepared direction segment to local arc length zero.
|
||||
$coarsePoints = [Array]::CreateInstance($coarsePointType, 4)
|
||||
$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 0.0 $forward), 0)
|
||||
$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $forward), 1)
|
||||
$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $reverse $true), 2)
|
||||
$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 2.0 $reverse), 3)
|
||||
$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
|
||||
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
|
||||
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = 1.0
|
||||
$vehicle.WidthMeters = 0.5
|
||||
$vehicle.SafetyMarginMeters = 0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = 1.0
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$uninitializedMap = [System.Runtime.Serialization.FormatterServices]::GetUninitializedObject($mapType)
|
||||
$request = [Activator]::CreateInstance($requestType, @($coarsePoints, $coarseSegments, $uninitializedMap, $vehicle, $configuration))
|
||||
$preprocessor = [Activator]::CreateInstance($preprocessorType)
|
||||
$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
|
||||
$prepareArguments = [object[]]@($request, $null, $null)
|
||||
$prepared = $prepareMethod.Invoke($preprocessor, $prepareArguments)
|
||||
Assert-True $prepared ('Preprocessor must accept a legal forward/reverse raw coarse path. Reason=' + $prepareArguments[2])
|
||||
Assert-Near 0.0 $prepareArguments[1].Segments[1].Points[0].ArcLength 0.0 'Every prepared direction segment must begin at local arc length zero.'
|
||||
Assert-True $prepareArguments[1].Segments[1].Points[0].IsGearSwitchPoint 'The reverse prepared segment must retain its gear-switch point.'
|
||||
|
||||
# Finite coordinates can still overflow distance arithmetic; public resampling must reject them instead of emitting NaN/Infinity.
|
||||
$overflowSegment = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint -1.0e308 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0e308 0.0 1.0 0.0 0.0))
|
||||
$resampler = [Activator]::CreateInstance($resamplerType)
|
||||
$segmentResampleMethod = @($resamplerType.GetMethods() | Where-Object {
|
||||
$_.Name -eq 'TryResample' -and $_.GetParameters()[0].ParameterType -eq $segmentType
|
||||
})[0]
|
||||
$resampleArguments = [object[]]@($overflowSegment, [double]0.05, $null, $null)
|
||||
$resampled = $segmentResampleMethod.Invoke($resampler, $resampleArguments)
|
||||
Assert-True (-not $resampled) ('Resampling must reject a distance overflow. Reason=' + $resampleArguments[3])
|
||||
|
||||
# A prepared path must reject null direction segments rather than silently dropping them during flattening.
|
||||
$nullSegmentArray = [Array]::CreateInstance($segmentType, 1)
|
||||
$nullSegmentRejected = $false
|
||||
try { [void][Activator]::CreateInstance($preparedPathType, @($nullSegmentArray)) } catch { $nullSegmentRejected = $true }
|
||||
Assert-True $nullSegmentRejected 'PreparedPath must reject a null direction segment.'
|
||||
|
||||
# Unwrapped heading must not jump by 2π when tangents cross the -π/π branch cut.
|
||||
$crossing = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
|
||||
(New-GeometryPoint -1.0 ([Math]::Tan(10.0 * [Math]::PI / 180.0)) 1.015 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
|
||||
(New-GeometryPoint -2.0 0.0 2.03 (-170.0 * [Math]::PI / 180.0) (-170.0 * [Math]::PI / 180.0)))
|
||||
$crossingAnalysis = Invoke-Analysis @($crossing)
|
||||
for ($index = 1; $index -lt $crossingAnalysis.Path.Count; $index++) {
|
||||
$difference = [Math]::Abs($crossingAnalysis.Path[$index].UnwrappedHeading - $crossingAnalysis.Path[$index - 1].UnwrappedHeading)
|
||||
Assert-True ($difference -lt [Math]::PI) 'Unwrapped headings must remain continuous across the ±π branch cut.'
|
||||
}
|
||||
|
||||
# The request preprocessor must reset arc length independently for every direction segment,
|
||||
# while retaining the duplicated pose that represents a legal forward-to-reverse gear switch.
|
||||
$preprocessor = [Activator]::CreateInstance($preprocessorType)
|
||||
$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
|
||||
Assert-True ($null -ne $prepareMethod) 'PathSmoothingPreprocessor must expose TryPrepare.'
|
||||
$coarsePath = [Array]::CreateInstance($coarsePointType, 5)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 0.0 0.0 0.0 $forward $false 'Start'), 0)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 1.0 $forward), 1)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $forward), 2)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $reverse $true), 3)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 3.0 $reverse), 4)
|
||||
$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
|
||||
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 2, $false, $true)), 0)
|
||||
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 3, 4, $true, $false)), 1)
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.80
|
||||
$vehicle.WidthMeters = [double]0.60
|
||||
$vehicle.SafetyMarginMeters = [double]0.05
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]0.80
|
||||
$configuration = [Activator]::CreateInstance($smoothingConfigurationType)
|
||||
$smoothingRequest = [Activator]::CreateInstance($smoothingRequestType, @(
|
||||
$coarsePath, $coarseSegments, (New-EmptyGeometryMap), $vehicle, $configuration))
|
||||
$prepareArguments = [object[]]@($smoothingRequest, $null, $null)
|
||||
Assert-True $prepareMethod.Invoke($preprocessor, $prepareArguments) ('Preprocessor must accept legal forward/reverse topology. Reason=' + $prepareArguments[2])
|
||||
$preparedPath = $prepareArguments[1]
|
||||
Assert-Equal 2 $preparedPath.Segments.Count 'Preprocessor must preserve both direction segments.'
|
||||
Assert-Near 0.0 $preparedPath.Segments[1].Points[0].ArcLength 0.0 'The reverse segment must restart local arc length at zero.'
|
||||
Assert-True $preparedPath.Segments[1].Points[0].IsGearSwitchPoint 'The duplicate reverse gear-switch point must be retained.'
|
||||
|
||||
# Segments may meet only at a paired, coincident forward/reverse gear switch.
|
||||
$illegalGearJump = New-DirectionSegment 1 $reverse @(
|
||||
(New-GeometryPoint 1.25 0.0 1.0 0.0 0.0 $true),
|
||||
(New-GeometryPoint 0.25 0.0 2.0 0.0 0.0)) $true $false
|
||||
Invoke-RejectedAnalysis @($forwardBeforeSwitch, $illegalGearJump) 'A gear-switch boundary whose poses differ must be rejected.'
|
||||
$illegalNormalBoundary = New-DirectionSegment 1 $forward @(
|
||||
(New-GeometryPoint 2.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 3.0 0.0 1.0 0.0 0.0)) $false $false
|
||||
Invoke-RejectedAnalysis @($straight, $illegalNormalBoundary) 'A non-gear segment boundary must be rejected.'
|
||||
|
||||
# Finite endpoint coordinates can still overflow while computing their separation; reject before interpolation.
|
||||
$resampler = [Activator]::CreateInstance($resamplerType)
|
||||
$resamplePointsMethod = $resamplerType.GetMethods() | Where-Object {
|
||||
$_.Name -eq 'TryResample' -and $_.GetParameters().Length -eq 4 -and
|
||||
$_.GetParameters()[0].ParameterType -eq [System.Collections.Generic.IReadOnlyList``1].MakeGenericType($pointType)
|
||||
} | Select-Object -First 1
|
||||
Assert-True ($null -ne $resamplePointsMethod) 'ArcLengthResampler must expose point-list TryResample.'
|
||||
$hugePoints = [Array]::CreateInstance($pointType, 2)
|
||||
$hugeCoordinate = [double]::MaxValue / 2.0
|
||||
$hugePoints.SetValue((New-GeometryPoint (-$hugeCoordinate) 0.0 0.0 0.0 0.0), 0)
|
||||
$hugePoints.SetValue((New-GeometryPoint $hugeCoordinate 0.0 1.0 0.0 0.0), 1)
|
||||
$resampleArguments = [object[]]@($hugePoints, [double]0.05, $null, $null)
|
||||
Assert-False $resamplePointsMethod.Invoke($resampler, $resampleArguments) 'Resampling must reject an infinite geometric distance caused by finite coordinates.'
|
||||
|
||||
# PreparedPath is an all-or-nothing immutable topology snapshot: null direction segments are invalid.
|
||||
$nullPreparedSegments = [Array]::CreateInstance($segmentType, 1)
|
||||
Assert-Throws { [Activator]::CreateInstance($preparedPathType, @($nullPreparedSegments)) } 'PreparedPath must reject null direction segments.'
|
||||
|
||||
# Segment indices are deliberately dense and equal to their position in the candidate array.
|
||||
$sparseSegment = New-DirectionSegment 2 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
|
||||
Invoke-RejectedAnalysis @($sparseSegment) 'Prepared direction-segment indices must match their dense array position.'
|
||||
|
||||
Write-Output 'Path smoothing geometry checks passed.'
|
||||
Reference in New Issue
Block a user