diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs index eb17400..8597d96 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs @@ -389,16 +389,51 @@ internal sealed class LocalG2CandidateBuilder private static bool ContainsOrigin(DerivativeControlPoint first, DerivativeControlPoint second, DerivativeControlPoint third, double margin) { + double sideX = second.X - first.X; + double sideY = second.Y - first.Y; + double thirdOffsetX = third.X - first.X; + double thirdOffsetY = third.Y - first.Y; + double triangleArea = sideX * thirdOffsetY - sideY * thirdOffsetX; double crossFirstSecond = Cross(first, second); double crossSecondThird = Cross(second, third); double crossThirdFirst = Cross(third, first); - if (!NumericGuard.IsFinite(crossFirstSecond) || !NumericGuard.IsFinite(crossSecondThird) || !NumericGuard.IsFinite(crossThirdFirst)) + if (!NumericGuard.IsFinite(triangleArea) || !NumericGuard.IsFinite(crossFirstSecond) || + !NumericGuard.IsFinite(crossSecondThird) || !NumericGuard.IsFinite(crossThirdFirst)) + { return true; + } double areaMargin = margin * Math.Max(1d, Math.Max(first.Norm, Math.Max(second.Norm, third.Norm))); + if (!NumericGuard.IsFinite(areaMargin)) return true; + if (Math.Abs(triangleArea) <= areaMargin) + { + // A degenerate triangle is only a closed line segment (or a point), not a + // two-dimensional region. Treat it as origin-containing only when the origin + // lies on one of its actual closed segments; otherwise its distance is positive. + return IsOriginOnSegment(first, second, margin) || IsOriginOnSegment(second, third, margin) || + IsOriginOnSegment(third, first, margin); + } return (crossFirstSecond >= -areaMargin && crossSecondThird >= -areaMargin && crossThirdFirst >= -areaMargin) || (crossFirstSecond <= areaMargin && crossSecondThird <= areaMargin && crossThirdFirst <= areaMargin); } + private static bool IsOriginOnSegment(DerivativeControlPoint start, DerivativeControlPoint end, double margin) + { + double dx = end.X - start.X; + double dy = end.Y - start.Y; + double lengthSquared = dx * dx + dy * dy; + if (!NumericGuard.IsFinite(lengthSquared)) return true; + if (lengthSquared == 0d) return start.Norm <= margin; + double length = Math.Sqrt(lengthSquared); + double cross = Cross(start, end); + double projection = -(start.X * dx + start.Y * dy); + if (!NumericGuard.IsFinite(length) || !NumericGuard.IsFinite(cross) || !NumericGuard.IsFinite(projection)) return true; + double lineMargin = margin * Math.Max(1d, length); + double projectionMargin = margin * Math.Max(1d, length); + if (!NumericGuard.IsFinite(lineMargin) || !NumericGuard.IsFinite(projectionMargin)) return true; + return Math.Abs(cross) <= lineMargin && projection >= -projectionMargin && + projection <= lengthSquared + projectionMargin; + } + private static double DistanceToSegment(DerivativeControlPoint start, DerivativeControlPoint end) { double dx = end.X - start.X; @@ -523,6 +558,7 @@ internal sealed class LocalG2CandidateBuilder case "Spliced": return BuildSpliced(); case "StartBoundary": return BuildStartBoundary(); case "InteriorStationaryCurve": return BuildInteriorStationaryCurve(); + case "ConstantVelocityCurve": return BuildConstantVelocityCurve(); case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints(); case "GearBoundary": return BuildGearBoundary(); default: throw new ArgumentOutOfRangeException(nameof(scenario)); @@ -536,7 +572,7 @@ internal sealed class LocalG2CandidateBuilder int outputRegionCount, bool internalConnectionsAreG2, string direction, bool vehicleAndGeometricCurvatureSignsAreOpposite, bool noDuplicateNonGearPoints, bool endpointsUnchanged, bool rejected = false, bool endpointsAreExact = false, - bool gearBoundaryMarkerPreserved = false) + bool gearBoundaryMarkerPreserved = false, bool accepted = false) { CandidateCount = candidateCount; StartPositionError = startPositionError; @@ -553,6 +589,7 @@ internal sealed class LocalG2CandidateBuilder Rejected = rejected; EndpointsAreExact = endpointsAreExact; GearBoundaryMarkerPreserved = gearBoundaryMarkerPreserved; + Accepted = accepted; } public int CandidateCount { get; } public double StartPositionError { get; } @@ -569,6 +606,7 @@ internal sealed class LocalG2CandidateBuilder public bool Rejected { get; } public bool EndpointsAreExact { get; } public bool GearBoundaryMarkerPreserved { get; } + public bool Accepted { get; } } private static CandidateTestSnapshot BuildIsolated() @@ -711,6 +749,28 @@ internal sealed class LocalG2CandidateBuilder false, false, false, !accepted); } + private static CandidateTestSnapshot BuildConstantVelocityCurve() + { + if (!QuinticHermiteCurve2D.TryCreate(0d, 0d, 1d, 0d, 0d, 0d, + 1d, 0d, 1d, 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(1d, 0d, 1d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), + }; + var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false); + var sampled = new List(); + bool accepted = TryAppendCurveSamples(curve, + new BoundaryNode(0d, 0d, 0d, 0d, 0d), + new BoundaryNode(1d, 1d, 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, false, false, false, accepted); + } + private static CandidateTestSnapshot BuildExactSpliceEndpoints() { PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null); diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 index 4483bc5..01d8edb 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 @@ -62,6 +62,10 @@ $interiorDerivative = Invoke-Scenario 'InteriorStationaryCurve' Assert-True $interiorDerivative.Rejected ` 'A curve with a stationary interior derivative must be rejected even when its endpoint chord is short.' +$constantVelocity = Invoke-Scenario 'ConstantVelocityCurve' +Assert-True $constantVelocity.Accepted ` + 'A nonstationary constant-velocity curve with collinear derivative controls must be accepted.' + $exactSplice = Invoke-Scenario 'ExactSpliceEndpoints' Assert-True $exactSplice.EndpointsAreExact ` 'Splicing must write interpolated exact endpoints instead of accepting approximate candidate endpoints.'