feat: preserve EM planner segment boundaries

2026-08-03 22:27:00 +08:00
parent b326431d63
commit e492e1610a
8 changed files with 407 additions and 4 deletions
@@ -0,0 +1,49 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public sealed class ReferenceBoundary : IEquatable<ReferenceBoundary>
+{
+    public ReferenceBoundary(int segmentIndex, double segmentLocalS, EmBoundaryType boundaryType, double sourceArcLength)
+    {
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (double.IsNaN(segmentLocalS) || double.IsInfinity(segmentLocalS) || segmentLocalS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(segmentLocalS));
+        if (double.IsNaN(sourceArcLength) || double.IsInfinity(sourceArcLength) || sourceArcLength < 0d)
+            throw new ArgumentOutOfRangeException(nameof(sourceArcLength));
+        if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType))
+            throw new ArgumentOutOfRangeException(nameof(boundaryType));
+
+        SegmentIndex = segmentIndex;
+        SegmentLocalS = segmentLocalS;
+        BoundaryType = boundaryType;
+        SourceArcLength = sourceArcLength;
+    }
+
+    public int SegmentIndex { get; }
+
+    public double SegmentLocalS { get; }
+
+    public EmBoundaryType BoundaryType { get; }
+
+    public double SourceArcLength { get; }
+
+    public bool Equals(ReferenceBoundary other)
+    {
+        return other != null && SegmentIndex == other.SegmentIndex && SegmentLocalS.Equals(other.SegmentLocalS) &&
+            BoundaryType == other.BoundaryType;
+    }
+
+    public override bool Equals(object obj) { return Equals(obj as ReferenceBoundary); }
+
+    public override int GetHashCode()
+    {
+        unchecked
+        {
+            int hash = SegmentIndex;
+            hash = (hash * 397) ^ SegmentLocalS.GetHashCode();
+            return (hash * 397) ^ (int)BoundaryType;
+        }
+    }
+}