429 lines
18 KiB
C#
429 lines
18 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
|
|
|
/// <summary>按硬方向边界生成并合并局部 G2 曲率事件的候选窗口。</summary>
|
|
internal sealed class LocalG2WindowPlanner
|
|
{
|
|
private const double MergeToleranceMeters = 1e-9d;
|
|
|
|
internal bool TryPlan(
|
|
PreparedPath originalPath,
|
|
IReadOnlyList<CurvatureTransition> transitions,
|
|
LocalG2OptionsSnapshot options,
|
|
out IReadOnlyList<LocalG2SmoothingRegion> regions,
|
|
out string reason)
|
|
{
|
|
regions = Empty<LocalG2SmoothingRegion>();
|
|
reason = string.Empty;
|
|
if (originalPath == null || transitions == null || options == null)
|
|
{
|
|
reason = "局部 G2 窗口规划输入无效。";
|
|
return false;
|
|
}
|
|
if (!TryGetSegmentLengths(originalPath, out Dictionary<int, double> segmentLengths, out reason)) return false;
|
|
|
|
var ordered = new List<CurvatureTransition>(transitions.Count);
|
|
for (int index = 0; index < transitions.Count; index++)
|
|
{
|
|
CurvatureTransition transition = transitions[index];
|
|
if (transition == null || !segmentLengths.TryGetValue(transition.SegmentIndex, out double length) ||
|
|
!NumericGuard.IsFinite(transition.LocalArcLengthMeters) || transition.LocalArcLengthMeters < 0d ||
|
|
transition.LocalArcLengthMeters > length + MergeToleranceMeters)
|
|
{
|
|
reason = "局部 G2 曲率事件不属于有效方向分段。";
|
|
return false;
|
|
}
|
|
ordered.Add(transition);
|
|
}
|
|
ordered.Sort(CompareTransitions);
|
|
|
|
var planned = new List<LocalG2SmoothingRegion>();
|
|
int cursor = 0;
|
|
while (cursor < ordered.Count)
|
|
{
|
|
CurvatureTransition first = ordered[cursor];
|
|
double segmentLength = segmentLengths[first.SegmentIndex];
|
|
var group = new List<CurvatureTransition> { first };
|
|
IReadOnlyList<LocalG2WindowVariant> variants =
|
|
BuildVariants(group, segmentLength, options);
|
|
if (variants.Count == 0)
|
|
{
|
|
reason = "局部 G2 单事件无法生成满足总长度约束的窗口。";
|
|
return false;
|
|
}
|
|
cursor++;
|
|
|
|
while (cursor < ordered.Count &&
|
|
ordered[cursor].SegmentIndex == first.SegmentIndex)
|
|
{
|
|
var tentative = new List<CurvatureTransition>(group)
|
|
{
|
|
ordered[cursor],
|
|
};
|
|
IReadOnlyList<LocalG2WindowVariant> tentativeVariants =
|
|
BuildVariants(tentative, segmentLength, options);
|
|
if (tentativeVariants.Count == 0) break;
|
|
group = tentative;
|
|
variants = tentativeVariants;
|
|
cursor++;
|
|
}
|
|
|
|
double minimumStart = double.PositiveInfinity;
|
|
double maximumEnd = double.NegativeInfinity;
|
|
for (int variantIndex = 0; variantIndex < variants.Count; variantIndex++)
|
|
{
|
|
minimumStart = Math.Min(
|
|
minimumStart,
|
|
variants[variantIndex].StartArcLengthMeters);
|
|
maximumEnd = Math.Max(
|
|
maximumEnd,
|
|
variants[variantIndex].EndArcLengthMeters);
|
|
}
|
|
|
|
planned.Add(new LocalG2SmoothingRegion(
|
|
first.SegmentIndex,
|
|
group,
|
|
minimumStart,
|
|
maximumEnd,
|
|
variants));
|
|
}
|
|
regions = new ReadOnlyCollection<LocalG2SmoothingRegion>(planned);
|
|
return true;
|
|
}
|
|
|
|
private static IReadOnlyList<LocalG2WindowVariant> BuildVariants(
|
|
IReadOnlyList<CurvatureTransition> transitions,
|
|
double segmentLength,
|
|
LocalG2OptionsSnapshot options)
|
|
{
|
|
var variants = new List<LocalG2WindowVariant>();
|
|
double firstEvent = transitions[0].LocalArcLengthMeters;
|
|
double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters;
|
|
double anchor = (firstEvent + lastEvent) / 2d;
|
|
IReadOnlyList<double> targets = BuildTargets(options, segmentLength);
|
|
double[] ratios = { 0.5d, 0.4d, 0.6d };
|
|
for (int ratioIndex = 0; ratioIndex < ratios.Length; ratioIndex++)
|
|
{
|
|
for (int targetIndex = 0; targetIndex < targets.Count; targetIndex++)
|
|
{
|
|
if (variants.Count >= options.MaximumCandidatesPerRegion)
|
|
return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
|
|
AddIfLegal(
|
|
variants,
|
|
targets[targetIndex],
|
|
ratios[ratioIndex],
|
|
anchor,
|
|
firstEvent,
|
|
lastEvent,
|
|
segmentLength,
|
|
ratioIndex == 0,
|
|
options);
|
|
}
|
|
}
|
|
return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
|
|
}
|
|
|
|
private static void AddIfLegal(List<LocalG2WindowVariant> variants, double target, double leftRatio,
|
|
double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift,
|
|
LocalG2OptionsSnapshot options)
|
|
{
|
|
if (variants.Count >= options.MaximumCandidatesPerRegion) return;
|
|
double left = target * leftRatio;
|
|
double right = target - left;
|
|
double availableLeft = anchor;
|
|
double availableRight = segmentLength - anchor;
|
|
if (permitBoundaryShift)
|
|
{
|
|
left = Math.Min(left, availableLeft);
|
|
right = Math.Min(right, availableRight);
|
|
double missing = target - left - right;
|
|
double addRight = Math.Min(missing, availableRight - right);
|
|
right += addRight;
|
|
left += Math.Min(missing - addRight, availableLeft - left);
|
|
}
|
|
else if (left > availableLeft + MergeToleranceMeters || right > availableRight + MergeToleranceMeters)
|
|
{
|
|
return;
|
|
}
|
|
|
|
double start = anchor - left;
|
|
double end = anchor + right;
|
|
if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target)
|
|
return;
|
|
double actualLength = end - start;
|
|
if (actualLength + MergeToleranceMeters < options.MinimumWindowLengthMeters ||
|
|
actualLength > options.MaximumWindowLengthMeters + MergeToleranceMeters)
|
|
{
|
|
return;
|
|
}
|
|
variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right));
|
|
}
|
|
|
|
private static IReadOnlyList<double> BuildTargets(LocalG2OptionsSnapshot options, double segmentLength)
|
|
{
|
|
if (segmentLength + MergeToleranceMeters < options.MinimumWindowLengthMeters)
|
|
return new ReadOnlyCollection<double>(new List<double>());
|
|
|
|
double[] requested =
|
|
{
|
|
options.PreferredWindowLengthMeters,
|
|
options.MinimumWindowLengthMeters,
|
|
options.MaximumWindowLengthMeters,
|
|
0.75d * options.PreferredWindowLengthMeters,
|
|
1.25d * options.PreferredWindowLengthMeters,
|
|
};
|
|
var targets = new List<double>(requested.Length);
|
|
for (int index = 0; index < requested.Length; index++)
|
|
{
|
|
double target = Math.Min(segmentLength,
|
|
Math.Max(options.MinimumWindowLengthMeters, Math.Min(options.MaximumWindowLengthMeters, requested[index])));
|
|
bool duplicate = false;
|
|
for (int prior = 0; prior < targets.Count; prior++)
|
|
{
|
|
if (Math.Abs(targets[prior] - target) <= MergeToleranceMeters)
|
|
{
|
|
duplicate = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!duplicate) targets.Add(target);
|
|
}
|
|
return targets;
|
|
}
|
|
|
|
private static bool TryGetSegmentLengths(PreparedPath originalPath, out Dictionary<int, double> lengths, out string reason)
|
|
{
|
|
lengths = new Dictionary<int, double>();
|
|
reason = string.Empty;
|
|
for (int position = 0; position < originalPath.Segments.Count; position++)
|
|
{
|
|
PreparedDirectionSegment segment = originalPath.Segments[position];
|
|
if (segment == null || segment.SegmentIndex != position || segment.Points == null || segment.Points.Count == 0)
|
|
{
|
|
reason = "局部 G2 窗口规划的预处理方向分段无效。";
|
|
return false;
|
|
}
|
|
double previousArc = -1d;
|
|
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
|
{
|
|
double arc = segment.Points[pointIndex].ArcLength;
|
|
if (!NumericGuard.IsFinite(arc) || arc < 0d || arc < previousArc)
|
|
{
|
|
reason = "局部 G2 窗口规划要求分段弧长有限且非递减。";
|
|
return false;
|
|
}
|
|
previousArc = arc;
|
|
}
|
|
lengths.Add(segment.SegmentIndex, previousArc);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static int CompareTransitions(CurvatureTransition left, CurvatureTransition right)
|
|
{
|
|
int segment = left.SegmentIndex.CompareTo(right.SegmentIndex);
|
|
if (segment != 0) return segment;
|
|
int arc = left.LocalArcLengthMeters.CompareTo(right.LocalArcLengthMeters);
|
|
return arc != 0 ? arc : left.LeftCoarsePathIndex.CompareTo(right.LeftCoarsePathIndex);
|
|
}
|
|
|
|
private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());
|
|
|
|
public static class TestHooks
|
|
{
|
|
public static WindowPlanningTestSnapshot Execute(string scenario)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(scenario))
|
|
throw new ArgumentException("A scenario is required.", nameof(scenario));
|
|
|
|
IReadOnlyList<CurvatureTransition> transitions;
|
|
double segmentLength;
|
|
switch (scenario)
|
|
{
|
|
case "SeparatedByOneMeter":
|
|
transitions = new[]
|
|
{
|
|
Transition(0.2d, 0),
|
|
Transition(1.2d, 1),
|
|
};
|
|
segmentLength = 1.4d;
|
|
break;
|
|
case "Mergeable":
|
|
transitions = new[]
|
|
{
|
|
Transition(0.4d, 0),
|
|
Transition(0.7d, 1),
|
|
};
|
|
segmentLength = 1.4d;
|
|
break;
|
|
case "ThreeEventPartition":
|
|
transitions = new[]
|
|
{
|
|
Transition(0.2d, 0),
|
|
Transition(0.6d, 1),
|
|
Transition(1.2d, 2),
|
|
};
|
|
segmentLength = 1.4d;
|
|
break;
|
|
case "NearBoundary":
|
|
transitions = new[] { Transition(0.1d, 0) };
|
|
segmentLength = 1d;
|
|
break;
|
|
case "InteriorCoverage":
|
|
transitions = new[] { Transition(1d, 0) };
|
|
segmentLength = 2d;
|
|
break;
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(scenario));
|
|
}
|
|
|
|
var planner = new LocalG2WindowPlanner();
|
|
if (!planner.TryPlan(
|
|
CreatePreparedPath(segmentLength),
|
|
transitions,
|
|
new LocalG2OptionsSnapshot(new PathSmoothingConfiguration()),
|
|
out IReadOnlyList<LocalG2SmoothingRegion> regions,
|
|
out string reason))
|
|
{
|
|
throw new InvalidOperationException(reason);
|
|
}
|
|
|
|
double maximumLength = 0d;
|
|
bool exactEnvelope = true;
|
|
var counts = new List<string>(regions.Count);
|
|
var signature = new List<string>();
|
|
for (int regionIndex = 0; regionIndex < regions.Count; regionIndex++)
|
|
{
|
|
LocalG2SmoothingRegion region = regions[regionIndex];
|
|
counts.Add(region.Transitions.Count.ToString());
|
|
double minimumStart = double.PositiveInfinity;
|
|
double maximumEnd = double.NegativeInfinity;
|
|
for (int variantIndex = 0; variantIndex < region.WindowVariants.Count; variantIndex++)
|
|
{
|
|
LocalG2WindowVariant variant = region.WindowVariants[variantIndex];
|
|
maximumLength = Math.Max(
|
|
maximumLength,
|
|
variant.EndArcLengthMeters - variant.StartArcLengthMeters);
|
|
minimumStart = Math.Min(minimumStart, variant.StartArcLengthMeters);
|
|
maximumEnd = Math.Max(maximumEnd, variant.EndArcLengthMeters);
|
|
signature.Add(
|
|
region.SegmentIndex + ":" +
|
|
variant.CandidateIndex + ":" +
|
|
variant.StartArcLengthMeters.ToString("R") + ":" +
|
|
variant.EndArcLengthMeters.ToString("R"));
|
|
}
|
|
exactEnvelope &= Math.Abs(region.MaximumStartArcLengthMeters - minimumStart) <= 1e-9d;
|
|
exactEnvelope &= Math.Abs(region.MaximumEndArcLengthMeters - maximumEnd) <= 1e-9d;
|
|
}
|
|
|
|
IReadOnlyList<LocalG2WindowVariant> firstVariants = regions[0].WindowVariants;
|
|
bool representativeTargetsFirst =
|
|
firstVariants.Count >= 3 &&
|
|
Math.Abs(WindowLength(firstVariants[0]) - 0.50d) <= 1e-9d &&
|
|
Math.Abs(WindowLength(firstVariants[1]) - 0.20d) <= 1e-9d &&
|
|
Math.Abs(WindowLength(firstVariants[2]) - 0.80d) <= 1e-9d;
|
|
bool hasAsymmetricVariant = false;
|
|
for (int index = 0; index < firstVariants.Count; index++)
|
|
{
|
|
if (Math.Abs(
|
|
firstVariants[index].LeftWindowLengthMeters -
|
|
firstVariants[index].RightWindowLengthMeters) > 1e-9d)
|
|
{
|
|
hasAsymmetricVariant = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
LocalG2WindowVariant first = firstVariants[0];
|
|
return new WindowPlanningTestSnapshot(
|
|
regions.Count,
|
|
string.Join(",", counts),
|
|
maximumLength,
|
|
exactEnvelope,
|
|
first.LeftWindowLengthMeters,
|
|
first.RightWindowLengthMeters,
|
|
string.Join("|", signature),
|
|
firstVariants.Count,
|
|
representativeTargetsFirst,
|
|
hasAsymmetricVariant);
|
|
}
|
|
|
|
public sealed class WindowPlanningTestSnapshot
|
|
{
|
|
internal WindowPlanningTestSnapshot(
|
|
int regionCount,
|
|
string transitionCounts,
|
|
double maximumWindowLength,
|
|
bool exactEnvelope,
|
|
double firstLeftLength,
|
|
double firstRightLength,
|
|
string signature,
|
|
int firstRegionVariantCount,
|
|
bool representativeTargetsFirst,
|
|
bool hasAsymmetricVariant)
|
|
{
|
|
RegionCount = regionCount;
|
|
TransitionCounts = transitionCounts;
|
|
MaximumWindowLength = maximumWindowLength;
|
|
ExactEnvelope = exactEnvelope;
|
|
FirstLeftLength = firstLeftLength;
|
|
FirstRightLength = firstRightLength;
|
|
Signature = signature;
|
|
FirstRegionVariantCount = firstRegionVariantCount;
|
|
RepresentativeTargetsFirst = representativeTargetsFirst;
|
|
HasAsymmetricVariant = hasAsymmetricVariant;
|
|
}
|
|
|
|
public int RegionCount { get; }
|
|
public string TransitionCounts { get; }
|
|
public double MaximumWindowLength { get; }
|
|
public bool ExactEnvelope { get; }
|
|
public double FirstLeftLength { get; }
|
|
public double FirstRightLength { get; }
|
|
public string Signature { get; }
|
|
public int FirstRegionVariantCount { get; }
|
|
public bool RepresentativeTargetsFirst { get; }
|
|
public bool HasAsymmetricVariant { get; }
|
|
}
|
|
|
|
private static double WindowLength(LocalG2WindowVariant variant) =>
|
|
variant.EndArcLengthMeters - variant.StartArcLengthMeters;
|
|
|
|
private static CurvatureTransition Transition(double arcLength, int index)
|
|
{
|
|
return new CurvatureTransition(
|
|
0,
|
|
index,
|
|
index + 1,
|
|
arcLength,
|
|
arcLength,
|
|
0d,
|
|
0d,
|
|
index % 2 == 0 ? 0d : 0.5d,
|
|
index % 2 == 0 ? 0.5d : 0d);
|
|
}
|
|
|
|
private static PreparedPath CreatePreparedPath(double length)
|
|
{
|
|
var points = new[]
|
|
{
|
|
new SmoothingPoint2D(
|
|
0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
|
new SmoothingPoint2D(
|
|
length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
|
};
|
|
return new PreparedPath(new[]
|
|
{
|
|
new PreparedDirectionSegment(
|
|
0, TravelDirection.Forward, points, false, false),
|
|
});
|
|
}
|
|
}
|
|
}
|