642 lines
33 KiB
C#
642 lines
33 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
|
|
|
/// <summary>以固定窗口和尺度顺序构造有限的局部五次 Hermite G2 候选。</summary>
|
|
internal sealed class LocalG2CandidateBuilder
|
|
{
|
|
private const double MinimumDerivativeNorm = 1e-10d;
|
|
private const int MinimumCurveEvaluationIntervals = 32;
|
|
private const int MaximumSubdivisionDepth = 32;
|
|
private static readonly double[] DerivativeScaleMultipliers = { 1d, 0.85d, 1.15d };
|
|
|
|
internal IReadOnlyList<LocalG2CandidateGeometry> Build(
|
|
PreparedDirectionSegment originalSegment,
|
|
LocalG2SmoothingRegion region,
|
|
double outputSpacingMeters,
|
|
LocalG2OptionsSnapshot options,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (!IsValidInput(originalSegment, region, outputSpacingMeters, options)) return Empty();
|
|
|
|
var candidates = new List<LocalG2CandidateGeometry>();
|
|
for (int windowIndex = 0; windowIndex < region.WindowVariants.Count; windowIndex++)
|
|
{
|
|
LocalG2WindowVariant window = region.WindowVariants[windowIndex];
|
|
for (int scaleIndex = 0; scaleIndex < DerivativeScaleMultipliers.Length; scaleIndex++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
if (candidates.Count >= Math.Min(options.MaximumCandidatesPerRegion, 12)) return ReadOnly(candidates);
|
|
if (TryBuildCandidate(
|
|
candidates.Count,
|
|
originalSegment,
|
|
region,
|
|
window,
|
|
DerivativeScaleMultipliers[scaleIndex],
|
|
outputSpacingMeters,
|
|
cancellationToken,
|
|
out LocalG2CandidateGeometry candidate))
|
|
{
|
|
candidates.Add(candidate);
|
|
}
|
|
}
|
|
}
|
|
return ReadOnly(candidates);
|
|
}
|
|
|
|
private static bool TryBuildCandidate(
|
|
int candidateIndex,
|
|
PreparedDirectionSegment segment,
|
|
LocalG2SmoothingRegion region,
|
|
LocalG2WindowVariant window,
|
|
double multiplier,
|
|
double outputSpacingMeters,
|
|
CancellationToken cancellationToken,
|
|
out LocalG2CandidateGeometry candidate)
|
|
{
|
|
candidate = null;
|
|
if (!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, window.StartArcLengthMeters, out SmoothingPoint2D startReference, out _) ||
|
|
!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, window.EndArcLengthMeters, out SmoothingPoint2D endReference, out _))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
|
|
CurvatureTransition first = region.Transitions[0];
|
|
CurvatureTransition last = region.Transitions[region.Transitions.Count - 1];
|
|
bool firstAtStart = SameArc(first.LocalArcLengthMeters, window.StartArcLengthMeters);
|
|
bool lastAtEnd = SameArc(last.LocalArcLengthMeters, window.EndArcLengthMeters);
|
|
double startVehicleCurvature = firstAtStart && segment.StartVehicleCurvaturePerMeter.HasValue
|
|
? segment.StartVehicleCurvaturePerMeter.Value
|
|
: first.LeftVehicleCurvaturePerMeter;
|
|
double endVehicleCurvature = last.RightVehicleCurvaturePerMeter;
|
|
if (!NumericGuard.IsFinite(startVehicleCurvature) || !NumericGuard.IsFinite(endVehicleCurvature)) return false;
|
|
|
|
var nodes = new List<BoundaryNode>();
|
|
nodes.Add(new BoundaryNode(window.StartArcLengthMeters, startReference.X, startReference.Y,
|
|
startReference.Heading, startVehicleCurvature));
|
|
for (int transitionIndex = 0; transitionIndex < region.Transitions.Count; transitionIndex++)
|
|
{
|
|
CurvatureTransition transition = region.Transitions[transitionIndex];
|
|
bool atStart = SameArc(transition.LocalArcLengthMeters, window.StartArcLengthMeters);
|
|
bool atEnd = SameArc(transition.LocalArcLengthMeters, window.EndArcLengthMeters);
|
|
// A one-sided transition at a hard segment boundary has no left/right zero-length piece.
|
|
if (atStart || atEnd) continue;
|
|
double leftAvailable = transition.LocalArcLengthMeters - window.StartArcLengthMeters;
|
|
double rightAvailable = window.EndArcLengthMeters - transition.LocalArcLengthMeters;
|
|
double denominator = leftAvailable + rightAvailable;
|
|
if (!NumericGuard.IsPositiveFinite(denominator)) return false;
|
|
double sharedCurvature = transition.LeftVehicleCurvaturePerMeter +
|
|
(transition.RightVehicleCurvaturePerMeter - transition.LeftVehicleCurvaturePerMeter) * leftAvailable / denominator;
|
|
if (!NumericGuard.IsFinite(sharedCurvature)) return false;
|
|
nodes.Add(new BoundaryNode(transition.LocalArcLengthMeters, transition.X, transition.Y,
|
|
transition.VehicleHeadingRadians, sharedCurvature));
|
|
}
|
|
nodes.Add(new BoundaryNode(window.EndArcLengthMeters, endReference.X, endReference.Y,
|
|
endReference.Heading, endVehicleCurvature));
|
|
|
|
if (!TryAssignDerivativeScales(nodes, multiplier) || nodes.Count < 2) return false;
|
|
var sampled = new List<SmoothingPoint2D>();
|
|
for (int nodeIndex = 1; nodeIndex < nodes.Count; nodeIndex++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
BoundaryNode left = nodes[nodeIndex - 1];
|
|
BoundaryNode right = nodes[nodeIndex];
|
|
if (!TryCreateCurve(left, right, directionSign, out QuinticHermiteCurve2D curve)) return false;
|
|
if (!TryAppendCurveSamples(
|
|
curve,
|
|
left,
|
|
right,
|
|
segment,
|
|
outputSpacingMeters,
|
|
sampled,
|
|
cancellationToken))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
if (sampled.Count < 2) return false;
|
|
candidate = new LocalG2CandidateGeometry(
|
|
candidateIndex,
|
|
segment.SegmentIndex,
|
|
window.StartArcLengthMeters,
|
|
window.EndArcLengthMeters,
|
|
window.LeftWindowLengthMeters,
|
|
window.RightWindowLengthMeters,
|
|
sampled,
|
|
startVehicleCurvature,
|
|
endVehicleCurvature,
|
|
directionSign * startVehicleCurvature,
|
|
directionSign * endVehicleCurvature,
|
|
true);
|
|
return true;
|
|
}
|
|
|
|
private static bool TryAssignDerivativeScales(List<BoundaryNode> nodes, double multiplier)
|
|
{
|
|
if (!NumericGuard.IsPositiveFinite(multiplier)) return false;
|
|
for (int index = 0; index < nodes.Count; index++)
|
|
{
|
|
double scale;
|
|
if (index == 0) scale = nodes[1].ArcLengthMeters - nodes[0].ArcLengthMeters;
|
|
else if (index == nodes.Count - 1) scale = nodes[index].ArcLengthMeters - nodes[index - 1].ArcLengthMeters;
|
|
else scale = ((nodes[index].ArcLengthMeters - nodes[index - 1].ArcLengthMeters) +
|
|
(nodes[index + 1].ArcLengthMeters - nodes[index].ArcLengthMeters)) / 2d;
|
|
scale *= multiplier;
|
|
if (!NumericGuard.IsPositiveFinite(scale)) return false;
|
|
nodes[index] = nodes[index].WithDerivativeScale(scale);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static bool TryCreateCurve(BoundaryNode left, BoundaryNode right, double directionSign, out QuinticHermiteCurve2D curve)
|
|
{
|
|
curve = null;
|
|
if (!TryGetDerivatives(left, directionSign, out double ldx, out double ldy, out double lddx, out double lddy) ||
|
|
!TryGetDerivatives(right, directionSign, out double rdx, out double rdy, out double rddx, out double rddy))
|
|
{
|
|
return false;
|
|
}
|
|
return QuinticHermiteCurve2D.TryCreate(left.X, left.Y, ldx, ldy, lddx, lddy,
|
|
right.X, right.Y, rdx, rdy, rddx, rddy, out curve, out _);
|
|
}
|
|
|
|
private static bool TryGetDerivatives(BoundaryNode node, double directionSign,
|
|
out double dx, out double dy, out double ddx, out double ddy)
|
|
{
|
|
dx = dy = ddx = ddy = 0d;
|
|
double travelHeading = directionSign > 0d ? node.VehicleHeadingRadians : node.VehicleHeadingRadians - Math.PI;
|
|
double geometricCurvature = directionSign * node.VehicleCurvaturePerMeter;
|
|
double tx = Math.Cos(travelHeading);
|
|
double ty = Math.Sin(travelHeading);
|
|
double nx = -ty;
|
|
double ny = tx;
|
|
dx = node.DerivativeScale * tx;
|
|
dy = node.DerivativeScale * ty;
|
|
ddx = node.DerivativeScale * node.DerivativeScale * geometricCurvature * nx;
|
|
ddy = node.DerivativeScale * node.DerivativeScale * geometricCurvature * ny;
|
|
return NumericGuard.IsFinite(dx) && NumericGuard.IsFinite(dy) && NumericGuard.IsFinite(ddx) && NumericGuard.IsFinite(ddy) &&
|
|
Math.Sqrt(dx * dx + dy * dy) >= MinimumDerivativeNorm;
|
|
}
|
|
|
|
private static bool TryAppendCurveSamples(
|
|
QuinticHermiteCurve2D curve,
|
|
BoundaryNode left,
|
|
BoundaryNode right,
|
|
PreparedDirectionSegment segment,
|
|
double outputSpacingMeters,
|
|
List<SmoothingPoint2D> output,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
// A short endpoint chord does not prove that a fifth-degree curve has no interior cusp.
|
|
// Check a deterministic interior grid before the chord-driven subdivision below; every
|
|
// subsequently emitted subdivision parameter is checked again by TryEvaluate.
|
|
for (int index = 0; index <= MinimumCurveEvaluationIntervals; index++)
|
|
{
|
|
if (!TryEvaluate(curve, (double)index / MinimumCurveEvaluationIntervals, out _)) return false;
|
|
}
|
|
if (!TryEvaluate(curve, 0d, out CurveSample start) || !TryEvaluate(curve, 1d, out CurveSample end)) return false;
|
|
if (output.Count == 0 && !TryAppendPoint(start, left, right, segment, output)) return false;
|
|
return TrySubdivide(curve, left, right, segment, outputSpacingMeters, 0d, start, 1d, end, 0, output, cancellationToken);
|
|
}
|
|
|
|
private static bool TrySubdivide(
|
|
QuinticHermiteCurve2D curve,
|
|
BoundaryNode left,
|
|
BoundaryNode right,
|
|
PreparedDirectionSegment segment,
|
|
double outputSpacingMeters,
|
|
double u0,
|
|
CurveSample sample0,
|
|
double u1,
|
|
CurveSample sample1,
|
|
int depth,
|
|
List<SmoothingPoint2D> output,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
if (Distance(sample0, sample1) <= outputSpacingMeters)
|
|
{
|
|
return TryAppendPoint(sample1, left, right, segment, output);
|
|
}
|
|
if (depth >= MaximumSubdivisionDepth) return false;
|
|
double middle = (u0 + u1) / 2d;
|
|
if (!TryEvaluate(curve, middle, out CurveSample middleSample)) return false;
|
|
return TrySubdivide(curve, left, right, segment, outputSpacingMeters, u0, sample0, middle, middleSample,
|
|
depth + 1, output, cancellationToken) &&
|
|
TrySubdivide(curve, left, right, segment, outputSpacingMeters, middle, middleSample, u1, sample1,
|
|
depth + 1, output, cancellationToken);
|
|
}
|
|
|
|
private static bool TryAppendPoint(
|
|
CurveSample sample,
|
|
BoundaryNode left,
|
|
BoundaryNode right,
|
|
PreparedDirectionSegment segment,
|
|
List<SmoothingPoint2D> output)
|
|
{
|
|
double referenceArcLength = left.ArcLengthMeters + (right.ArcLengthMeters - left.ArcLengthMeters) * sample.Parameter;
|
|
if (!PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, referenceArcLength, out SmoothingPoint2D reference, out _))
|
|
return false;
|
|
double x = SameArc(sample.Parameter, 0d) ? left.X : SameArc(sample.Parameter, 1d) ? right.X : sample.X;
|
|
double y = SameArc(sample.Parameter, 0d) ? left.Y : SameArc(sample.Parameter, 1d) ? right.Y : sample.Y;
|
|
double travelHeading = Math.Atan2(sample.Dy, sample.Dx);
|
|
double vehicleHeading = segment.Direction == TravelDirection.Forward ? travelHeading : travelHeading + Math.PI;
|
|
if (!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(travelHeading) || !NumericGuard.IsFinite(vehicleHeading)) return false;
|
|
if (output.Count > 0 && Distance(output[output.Count - 1], x, y) <= 1e-12d) return false;
|
|
output.Add(new SmoothingPoint2D(
|
|
x,
|
|
y,
|
|
referenceArcLength,
|
|
AngleMath.NormalizeRadians(vehicleHeading),
|
|
vehicleHeading,
|
|
reference.BodyClearance,
|
|
false,
|
|
SmoothedPathPointSource.LocalG2Transition));
|
|
return true;
|
|
}
|
|
|
|
private static bool TryEvaluate(QuinticHermiteCurve2D curve, double parameter, out CurveSample sample)
|
|
{
|
|
sample = default;
|
|
curve.Evaluate(parameter, out double x, out double y, out double dx, out double dy, out double ddx, out double ddy);
|
|
double derivativeNorm = Math.Sqrt(dx * dx + dy * dy);
|
|
if (!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(dx) || !NumericGuard.IsFinite(dy) ||
|
|
!NumericGuard.IsFinite(ddx) || !NumericGuard.IsFinite(ddy) || !NumericGuard.IsFinite(derivativeNorm) ||
|
|
derivativeNorm < MinimumDerivativeNorm)
|
|
{
|
|
return false;
|
|
}
|
|
sample = new CurveSample(parameter, x, y, dx, dy);
|
|
return true;
|
|
}
|
|
|
|
private static bool IsValidInput(PreparedDirectionSegment segment, LocalG2SmoothingRegion region,
|
|
double outputSpacingMeters, LocalG2OptionsSnapshot options)
|
|
{
|
|
if (segment == null || region == null || options == null || segment.SegmentIndex != region.SegmentIndex ||
|
|
!NumericGuard.IsPositiveFinite(outputSpacingMeters) || segment.Points == null || segment.Points.Count < 2 ||
|
|
region.Transitions == null || region.Transitions.Count == 0 || region.WindowVariants == null)
|
|
{
|
|
return false;
|
|
}
|
|
for (int index = 0; index < region.Transitions.Count; index++)
|
|
{
|
|
CurvatureTransition transition = region.Transitions[index];
|
|
if (transition == null || transition.SegmentIndex != segment.SegmentIndex) return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private static double Distance(CurveSample left, CurveSample right) => Distance(left.X, left.Y, right.X, right.Y);
|
|
private static double Distance(SmoothingPoint2D point, double x, double y) => Distance(point.X, point.Y, x, y);
|
|
private static double Distance(double x0, double y0, double x1, double y1)
|
|
{
|
|
double dx = x1 - x0;
|
|
double dy = y1 - y0;
|
|
return Math.Sqrt(dx * dx + dy * dy);
|
|
}
|
|
private static bool SameArc(double left, double right) => Math.Abs(left - right) <= 1e-10d;
|
|
private static IReadOnlyList<LocalG2CandidateGeometry> Empty() => new ReadOnlyCollection<LocalG2CandidateGeometry>(new List<LocalG2CandidateGeometry>());
|
|
private static IReadOnlyList<LocalG2CandidateGeometry> ReadOnly(List<LocalG2CandidateGeometry> values) =>
|
|
new ReadOnlyCollection<LocalG2CandidateGeometry>(values);
|
|
|
|
private readonly struct BoundaryNode
|
|
{
|
|
internal BoundaryNode(double arcLengthMeters, double x, double y, double vehicleHeadingRadians, double vehicleCurvaturePerMeter)
|
|
{
|
|
ArcLengthMeters = arcLengthMeters;
|
|
X = x;
|
|
Y = y;
|
|
VehicleHeadingRadians = vehicleHeadingRadians;
|
|
VehicleCurvaturePerMeter = vehicleCurvaturePerMeter;
|
|
DerivativeScale = 0d;
|
|
}
|
|
private BoundaryNode(double arcLengthMeters, double x, double y, double vehicleHeadingRadians,
|
|
double vehicleCurvaturePerMeter, double derivativeScale)
|
|
{
|
|
ArcLengthMeters = arcLengthMeters;
|
|
X = x;
|
|
Y = y;
|
|
VehicleHeadingRadians = vehicleHeadingRadians;
|
|
VehicleCurvaturePerMeter = vehicleCurvaturePerMeter;
|
|
DerivativeScale = derivativeScale;
|
|
}
|
|
internal double ArcLengthMeters { get; }
|
|
internal double X { get; }
|
|
internal double Y { get; }
|
|
internal double VehicleHeadingRadians { get; }
|
|
internal double VehicleCurvaturePerMeter { get; }
|
|
internal double DerivativeScale { get; }
|
|
internal BoundaryNode WithDerivativeScale(double derivativeScale) => new BoundaryNode(
|
|
ArcLengthMeters, X, Y, VehicleHeadingRadians, VehicleCurvaturePerMeter, derivativeScale);
|
|
}
|
|
|
|
private readonly struct CurveSample
|
|
{
|
|
internal CurveSample(double parameter, double x, double y, double dx, double dy)
|
|
{
|
|
Parameter = parameter;
|
|
X = x;
|
|
Y = y;
|
|
Dx = dx;
|
|
Dy = dy;
|
|
}
|
|
internal double Parameter { get; }
|
|
internal double X { get; }
|
|
internal double Y { get; }
|
|
internal double Dx { get; }
|
|
internal double Dy { get; }
|
|
}
|
|
|
|
/// <summary>反射脚本使用的窄范围候选与拼接场景入口。</summary>
|
|
public static class TestHooks
|
|
{
|
|
public static CandidateTestSnapshot Execute(string scenario)
|
|
{
|
|
if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
|
|
switch (scenario)
|
|
{
|
|
case "Isolated": return BuildIsolated();
|
|
case "Cluster": return BuildCluster();
|
|
case "Reverse": return BuildReverse();
|
|
case "Spliced": return BuildSpliced();
|
|
case "StartBoundary": return BuildStartBoundary();
|
|
case "InteriorStationaryCurve": return BuildInteriorStationaryCurve();
|
|
case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints();
|
|
case "GearBoundary": return BuildGearBoundary();
|
|
default: throw new ArgumentOutOfRangeException(nameof(scenario));
|
|
}
|
|
}
|
|
|
|
public sealed class CandidateTestSnapshot
|
|
{
|
|
internal CandidateTestSnapshot(int candidateCount, double startPositionError, double endPositionError,
|
|
double startCurvatureError, double endCurvatureError, bool containsLocalG2Source,
|
|
int outputRegionCount, bool internalConnectionsAreG2, string direction,
|
|
bool vehicleAndGeometricCurvatureSignsAreOpposite, bool noDuplicateNonGearPoints,
|
|
bool endpointsUnchanged, bool rejected = false, bool endpointsAreExact = false,
|
|
bool gearBoundaryMarkerPreserved = false)
|
|
{
|
|
CandidateCount = candidateCount;
|
|
StartPositionError = startPositionError;
|
|
EndPositionError = endPositionError;
|
|
StartCurvatureError = startCurvatureError;
|
|
EndCurvatureError = endCurvatureError;
|
|
ContainsLocalG2Source = containsLocalG2Source;
|
|
OutputRegionCount = outputRegionCount;
|
|
InternalConnectionsAreG2 = internalConnectionsAreG2;
|
|
Direction = direction;
|
|
VehicleAndGeometricCurvatureSignsAreOpposite = vehicleAndGeometricCurvatureSignsAreOpposite;
|
|
NoDuplicateNonGearPoints = noDuplicateNonGearPoints;
|
|
EndpointsUnchanged = endpointsUnchanged;
|
|
Rejected = rejected;
|
|
EndpointsAreExact = endpointsAreExact;
|
|
GearBoundaryMarkerPreserved = gearBoundaryMarkerPreserved;
|
|
}
|
|
public int CandidateCount { get; }
|
|
public double StartPositionError { get; }
|
|
public double EndPositionError { get; }
|
|
public double StartCurvatureError { get; }
|
|
public double EndCurvatureError { get; }
|
|
public bool ContainsLocalG2Source { get; }
|
|
public int OutputRegionCount { get; }
|
|
public bool InternalConnectionsAreG2 { get; }
|
|
public string Direction { get; }
|
|
public bool VehicleAndGeometricCurvatureSignsAreOpposite { get; }
|
|
public bool NoDuplicateNonGearPoints { get; }
|
|
public bool EndpointsUnchanged { get; }
|
|
public bool Rejected { get; }
|
|
public bool EndpointsAreExact { get; }
|
|
public bool GearBoundaryMarkerPreserved { get; }
|
|
}
|
|
|
|
private static CandidateTestSnapshot BuildIsolated()
|
|
{
|
|
PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
|
|
LocalG2SmoothingRegion region = CreateRegion(new[]
|
|
{
|
|
new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0d, 0.4d),
|
|
}, 0.25d, 0.75d);
|
|
IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
|
|
segment, region, 0.025d, CreateOptions(), CancellationToken.None);
|
|
if (candidates.Count == 0) throw new InvalidOperationException("The isolated candidate scenario produced no candidate.");
|
|
LocalG2CandidateGeometry candidate = candidates[0];
|
|
PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.25d, out SmoothingPoint2D start, out _);
|
|
PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.75d, out SmoothingPoint2D end, out _);
|
|
return new CandidateTestSnapshot(candidates.Count,
|
|
Distance(start, candidate.RegionPoints[0]), Distance(end, candidate.RegionPoints[candidate.RegionPoints.Count - 1]),
|
|
Math.Abs(candidate.StartVehicleCurvaturePerMeter - 0d),
|
|
Math.Abs(candidate.EndVehicleCurvaturePerMeter - 0.4d),
|
|
ContainsLocalG2(candidate.RegionPoints), 0, false, string.Empty, false, false, false);
|
|
}
|
|
|
|
private static CandidateTestSnapshot BuildCluster()
|
|
{
|
|
PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
|
|
var transitions = new[]
|
|
{
|
|
new CurvatureTransition(0, 1, 2, 0.4d, 0.4d, 0d, 0d, 0d, 0.4d),
|
|
new CurvatureTransition(0, 2, 3, 0.6d, 0.6d, 0d, 0d, 0.4d, 0d),
|
|
};
|
|
var planner = new LocalG2WindowPlanner();
|
|
if (!planner.TryPlan(new PreparedPath(new[] { segment }), transitions, CreateOptions(),
|
|
out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
|
|
{
|
|
throw new InvalidOperationException(reason);
|
|
}
|
|
IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
|
|
segment, regions[0], 0.025d, CreateOptions(), CancellationToken.None);
|
|
if (candidates.Count == 0) throw new InvalidOperationException("The clustered candidate scenario produced no candidate.");
|
|
return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, regions.Count,
|
|
candidates[0].InternalConnectionsAreG2, string.Empty, false, false, false);
|
|
}
|
|
|
|
private static CandidateTestSnapshot BuildReverse()
|
|
{
|
|
PreparedDirectionSegment segment = CreateSegment(TravelDirection.Reverse, null);
|
|
LocalG2SmoothingRegion region = CreateRegion(new[]
|
|
{
|
|
new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, Math.PI, 0.2d, 0.4d),
|
|
}, 0.25d, 0.75d);
|
|
IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
|
|
segment, region, 0.025d, CreateOptions(), CancellationToken.None);
|
|
if (candidates.Count == 0) throw new InvalidOperationException("The reverse candidate scenario produced no candidate.");
|
|
LocalG2CandidateGeometry candidate = candidates[0];
|
|
bool opposite = candidate.StartVehicleCurvaturePerMeter * candidate.StartGeometricCurvaturePerMeter < 0d &&
|
|
candidate.EndVehicleCurvaturePerMeter * candidate.EndGeometricCurvaturePerMeter < 0d;
|
|
return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false,
|
|
segment.Direction.ToString(), opposite, false, false);
|
|
}
|
|
|
|
private static CandidateTestSnapshot BuildSpliced()
|
|
{
|
|
PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, 0.17d);
|
|
LocalG2SmoothingRegion region = CreateRegion(new[]
|
|
{
|
|
new CurvatureTransition(0, 1, 2, 0.5d, 0.5d, 0d, 0d, 0.17d, 0.4d),
|
|
}, 0.25d, 0.75d);
|
|
IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
|
|
segment, region, 0.025d, CreateOptions(), CancellationToken.None);
|
|
if (candidates.Count == 0) throw new InvalidOperationException("The splice candidate scenario produced no candidate.");
|
|
PreparedDirectionSegment untouched = CreateSegment(TravelDirection.Reverse, null);
|
|
var path = new PreparedPath(new[] { segment, untouched });
|
|
if (!new LocalG2PathSplicer().TryReplace(path, candidates[0], out PreparedPath replaced, out string reason))
|
|
throw new InvalidOperationException(reason);
|
|
PreparedDirectionSegment result = replaced.Segments[0];
|
|
bool noDuplicates = true;
|
|
for (int index = 1; index < result.Points.Count; index++)
|
|
{
|
|
if (!result.Points[index - 1].IsGearSwitchPoint && !result.Points[index].IsGearSwitchPoint &&
|
|
Distance(result.Points[index - 1], result.Points[index]) <= 1e-12d)
|
|
{
|
|
noDuplicates = false;
|
|
break;
|
|
}
|
|
}
|
|
bool monotonicallyIncreasing = true;
|
|
for (int index = 1; index < result.Points.Count; index++)
|
|
{
|
|
if (result.Points[index].ArcLength <= result.Points[index - 1].ArcLength)
|
|
{
|
|
monotonicallyIncreasing = false;
|
|
break;
|
|
}
|
|
}
|
|
bool endpoints = Distance(segment.Points[0], result.Points[0]) <= 1e-12d &&
|
|
Distance(segment.Points[segment.Points.Count - 1], result.Points[result.Points.Count - 1]) <= 1e-12d &&
|
|
result.StartVehicleCurvaturePerMeter == segment.StartVehicleCurvaturePerMeter && monotonicallyIncreasing &&
|
|
ReferenceEquals(untouched, replaced.Segments[1]);
|
|
return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
|
|
false, noDuplicates, endpoints);
|
|
}
|
|
|
|
private static CandidateTestSnapshot BuildStartBoundary()
|
|
{
|
|
PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, 0.17d);
|
|
LocalG2SmoothingRegion region = CreateRegion(new[]
|
|
{
|
|
new CurvatureTransition(0, 0, 1, 0d, 0d, 0d, 0d, 0.17d, 0.4d),
|
|
}, 0d, 0.5d);
|
|
IReadOnlyList<LocalG2CandidateGeometry> candidates = new LocalG2CandidateBuilder().Build(
|
|
segment, region, 0.025d, CreateOptions(), CancellationToken.None);
|
|
if (candidates.Count == 0) throw new InvalidOperationException("The start-boundary transition must not create a zero-length left curve.");
|
|
LocalG2CandidateGeometry candidate = candidates[0];
|
|
return new CandidateTestSnapshot(candidates.Count,
|
|
Math.Abs(candidate.StartVehicleCurvaturePerMeter - 0.17d), 0d,
|
|
Math.Abs(candidate.StartVehicleCurvaturePerMeter - 0.17d), 0d,
|
|
ContainsLocalG2(candidate.RegionPoints), 0, false, string.Empty, false, false, false);
|
|
}
|
|
|
|
private static CandidateTestSnapshot BuildInteriorStationaryCurve()
|
|
{
|
|
const double endpointX = 7d / 1500d;
|
|
if (!QuinticHermiteCurve2D.TryCreate(0d, 0d, 0.01d, 0d, 0d, 0d,
|
|
endpointX, 0d, 0.01d, 0d, 0d, 0d, out QuinticHermiteCurve2D curve, out string reason))
|
|
{
|
|
throw new InvalidOperationException(reason);
|
|
}
|
|
var points = new[]
|
|
{
|
|
new SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
|
new SmoothingPoint2D(endpointX, 0d, endpointX, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
|
};
|
|
var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false);
|
|
var sampled = new List<SmoothingPoint2D>();
|
|
bool accepted = TryAppendCurveSamples(curve,
|
|
new BoundaryNode(0d, 0d, 0d, 0d, 0d),
|
|
new BoundaryNode(endpointX, endpointX, 0d, 0d, 0d),
|
|
segment, 0.025d, sampled, CancellationToken.None);
|
|
return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
|
|
false, false, false, !accepted);
|
|
}
|
|
|
|
private static CandidateTestSnapshot BuildExactSpliceEndpoints()
|
|
{
|
|
PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null);
|
|
var candidate = new LocalG2CandidateGeometry(0, 0, 0.25d, 0.75d, 0.25d, 0.25d,
|
|
new[]
|
|
{
|
|
Point(0.2500000005d, 0d, 0.25d, false),
|
|
Point(0.5d, 0d, 0.5d, false),
|
|
Point(0.7499999995d, 0d, 0.75d, false),
|
|
}, 0d, 0d, 0d, 0d, true);
|
|
if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate,
|
|
out PreparedPath replaced, out string reason))
|
|
{
|
|
throw new InvalidOperationException(reason);
|
|
}
|
|
PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.25d, out SmoothingPoint2D start, out _);
|
|
PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.75d, out SmoothingPoint2D end, out _);
|
|
IReadOnlyList<SmoothingPoint2D> result = replaced.Segments[0].Points;
|
|
bool exact = result[1].X == start.X && result[1].Y == start.Y &&
|
|
result[result.Count - 2].X == end.X && result[result.Count - 2].Y == end.Y;
|
|
return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
|
|
false, false, false, false, exact);
|
|
}
|
|
|
|
private static CandidateTestSnapshot BuildGearBoundary()
|
|
{
|
|
var sourcePoints = new[]
|
|
{
|
|
Point(0d, 0d, 0d, true), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false),
|
|
};
|
|
var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, sourcePoints, true, false);
|
|
var candidate = new LocalG2CandidateGeometry(0, 0, 0d, 0.5d, 0d, 0.5d,
|
|
new[] { Point(0d, 0d, 0d, false), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false) },
|
|
0d, 0d, 0d, 0d, true);
|
|
if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate,
|
|
out PreparedPath replaced, out string reason))
|
|
{
|
|
throw new InvalidOperationException(reason);
|
|
}
|
|
bool preserved = replaced.Segments[0].StartsAtGearSwitch && replaced.Segments[0].Points[0].IsGearSwitchPoint;
|
|
return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty,
|
|
false, false, false, false, false, preserved);
|
|
}
|
|
|
|
private static PreparedDirectionSegment CreateSegment(TravelDirection direction, double? startCurvature)
|
|
{
|
|
double heading = direction == TravelDirection.Forward ? 0d : Math.PI;
|
|
var points = new List<SmoothingPoint2D>();
|
|
for (int index = 0; index <= 4; index++)
|
|
{
|
|
double arc = index * 0.25d;
|
|
points.Add(new SmoothingPoint2D(arc, 0d, arc, heading, heading, 1d, false, SmoothedPathPointSource.Anchor));
|
|
}
|
|
return new PreparedDirectionSegment(0, direction, points, false, false, startCurvature);
|
|
}
|
|
|
|
private static LocalG2SmoothingRegion CreateRegion(
|
|
IReadOnlyList<CurvatureTransition> transitions,
|
|
double startArcLengthMeters,
|
|
double endArcLengthMeters)
|
|
{
|
|
return new LocalG2SmoothingRegion(0, transitions, startArcLengthMeters, endArcLengthMeters,
|
|
new[] { new LocalG2WindowVariant(0, startArcLengthMeters, endArcLengthMeters,
|
|
transitions[0].LocalArcLengthMeters - startArcLengthMeters,
|
|
endArcLengthMeters - transitions[transitions.Count - 1].LocalArcLengthMeters) });
|
|
}
|
|
|
|
private static LocalG2OptionsSnapshot CreateOptions() => new LocalG2OptionsSnapshot(new PathSmoothingConfiguration());
|
|
private static bool ContainsLocalG2(IReadOnlyList<SmoothingPoint2D> points)
|
|
{
|
|
for (int index = 0; index < points.Count; index++)
|
|
if (points[index].Source == SmoothedPathPointSource.LocalG2Transition) return true;
|
|
return false;
|
|
}
|
|
private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right) =>
|
|
Math.Sqrt((right.X - left.X) * (right.X - left.X) + (right.Y - left.Y) * (right.Y - left.Y));
|
|
private static SmoothingPoint2D Point(double x, double y, double arcLength, bool isGearSwitch) =>
|
|
new SmoothingPoint2D(x, y, arcLength, 0d, 0d, 1d, isGearSwitch,
|
|
isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.LocalG2Transition);
|
|
}
|
|
}
|