Add fleet curve walk support

2026-07-07 20:28:26 +08:00
parent 6ca401cf68
commit 0d44fb8fde
5 changed files with 1099 additions and 504 deletions
@@ -400,6 +400,102 @@ namespace MultiWheelC
                }
            }
        }
+
+        public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
+            float speed, params float[] trackTypeInfo)
+        {
+            if (trackTypeInfo == null || trackTypeInfo.Length < 2)
+            {
+                DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            var trackType = (int)trackTypeInfo[0];
+            if (trackType != 2)
+            {
+                DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
+                return;
+            }
+
+            var controlPointNum = (int)trackTypeInfo[1];
+            var expectedLength = 2 + controlPointNum * 2;
+            if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
+            {
+                DLog.Log(
+                    $"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
+                    $"length={trackTypeInfo.Length}, expected>={expectedLength}.",
+                    "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
+                return;
+            }
+
+            BezierTrack track;
+            try
+            {
+                track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
+            }
+            catch (Exception ex)
+            {
+                DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
+                return;
+            }
+
+            if (track == null)
+            {
+                DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
+                Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
+                return;
+            }
+
+            track.Speed = speed;
+            track.CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias;
+
+            DLog.Log(
+                $"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
+                $"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
+                $"carDirectionBias={PilotDefinition.Conf.FleetCurveCarDirectionBias:0.0})",
+                "FleetCurveDbg");
+
+            if (dstId != -1)
+            {
+                while (!TryLock(dstId))
+                {
+                    Thread.Sleep(50);
+                }
+                DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
+            }
+
+            var action = new MultiWheelC.FleetCurveWalk
+            {
+                Track = track,
+                CurveSpeed = speed,
+                CarDirectionBias = PilotDefinition.Conf.FleetCurveCarDirectionBias,
+                BezierResolution = PilotDefinition.Conf.FleetCurveBezierResolution,
+                SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
+                FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
+                FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
+                SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
+                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
+                StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
+            };
+
+            try
+            {
+                new DriveTask(action.Get()).Wait();
+            }
+            finally
+            {
+                if (srcId != -1)
+                {
+                    Leave(srcId);
+                    DLog.Log($"閲婃斁鏀捐溅鐐箋srcId}", "FleetCurveDbg");
+                }
+            }
+        }
        
        public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
        {