837 lines
42 KiB
C#
837 lines
42 KiB
C#
using Ananta.SDK.Network;
|
||
using Ananta.SDK.Rpc;
|
||
using Ananta.SDK.Serialization;
|
||
using Ananta.Server.Configuration;
|
||
using Ananta.Server.Protocol.Client4229938;
|
||
using GameMethods = Ananta.Server.RpcTypes.Client4229938.Methods.Game;
|
||
using NavMethods = Ananta.Server.RpcTypes.Client4229938.Methods.GameScene;
|
||
|
||
namespace Ananta.Server.Handlers.Game;
|
||
|
||
/// <summary>
|
||
/// 载具路线规划 / 自驾（2026-09-24 移植自第九份捐赠端 <c>GameRouter.VehicleNav.cs</c>，但图换成我们自己的）。
|
||
///
|
||
/// ★★ 为什么值得做：`logs/unknown-methods.log` 里今天 06:05 UTC 连着 **89 次**
|
||
/// `AskVehicleNavigationPathPointsFromPos`（每秒一次，起点在动、终点固定）——玩家正在开车跑导航，
|
||
/// 而我们每条都回的是 `DefaultReturnCatalog` 的 typed default：**空点集 + 长度 0**。
|
||
/// 也就是客户端在问「怎么开过去」，服务端一直答「不知道」。
|
||
///
|
||
/// ★★ 为什么不搬捐赠端的 `vehicle_nav_graph.bin`(2.36MB)：那份是他们自己 generator 从另一份
|
||
/// 数据打的私有格式（magic `ANGV` / ver 2 / 每车道 40B），provenance 不可考；我们手上已经有
|
||
/// `vehicle_lanes.csv`(14171 条) + `vehicle_lane_links.csv`（多值后继覆盖 98.2%，见
|
||
/// `GameRouter.VehicleLanes.cs` 头注），用同一份数据算出来的路线才是自洽的。
|
||
///
|
||
/// ⛑️ 我们这份表的几何是「弦」不是真实样条 —— `_gen_vehicle_lanes.py` 压掉了中间点。
|
||
/// 所以画出来的导航线是折线、切内弯；想提高精度要把 lane 折线点补回来，不是改这里。
|
||
/// ⛑️ 只走**前向**后继（本帧生成车道的方向），与 T3-7 车流 `Advance` 的规则一致；
|
||
/// 客户端这次带的是 `IgnoreDirection=true`，它接受单方向绕行，不构成违约。
|
||
/// ⛑️ 回滚开关 = `gameplay.vehicleNavigation.enabled=false` ⇒ 逐字节回到空结果（见 <c>EmptyResult</c>）。
|
||
/// </summary>
|
||
internal sealed partial class GameRouter
|
||
{
|
||
private static VehicleNavigationSettings VehicleNavCfg
|
||
=> PrivateServerConfigStore.Current.Gameplay.VehicleNavigation;
|
||
|
||
private static NavMethods.VehicleNavResult4229938 EmptyResult(uint navReqId)
|
||
=> new() { NavReqId = navReqId, Points = [], CenterPoints = [] };
|
||
|
||
/// <summary>
|
||
/// 当前生效的巡航速度（<b>米/秒</b>）。
|
||
///
|
||
/// 优先级：<c>config/autonomous-driving.json</c>（GM 面板写的） > <c>gameplay.vehicleNavigation.cruiseSpeed</c>。
|
||
/// 前者不存在 = 没在 GM 里调过速 ⇒ 完全用主配置的值，与 2026-09-30 之前逐字节一致。
|
||
/// ⛑ 单位换算（km/h → m/s）<b>只在这里做</b>；<see cref="AutonomousDriveSpeedSettings"/> 对外一律 km/h。
|
||
/// ⛑ 文件被写坏时<b>不能</b>让玩家开不了车：吞掉异常退回主配置，GM 重新保存即可恢复。
|
||
/// </summary>
|
||
private static float EffectiveCruiseSpeedMs()
|
||
{
|
||
try
|
||
{
|
||
if (File.Exists(AutonomousDriveSpeedSettings.FilePath))
|
||
return AutonomousDriveSpeedSettings.Read() / 3.6f;
|
||
}
|
||
catch (Exception ex) when (ex is InvalidDataException or IOException or UnauthorizedAccessException)
|
||
{
|
||
// 同上：坏配置只影响「用哪个速度」，不影响「能不能开车」。
|
||
}
|
||
|
||
return VehicleNavCfg.CruiseSpeed;
|
||
}
|
||
|
||
// -----------------------------------------------------------------------------------------
|
||
// AskVehicleNavigationPathPoints (67317431) — 起点由服务端自己找（最后一次上报的位置）
|
||
// -----------------------------------------------------------------------------------------
|
||
[Handler(MethodId.AskVehicleNavigationPathPoints)]
|
||
private Task OnAskVehicleNavigationPathPoints(Connection conn, UxRpcMessage msg)
|
||
{
|
||
if (!VehicleNavCfg.Enabled)
|
||
return AnswerNavAsync(conn, msg, EmptyResult(0), null);
|
||
|
||
if (!msg.TryGetArgs<NavMethods.AskVehicleNavigationPathPoints4229938>(out var args) || args is null)
|
||
{
|
||
LogNavOnce(conn, "[VEHNAV] AskVehicleNavigationPathPoints 参数解析失败，回空结果", msg.Body.Length);
|
||
return AnswerNavAsync(conn, msg, EmptyResult(0), null);
|
||
}
|
||
|
||
var (sx, sy, sz) = CurrentVehiclePosition(msg);
|
||
return AnswerNavAsync(
|
||
conn, msg, EmptyResult(args.NavReqId),
|
||
SolveRoute(conn, sx, sy, sz, args.TargetPosition.X, args.TargetPosition.Y, args.TargetPosition.Z,
|
||
args.NeedCenterPoints && VehicleNavCfg.IncludeCenterPoints, "AskVehicleNavigationPathPoints"));
|
||
}
|
||
|
||
// -----------------------------------------------------------------------------------------
|
||
// AskVehicleNavigationPathPointsFromPos (67650626) — 客户端自带起点（今天实际在用的就是这条）
|
||
// -----------------------------------------------------------------------------------------
|
||
[Handler(MethodId.AskVehicleNavigationPathPointsFromPos)]
|
||
private Task OnAskVehicleNavigationPathPointsFromPos(Connection conn, UxRpcMessage msg)
|
||
{
|
||
if (!VehicleNavCfg.Enabled)
|
||
return AnswerNavAsync(conn, msg, EmptyResult(0), null);
|
||
|
||
if (!msg.TryGetArgs<NavMethods.AskVehicleNavigationPathPointsFromPos4229938>(out var args) || args is null)
|
||
{
|
||
LogNavOnce(conn, "[VEHNAV] AskVehicleNavigationPathPointsFromPos 参数解析失败，回空结果", msg.Body.Length);
|
||
return AnswerNavAsync(conn, msg, EmptyResult(0), null);
|
||
}
|
||
|
||
var start = args.StartPosition;
|
||
if (start.X == 0f && start.Z == 0f)
|
||
(start.X, _, start.Z) = CurrentVehiclePosition(msg);
|
||
|
||
return AnswerNavAsync(
|
||
conn, msg, EmptyResult(args.NavReqId),
|
||
SolveRoute(conn, start.X, start.Y, start.Z, args.TargetPosition.X, args.TargetPosition.Y,
|
||
args.TargetPosition.Z, args.NeedCenterPoints && VehicleNavCfg.IncludeCenterPoints,
|
||
"AskVehicleNavigationPathPointsFromPos"));
|
||
}
|
||
|
||
// -----------------------------------------------------------------------------------------
|
||
// AskVehicleNavigationPathLength (67519267) — 只要里程（float）
|
||
// -----------------------------------------------------------------------------------------
|
||
[Handler(MethodId.AskVehicleNavigationPathLength)]
|
||
private Task OnAskVehicleNavigationPathLength(Connection conn, UxRpcMessage msg)
|
||
{
|
||
if (!VehicleNavCfg.Enabled)
|
||
return conn.ReturnAsync(msg, 0f);
|
||
|
||
if (!msg.TryGetArgs<NavMethods.AskVehicleNavigationPathLength4229938>(out var args) || args is null)
|
||
return conn.ReturnAsync(msg, 0f);
|
||
|
||
var (sx, sy, sz) = CurrentVehiclePosition(msg);
|
||
var route = SolveRoute(conn, sx, sy, sz, args.TargetPosition.X, args.TargetPosition.Y,
|
||
args.TargetPosition.Z, needCenter: false, "AskVehicleNavigationPathLength");
|
||
return conn.ReturnAsync(msg, route is { Ok: true } ok ? ok.Length : 0f);
|
||
}
|
||
|
||
// -----------------------------------------------------------------------------------------
|
||
// 自驾开关 Start / Stop / ChangeTarget / CancelTarget
|
||
//
|
||
// ★★ 2026-09-27 修「点了自动驾驶车不动」。根因不在 Start 的回执 —— 客户端的自动驾驶
|
||
// 开关是 **服务端推下来的 S2C**，不是本地状态：
|
||
// BaseVehicleControllerStore.OnAutoDriveBtnClick → AskVehicleStartAutonomousDriving
|
||
// GameSceneToClientImpl:1438 SyncVehicleAutonomousDrivingState(u64 车, bool isStart)
|
||
// → DriveVehiclesManager.autoDrivingVehicleDict[车] = true / nil
|
||
// → BaseVehicleControllerStore.SetAutoDriving(GetVehicleInAutonomousDriving(车))
|
||
// 真正的循迹 AI 在客户端 C# 的 LX6.Drive.DriveManager（包是 LuaAndCSharp，两边都收）。
|
||
// ⇒ 只要服务端不发 68349264，UI 上那个按钮按下去会立刻弹回来、车纹丝不动。
|
||
// ⇒ 反过来也解释了为什么 previously「挂了 Start/Stop 回执」毫无效果：回执只影响
|
||
// `AutoDrive.AskWait`，不影响 Driving 状态。
|
||
//
|
||
// ⛑️ 78636393(CancelTarget) 之前压根没注册 ⇒ 玩家在地图上删掉导航点时会 INVOKE 无回执、await 卡死（规则 84）。
|
||
// ⛑️ 服务端**不负责每帧开车**：车还是客户端本地物理。服务端只管「给不给自动驾驶许可」。
|
||
// 真要让另一端的观众也看到车在动，是另一件事（要服务端推位置环回），本次不做。
|
||
// -----------------------------------------------------------------------------------------
|
||
[Handler(MethodId.AskVehicleStartAutonomousDriving)]
|
||
private async Task OnAskVehicleStartAutonomousDriving(Connection conn, UxRpcMessage msg)
|
||
{
|
||
var runtime = GetVehicleRuntimeState(msg.Context.Session);
|
||
|
||
if (!msg.TryGetArgs<NavMethods.AskVehicleStartAutonomousDriving4229938>(out var args) || args is null)
|
||
{
|
||
LogNavOnce(conn, "[VEHNAV] AskVehicleStartAutonomousDriving 参数解析失败", msg.Body.Length);
|
||
await conn.ReturnEmptyOkAsync(msg);
|
||
return;
|
||
}
|
||
|
||
ulong vehicleId;
|
||
lock (runtime.SyncRoot)
|
||
{
|
||
vehicleId = runtime.CurrentVehicleId;
|
||
runtime.AutoDriveHasTarget = args.HasValidTargetPosition;
|
||
runtime.AutoDriveTargetX = args.TargetPosition.X;
|
||
runtime.AutoDriveTargetY = args.TargetPosition.Y;
|
||
runtime.AutoDriveTargetZ = args.TargetPosition.Z;
|
||
}
|
||
|
||
conn.Log.Info($"[VEHNAV] start autonomous driving vehicle={vehicleId} " +
|
||
$"validTarget={args.HasValidTargetPosition} " +
|
||
$"target=({args.TargetPosition.X:0.#},{args.TargetPosition.Y:0.#},{args.TargetPosition.Z:0.#})");
|
||
|
||
if (vehicleId == 0)
|
||
{
|
||
// 没在车上就没法说「哪辆车开始自动驾驶」——这不构成协议错误，回 Ok 让按钮解锁即可。
|
||
LogNavOnce(conn, "[VEHNAV] 收到 Start 但当前不在任何车上，不下发自动驾驶状态", 0);
|
||
await conn.ReturnEmptyOkAsync(msg);
|
||
return;
|
||
}
|
||
|
||
if (!VehicleNavCfg.AutonomousDrivingEnabled)
|
||
{
|
||
LogNavOnce(conn, "[VEHNAV] 自驾已由 gameplay.vehicleNavigation.autonomousDrivingEnabled=false 关闭", 0);
|
||
await conn.ReturnEmptyOkAsync(msg);
|
||
return;
|
||
}
|
||
|
||
// 先把玩家的三个开关同步一次：这样 UI 的 blocked/interactable 有确定值，
|
||
// 而不是靠 lua 里 nil 的巧合。三个全 false = 「允许自驾、不锁沉浸模式」。
|
||
await PushPlayerAutonomousStateAsync(conn);
|
||
|
||
// ★★ 真正让车动起来的一步（2026-09-28 第二轮）。
|
||
// 第一轮只推了状态包，结果「能开自动驾驶但车不动」——因为 68349264 只管 UI 与许可，
|
||
// 车的循迹由 SyncAssignVehicleAITask 的 **Cruise 任务**驱动，而它带 TargetPointList
|
||
// （VehicleCruiseParameters4229938），正是「沿一串点巡航」。路径就用 SolveRoute 算的。
|
||
var (sx, sy, sz) = VehiclePositionFor(runtime, vehicleId, msg);
|
||
var route = SolveRoute(conn, sx, sy, sz, args.TargetPosition.X, args.TargetPosition.Y,
|
||
args.TargetPosition.Z, needCenter: false, "auto-drive-cruise");
|
||
|
||
if (route is { Ok: true } ok && ok.DrivePoints.Count >= 2)
|
||
{
|
||
// ★★ 2026-09-30：状态包必须**先于** AI 任务下发。
|
||
// 捐赠端 `StartAutonomousDriving4229938` 的顺序是 controller → PublishAutoDriveState
|
||
// → SyncAssignVehicleAITask；而我们以前是发完任务、确认完才补状态包。
|
||
// 客户端只有先进入「自动驾驶中」才会按自动驾驶那套表现搭好车内视角/角色动作，
|
||
// 否则它就是收到一个**陌生人发来的 AI 任务**（和骇入自动驾驶同一条通道），
|
||
// 表现层可能按「AI 接管」处理 —— 这正是「车门消失 / 角色缺模 / 没驾驶动作」的嫌疑点。
|
||
await PushVehicleAutonomousStateAsync(conn, vehicleId, true);
|
||
await AssignAutonomousCruiseAsync(conn.Session, runtime, vehicleId, ok.DrivePoints);
|
||
lock (runtime.SyncRoot)
|
||
{
|
||
runtime.AutoDriving = true;
|
||
runtime.AutoDrivingVehicleId = vehicleId;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 解不出路就别点亮 UI：否则玩家看到「自动驾驶中」而车不动，比不点亮更迷惑。
|
||
// ⛑⛑ 2026-09-30 顺手修一个真 bug：以前 `PushVehicleAutonomousStateAsync(true)`
|
||
// 写在 if/else **外面**，于是这条日志说「不点亮」、下一行照样点亮了。
|
||
// 表现就是「按了自动驾驶，按钮亮了，车纹丝不动」。
|
||
LogNavOnce(conn, "[VEHNAV] 自驾路线解不出来，不点亮自动驾驶状态（见上一条车道/图日志）", 0);
|
||
}
|
||
|
||
await conn.ReturnEmptyOkAsync(msg);
|
||
}
|
||
|
||
[Handler(MethodId.AskVehicleStopAutonomousDriving)]
|
||
private async Task OnAskVehicleStopAutonomousDriving(Connection conn, UxRpcMessage msg)
|
||
{
|
||
var runtime = GetVehicleRuntimeState(msg.Context.Session);
|
||
ulong vehicleId;
|
||
lock (runtime.SyncRoot)
|
||
{
|
||
vehicleId = runtime.AutoDrivingVehicleId != 0 ? runtime.AutoDrivingVehicleId : runtime.CurrentVehicleId;
|
||
runtime.AutoDriving = false;
|
||
runtime.AutoDrivingVehicleId = 0;
|
||
// 已经不在自驾了：丢掉巡航缓存，免得下次 GM 调速拿着过期路线重发。
|
||
runtime.AutoDriveLastPoints = null;
|
||
runtime.AutoDriveSpeedKmh = 0f;
|
||
}
|
||
|
||
conn.Log.Info($"[VEHNAV] stop autonomous driving vehicle={vehicleId}");
|
||
if (vehicleId != 0)
|
||
{
|
||
// 撤掉 AI 任务（action 0 = 停止），否则车会继续跑完下发的巡航路线。
|
||
await AssignAutonomousStopAsync(conn.Session, runtime, vehicleId);
|
||
await PushVehicleAutonomousStateAsync(conn, vehicleId, false);
|
||
}
|
||
|
||
await conn.ReturnEmptyOkAsync(msg);
|
||
}
|
||
|
||
[Handler(MethodId.AskVehicleChangeAutonomousDrivingTarget)]
|
||
private async Task OnAskVehicleChangeAutonomousDrivingTarget(Connection conn, UxRpcMessage msg)
|
||
{
|
||
var runtime = GetVehicleRuntimeState(msg.Context.Session);
|
||
|
||
if (!msg.TryGetArgs<NavMethods.AskVehicleChangeAutonomousDrivingTarget4229938>(out var args) || args is null)
|
||
{
|
||
LogNavOnce(conn, "[VEHNAV] AskVehicleChangeAutonomousDrivingTarget 参数解析失败", msg.Body.Length);
|
||
await conn.ReturnEmptyOkAsync(msg);
|
||
return;
|
||
}
|
||
|
||
var (tx, ty, tz) = (args.TargetPosition.X, args.TargetPosition.Y, args.TargetPosition.Z);
|
||
lock (runtime.SyncRoot)
|
||
{
|
||
runtime.AutoDriveHasTarget = true;
|
||
runtime.AutoDriveTargetX = tx;
|
||
runtime.AutoDriveTargetY = ty;
|
||
runtime.AutoDriveTargetZ = tz;
|
||
}
|
||
|
||
conn.Log.Info($"[VEHNAV] change autonomous target=({tx:0.#},{ty:0.#},{tz:0.#})");
|
||
|
||
// ★★ 2026-09-30 修「开到一半换终点没反应，必须等跑完当前点」。
|
||
// 客户端 lua（BaseVehicleControllerStore.lua:600 OnVehicleNavTargetChange）在换/删导航点
|
||
// 时只发这一条 Change，**不会** Stop 再 Start。而我们以前这里只记账、不下发任何东西
|
||
// ⇒ 车继续开向老终点，玩家只能手动停了再开。现在：正在自驾就立刻换掉 Cruise 任务。
|
||
await RetargetAutonomousDrivingAsync(conn, runtime, msg, tx, ty, tz);
|
||
await conn.ReturnEmptyOkAsync(msg);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 把正在进行的自动驾驶改道到新终点（<see cref="MethodId.AskVehicleChangeAutonomousDrivingTarget"/> 用）。
|
||
///
|
||
/// ⛑ 只重算 Cruise，**不重推** <c>SyncVehicleAutonomousDrivingState</c>：捐赠端那句
|
||
/// "Keep the native autonomous HUD active; replace only our own Cruise." 就是这个意思 ——
|
||
/// 重推会闪一次 UI，且客户端本来就在自动驾驶状态里。
|
||
/// ⛑ 改道起点用<b>车当前位置</b>（<see cref="VehiclePositionFor"/>），不是原来的出发位置。
|
||
/// ⛑ 解不出新路时<b>保持现状</b>（继续开向老终点）而不是停手：中途抛锚比开到旧目的地更难受，
|
||
/// 玩家可以自己按 Stop。这与捐赠端不同（他们 no-route 就 Stop），是他们明确的选择差异。
|
||
/// </summary>
|
||
private async Task RetargetAutonomousDrivingAsync(
|
||
Connection conn, VehicleRuntimeState runtime, UxRpcMessage msg, float tx, float ty, float tz)
|
||
{
|
||
ulong vehicleId;
|
||
ulong previousToken;
|
||
lock (runtime.SyncRoot)
|
||
{
|
||
// 没在自驾 ⇒ 什么都不做：下次按下自动驾驶就带着新目标 Start。
|
||
if (!runtime.AutoDriving)
|
||
return;
|
||
vehicleId = runtime.AutoDrivingVehicleId != 0 ? runtime.AutoDrivingVehicleId : runtime.CurrentVehicleId;
|
||
previousToken = runtime.AutoDriveTaskToken;
|
||
}
|
||
|
||
if (vehicleId == 0 || !VehicleNavCfg.AutonomousDrivingEnabled)
|
||
return;
|
||
|
||
var (sx, sy, sz) = VehiclePositionFor(runtime, vehicleId, msg);
|
||
var route = SolveRoute(conn, sx, sy, sz, tx, ty, tz, needCenter: false, "auto-drive-retarget");
|
||
if (route is not { Ok: true } ok || ok.DrivePoints.Count < 2)
|
||
{
|
||
conn.Log.Info("[VEHNAV] retarget 解不出路线，保持当前任务（详见上面的车道/图日志）");
|
||
return;
|
||
}
|
||
|
||
if (previousToken != 0)
|
||
{
|
||
await AcknowledgeAutoDriveTaskAsync(conn.Session, vehicleId, previousToken, 4); // Abort
|
||
conn.Log.Info($"[VEHNAV] retarget abort previous token={previousToken}");
|
||
}
|
||
|
||
await AssignAutonomousCruiseAsync(conn.Session, runtime, vehicleId, ok.DrivePoints);
|
||
conn.Log.Info($"[VEHNAV] retarget applied vehicle={vehicleId} points={ok.DrivePoints.Count}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// AskVehicleCancelAutonomousDrivingTarget（67836393）—— 2026-09-27 补，之前漏注册。
|
||
/// 客户端在地图上删掉导航点时发，是 INVOKE ⇒ 不回执会让 lua 的 await 卡住。
|
||
/// 语义只是「忘记导航目标」：车照开，只是没了终点（客户端会自己决定何时停下）。
|
||
/// </summary>
|
||
[Handler(MethodId.AskVehicleCancelAutonomousDrivingTarget)]
|
||
private async Task OnAskVehicleCancelAutonomousDrivingTarget(Connection conn, UxRpcMessage msg)
|
||
{
|
||
var runtime = GetVehicleRuntimeState(msg.Context.Session);
|
||
lock (runtime.SyncRoot)
|
||
runtime.AutoDriveHasTarget = false;
|
||
|
||
conn.Log.Info("[VEHNAV] cancel autonomous target");
|
||
await conn.ReturnEmptyOkAsync(msg);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 自驾起点：优先用<b>车</b>自己上报的位置（<c>AskVehicleMove</c> 写进 VehicleRecord），
|
||
/// 退化才用玩家位置。Start 请求不带起点，而玩家坐在车里时这两个坐标差着车长，
|
||
/// 用错了会把起点吸到旁边的车道上。
|
||
/// </summary>
|
||
private static (float X, float Y, float Z) VehiclePositionFor(
|
||
VehicleRuntimeState runtime, ulong vehicleId, UxRpcMessage msg)
|
||
{
|
||
lock (runtime.SyncRoot)
|
||
if (runtime.Vehicles.TryGetValue(vehicleId, out var vehicle))
|
||
return (vehicle.Position.X, vehicle.Position.Y, vehicle.Position.Z);
|
||
|
||
return CurrentVehiclePosition(msg);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 下发「沿路径巡航」的 AI 任务（<c>SyncAssignVehicleAITask</c> + <c>VehicleCruiseParameters</c>）。
|
||
///
|
||
/// 取证：<c>GameRouter.VehicleControls.cs</c> 的 action 4 就是用这个参数类型（带
|
||
/// <c>TargetPointList</c>）让空车倒车挪位；车流/T3-7 与载具骇入都走同一条
|
||
/// <c>SyncAssignVehicleAITask</c> 通道。 ⇒ 这就是服务端驱动车动的唯一入口。
|
||
///
|
||
/// ⛑️ <c>Count</c> 填的是点数量（语义未实测：若车只开到第一个点，改成 1 再试）。
|
||
///
|
||
/// ⛑️⛑️ 2026-09-30 两处按捐赠端 v0.5-2 对齐（都有回滚开关，见 <c>PrivateServerConfig</c>）：
|
||
/// ① <c>DrivingFlags</c>：以前只留 <c>DFStopForCars(1)</c> ⇒ 车不会绕开静止车辆/行人/物件，
|
||
/// 前面一停就顶死，也不等红灯。现在默认 <see cref="VehicleNavigationSettings.CruiseDrivingFlags"/>
|
||
/// = <c>1|2|8|16|32|128|16384</c>（枚举语义见 <c>ux_rpc/payload_types.dump.cs:193</c>）。
|
||
/// ② <c>TargetPointList</c>：默认只给<b>终点一个点</b>（<see cref="VehicleNavigationSettings.CruiseSingleTarget"/>），
|
||
/// 让客户端 C# 的 <c>NavigationHelper</c> 自己导航 —— 我们这份车道几何是「弦」不是样条，
|
||
/// 喂整串点会切内弯。<c>PathFindFlags=0</c>（正向，按道路方向）。
|
||
///
|
||
/// <paramref name="session"/> 而不是 <c>Connection</c>：GM 面板没有 Connection，
|
||
/// 热应用调速（<see cref="ApplyAutonomousDriveSpeedAsync"/>）要复用同一条下发路径。
|
||
/// </summary>
|
||
/// <summary>
|
||
/// 下发 Cruise（沿一串点巡航）任务。自驾与<b>出租车</b>共用这一条通道
|
||
/// （2026-09-30：出租车接入）。
|
||
///
|
||
/// <param name="speedMsOverride">
|
||
/// 速度覆盖（m/s）。<b>null = 用自驾配置的速度</b>（<see cref="EffectiveCruiseSpeedMs"/>）。
|
||
/// 出租车必须传自己的值：正常 <c>15</c>、加速 <c>25</c>（捐赠端 <c>BuildTaxiDriveTask4229938</c>
|
||
/// 原值），不能跟着 GM 的自驾调速走 —— 那会让「加速」按钮和 GM 滑条互相打架。
|
||
/// <param name="tokenOverride">
|
||
/// 复用一个<b>已经有了的</b> token，而不是自己 mint 一个新的（2026-10-03，召唤载具开过来）。
|
||
/// 客户端 <c>SummonVehicleManager</c> 在 <c>AskSummonVehicle</c> 回执里拿到
|
||
/// <c>SummonVehicleResult.TaskToken</c> 后就用它 <c>RegisterTaskAIStartedEvent(token)</c>
|
||
/// —— 两边对不上，原生那套「召唤演出」永远不会触发。⇒ 那台车的到达任务必须带同一个 token。
|
||
/// </param>
|
||
/// </summary>
|
||
private static async Task AssignAutonomousCruiseAsync(
|
||
TcpSession session, VehicleRuntimeState runtime, ulong vehicleId, List<NavMethods.UxVector3> path,
|
||
float? speedMsOverride = null, ulong? tokenOverride = null)
|
||
{
|
||
var cfg = VehicleNavCfg;
|
||
var speedMs = speedMsOverride ?? EffectiveCruiseSpeedMs();
|
||
|
||
List<NavMethods.UxVector3> points;
|
||
if (cfg.CruiseSingleTarget)
|
||
points = [path[^1]]; // 只给终点，交给客户端导航
|
||
else
|
||
points = cfg.CruiseMaxPoints > 0 && path.Count > cfg.CruiseMaxPoints
|
||
? Decimate(path, cfg.CruiseMaxPoints)
|
||
: path;
|
||
|
||
var token = tokenOverride ?? unchecked((ulong)IdCounterStore.Next(VehicleTokenCounter, VehicleTokenFloor));
|
||
var packet = UxSerializer.Serialize(
|
||
new GameMethods.AssignVehicleControl4229938<GameMethods.VehicleCruiseParameters4229938>
|
||
{
|
||
VehicleUId = vehicleId,
|
||
Parameters = new GameMethods.VehicleCruiseParameters4229938
|
||
{
|
||
TargetPointList = points,
|
||
CruiseType = 0, // GoToOneTarget
|
||
Count = points.Count,
|
||
ConfigFlags = 4, // CF_SlowDownWhenArriveTarget
|
||
PathFindFlags = 0, // 正向，按道路方向
|
||
MaxSpeed = speedMs,
|
||
ArrivalDistance = cfg.CruiseArrivalMeters,
|
||
// 捐赠端显式 false：true 会让车按「离终点还有多远」压速度，GM 拉的 MaxSpeed 就不准了。
|
||
AdaptSpeedToTargetDistance = false,
|
||
Base = new GameMethods.VehicleTaskTail4229938
|
||
{
|
||
Token = token,
|
||
DefaultSpeed = speedMs,
|
||
DrivingFlags = cfg.CruiseDrivingFlags,
|
||
},
|
||
},
|
||
});
|
||
|
||
// 主控权交给服务端：不给的话玩家的方向盘输入会盖掉 AI 任务。
|
||
await session.NotifyAsync(MethodId.SyncChangeVehicleController, UxSerializer.Serialize(
|
||
new GameMethods.SyncChangeVehicleController4229938
|
||
{
|
||
vehicleInstanceId = vehicleId,
|
||
controllerPid = Profile.PlayerPid,
|
||
}), CancellationToken.None);
|
||
|
||
lock (runtime.SyncRoot)
|
||
{
|
||
runtime.AutoDriveTaskToken = token;
|
||
runtime.AutoDriveTaskStatus = 0; // Pending：等客户端请求 Running，我们再确认。
|
||
// 缓存「这次真正下发了什么」，供 GM 热应用调速原样复用（换速度不换路线）。
|
||
runtime.AutoDriveLastPoints = new List<NavMethods.UxVector3>(points);
|
||
runtime.AutoDriveSpeedKmh = speedMs * 3.6f;
|
||
}
|
||
|
||
await session.NotifyAsync(MethodId.SyncAssignVehicleAITask, packet, CancellationToken.None);
|
||
|
||
// ⛔ 这里曾经有一段 `SyncAetherAIVehicleForceGo(forceGo=true)`（2026-09-30 加，同日删除）：
|
||
// 当时怀疑「车门消失 / 角色缺模」是车被降级成 Aether 简化表示。已证伪 ——
|
||
// 真因是状态包必须在 AI 任务之前下发（见本文件 Start handler 的注释）。
|
||
// ⛑ 别再把 `GameRouter.Traffic.cs` 里车流那条 forceGo=false 也删了，那个是有用的。
|
||
session.Log.Info($"[VEHNAV] cruise assigned entity={vehicleId} token={token} " +
|
||
$"points={points.Count} speed={speedMs * 3.6f:0.#}km/h flags={cfg.CruiseDrivingFlags} " +
|
||
$"bytes={packet.Length}");
|
||
|
||
// ★★ 主动确认一次（2026-09-28 第三轮）。
|
||
// 第二轮的症状：包发出去了、客户端也回了 status=1/2，但车不动 —— 因为
|
||
// AskUpdateVehicleAITaskStatus 的 handler 只认骇入的 HackTaskToken，把自驾 token
|
||
// 当成陌生包 ignore 了。而客户端「keeps its task Pending until SyncChangeVehicleAITaskState
|
||
// acknowledges the SAME vehicle and task token」（见 VehicleHacking.cs 的注释）。
|
||
// ⇒ 任务从没被确认 ⇒ 车不动。这里先主动推一次 Running(1)，响应式确认在
|
||
// AskUpdateVehicleAITaskStatus 里也补上了（双保险，幂等）。
|
||
await AcknowledgeAutoDriveTaskAsync(session, vehicleId, token, 1);
|
||
}
|
||
|
||
/// <summary>
|
||
/// GM 实时调速（2026-09-30，<c>/api/vehicle/autodrive-speed</c>）。
|
||
///
|
||
/// ① 先落盘 <c>config/autonomous-driving.json</c>（<b>一定执行</b>，与有没有在开车无关）；
|
||
/// ② 再对<b>正在</b>自动驾驶的那辆车热应用：撤回旧任务 → 用新速度重建 → 再确认一次。
|
||
///
|
||
/// ⛑ 返回 false 只代表「这次没法热应用」（没在车上 / 没在自驾 / 上次点集没缓存），
|
||
/// <b>速度仍然已经保存</b>，下一次按自动驾驶就会生效 —— 别把它当失败提示给用户。
|
||
///
|
||
/// ⛑⛑ 为什么复用 <c>AutoDriveLastPoints</c> 而不是重算路线：<c>SolveRoute</c> 是<b>实例方法</b>
|
||
/// （要 <c>Connection</c> + <c>LogNavOnce</c>），GM 这条路径上根本没有 Connection。
|
||
/// 缓存还有个好处：换速度时路线<b>逐点不变</b>，不会因为重算而突然改道。
|
||
/// </summary>
|
||
internal static async Task<bool> ApplyAutonomousDriveSpeedAsync(TcpSession session, float kmh)
|
||
{
|
||
if (!AutonomousDriveSpeedSettings.Valid(kmh))
|
||
throw new ArgumentOutOfRangeException(nameof(kmh),
|
||
$"速度必须为 {AutonomousDriveSpeedSettings.MinKmh:0.#}–{AutonomousDriveSpeedSettings.MaxKmh:0.#} km/h。");
|
||
|
||
var saved = AutonomousDriveSpeedSettings.Save(kmh);
|
||
var runtime = GetVehicleRuntimeState(session);
|
||
|
||
ulong vehicleId;
|
||
ulong previousToken;
|
||
float previousKmh;
|
||
List<NavMethods.UxVector3>? points;
|
||
lock (runtime.SyncRoot)
|
||
{
|
||
if (!runtime.AutoDriving)
|
||
return false;
|
||
vehicleId = runtime.AutoDrivingVehicleId;
|
||
points = runtime.AutoDriveLastPoints;
|
||
previousKmh = runtime.AutoDriveSpeedKmh;
|
||
previousToken = runtime.AutoDriveTaskToken;
|
||
}
|
||
|
||
if (vehicleId == 0 || points is null || points.Count == 0)
|
||
return false;
|
||
|
||
// 值没变就不重发：GM 手抖连点会刷出一串同速任务，客户端要一个个 Abort。
|
||
if (MathF.Abs(previousKmh - saved) < 0.05f)
|
||
return true;
|
||
|
||
// 旧任务必须先 Abort(4)。不撤的话客户端会同时挂着两个 Pending 任务，
|
||
// 车会照旧速度跑完再切 —— 观感上就是「拖了半天才变速」。
|
||
if (previousToken != 0)
|
||
{
|
||
await AcknowledgeAutoDriveTaskAsync(session, vehicleId, previousToken, 4);
|
||
session.Log.Info($"[VEHNAV] auto-drive speed abort previous token={previousToken}");
|
||
}
|
||
|
||
await AssignAutonomousCruiseAsync(session, runtime, vehicleId, points);
|
||
session.Log.Info($"[VEHNAV] auto-drive speed hot-applied vehicle={vehicleId} " +
|
||
$"{previousKmh:0.#}→{saved:0.#} km/h points={points.Count}");
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 回推 <c>SyncChangeVehicleAITaskState</c> 设置自驾任务状态。客户端靠这个包把任务从 Pending
|
||
/// 推进到 Running —— **没有这一步车就不会动**。
|
||
/// <c>VehicleAIStatus</c>：0 Pending / 1 Running / 2 Success / 3 Pause / <b>4 Abort</b> / 5 Override
|
||
/// （客户端枚举，<c>ux_rpc/ux_game_all.dump.cs:181155</c>）。
|
||
/// </summary>
|
||
private static async Task AcknowledgeAutoDriveTaskAsync(TcpSession session, ulong vehicleId, ulong token, byte status)
|
||
{
|
||
await session.NotifyAsync(MethodId.SyncChangeVehicleAITaskState, UxSerializer.Serialize(
|
||
new GameMethods.VehicleTaskState4229938
|
||
{
|
||
VehicleEntityId = vehicleId,
|
||
TaskToken = token,
|
||
Status = status,
|
||
}), CancellationToken.None);
|
||
session.Log.Info($"[VEHNAV] AI task ack entity={vehicleId} token={token} status={status}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 自驾任务的状态机：只要求 token 匹配。客户端会连发 Running(1)，
|
||
/// 所以 1→1 必须被接受（骇入那套 IsAllowedHackTaskTransition 会拒掉重复，自驾不能照抄）。
|
||
/// </summary>
|
||
internal static bool TryApplyAutoDriveTaskStatus(VehicleRuntimeState runtime, ulong token, byte next)
|
||
{
|
||
if (token == 0 || token != runtime.AutoDriveTaskToken)
|
||
return false;
|
||
if (next is 0 or > 5)
|
||
return false;
|
||
runtime.AutoDriveTaskStatus = next;
|
||
return true;
|
||
}
|
||
|
||
/// <summary>停止 AI 任务（action 0）。与 <see cref="AssignAutonomousCruiseAsync"/> 成对。</summary>
|
||
private static async Task AssignAutonomousStopAsync(TcpSession session, VehicleRuntimeState runtime, ulong vehicleId)
|
||
{
|
||
var token = unchecked((ulong)IdCounterStore.Next(VehicleTokenCounter, VehicleTokenFloor));
|
||
var packet = UxSerializer.Serialize(
|
||
new GameMethods.AssignVehicleControl4229938<GameMethods.VehicleStopParameters4229938>
|
||
{
|
||
VehicleUId = vehicleId,
|
||
Parameters = new GameMethods.VehicleStopParameters4229938
|
||
{
|
||
Base = new GameMethods.VehicleTaskTail4229938
|
||
{
|
||
Token = token,
|
||
DefaultSpeed = 0f,
|
||
DrivingFlags = 1,
|
||
},
|
||
},
|
||
});
|
||
lock (runtime.SyncRoot)
|
||
{
|
||
// 停止任务同样要能收到确认，否则车停不下来（同一个 Pending 陷阱）。
|
||
runtime.AutoDriveTaskToken = token;
|
||
runtime.AutoDriveTaskStatus = 0;
|
||
}
|
||
await session.NotifyAsync(MethodId.SyncAssignVehicleAITask, packet, CancellationToken.None);
|
||
session.Log.Info($"[VEHNAV] cruise stop entity={vehicleId} token={token} bytes={packet.Length}");
|
||
await AcknowledgeAutoDriveTaskAsync(session, vehicleId, token, 1);
|
||
}
|
||
|
||
private static Task PushVehicleAutonomousStateAsync(Connection conn, ulong vehicleEntityId, bool isStart)
|
||
=> conn.Session.NotifyAsync(MethodId.IGameSceneToClient_SyncVehicleAutonomousDrivingState,
|
||
UxSerializer.Serialize(new NavMethods.SyncVehicleAutonomousDrivingState4229938
|
||
{
|
||
vehicleEntityId = vehicleEntityId,
|
||
isStart = isStart,
|
||
}), CancellationToken.None);
|
||
|
||
private static Task PushPlayerAutonomousStateAsync(Connection conn)
|
||
=> conn.Session.NotifyAsync(MethodId.IGameSceneToClient_SyncPlayerAutonomousDrivingState,
|
||
UxSerializer.Serialize(new NavMethods.SyncPlayerAutonomousDrivingState4229938
|
||
{
|
||
isInOverrideMode = false,
|
||
isAutoDrivingBlocked = false,
|
||
isImmersiveModeBlocked = false,
|
||
}), CancellationToken.None);
|
||
|
||
// -----------------------------------------------------------------------------------------
|
||
// 求解：把两张点吸到车道上，再在车道图上跑一次 Dijkstra（代价 = 米）
|
||
// -----------------------------------------------------------------------------------------
|
||
|
||
/// <summary>
|
||
/// <paramref name="Points"/> = 给<b>导航线</b>的点（可能被 <c>ReversePoints</c> 翻成「终点 → 车」）；
|
||
/// <paramref name="DrivePoints"/> = 给 <b>AI 巡航任务</b>的点，恒为「车 → 终点」正向
|
||
/// （2026-09-28：反转只对画线的箭头有意义，喂给 AI 会让车掉头）。
|
||
/// </summary>
|
||
private readonly record struct NavRoute(
|
||
bool Ok, List<NavMethods.UxVector3> Points, List<NavMethods.UxVector3> Center,
|
||
List<NavMethods.UxVector3> DrivePoints, float Length, int Hops, string Note);
|
||
|
||
private NavRoute? SolveRoute(
|
||
Connection conn, float sx, float sy, float sz, float tx, float ty, float tz, bool needCenter, string tag)
|
||
{
|
||
var cfg = VehicleNavCfg;
|
||
if (!VehicleLaneGraph.Available)
|
||
{
|
||
LogNavOnce(conn, "[VEHNAV] 车道表未载入（vehicle_lanes.csv / vehicle_lane_links.csv 缺失）", 0);
|
||
return null;
|
||
}
|
||
|
||
if (!VehicleLaneGraph.TrySnapLane(sx, sz, cfg.StartSnapMeters, out var start))
|
||
{
|
||
LogNavOnce(conn, $"[VEHNAV] {tag} 起点 ({sx:0.#},{sz:0.#}) 在 {cfg.StartSnapMeters:0.#}m 内找不到车道", 0);
|
||
return null;
|
||
}
|
||
|
||
if (!VehicleLaneGraph.TrySnapLane(tx, tz, cfg.TargetSnapMeters, out var dst))
|
||
{
|
||
LogNavOnce(conn, $"[VEHNAV] {tag} 终点 ({tx:0.#},{tz:0.#}) 在 {cfg.TargetSnapMeters:0.#}m 内找不到车道", 0);
|
||
return null;
|
||
}
|
||
|
||
var hops = Dijkstra(start.Handle, dst.Handle, cfg.MaxExpansions, out var laneList);
|
||
if (hops == 0)
|
||
{
|
||
LogNavOnce(conn, $"[VEHNAV] {tag} 车道图不通 start={start.Handle} target={dst.Handle}", 0);
|
||
return null;
|
||
}
|
||
|
||
// 几何：车的实际位置 →（每条车道的末端）→ 终点在最后一条车道上的投影点。
|
||
// 里程同理：首条只算「剩下的那截」，中间整条计入，末条只算到投影点为止。
|
||
var points = new List<NavMethods.UxVector3>(laneList.Count + 2) { new(sx, sy, sz) };
|
||
var center = needCenter ? new List<NavMethods.UxVector3>(laneList.Count) : [];
|
||
var length = 0f;
|
||
var prevX = sx;
|
||
var prevZ = sz;
|
||
|
||
for (var i = 0; i < laneList.Count; i++)
|
||
{
|
||
if (!VehicleLaneGraph.TryLaneGeometry(laneList[i], out var bx, out var by, out var bz,
|
||
out var ex, out var ey, out var ez, out var laneLength))
|
||
continue;
|
||
|
||
if (needCenter)
|
||
center.Add(new NavMethods.UxVector3((bx + ex) * 0.5f, (by + ey) * 0.5f, (bz + ez) * 0.5f));
|
||
|
||
if (i == laneList.Count - 1)
|
||
{
|
||
var endT = i == 0 ? dst.T - start.T : dst.T;
|
||
length += laneLength * (i == 0 ? endT : dst.T);
|
||
AddPoint(points, ref prevX, ref prevZ, dst.X, ty, dst.Z, cfg.MinPointSpacing);
|
||
break;
|
||
}
|
||
|
||
length += laneLength * (i == 0 ? 1f - start.T : 1f);
|
||
AddPoint(points, ref prevX, ref prevZ, ex, ey, ez, cfg.MinPointSpacing);
|
||
}
|
||
|
||
length = MathF.Max(0f, length);
|
||
|
||
if (points.Count < 2)
|
||
{
|
||
LogNavOnce(conn, $"[VEHNAV] {tag} 生成点数不足 ({points.Count})", 0);
|
||
return null;
|
||
}
|
||
|
||
var forward = Decimate(points, cfg.MaxPathPoints);
|
||
var final = new List<NavMethods.UxVector3>(forward);
|
||
// ★ 捐赠端自己探针的结论：下行顺序是「终点 → 车」，不是「车 → 终点」。
|
||
// 他们用 [VEHNAV] 线顺序自检 断言过（Points[0] 距目标 <1m、末点距车 <1m）。
|
||
// ⛑️ 这一条我们还没实测过，所以做成开关而不是写死。
|
||
if (cfg.ReversePoints)
|
||
final.Reverse();
|
||
|
||
if (cfg.LogEveryRequest)
|
||
conn.Log.Info($"[VEHNAV] {tag} start={start.Handle}({start.Distance:0.#}m) target={dst.Handle}" +
|
||
$"({dst.Distance:0.#}m) hops={hops} length={length:0.#}m points={final.Count} center={center.Count}");
|
||
|
||
return new NavRoute(true, final, center, forward, length, hops, "ok");
|
||
}
|
||
|
||
private Task AnswerNavAsync(Connection conn, UxRpcMessage msg,
|
||
NavMethods.VehicleNavResult4229938 fallback, NavRoute? route)
|
||
{
|
||
if (route is { Ok: true } ok)
|
||
{
|
||
fallback.Points = ok.Points;
|
||
fallback.CenterPoints = ok.Center;
|
||
}
|
||
|
||
return conn.ReturnAsync(msg, fallback);
|
||
}
|
||
|
||
private void LogNavOnce(Connection conn, string message, int bytes)
|
||
{
|
||
// 开车时客户端每秒问一次：抓不到车道的失败不能每秒刷屏，10 秒最多一条。
|
||
const string key = "vehnav.lastWarnTicks";
|
||
var now = Environment.TickCount64;
|
||
if (conn.Session.Items.TryGetValue(key, out var raw) && raw is long last && now - last < 10_000)
|
||
return;
|
||
|
||
conn.Session.Items[key] = now;
|
||
conn.Log.Info(bytes > 0 ? $"{message} bytes={bytes}" : message);
|
||
}
|
||
|
||
/// <summary>服务端最后一次看到的玩家位置，退化时退回世界出生点。</summary>
|
||
private static (float X, float Y, float Z) CurrentVehiclePosition(UxRpcMessage msg)
|
||
{
|
||
var state = GetWorldStateStatic(msg.Context);
|
||
if (state.HasLastReportedPlayerTransform)
|
||
{
|
||
var p = state.LastReportedPlayerPosition;
|
||
return (p.X, p.Y, p.Z);
|
||
}
|
||
|
||
var spawn = Profile.WorldSpawn;
|
||
return (spawn.X, spawn.Y, spawn.Z);
|
||
}
|
||
|
||
private static void AddPoint(List<NavMethods.UxVector3> list, ref float prevX, ref float prevZ,
|
||
float x, float y, float z, float minSpacing)
|
||
{
|
||
if (minSpacing > 0f)
|
||
{
|
||
var dx = x - prevX;
|
||
var dz = z - prevZ;
|
||
if (dx * dx + dz * dz < minSpacing * minSpacing)
|
||
return;
|
||
}
|
||
|
||
list.Add(new NavMethods.UxVector3(x, y, z));
|
||
prevX = x;
|
||
prevZ = z;
|
||
}
|
||
|
||
/// <summary>点数超过上限时按等距抽稀，首位必有。</summary>
|
||
private static List<NavMethods.UxVector3> Decimate(List<NavMethods.UxVector3> source, int limit)
|
||
{
|
||
if (limit <= 0 || source.Count <= limit)
|
||
return source;
|
||
|
||
var result = new List<NavMethods.UxVector3>(limit);
|
||
var last = source.Count - 1;
|
||
for (var i = 0; i < limit - 1; i++)
|
||
result.Add(source[(int)MathF.Floor(i * last / (float)(limit - 1))]);
|
||
result.Add(source[last]);
|
||
return result;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 车道图上的前向 Dijkstra（代价 = 米）。返回跳数 0 表示不通，<paramref name="lanes"/> 是
|
||
/// 起点→终点的车道序列。第一跳的代价是「起点所在车道剩余里程」，这样同车道起步也算得准。
|
||
/// </summary>
|
||
private static int Dijkstra(int startLane, int targetLane, int maxExpansions, out List<int> lanes)
|
||
{
|
||
lanes = [];
|
||
if (startLane == targetLane)
|
||
{
|
||
lanes.Add(startLane);
|
||
return 1;
|
||
}
|
||
|
||
var dist = new Dictionary<int, float>();
|
||
var prev = new Dictionary<int, int>();
|
||
var queue = new PriorityQueue<int, float>();
|
||
var visited = new HashSet<int>();
|
||
|
||
VehicleLaneGraph.TryLaneGeometry(startLane, out _, out _, out _, out _, out _, out _, out var firstLength);
|
||
dist[startLane] = firstLength;
|
||
queue.Enqueue(startLane, firstLength);
|
||
|
||
var expansions = 0;
|
||
while (queue.Count > 0 && expansions < maxExpansions)
|
||
{
|
||
queue.TryDequeue(out var lane, out var cost);
|
||
if (cost > 0f && dist.TryGetValue(lane, out var best) && cost > best + 0.01f)
|
||
continue;
|
||
if (!visited.Add(lane))
|
||
continue;
|
||
expansions++;
|
||
|
||
if (lane == targetLane)
|
||
break;
|
||
|
||
foreach (var next in VehicleLaneGraph.NextOf(lane))
|
||
{
|
||
if (!VehicleLaneGraph.TryLaneGeometry(next, out _, out _, out _, out _, out _, out _, out var len))
|
||
continue;
|
||
var candidate = cost + MathF.Max(0.5f, len);
|
||
if (dist.TryGetValue(next, out var known) && known <= candidate)
|
||
continue;
|
||
|
||
dist[next] = candidate;
|
||
prev[next] = lane;
|
||
queue.Enqueue(next, candidate);
|
||
}
|
||
}
|
||
|
||
if (!visited.Contains(targetLane))
|
||
return 0;
|
||
|
||
var chain = new List<int> { targetLane };
|
||
var cursor = targetLane;
|
||
while (cursor != startLane)
|
||
{
|
||
if (!prev.TryGetValue(cursor, out var parent))
|
||
return 0;
|
||
chain.Add(parent);
|
||
cursor = parent;
|
||
if (chain.Count > 4096)
|
||
return 0;
|
||
}
|
||
|
||
chain.Reverse();
|
||
lanes = chain;
|
||
return lanes.Count;
|
||
}
|
||
}
|