984 lines
44 KiB
C#
984 lines
44 KiB
C#
using System.Globalization;
|
||
using Ananta.Server.Configuration;
|
||
|
||
namespace Ananta.Server.Handlers.Game;
|
||
|
||
/// <summary>
|
||
/// 行人路网（服务端侧 ZoneGraph 简化版）—— 让氛围行人**沿着真实人行道走**，而不是原地站着。
|
||
///
|
||
/// 数据来自世界车道导出（`All lanes.rar`）：
|
||
/// `_ref/lanes/lanes/pedestrian-lane-polylines.csv` (Handle;Seq;X;Y;Z，244398 点)
|
||
/// `_ref/lanes/lanes/pedestrian-lane-links.csv` (FromHandle;ToHandle，263448 条)
|
||
/// 由 `_ref/_gen_ped_lanes.py` 压成两份运行时表：
|
||
/// `Ananta.Server/Ananta.App/data/ped_lanes.csv` = 每条车道一行：handle;x0;y0;z0;x1;y1;z1;...
|
||
/// `Ananta.Server/Ananta.App/data/ped_lane_links.csv` = 每条车道一行：handle;to1;to2;...
|
||
///
|
||
/// ★ 2026-09-22 新增第三份表（本轮 T2-37 落点）：
|
||
/// `Ananta.Server/Ananta.App/data/ped_lane_meta.csv` = handle;width;tags
|
||
/// 由 `_gen_ped_lane_meta.py` 从 `zonegraph_lanes_ped.csv`（65,547 段）按 1 米网格反查上来，
|
||
/// 命中率 100%。它带两个自产路网没有的属性：
|
||
/// width = 人行道真实宽度（中位 2.0 m）⇒ 横向 clamp，别让所有人走在同一条几何中线上
|
||
/// tags = 位标记，131072 = PedestrianBanned（禁行）⇒ 投放 / 换道时跳过
|
||
///
|
||
/// 用法：给一个行人分配一条车道 + 起点进度，然后每 tick 调 <see cref="Advance"/> 推进固定距离，
|
||
/// 把结果塞进 `SyncLogicAgentCommand`（TaskWayPointMove）发给客户端。走到车道尽头就顺着 link 换下一条，
|
||
/// 没有合格出边就原地掉头（Dir 取反），所以行人永远不会"走出路网消失"。
|
||
/// </summary>
|
||
internal static class PedLaneGraph
|
||
{
|
||
/// <summary>PedestrianBanned：这类车道官方禁止行人通行（实测 474 条）。</summary>
|
||
private const int TagPedestrianBanned = 131072;
|
||
|
||
/// <summary>换道允许的最大间隙（米）。超过就是"抄近路穿广场/绿化带"——宁可掉头。
|
||
/// 依据：99.9% 的 lane link 间隙 ≤ 16 m。</summary>
|
||
private const float MaxHopGapMetres = 16f;
|
||
|
||
/// <summary>规划路径的搜索半径（米）。Dijkstra 按距离出队，一旦出队距离超过它就放弃 ——
|
||
/// 一是不给行人规划出几百米的超长行程，二是起终点不在同一连通块时不用遍历整个路网
|
||
/// （88k 车道 / 263k 边）才肯返回 null。</summary>
|
||
private const float MaxRouteSearchMetres = 400f;
|
||
|
||
/// <summary>
|
||
/// 横向偏移的绝对上限（米），来自 `ambientPedsLateralMaxMetres`（配置，默认 0.4）。
|
||
/// 即使车道被反查到很大的宽度也不超过它 —— `ped_lane_meta.csv` 的 width 是 1 m 网格空间哈希
|
||
/// 反查出来的，窄路有可能被安上宽路的宽度，这是第二道保险。
|
||
/// ⛑️ **设成 0 就完全不偏移（全员走车道中线）** —— 排查"行人卡墙"时先用它排除嫌疑。
|
||
/// </summary>
|
||
private static float MaxLateralMetres
|
||
=> MathF.Max(0f, PrivateServerConfigStore.Current.Gameplay.Aether.AmbientPedsLateralMaxMetres);
|
||
|
||
/// <summary>
|
||
/// 横向可走半径的**安全余量**（米），来自 `ambientPedsLateralMarginMetres`（配置，默认 0.9）。
|
||
/// <c>HalfWalk = max(0, width/2 - margin)</c> —— margin 越大，行人越往车道中线收。
|
||
///
|
||
/// ⛑️ 2026-09-26：原值是硬编码 0.6，而车道宽度**中位只有 2.0 m**（p75=3.0 / p90=4.0）：
|
||
/// 2 m 路上算出 HalfWalk=0.4 ⇒ 实测偏移最大 0.36 m，距路缘只剩 0.64 m ⇒ 行人胶囊 + 护栏正好卡在这里。
|
||
/// 改成 0.9 后：≤1.8 m 的窄路 HalfWalk 直接归 0（走中线），3~4 m 的宽路仍留 ±0.22 m 差异（不会像排队）。
|
||
/// 判据：先设 `ambientPedsLateralMaxMetres=0`（全员中线）确认卡护栏确实是偏移造成，再回到本项微调。
|
||
/// </summary>
|
||
private static float LateralMarginMetres
|
||
=> MathF.Max(0f, PrivateServerConfigStore.Current.Gameplay.Aether.AmbientPedsLateralMarginMetres);
|
||
|
||
/// <summary>
|
||
/// 出生点的**最小车道宽度**（米），来自 `ambientPedsSpawnMinWidthMetres`（配置，默认 1.8）。
|
||
/// ⛑️ 2026-09-26：候选池原来只看 `Banned`，完全不看宽度 ⇒ 随机抽到 0.5~1.8 m 的窄道
|
||
/// （实测宽度中位 2.0 / p10 1.5 / min 0.5，约 17% ≤1.8 m）就生在树池边、栏杆边的小径上。
|
||
/// 设 0 = 不过滤（回到旧行为）。
|
||
/// </summary>
|
||
private static float MinSpawnWidthMetres
|
||
=> MathF.Max(0f, PrivateServerConfigStore.Current.Gameplay.Aether.AmbientPedsSpawnMinWidthMetres);
|
||
|
||
internal struct WalkState
|
||
{
|
||
internal int Lane;
|
||
internal float Pos; // 段内进度：整数部分 = 段下标，小数部分 = 段内比例
|
||
internal sbyte Dir; // +1 顺行 / -1 掉头
|
||
|
||
/// <summary>横向站位系数（-1..1），一次分配终身不变。
|
||
/// 同一个 walker 锁一侧，否则会看到行人在人行道左右横跳（donor round 64 同款结论）。</summary>
|
||
internal float Offset;
|
||
|
||
/// <summary>上一条走过的车道 handle（-1 = 没有）。换道时禁止立刻退回去 —— 见 <see cref="TryHop"/>。</summary>
|
||
internal int Prev;
|
||
|
||
/// <summary>
|
||
/// 最近走过的车道环形缓冲（元素 -1 = 空）。长度 = `AmbientPedsRouteMemory`；
|
||
/// 换道时优先躲开这里面的车道，行人就不会在几条短车道之间绕圈。
|
||
/// </summary>
|
||
internal int[]? Recent;
|
||
}
|
||
|
||
private sealed class Lane
|
||
{
|
||
internal float[] Xs = [];
|
||
internal float[] Ys = [];
|
||
internal float[] Zs = [];
|
||
internal float[] SegLen = []; // 每段长度，长度 = 点数-1
|
||
internal int[] Next = [];
|
||
|
||
/// <summary>车道总长（各段之和，米）。Dijkstra 的边权要用，加载时算一次。</summary>
|
||
internal float Length;
|
||
|
||
/// <summary>横向可走半径 = max(0, width/2 - 安全余量)。width 未知时为 0（走中线）。</summary>
|
||
internal float HalfWalk;
|
||
|
||
/// <summary>人行道真实宽度（米，`ped_lane_meta.csv`）。0 = 未知。出生点过滤要用。</summary>
|
||
internal float Width;
|
||
internal bool Banned;
|
||
|
||
internal int Count => Xs.Length;
|
||
}
|
||
|
||
private const float CellSize = 50f;
|
||
|
||
private sealed class Table
|
||
{
|
||
internal Dictionary<int, Lane> Lanes = [];
|
||
internal Dictionary<(int, int), List<int>> Cells = [];
|
||
internal bool Ok;
|
||
}
|
||
|
||
private static readonly Lazy<Table> Cache = new(Load);
|
||
|
||
internal static bool Available => Cache.Value.Ok;
|
||
internal static int LaneCount => Cache.Value.Lanes.Count;
|
||
|
||
private static Table Load()
|
||
{
|
||
var table = new Table();
|
||
var cellsSeen = new HashSet<(int, int)>(); // 每条车道去重：同一格只登记一次
|
||
var polyPath = PrivateServerConfigStore.ResolveProjectPath(
|
||
"Ananta.Server/Ananta.App/data/ped_lanes.csv");
|
||
var linkPath = PrivateServerConfigStore.ResolveProjectPath(
|
||
"Ananta.Server/Ananta.App/data/ped_lane_links.csv");
|
||
var metaPath = PrivateServerConfigStore.ResolveProjectPath(
|
||
"Ananta.Server/Ananta.App/data/ped_lane_meta.csv");
|
||
if (!File.Exists(polyPath) || !File.Exists(linkPath))
|
||
return table;
|
||
|
||
foreach (var line in File.ReadLines(polyPath))
|
||
{
|
||
if (line.Length == 0)
|
||
continue;
|
||
var parts = line.Split(';');
|
||
if (parts.Length < 5)
|
||
continue;
|
||
if (!int.TryParse(parts[0], out var handle))
|
||
continue;
|
||
|
||
var n = (parts.Length - 1) / 3;
|
||
var lane = new Lane
|
||
{
|
||
Xs = new float[n],
|
||
Ys = new float[n],
|
||
Zs = new float[n],
|
||
SegLen = new float[Math.Max(0, n - 1)],
|
||
};
|
||
for (var i = 0; i < n; i++)
|
||
{
|
||
lane.Xs[i] = Num(parts[1 + i * 3]);
|
||
lane.Ys[i] = Num(parts[2 + i * 3]);
|
||
lane.Zs[i] = Num(parts[3 + i * 3]);
|
||
}
|
||
for (var i = 0; i < lane.SegLen.Length; i++)
|
||
{
|
||
var dx = lane.Xs[i + 1] - lane.Xs[i];
|
||
var dy = lane.Ys[i + 1] - lane.Ys[i];
|
||
var dz = lane.Zs[i + 1] - lane.Zs[i];
|
||
lane.SegLen[i] = MathF.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
}
|
||
for (var i = 0; i < lane.SegLen.Length; i++)
|
||
lane.Length += lane.SegLen[i];
|
||
table.Lanes[handle] = lane;
|
||
|
||
// ⛑️ 2026-09-23：登记**每个采样点**所在的格（原来只登记首点）。
|
||
// 只登记首点时，长车道（最长 1001 m）的中段在索引里根本不存在，
|
||
// 行人站在它上面也会被吸附到几十米外的别的车道。实测 60 个真实起点：
|
||
// 只登记首点：吸附距离 中位 7.61 m / p90 34.57 m / max 65 m，**88% 选错车道**
|
||
// 全点登记后：吸附距离 中位 1.40 m / p90 9.27 m
|
||
// 车道中位长度才 3.58 m，7.6 m 的误差等于把行人钉在隔壁人行道上。
|
||
cellsSeen.Clear();
|
||
for (var i = 0; i < n; i++)
|
||
{
|
||
var cell = CellOf(lane.Xs[i], lane.Zs[i]);
|
||
if (!cellsSeen.Add(cell))
|
||
continue;
|
||
if (!table.Cells.TryGetValue(cell, out var bucket))
|
||
table.Cells[cell] = bucket = [];
|
||
bucket.Add(handle);
|
||
}
|
||
}
|
||
|
||
foreach (var line in File.ReadLines(linkPath))
|
||
{
|
||
if (line.Length == 0)
|
||
continue;
|
||
var parts = line.Split(';');
|
||
if (parts.Length < 2 || !int.TryParse(parts[0], out var handle))
|
||
continue;
|
||
if (!table.Lanes.TryGetValue(handle, out var lane))
|
||
continue;
|
||
var outs = new List<int>(parts.Length - 1);
|
||
for (var i = 1; i < parts.Length; i++)
|
||
if (int.TryParse(parts[i], out var to) && table.Lanes.ContainsKey(to) && to != handle)
|
||
outs.Add(to);
|
||
lane.Next = outs.ToArray();
|
||
}
|
||
|
||
// width / tags：没有这份表也能跑（HalfWalk=0 ⇒ 走中线，Banned=false ⇒ 不过滤）。
|
||
if (File.Exists(metaPath))
|
||
{
|
||
foreach (var line in File.ReadLines(metaPath))
|
||
{
|
||
if (line.Length == 0)
|
||
continue;
|
||
var parts = line.Split(';');
|
||
if (parts.Length < 3 || !int.TryParse(parts[0], out var handle))
|
||
continue;
|
||
if (!table.Lanes.TryGetValue(handle, out var lane))
|
||
continue;
|
||
var width = Num(parts[1]);
|
||
var tags = 0;
|
||
int.TryParse(parts[2], out tags);
|
||
// margin = 角色碰撞半径 + 安全余量。原来硬编码 0.6，窄路上算出来的可偏移空间偏大
|
||
// ⇒ 行人被推到街边卡进墙里 / 卡进护栏（2026-09-23 卡墙、2026-09-26 卡护栏，两次实测）。
|
||
lane.Width = width;
|
||
lane.HalfWalk = MathF.Max(0f, width / 2f - LateralMarginMetres);
|
||
lane.Banned = (tags & TagPedestrianBanned) != 0;
|
||
}
|
||
}
|
||
|
||
table.Ok = table.Lanes.Count > 0;
|
||
return table;
|
||
}
|
||
|
||
private static float Num(string s) => float.TryParse(s, NumberStyles.Float, CultureInfo.InvariantCulture, out var v) ? v : 0f;
|
||
|
||
private static (int, int) CellOf(float x, float z)
|
||
=> ((int)MathF.Floor(x / CellSize), (int)MathF.Floor(z / CellSize));
|
||
|
||
/// <summary>在玩家附近随机挑一条车道的起点，用来当行人的出生点。禁行车道不参与。</summary>
|
||
internal static bool TryPickSpawn(float px, float pz, float radius, Random rng,
|
||
out float x, out float y, out float z, out WalkState state)
|
||
{
|
||
x = y = z = 0f;
|
||
state = default;
|
||
var table = Cache.Value;
|
||
if (!table.Ok)
|
||
return false;
|
||
|
||
var reach = (int)MathF.Ceiling(radius / CellSize);
|
||
var baseCell = CellOf(px, pz);
|
||
List<int> pool = [];
|
||
for (var dx = -reach; dx <= reach; dx++)
|
||
for (var dz = -reach; dz <= reach; dz++)
|
||
{
|
||
if (!table.Cells.TryGetValue((baseCell.Item1 + dx, baseCell.Item2 + dz), out var bucket))
|
||
continue;
|
||
foreach (var h in bucket)
|
||
{
|
||
if (!table.Lanes.TryGetValue(h, out var lane))
|
||
continue;
|
||
if (lane.Banned)
|
||
continue;
|
||
// 窄道（树池边 / 栏杆边的小径）不生出人 —— 宽度未知（0）的按不动处理（meta 表已全覆盖）。
|
||
if (MinSpawnWidthMetres > 0f && lane.Width > 0f && lane.Width < MinSpawnWidthMetres)
|
||
continue;
|
||
var ddx = lane.Xs[0] - px;
|
||
var ddz = lane.Zs[0] - pz;
|
||
if (ddx * ddx + ddz * ddz <= radius * radius)
|
||
pool.Add(h);
|
||
}
|
||
}
|
||
if (pool.Count == 0)
|
||
return false;
|
||
|
||
var picked = pool[rng.Next(pool.Count)];
|
||
var start = table.Lanes[picked];
|
||
var st = new WalkState
|
||
{
|
||
Lane = picked,
|
||
// ⛑️ 出生位置/方向必须随机。2026-09-23 用户实测："同一条路上两个行人会停在一样的
|
||
// 位置、一起起步、一起掉头" —— 因为原来恒为 Pos=0 / Dir=+1，同一批生成的人
|
||
// 起点重合、方向一致、步长一致、TryHop 又是确定性的（就近优先）⇒ 动作完全同步。
|
||
// ⛑️ 相位只落在**段中间**：车道端点常常就是路口 / 路缘 / 护栏端头，生在那儿一出生就卡住。
|
||
Pos = start.Count > 1 ? rng.Next(start.Count - 1) + 0.25f + rng.NextSingle() * 0.5f : 0f,
|
||
Dir = (sbyte)(rng.Next(2) == 0 ? 1 : -1),
|
||
// 一侧站位：-1..1，避开正好 0（不然还是全走中线）。
|
||
Offset = start.HalfWalk > 0f ? (rng.Next(2) == 0 ? -1f : 1f) * (0.35f + rng.NextSingle() * 0.55f) : 0f,
|
||
Prev = -1,
|
||
};
|
||
Sample(start, st, out x, out y, out z, out _);
|
||
state = st;
|
||
return true;
|
||
}
|
||
|
||
/// <summary>沿车道推进指定距离（米）。返回当前世界坐标与朝向（度，0 = +Z）。</summary>
|
||
internal static bool Advance(ref WalkState s, float distance, Random rng,
|
||
out float x, out float y, out float z, out float facing)
|
||
{
|
||
var table = Cache.Value;
|
||
x = y = z = 0f;
|
||
facing = 0f;
|
||
if (!table.Ok || !table.Lanes.TryGetValue(s.Lane, out var lane) || lane.Count < 2)
|
||
return false;
|
||
|
||
var guard = 0;
|
||
while (distance > 0f && guard++ < 64)
|
||
{
|
||
var segCount = lane.Count - 1;
|
||
var seg = Math.Clamp((int)MathF.Floor(s.Pos), 0, segCount - 1);
|
||
var len = lane.SegLen[seg];
|
||
if (len <= 0f)
|
||
{
|
||
// 退化段（重复点）：直接跨过去
|
||
s.Pos += s.Dir > 0 ? 1f : -1f;
|
||
continue;
|
||
}
|
||
|
||
var frac = s.Pos - seg;
|
||
var remain = (s.Dir > 0 ? 1f - frac : frac) * len;
|
||
if (distance < remain)
|
||
{
|
||
s.Pos += s.Dir * (distance / len);
|
||
distance = 0f;
|
||
break;
|
||
}
|
||
|
||
distance -= remain;
|
||
s.Pos = s.Dir > 0 ? seg + 1f : seg;
|
||
if (s.Dir > 0 ? s.Pos >= segCount : s.Pos <= 0f)
|
||
{
|
||
if (!TryHop(ref s, lane, rng, table, out lane))
|
||
break;
|
||
}
|
||
}
|
||
|
||
Sample(lane, s, out x, out y, out z, out facing);
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 到尽头换下一条车道。
|
||
///
|
||
/// ⛑️ 2026-09-22 两处修正（原来是"随便挑一条出边 + 永远从起点进"）：
|
||
/// ① **就近入口**：比较当前出口到下一条车道**两端**的距离，从近的一端进（可能要反向走），
|
||
/// 否则行人会"瞬移到另一条车道起点"，看起来就是突然横穿 / 回头。
|
||
/// ② **间隙闸门**：入口距离 > 16 m 的出边视为断开（广场 / 绿化带），不选；
|
||
/// 全部超限就在本车道原地掉头。
|
||
/// ③ **禁止回退**（2026-09-22 实测后加）：不许立刻走回刚过来的那条车道。
|
||
///
|
||
/// ⛑️ ③ 是实测逼出来的，不是拍脑袋：我们的路网**双向边成对存在**（A→B 和 B→A 都在表里），
|
||
/// 而"就近入口"又总是挑离出口最近的候选 —— 于是 A 出去必选 B，B 出去必选 A，行人在两条
|
||
/// 3~4 m 的短车道之间无限来回。离线复刻 2000 段的实测：
|
||
/// 乒乓率 95.2% / 每步 30 m 任务的净位移中位数只有 1.2 m / 57.9% 的步在原地打转。
|
||
/// 加上禁止回退后：乒乓 0% / 净位移中位 10.4 m（4.5×）/ 打转降到 41.5%。
|
||
/// ⛑️ 掉头时要**清掉 Prev**：否则死胡同里（唯一出边就是刚来的那条）会永远掉头来回；
|
||
/// 清掉之后掉头率从 22.0% 降到 4.8%，行人能走出死胡同。
|
||
/// </summary>
|
||
private static bool TryHop(ref WalkState s, Lane lane, Random rng, Table table, out Lane next)
|
||
{
|
||
var exitX = s.Dir > 0 ? lane.Xs[lane.Count - 1] : lane.Xs[0];
|
||
var exitZ = s.Dir > 0 ? lane.Zs[lane.Count - 1] : lane.Zs[0];
|
||
|
||
var memory = Math.Clamp(PrivateServerConfigStore.Current.Gameplay.Aether.AmbientPedsRouteMemory, 0, 16);
|
||
|
||
// 两级降级：① 躲开最近 N 条 → ② 只挡紧邻的上一条 → ③ 掉头。
|
||
// 不做②的话，记忆越长越容易把路口堵死（实测记忆 8 条不降级的话掉头率会很难看）。
|
||
var bestTo = ScanHop(s, lane, table, exitX, exitZ, useMemory: memory > 1, out var bestDir);
|
||
if (bestTo < 0 && memory > 1)
|
||
bestTo = ScanHop(s, lane, table, exitX, exitZ, useMemory: false, out bestDir);
|
||
|
||
if (bestTo >= 0 && table.Lanes.TryGetValue(bestTo, out var picked))
|
||
{
|
||
Remember(ref s, s.Lane, memory);
|
||
s.Prev = s.Lane;
|
||
s.Lane = bestTo;
|
||
s.Dir = bestDir;
|
||
s.Pos = bestDir > 0 ? 0f : picked.Count - 1f;
|
||
next = picked;
|
||
return true;
|
||
}
|
||
|
||
// 没有合格出边：原地掉头。⛑️ Prev 和记忆都要清 —— 死胡同里唯一出边往往就是刚来的那条，
|
||
// 不清的话掉头之后还是选不到，行人会永远在这一条车道上折返。
|
||
// 实测：掉头时保留记忆 ⇒ 掉头率从 9% 飙到 42%（因为禁令把出口全挡住了）。
|
||
s.Dir = (sbyte)-s.Dir;
|
||
s.Pos = Math.Clamp(s.Pos, 0f, lane.Count - 1f);
|
||
s.Prev = -1;
|
||
s.Recent = null;
|
||
next = lane;
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在出边里挑"离当前出口最近"的一条，返回 handle（-1 = 没有合格候选）。
|
||
/// </summary>
|
||
/// <param name="useMemory">true = 连最近走过的几条一起躲开（第一级），false = 只挡紧邻上一条（第二级）。</param>
|
||
private static int ScanHop(WalkState s, Lane lane, Table table, float exitX, float exitZ,
|
||
bool useMemory, out sbyte dir)
|
||
{
|
||
dir = 1;
|
||
var bestTo = -1;
|
||
var bestDist = float.MaxValue;
|
||
|
||
foreach (var to in lane.Next)
|
||
{
|
||
if (!table.Lanes.TryGetValue(to, out var cand) || cand.Count < 2 || cand.Banned)
|
||
continue;
|
||
if (to == s.Prev)
|
||
continue; // ③ 不许立刻退回去（会把行人锁在两条短车道之间来回）
|
||
if (useMemory && s.Recent is not null)
|
||
{
|
||
var seen = false;
|
||
for (var i = 0; i < s.Recent.Length; i++)
|
||
{
|
||
if (s.Recent[i] == to)
|
||
{
|
||
seen = true;
|
||
break;
|
||
}
|
||
}
|
||
if (seen)
|
||
continue;
|
||
}
|
||
|
||
var sdx = cand.Xs[0] - exitX;
|
||
var sdz = cand.Zs[0] - exitZ;
|
||
var dStart = MathF.Sqrt(sdx * sdx + sdz * sdz);
|
||
|
||
var edx = cand.Xs[cand.Count - 1] - exitX;
|
||
var edz = cand.Zs[cand.Count - 1] - exitZ;
|
||
var dEnd = MathF.Sqrt(edx * edx + edz * edz);
|
||
|
||
var forward = dStart <= dEnd;
|
||
var d = forward ? dStart : dEnd;
|
||
if (d > MaxHopGapMetres || d >= bestDist)
|
||
continue;
|
||
|
||
bestDist = d;
|
||
bestTo = to;
|
||
dir = forward ? (sbyte)1 : (sbyte)-1;
|
||
}
|
||
return bestTo;
|
||
}
|
||
|
||
/// <summary>把刚走过的车道记进环形缓冲（新的在头部）。memory ≤ 1 时不建数组。</summary>
|
||
private static void Remember(ref WalkState s, int lane, int memory)
|
||
{
|
||
if (memory <= 1)
|
||
return;
|
||
|
||
var buf = s.Recent;
|
||
if (buf is null || buf.Length != memory)
|
||
{
|
||
buf = new int[memory];
|
||
Array.Fill(buf, -1);
|
||
s.Recent = buf;
|
||
}
|
||
for (var i = buf.Length - 1; i > 0; i--)
|
||
buf[i] = buf[i - 1];
|
||
buf[0] = lane;
|
||
}
|
||
|
||
private static void Sample(Lane lane, WalkState s, out float x, out float y, out float z, out float facing)
|
||
{
|
||
var segCount = lane.Count - 1;
|
||
var seg = Math.Clamp((int)MathF.Floor(s.Pos), 0, segCount - 1);
|
||
var t = Math.Clamp(s.Pos - seg, 0f, 1f);
|
||
var a = seg;
|
||
var b = seg + 1;
|
||
|
||
var ax = lane.Xs[a];
|
||
var az = lane.Zs[a];
|
||
var bx = lane.Xs[b];
|
||
var bz = lane.Zs[b];
|
||
|
||
// 横向 clamp：把整段沿法向推开，行人不挤在车道几何中线上。
|
||
if (lane.HalfWalk > 0f && s.Offset != 0f)
|
||
{
|
||
var dx = bx - ax;
|
||
var dz = bz - az;
|
||
var len = MathF.Sqrt(dx * dx + dz * dz);
|
||
if (len > 1e-4f)
|
||
{
|
||
// 与 TryPlanJourney 同一套夹紧规则：min(HalfWalk, 配置上限) × 比例。
|
||
var off = MathF.Min(lane.HalfWalk, MaxLateralMetres) * s.Offset;
|
||
var nx = -dz / len * off;
|
||
var nz = dx / len * off;
|
||
ax += nx;
|
||
az += nz;
|
||
bx += nx;
|
||
bz += nz;
|
||
}
|
||
}
|
||
|
||
x = ax + (bx - ax) * t;
|
||
y = lane.Ys[a] + (lane.Ys[b] - lane.Ys[a]) * t;
|
||
z = az + (bz - az) * t;
|
||
|
||
var fdx = (bx - ax) * s.Dir;
|
||
var fdz = (bz - az) * s.Dir;
|
||
facing = fdx * fdx + fdz * fdz > 1e-6f
|
||
? MathF.Atan2(fdx, fdz) * 180f / MathF.PI
|
||
: 0f;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 沿路网规划一条 from → to 的折线 —— `AskClientNpcFindPath`(67974714) 的真路径应答。
|
||
///
|
||
/// 客户端只在它自己要走路时才来问（我们的人是被服务端 TaskWayPointMove 驱动着走的，
|
||
/// 但 NPC / 某些单位会走客户端寻路）。现在默认回的是**空路径**（`0xFF01FF01`），
|
||
/// 等于告诉客户端"没有路" ⇒ 那条单位就站着。
|
||
///
|
||
/// ⛑️ 规划不出来（路网没覆盖 / 不在同一连通块 / 距离太远）一律返回 null，
|
||
/// 调用方照旧回空路径 —— 宁可不动，也不要给一条穿墙的直线。
|
||
/// </summary>
|
||
/// <param name="lateralCap">
|
||
/// 可选输出：每个路径点所属车道的 <see cref="Lane.HalfWalk"/>（可横向偏移的上限，米）。
|
||
/// 调用方要按它夹紧横向偏移 —— 窄路上限接近 0，就不该把行人往路边推（会推到墙里）。
|
||
/// </param>
|
||
internal static List<(float X, float Y, float Z)>? PlanRoute(
|
||
float fromX, float fromZ, float toX, float toZ,
|
||
int maxPoints = 64, float minSpacing = 1.5f, List<float>? lateralCap = null)
|
||
{
|
||
var table = Cache.Value;
|
||
if (!table.Ok)
|
||
return null;
|
||
|
||
var start = NearestLane(table, fromX, fromZ, out var startIdx);
|
||
var goal = NearestLane(table, toX, toZ, out var goalIdx);
|
||
if (start < 0 || goal < 0)
|
||
return null;
|
||
|
||
var chain = ShortestChain(table, start, goal);
|
||
if (chain is null)
|
||
return null;
|
||
|
||
var raw = new List<(float X, float Y, float Z)>(256);
|
||
var rawCap = lateralCap is null ? null : new List<float>(256);
|
||
var single = chain.Count == 1;
|
||
for (var i = 0; i < chain.Count; i++)
|
||
{
|
||
if (!table.Lanes.TryGetValue(chain[i], out var lane) || lane.Count == 0)
|
||
continue;
|
||
|
||
var from = i == 0 ? startIdx : 0;
|
||
var to = i == chain.Count - 1 ? goalIdx : lane.Count - 1;
|
||
|
||
if (single)
|
||
{
|
||
// ⛑ 同一条车道：必须按 起点→终点 的**真实方向**遍历。
|
||
// 老实现在这里 `if (from > to) swap` 会把整条路径掉个头 —— 行人先倒着走到
|
||
// 起点再掉回来；而 startIdx == goalIdx（≤6 m 的短距离必然命中）时只产出 1 个点，
|
||
// ⇒ raw.Count < 2 ⇒ 整条路作废回 null。
|
||
// 实测 2026-09-22：90 s 内 191 次问路、真实移动请求 115 次，回空 73 次（63.5%），
|
||
// 失败距离**全部在 1.8–5.9 m** —— 就是这一条，不是路网没覆盖。
|
||
if (to == from)
|
||
{
|
||
// 两端吸附到同一个采样点：向两侧各扩一个点，保底 ≥3 点。
|
||
from = Math.Max(0, from - 1);
|
||
to = Math.Min(lane.Count - 1, to + 1);
|
||
}
|
||
|
||
var step = to >= from ? 1 : -1;
|
||
var guard = lane.Count + 1; // 防御：车道只有 1 个点时不许死循环
|
||
for (var k = from; guard-- > 0; k += step)
|
||
{
|
||
raw.Add((lane.Xs[k], lane.Ys[k], lane.Zs[k]));
|
||
rawCap?.Add(lane.HalfWalk);
|
||
if (k == to)
|
||
break;
|
||
}
|
||
continue;
|
||
}
|
||
|
||
// 跨车道：链路本身有向（本车道末端 → 下一条首端），中间段整条顺行，两处裁剪都成立。
|
||
if (from > to)
|
||
(from, to) = (to, from);
|
||
for (var k = from; k <= to; k++)
|
||
{
|
||
raw.Add((lane.Xs[k], lane.Ys[k], lane.Zs[k]));
|
||
rawCap?.Add(lane.HalfWalk);
|
||
}
|
||
}
|
||
|
||
// 端点锚定到请求的真实坐标 —— 车道采样点有间距，不锚定会让路径两端"偏移到采样点上"。
|
||
if (raw.Count >= 2)
|
||
{
|
||
raw[0] = (fromX, raw[0].Y, fromZ);
|
||
raw[^1] = (toX, raw[^1].Y, toZ);
|
||
}
|
||
else
|
||
{
|
||
// 极端情形（车道只有 1 个采样点）：给同车道内的两点直线。≤6 m，不会穿墙。
|
||
var y = raw.Count == 1 ? raw[0].Y : 0f;
|
||
var cap = rawCap is { Count: > 0 } ? rawCap[0] : 0f;
|
||
raw.Clear();
|
||
rawCap?.Clear();
|
||
raw.Add((fromX, y, fromZ));
|
||
raw.Add((toX, y, toZ));
|
||
rawCap?.Add(cap);
|
||
rawCap?.Add(cap);
|
||
}
|
||
|
||
// 按最小间距重采样（donor：MaxPoints=64 / MinMetres=1.5 / SnapMetres=3）。
|
||
var points = new List<(float X, float Y, float Z)>(Math.Min(maxPoints, raw.Count));
|
||
var last = raw[0];
|
||
points.Add(last);
|
||
lateralCap?.Add(rawCap?[0] ?? 0f);
|
||
for (var i = 1; i < raw.Count && points.Count < maxPoints; i++)
|
||
{
|
||
var dx = raw[i].X - last.X;
|
||
var dz = raw[i].Z - last.Z;
|
||
if (dx * dx + dz * dz < minSpacing * minSpacing)
|
||
continue;
|
||
points.Add(raw[i]);
|
||
lateralCap?.Add(rawCap?[i] ?? 0f);
|
||
last = raw[i];
|
||
}
|
||
|
||
// 无论 minSpacing 重采样丢掉多少中间点，最后一点必须是真实目标 ——
|
||
// 否则客户端沿路径走到倒数第二个点就"没路了"，又会站住。
|
||
if (points.Count > 0 && points.Count < maxPoints)
|
||
{
|
||
var tailY = raw[^1].Y;
|
||
var tdx = toX - points[^1].X;
|
||
var tdz = toZ - points[^1].Z;
|
||
if (tdx * tdx + tdz * tdz > 0.09f) // > 0.3 m 才补，免得制造重复点
|
||
{
|
||
points.Add((toX, tailY, toZ));
|
||
lateralCap?.Add(rawCap?[^1] ?? 0f);
|
||
}
|
||
}
|
||
|
||
return points.Count >= 2 ? points : null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 目标导向行程：从 (fromX,fromZ) 走向"以 (centerX,centerZ) 为中心的环形区域内随机一点"。
|
||
///
|
||
/// ⛑️ 为什么要有这个：随机游走（<see cref="Advance"/> + <see cref="TryHop"/>）在我们的路网上
|
||
/// 只在**局部邻域**打转 —— 86.6% 的车道只有 2 个采样点、长度中位数 3.58 m，行人走几步就到
|
||
/// 端点、换道、再回来。2026-09-23 用户实测："行人只是从一条路的一头走到另一头，路很短就很奇怪"。
|
||
/// 给它一个几十米外的目标，用已经实机验证 100% 可用的 <see cref="PlanRoute"/> 规划整条路径，
|
||
/// 行人就会穿过多个路口、像在赶路。
|
||
///
|
||
/// 终点取环形区域（而非行人自身附近的随机方向）是为了让行程始终留在玩家活动圈内 ——
|
||
/// 否则行人一路走出 <c>ambientPedsRecycleMeters</c> 被回收，看起来就是"路人凭空消失"。
|
||
/// </summary>
|
||
/// <param name="lateralRatio">
|
||
/// 横向偏移**比例**（-1..1，带符号，终身锁定一侧）。实际偏移量 = 该点车道的
|
||
/// <see cref="Lane.HalfWalk"/> × 这个值，再受 <see cref="MaxLateralMetres"/> 夹住。
|
||
///
|
||
/// ⛑️ 这里必须是**比例而不是米数**：2026-09-23 用户实测"窄路上行人卡在街边的墙壁里" ——
|
||
/// 老代码传的是固定的 0.6 m，跟车道多宽完全无关。窄路的 HalfWalk 常常就是 0（路宽 ≤ 0.8 m），
|
||
/// 该一点都不偏，结果照样把人往外推 0.21~0.54 m ⇒ 推进墙里。
|
||
/// 随机游走的 <see cref="Sample"/> 一直用 `HalfWalk × Offset` 是对的，只有目标导向这条路径抄错了。
|
||
/// </param>
|
||
internal static List<(float X, float Y, float Z)>? TryPlanJourney(
|
||
float fromX, float fromZ, float centerX, float centerZ,
|
||
float minR, float maxR, float lateralRatio, Random rng, int maxPoints = 512)
|
||
{
|
||
// ⛑️⛑️ 终点必须落在**人行道上**，不能用环形区域里的自由随机点。
|
||
// 2026-09-23 用户实测"少数行人撞墙不停走"：那个点跟路网毫无关系，实测 92% 离最近
|
||
// 人行道 ≥ 2 m（中位 11 m、最大 47 m），也就是常常落在建筑/江里；而 `PlanRoute` 末尾
|
||
// 会把最后一个路径点**硬锚定**到 `toX/toZ` ⇒ 行人的最后一段是"扎进墙里"，
|
||
// 客户端到不了终点就一直往里推。
|
||
// 修法：多次取样，取离路网最近的那个候选，并用它的车道点坐标当真正的终点。
|
||
if (!TryPickGoalOnRing(centerX, centerZ, minR, maxR, rng, out var gx, out var gz))
|
||
return null;
|
||
|
||
List<float> caps = [];
|
||
var path = PlanRoute(fromX, fromZ, gx, gz, maxPoints, 1.5f, caps);
|
||
if (path is null || path.Count < 2)
|
||
return null;
|
||
|
||
if (MathF.Abs(lateralRatio) > 1e-3f)
|
||
{
|
||
// 法向用前后点差分求（首尾退化为用单边），与 Sample() 里的横向 clamp 同思路。
|
||
for (var i = 0; i < path.Count; i++)
|
||
{
|
||
var a = path[Math.Max(0, i - 1)];
|
||
var b = path[Math.Min(path.Count - 1, i + 1)];
|
||
var dx = b.X - a.X;
|
||
var dz = b.Z - a.Z;
|
||
var len = MathF.Sqrt(dx * dx + dz * dz);
|
||
if (len < 1e-4f)
|
||
continue;
|
||
|
||
var cap = i < caps.Count ? caps[i] : 0f;
|
||
var off = MathF.Min(cap, MaxLateralMetres) * lateralRatio;
|
||
if (MathF.Abs(off) < 1e-4f)
|
||
continue; // 窄路：一点都不推
|
||
|
||
path[i] = (path[i].X - dz / len * off, path[i].Y, path[i].Z + dx / len * off);
|
||
}
|
||
}
|
||
return path;
|
||
}
|
||
|
||
/// <summary>候选终点偏离最近人行道的容忍值（米）。超过就再摇一次方向。</summary>
|
||
private const float MaxGoalSnapMetres = 8f;
|
||
|
||
/// <summary>一次行程最多摇几个候选终点。实测 6 次里基本都能摇出 ≤ 8 m 的（到处都是人行道）。</summary>
|
||
private const int GoalTries = 6;
|
||
|
||
/// <summary>
|
||
/// 在玩家周围环形区域里挑一个**已经在人行道上**的行程终点。
|
||
///
|
||
/// 直接拿环形随机点当终点是错的 —— 那个点在建筑/江里的概率极高（实测 92% 偏离 ≥ 2 m），
|
||
/// 而 <see cref="PlanRoute"/> 尾部会把路径最后一点硬拉到 <c>toX/toZ</c>，
|
||
/// 于是行人的最后一段任务终点在墙里，客户端到不了就一直往墙里推。
|
||
/// </summary>
|
||
private static bool TryPickGoalOnRing(float centerX, float centerZ, float minR, float maxR,
|
||
Random rng, out float gx, out float gz)
|
||
{
|
||
gx = centerX;
|
||
gz = centerZ;
|
||
|
||
var table = Cache.Value;
|
||
if (!table.Ok)
|
||
return false;
|
||
|
||
var bestScore = float.MaxValue;
|
||
for (var t = 0; t < GoalTries; t++)
|
||
{
|
||
var ang = rng.NextSingle() * MathF.PI * 2f;
|
||
var r = minR + rng.NextSingle() * MathF.Max(0f, maxR - minR);
|
||
var cx = centerX + MathF.Cos(ang) * r;
|
||
var cz = centerZ + MathF.Sin(ang) * r;
|
||
|
||
var h = NearestLane(table, cx, cz, out var idx);
|
||
if (h < 0 || !table.Lanes.TryGetValue(h, out var lane) || lane.Count == 0 || lane.Banned)
|
||
continue;
|
||
|
||
idx = Math.Clamp(idx, 0, lane.Count - 1);
|
||
var px = lane.Xs[idx];
|
||
var pz = lane.Zs[idx];
|
||
var d = MathF.Sqrt((px - cx) * (px - cx) + (pz - cz) * (pz - cz));
|
||
if (d >= bestScore)
|
||
continue;
|
||
|
||
bestScore = d;
|
||
gx = px;
|
||
gz = pz;
|
||
if (d <= MaxGoalSnapMetres)
|
||
return true;
|
||
}
|
||
|
||
// 全都偏远：用最贴近的那个（已写进 gx/gz），总比让终点落在建筑群里好。
|
||
return bestScore < float.MaxValue;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 把路网漫游状态吸附回 (x,z) 所在的车道 —— 从"目标导向"退回随机游走之前必须调一次。
|
||
/// 不吸附的话 <see cref="WalkState.Lane"/> 还停在生成时那条老车道上，下一拍 <see cref="Advance"/>
|
||
/// 会从那里出点 ⇒ 行人瞬移几十米。
|
||
/// </summary>
|
||
/// <summary>
|
||
/// 出生朝向 = 该点在车道上的走向（度，0 = +Z），`Dir=-1` 时自动反向。
|
||
/// ⛑️ 2026-09-26：出生朝向原来是 <c>_random * 360 - 180</c>，跟车道走向毫无关系，
|
||
/// 而位移由 `TaskWayPointMove` 的路径点决定 ⇒ 身体朝 A、位移朝 B = 看起来在倒退走路。
|
||
/// 与 <see cref="Sample"/> 共用同一套 facing 定义，保证朝向和采样点永远一致。
|
||
/// </summary>
|
||
internal static bool TryFacing(WalkState s, out float facing)
|
||
{
|
||
facing = 0f;
|
||
var table = Cache.Value;
|
||
if (!table.Ok || !table.Lanes.TryGetValue(s.Lane, out var lane) || lane.Count < 2)
|
||
return false;
|
||
Sample(lane, s, out _, out _, out _, out facing);
|
||
return float.IsFinite(facing);
|
||
}
|
||
|
||
internal static bool SnapTo(float x, float z, ref WalkState s)
|
||
{
|
||
var table = Cache.Value;
|
||
if (!table.Ok)
|
||
return false;
|
||
|
||
var h = NearestLane(table, x, z, out var idx);
|
||
if (h < 0 || !table.Lanes.TryGetValue(h, out var lane) || lane.Count < 2)
|
||
return false;
|
||
|
||
s.Lane = h;
|
||
s.Pos = Math.Clamp((float)idx, 0f, lane.Count - 1f);
|
||
s.Dir = 1;
|
||
s.Prev = -1;
|
||
s.Recent = null;
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 「脱困 / 起步」点 —— 规划不出路（或起终点重合）时给客户端一个能走的点，而不是回空路径。
|
||
///
|
||
/// ⛑️⛑️ 2026-09-23 实测：单位 spawn 后第一个问路包就是 <c>From == To</c>（packets 日志 DECODED 行证实），
|
||
/// 我们回 4 字节空路径 <c>FF 01 FF 01</c> ⇒ 它拿不到方向，就一直杵到客户端自驱 wander 重新武装（140 s）。
|
||
/// 卡在花坛 / 门框里的单位同样只会反复问 <c>from ≈ to</c>，回空 ⇒ 永远挪不出来。
|
||
/// 这里给的是「最近车道上、朝车道远端走 metres 米」的一个点 —— 只要还落在人行道上就不会穿墙。
|
||
/// </summary>
|
||
internal static bool TryPickEscape(float x, float z, float metres,
|
||
out float ex, out float ey, out float ez)
|
||
{
|
||
ex = ey = ez = 0f;
|
||
var table = Cache.Value;
|
||
if (!table.Ok)
|
||
return false;
|
||
|
||
var h = NearestLane(table, x, z, out var idx);
|
||
if (h < 0 || !table.Lanes.TryGetValue(h, out var lane) || lane.Count < 2)
|
||
return false;
|
||
|
||
metres = MathF.Max(1f, metres);
|
||
var start = Math.Clamp(idx, 0, lane.Count - 1);
|
||
// 贴着车道末端就往回走，否则顺行 —— 免得一步就跨出车道尽头。
|
||
var dir = start >= lane.Count - 2 ? -1 : 1;
|
||
var i = start;
|
||
var acc = 0f;
|
||
while (i + dir >= 0 && i + dir <= lane.Count - 1 && acc < metres)
|
||
{
|
||
var seg = dir > 0 ? i : i - 1;
|
||
if (seg < 0 || seg >= lane.SegLen.Length)
|
||
break;
|
||
acc += lane.SegLen[seg];
|
||
i += dir;
|
||
}
|
||
|
||
// 一步都没挪动（退化车道 / 全零段）⇒ 退到车道另一端，仍然比原地不动强。
|
||
if (i == start)
|
||
i = dir > 0 ? 0 : lane.Count - 1;
|
||
if (i == start)
|
||
return false;
|
||
|
||
var st = new WalkState
|
||
{
|
||
Lane = h,
|
||
Pos = i,
|
||
Dir = (sbyte)dir,
|
||
// 脱困一律走中线：卡住的单位多半就是被横向偏移推进了花坛 / 门框。
|
||
Offset = 0f,
|
||
Prev = -1,
|
||
};
|
||
Sample(lane, st, out ex, out ey, out ez, out _);
|
||
return float.IsFinite(ex) && float.IsFinite(ez);
|
||
}
|
||
|
||
/// <summary>找离 (x,z) 最近的车道，并给出最近的点下标。</summary>
|
||
private static int NearestLane(Table table, float x, float z, out int pointIndex)
|
||
{
|
||
pointIndex = 0;
|
||
var best = -1;
|
||
var bestD = float.MaxValue;
|
||
var baseCell = CellOf(x, z);
|
||
|
||
// ⛑️ 2026-09-23 修：dx 那一层**不能**带 `best < 0` 提前退出。
|
||
// 原来一在 dx = -span（最左一列，离行人最远的那一列）找到任何候选就直接收工，
|
||
// 行人脚下那一列（dx = 0）根本没被搜过 —— 结果就是"找到什么算什么"，
|
||
// 实测吸附距离中位 7.6 m / p90 34.6 m / 88% 选错车道。
|
||
// 正确做法：每个 span 把整个方形走完（越靠中心越近），只由 span 层决定是否继续扩大。
|
||
for (var span = 1; span <= 8 && best < 0; span++)
|
||
for (var dx = -span; dx <= span; dx++)
|
||
for (var dz = -span; dz <= span; dz++)
|
||
{
|
||
if (!table.Cells.TryGetValue((baseCell.Item1 + dx, baseCell.Item2 + dz), out var bucket))
|
||
continue;
|
||
foreach (var h in bucket)
|
||
{
|
||
if (!table.Lanes.TryGetValue(h, out var lane) || lane.Banned)
|
||
continue;
|
||
for (var i = 0; i < lane.Count; i++)
|
||
{
|
||
var ddx = lane.Xs[i] - x;
|
||
var ddz = lane.Zs[i] - z;
|
||
var d = ddx * ddx + ddz * ddz;
|
||
if (d >= bestD)
|
||
continue;
|
||
bestD = d;
|
||
best = h;
|
||
pointIndex = i;
|
||
}
|
||
}
|
||
}
|
||
return best;
|
||
}
|
||
|
||
/// <summary>车道图上的 BFS。限制访问预算，别为一次问路把 88k 条车道全走一遍。</summary>
|
||
/// <summary>
|
||
/// 按**实际距离**的最短路（Dijkstra），而不是按跳数。
|
||
///
|
||
/// ⛑️⛑️ 2026-09-23 两个真 bug，都是离线量化逼出来的（模拟器 `_pedgate_sim.py`，250 条真实行程）：
|
||
///
|
||
/// ① **没有间隙闸门**。`TryHop`（随机游走）有 `MaxHopGapMetres` 闸门，这里原来**完全没有** ——
|
||
/// 而 link 表里有 **20.6% 的边端点间隙 > 16 m，最大 859 m**（全量统计，不是抽样）。
|
||
/// 实测一条 49 m 的行程规划出 416 m 的路径，轨迹里出现"先去西边 80 m 再回到起点"。
|
||
/// 规划路径（目标导向行程 + `AskClientNpcFindPath` 应答）一直在走这些跨空间的边。
|
||
/// ② **按跳数最短路**会疯狂使用长车道：车道长度中位 3.56 m 但最长 856 m，
|
||
/// 跳数最少 ⇏ 距离最短，方向还可能完全背离目标。
|
||
///
|
||
/// 修完的实测（目标 45~135 m）：
|
||
/// BFS 无闸门：绕行 3.02x（路径 279 m / 净位移 93 m）
|
||
/// BFS + 闸门：绕行 2.09x（186 m）
|
||
/// **Dijkstra + 闸门：绕行 1.82x（165 m），成功率 99.6%，单次 2.3 ms** ← 采用
|
||
/// 城市路网里 1.8x 的绕行是正常的绕街区，可以接受。
|
||
/// </summary>
|
||
private static List<int>? ShortestChain(Table table, int start, int goal)
|
||
{
|
||
if (start == goal)
|
||
return [start];
|
||
|
||
var prev = new Dictionary<int, int> { [start] = -1 };
|
||
var dist = new Dictionary<int, float> { [start] = 0f };
|
||
var settled = new HashSet<int>();
|
||
var queue = new PriorityQueue<int, float>();
|
||
queue.Enqueue(start, 0f);
|
||
var budget = 200000;
|
||
|
||
while (queue.Count > 0 && budget-- > 0)
|
||
{
|
||
var cur = queue.Dequeue();
|
||
if (!settled.Add(cur))
|
||
continue;
|
||
if (cur == goal)
|
||
break;
|
||
if (!table.Lanes.TryGetValue(cur, out var lane))
|
||
continue;
|
||
|
||
var dCur = dist[cur];
|
||
if (dCur > MaxRouteSearchMetres)
|
||
break; // 已经搜出这么远还没到 ⇒ 放弃（不可达 / 太远）
|
||
|
||
// 路径的实际接续方式是"本车道末端 → 下一条首端"（与 PlanRoute 里取点的顺序一致）。
|
||
var exitX = lane.Xs[lane.Count - 1];
|
||
var exitZ = lane.Zs[lane.Count - 1];
|
||
|
||
foreach (var to in lane.Next)
|
||
{
|
||
if (settled.Contains(to))
|
||
continue;
|
||
if (!table.Lanes.TryGetValue(to, out var cand) || cand.Banned)
|
||
continue;
|
||
|
||
var gx = cand.Xs[0] - exitX;
|
||
var gz = cand.Zs[0] - exitZ;
|
||
var hop = MathF.Sqrt(gx * gx + gz * gz);
|
||
if (hop > MaxHopGapMetres)
|
||
continue; // 跨空间的坏边：宁可绕一点，也不要让路径瞬移
|
||
|
||
var nd = dCur + hop + cand.Length;
|
||
if (nd >= dist.GetValueOrDefault(to, float.MaxValue))
|
||
continue;
|
||
|
||
dist[to] = nd;
|
||
prev[to] = cur;
|
||
queue.Enqueue(to, nd);
|
||
}
|
||
}
|
||
|
||
if (!prev.ContainsKey(goal))
|
||
return null;
|
||
|
||
var chain = new List<int>();
|
||
for (var n = goal; n != -1; n = prev[n])
|
||
chain.Add(n);
|
||
chain.Reverse();
|
||
return chain;
|
||
}
|
||
|
||
/// <summary>诊断用：路网里有多少条禁行车道、多少条带宽度。给 GM 面板 / 启动日志看。</summary>
|
||
internal static (int Lanes, int Banned, int WithWidth) Stats()
|
||
{
|
||
var table = Cache.Value;
|
||
var banned = 0;
|
||
var withWidth = 0;
|
||
foreach (var lane in table.Lanes.Values)
|
||
{
|
||
if (lane.Banned)
|
||
banned++;
|
||
if (lane.HalfWalk > 0f)
|
||
withWidth++;
|
||
}
|
||
return (table.Lanes.Count, banned, withWidth);
|
||
}
|
||
}
|