fix: handle degenerate local G2 derivative hulls

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<SmoothingPoint2D>();
+            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);

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.'