chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -0,0 +1,378 @@
|
||||
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;
|
||||
foreach (double target in BuildTargets(options, segmentLength))
|
||||
{
|
||||
if (variants.Count >= options.MaximumCandidatesPerRegion) break;
|
||||
AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options);
|
||||
AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options);
|
||||
AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, 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,
|
||||
0.75d * options.PreferredWindowLengthMeters,
|
||||
1.25d * options.PreferredWindowLengthMeters,
|
||||
options.MinimumWindowLengthMeters,
|
||||
options.MaximumWindowLengthMeters,
|
||||
};
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
LocalG2WindowVariant first = regions[0].WindowVariants[0];
|
||||
return new WindowPlanningTestSnapshot(
|
||||
regions.Count,
|
||||
string.Join(",", counts),
|
||||
maximumLength,
|
||||
exactEnvelope,
|
||||
first.LeftWindowLengthMeters,
|
||||
first.RightWindowLengthMeters,
|
||||
string.Join("|", signature));
|
||||
}
|
||||
|
||||
public sealed class WindowPlanningTestSnapshot
|
||||
{
|
||||
internal WindowPlanningTestSnapshot(
|
||||
int regionCount,
|
||||
string transitionCounts,
|
||||
double maximumWindowLength,
|
||||
bool exactEnvelope,
|
||||
double firstLeftLength,
|
||||
double firstRightLength,
|
||||
string signature)
|
||||
{
|
||||
RegionCount = regionCount;
|
||||
TransitionCounts = transitionCounts;
|
||||
MaximumWindowLength = maximumWindowLength;
|
||||
ExactEnvelope = exactEnvelope;
|
||||
FirstLeftLength = firstLeftLength;
|
||||
FirstRightLength = firstRightLength;
|
||||
Signature = signature;
|
||||
}
|
||||
|
||||
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; }
|
||||
}
|
||||
|
||||
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),
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user