868 lines
37 KiB
C#
868 lines
37 KiB
C#
using Ananta.Server.Configuration;
|
||
|
||
namespace Ananta.Server.Handlers.Game;
|
||
|
||
/// <summary>
|
||
/// 车辆路网（服务端侧 ZoneGraph 简化版）—— 给 T3-7 车流提供「车道 handle + 沿车道里程」。
|
||
///
|
||
/// 数据来自捐赠端 `zonegraph_lanes.csv`（WorldMap_Release，14171 条车道），
|
||
/// 由 `_gen_vehicle_lanes.py` 压成两份运行时表：
|
||
/// `Ananta.Server/Ananta.App/data/vehicle_lanes.csv` = handle;bx;by;bz;ex;ey;ez;length;speed
|
||
/// `Ananta.Server/Ananta.App/data/vehicle_lane_links.csv` = handle;to1;to2;...
|
||
///
|
||
/// ★ 为什么用捐赠端那份而不是 `_ref/lanes/vehicle-lane-links.csv`：
|
||
/// 后者 Type==1 前向边只有 10911 条、只覆盖 56.9% 的车道，且是捐赠端 next 的**真子集**；
|
||
/// 捐赠端 next 是多值后继，覆盖 13910/14171 = 98.2%，换道接缝中位 0.00m。
|
||
///
|
||
/// ★ 与 PedLaneGraph 的关键差别：客户端按 <c>DistanceAlongLane</c> 在**它本地的真实车道曲线**上
|
||
/// 插值（MassTrafficInterpolationUtils），所以服务端不需要车道折线，只要里程单调推进。
|
||
/// 这里的 bx/ez 只用于「玩家附近挑车道」和 despawn 判定。
|
||
/// </summary>
|
||
internal static class VehicleLaneGraph
|
||
{
|
||
internal struct DriveState
|
||
{
|
||
internal int Lane;
|
||
internal float Distance;
|
||
}
|
||
|
||
private sealed class Lane
|
||
{
|
||
internal float Bx, By, Bz, Ex, Ey, Ez;
|
||
internal float Length;
|
||
internal float Speed;
|
||
internal int[] Next = [];
|
||
|
||
/// <summary>
|
||
/// 这条车道能不能用于<b>导航求解</b>（2026-09-30）＝它是否属于车道图的
|
||
/// <b>最大强连通分量</b>（既能被开进来、也能开出去）。见 <see cref="MarkRoutableLanes"/>。
|
||
/// ⛑ 只有 <c>TrySnapLane</c>（导航）在意它；<c>TryPickSpawn</c>（车流出生）不看，
|
||
/// 后者用的是更宽松的「有后继」判据，改了会动摇 T3-7 已经调好的出生分布。
|
||
/// </summary>
|
||
internal bool Routable;
|
||
|
||
/// <summary>
|
||
/// ★★ 2026-10-03：「弦中点偏离真实车道」的<b>弓高</b>（米）。
|
||
///
|
||
/// CSV 只给了车道<b>弦的两个端点</b>（bx/bz → ex/ez），真实车道是曲线，客户端按
|
||
/// <c>DistanceAlongLane</c> 在它本地的曲线上插值。我们沿<b>直线弦</b>取点，弯道时
|
||
/// 弦中点会落到路面之外 —— 召唤车「卡在人行道 / 花坛里」就是这个（实测一条 70m 弯车道
|
||
/// 弦长 62.96m、弧长 70.02m，弦中点偏离真实路面约 10m）。
|
||
///
|
||
/// 推导（把车道近似成圆弧）：
|
||
/// 弧弦比 <c>r = Length / chord ≈ 1 + θ²/24</c> ⇒ 圆心角 <c>θ ≈ sqrt(24(r-1))</c>
|
||
/// 弓高 <c>h ≈ chord · θ / 8</c>
|
||
/// 弦上参数 t 处的横向偏离 <c>δ(t) ≈ 4·h·t·(1-t)</c>（t=0/1 为 0，t=0.5 最大为 h）。
|
||
/// </summary>
|
||
internal float Sagitta;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 算一条车道的<b>弓高</b>（弦中点相对真实曲线的最大横向偏离，米）。见 <see cref="Lane.Sagitta"/>。
|
||
/// 直车道（弧长≈弦长）返回 0。
|
||
/// </summary>
|
||
private static float SagittaOf(Lane lane)
|
||
{
|
||
var dx = lane.Ex - lane.Bx;
|
||
var dz = lane.Ez - lane.Bz;
|
||
var chord = MathF.Sqrt(dx * dx + dz * dz);
|
||
if (chord < 1e-3f || lane.Length <= chord)
|
||
return 0f;
|
||
|
||
// 弧弦比 r ≈ 1 + θ²/24 ⇒ θ ≈ sqrt(24(r-1))；弓高 h ≈ chord·θ/8
|
||
var ratio = lane.Length / chord;
|
||
var theta = MathF.Sqrt(24f * (ratio - 1f));
|
||
return chord * theta / 8f;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 弦上参数 <c>t</c>（0..1）处的横向偏离（米）。t=0/1 落在端点 ⇒ 偏离 0；t=0.5 最大 ⇒ 等于弓高。
|
||
/// </summary>
|
||
private static float LateralDeviationAt(float sagitta, float t)
|
||
=> sagitta <= 0f ? 0f : 4f * sagitta * t * (1f - t);
|
||
|
||
private const float CellSize = 100f;
|
||
|
||
private sealed class Table
|
||
{
|
||
internal Dictionary<int, Lane> Lanes = [];
|
||
internal Dictionary<(int, int), List<int>> Cells = [];
|
||
internal bool Ok;
|
||
internal int RoutableLanes;
|
||
internal int RoutableComponents;
|
||
}
|
||
|
||
private static readonly Lazy<Table> Cache = new(Load);
|
||
|
||
internal static bool Available => Cache.Value.Ok;
|
||
internal static int LaneCount => Cache.Value.Lanes.Count;
|
||
|
||
/// <summary>最大强连通分量里的车道数（导航只会吸附到这些车道上）。排障用。</summary>
|
||
internal static int RoutableLaneCount => Cache.Value.RoutableLanes;
|
||
|
||
/// <summary>强连通分量个数。理想值 1；2026-09-30 实测 286（见 <see cref="MarkRoutableLanes"/>）。</summary>
|
||
internal static int ComponentCount => Cache.Value.RoutableComponents;
|
||
|
||
private static Table Load()
|
||
{
|
||
var table = new Table();
|
||
var lanePath = PrivateServerConfigStore.ResolveProjectPath(
|
||
"Ananta.Server/Ananta.App/data/vehicle_lanes.csv");
|
||
var linkPath = PrivateServerConfigStore.ResolveProjectPath(
|
||
"Ananta.Server/Ananta.App/data/vehicle_lane_links.csv");
|
||
if (!File.Exists(lanePath) || !File.Exists(linkPath))
|
||
return table;
|
||
|
||
foreach (var line in File.ReadLines(lanePath))
|
||
{
|
||
if (line.Length == 0)
|
||
continue;
|
||
var parts = line.Split(';');
|
||
if (parts.Length < 8 || !int.TryParse(parts[0], out var handle))
|
||
continue;
|
||
|
||
var lane = new Lane
|
||
{
|
||
Bx = Num(parts[1]), By = Num(parts[2]), Bz = Num(parts[3]),
|
||
Ex = Num(parts[4]), Ey = Num(parts[5]), Ez = Num(parts[6]),
|
||
Length = Num(parts[7]),
|
||
Speed = parts.Length >= 9 ? Num(parts[8]) : 9.72f,
|
||
};
|
||
if (lane.Length <= 0.5f)
|
||
{
|
||
// 退化车道：按端点距离补一个下限，避免除零。
|
||
var dx = lane.Ex - lane.Bx;
|
||
var dz = lane.Ez - lane.Bz;
|
||
lane.Length = MathF.Max(1f, MathF.Sqrt(dx * dx + dz * dz));
|
||
}
|
||
lane.Sagitta = SagittaOf(lane);
|
||
table.Lanes[handle] = lane;
|
||
|
||
var cell = CellOf(lane.Bx, lane.Bz);
|
||
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();
|
||
}
|
||
|
||
MarkRoutableLanes(table);
|
||
table.Ok = table.Lanes.Count > 0;
|
||
return table;
|
||
}
|
||
|
||
// -----------------------------------------------------------------------------------------
|
||
// ⛑⛑ 2026-09-30：车道图有 **285 条孤岛车道**，这是「自动驾驶开不到自定义标点」的真因。
|
||
//
|
||
// 体检脚本 `_vehicle_lane_graph_check.py` 的结果（可直接复跑）：
|
||
// 车道 14171 条、链接表只覆盖 13910 条 ⇒ **261 条没有任何后继、255 条没有任何前驱**，
|
||
// 两者的样例 id 完全重合（89/93/135/140/232/236/410/417…）⇒ 它们是<b>双向断开的孤儿车道</b>。
|
||
// Tarjan 的结果更狠：全图 **286 个强连通分量**，最大的那个有 13886 条（97%），
|
||
// 剩下 285 条各自成形单影只的分量。
|
||
//
|
||
// 后果：日志里 `车道图不通 start=1795 target=1972` 反复出现且**永远不解开** —— 因为 lane 1972 /
|
||
// 6901 / 13108 的「能到它的车道数」= 1（只有它自己）。而 <c>SolveRoute</c> 的 Dijkstra 只走前向
|
||
// 后继，玩家把目的地标点落到这种车道附近时，**永远解不出路** ⇒ Start 之后根本没有 cruise 下发
|
||
// ⇒ 现象就是「按了自动驾驶没反应」。
|
||
// 商店/POI 之所以大多没事：它们的 endPos 是走 `GpsAreaConnectMgr` 算出来的**路口/出入口点**，
|
||
// 天然落在主干道上；自定义标点给的是玩家自己点的原始坐标，撞上孤儿车道就是纯运气。
|
||
//
|
||
// ⇒ 治法：**导航吸附时只认最大强连通分量里的车道**。孤儿车道本来就没法参与路径，
|
||
// 拿它当起点/终点必然失败；跳过它去找次近的正常车道，代价是终点可能偏移几米。
|
||
// （真要根治得重跑 `_gen_vehicle_lanes.py` 把这 261 条接上去，本次不动数据。）
|
||
// -----------------------------------------------------------------------------------------
|
||
private static void MarkRoutableLanes(Table table)
|
||
{
|
||
// 迭代版 Tarjan（14171 个点，递归会爆栈）。
|
||
var index = new Dictionary<int, int>();
|
||
var low = new Dictionary<int, int>();
|
||
var onStack = new HashSet<int>();
|
||
var stack = new List<int>();
|
||
var comp = new Dictionary<int, int>();
|
||
var counter = 0;
|
||
var compCount = 0;
|
||
|
||
foreach (var root in table.Lanes.Keys)
|
||
{
|
||
if (index.ContainsKey(root))
|
||
continue;
|
||
|
||
var work = new List<(int Node, IEnumerator<int> It)> { (root, Next(table, root)) };
|
||
index[root] = low[root] = counter++;
|
||
stack.Add(root);
|
||
onStack.Add(root);
|
||
|
||
while (work.Count > 0)
|
||
{
|
||
var (v, it) = work[^1];
|
||
var advanced = false;
|
||
|
||
while (it.MoveNext())
|
||
{
|
||
var w = it.Current;
|
||
if (!table.Lanes.ContainsKey(w))
|
||
continue;
|
||
if (!index.ContainsKey(w))
|
||
{
|
||
index[w] = low[w] = counter++;
|
||
stack.Add(w);
|
||
onStack.Add(w);
|
||
work.Add((w, Next(table, w)));
|
||
advanced = true;
|
||
break;
|
||
}
|
||
if (onStack.Contains(w) && index[w] < low[v])
|
||
low[v] = index[w];
|
||
}
|
||
|
||
if (advanced)
|
||
continue;
|
||
|
||
work.RemoveAt(work.Count - 1);
|
||
if (low[v] == index[v])
|
||
{
|
||
var id = compCount++;
|
||
int popped;
|
||
do
|
||
{
|
||
popped = stack[^1];
|
||
stack.RemoveAt(stack.Count - 1);
|
||
onStack.Remove(popped);
|
||
comp[popped] = id;
|
||
}
|
||
while (popped != v);
|
||
}
|
||
|
||
if (work.Count > 0)
|
||
{
|
||
var parent = work[^1].Node;
|
||
if (low[v] < low[parent])
|
||
low[parent] = low[v];
|
||
}
|
||
}
|
||
}
|
||
|
||
// 取规模最大的分量作为「可用路网」。
|
||
var sizes = new Dictionary<int, int>();
|
||
foreach (var c in comp.Values)
|
||
sizes[c] = sizes.TryGetValue(c, out var n) ? n + 1 : 1;
|
||
|
||
var best = -1;
|
||
var bestSize = 0;
|
||
foreach (var (id, size) in sizes)
|
||
{
|
||
if (size <= bestSize)
|
||
continue;
|
||
bestSize = size;
|
||
best = id;
|
||
}
|
||
|
||
var routable = 0;
|
||
foreach (var (handle, lane) in table.Lanes)
|
||
{
|
||
lane.Routable = comp.TryGetValue(handle, out var c) && c == best;
|
||
if (lane.Routable)
|
||
routable++;
|
||
}
|
||
|
||
if (routable == 0)
|
||
{
|
||
// 兜底：一旦上面的分量算挂了（退化成全 false）导航会**彻底不可用**，
|
||
// 宁可退回老行为（全都可用）也不要让自动驾驶直接死掉。
|
||
foreach (var lane in table.Lanes.Values)
|
||
lane.Routable = true;
|
||
routable = table.Lanes.Count;
|
||
}
|
||
|
||
table.RoutableLanes = routable;
|
||
table.RoutableComponents = sizes.Count;
|
||
}
|
||
|
||
private static IEnumerator<int> Next(Table table, int handle)
|
||
=> table.Lanes.TryGetValue(handle, out var lane)
|
||
? ((IEnumerable<int>)lane.Next).GetEnumerator()
|
||
: Enumerable.Empty<int>().GetEnumerator();
|
||
|
||
private static float Num(string s) => float.TryParse(s, 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>
|
||
/// 在 (px,pz) 的**环形**区间内随机挑一条车道当作出生点：距玩家
|
||
/// <c>[minDistance, radius]</c>。
|
||
///
|
||
/// ⛑ 为什么必须有 <c>minDistance</c>（2026-09-29 修）：原来只筛「≤ radius」，
|
||
/// 于是车会直接刷在玩家脚边几米处；而且出生点固定取车道<b>起点</b>（Distance=0），
|
||
/// 而车道起点大量落在路口里 ⇒ 观感就是「车刷在我身边，还不在车道上」。
|
||
/// 更糟的是客户端 <c>AetherVehicleViewManager</c> 会在
|
||
/// <c>CANDIDATE_GO_DISTANCE_SQ=400</c>（20m）内把车流车提升成 GO（完整载具实体），
|
||
/// 提升后不再吃我们的 Aether 车道插值 ⇒ 冻在原地，于是玩家身边越积越多不动的车。
|
||
/// </summary>
|
||
internal static bool TryPickSpawn(float px, float pz, float radius, float minDistance, Random rng,
|
||
out DriveState state, out float x, out float y, out float z, out float speed)
|
||
{
|
||
state = default;
|
||
x = y = z = 0f;
|
||
speed = 9.72f;
|
||
|
||
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;
|
||
// ⛑ 2026-09-29：不要生在**没有后继**的车道上。
|
||
// 链接表只覆盖 13910/14171，剩下 261 条是无后继的真死路；
|
||
// 生在那种车道上 ⇒ 车开到尽头就被回收，实机表现为「刚刷出来就没了」
|
||
// （日志里 alive=3~20s、hops=0、after=13~211m 全是这批）。
|
||
if (lane.Next.Length == 0)
|
||
continue;
|
||
var ddx = lane.Bx - px;
|
||
var ddz = lane.Bz - pz;
|
||
var dSq = ddx * ddx + ddz * ddz;
|
||
if (dSq <= radius * radius && dSq >= minDistance * minDistance)
|
||
pool.Add(h);
|
||
}
|
||
}
|
||
if (pool.Count == 0)
|
||
{
|
||
// 环太窄（min 太大 / 附近没路）⇒ 退回「只要不在玩家脸上」，宁可近一点也别刷不出来。
|
||
var minSq = minDistance * minDistance;
|
||
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;
|
||
var ddx = lane.Bx - px;
|
||
var ddz = lane.Bz - pz;
|
||
if (ddx * ddx + ddz * ddz <= radius * radius)
|
||
pool.Add(h);
|
||
}
|
||
if (pool.Count > 0)
|
||
break;
|
||
}
|
||
if (pool.Count == 0)
|
||
return false;
|
||
}
|
||
|
||
var picked = pool[rng.Next(pool.Count)];
|
||
var start = table.Lanes[picked];
|
||
state = new DriveState { Lane = picked, Distance = 0f };
|
||
x = start.Bx;
|
||
y = start.By;
|
||
z = start.Bz;
|
||
speed = start.Speed;
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 「召唤载具从车道开过来」的生成点（2026-10-03）：在 (px,pz) 的<b>环形</b>区间
|
||
/// <c>[minDistance, maxDistance]</c> 内，挑一条车道上距离合适的点，并返回<b>车道朝向</b>。
|
||
///
|
||
/// ⛑⛑ 与 <see cref="TryPickSpawn"/>（T3-7 车流出生）的三点差别，别混用：
|
||
/// ① <b>只挑 <see cref="Lane.Routable"/> 的车道</b>。吸附到那 285 条孤岛车道上 ⇒
|
||
/// Dijkstra 解不出路 ⇒ 一条 Cruise 都不发 ⇒ 「叫了车但没反应」（2026-09-30 血泪）。
|
||
/// ② <b>不只取车道起点</b>：沿弦采样 5 个 t，用落在环内的那一段 —— 车道起点大量落在路口里，
|
||
/// 刷在路口的车开出来会横穿。
|
||
/// ③ <b>返回车道朝向</b>（<see cref="FacingOf"/> 同一套角度约定），车头顺着路，
|
||
/// 不然车要先原地掉头，观感是「车卡了一下才走」。
|
||
///
|
||
/// <paramref name="maxLateralMeters"/>：允许的<b>横向偏离</b>（米），0 = 不过滤（旧行为）。
|
||
/// 车道 CSV 只有弦端点，弯道时弦中点在真实路面之外 ⇒ 车会刷到人行道/花坛里。
|
||
/// 给 1.5 之类的值后，弯车道只保留<b>靠近端点</b>的那些采样点（偏离随 t 两端趋 0）。
|
||
/// <paramref name="rng"/>：多台车连叫时不想每次都刷在同一个位置，交给调用方给随机源。
|
||
/// </summary>
|
||
internal static bool TryPickDriveInSpawn(float px, float pz, float minDistance, float maxDistance,
|
||
float maxLateralMeters, Random rng, out float x, out float y, out float z, out float facingDegrees)
|
||
{
|
||
x = y = z = 0f;
|
||
facingDegrees = 0f;
|
||
|
||
var table = Cache.Value;
|
||
if (!table.Ok)
|
||
return false;
|
||
|
||
var minSq = minDistance * minDistance;
|
||
var maxSq = maxDistance * maxDistance;
|
||
var reach = (int)MathF.Ceiling(maxDistance / CellSize) + 1;
|
||
var baseCell = CellOf(px, pz);
|
||
|
||
List<(float X, float Y, float Z, float Facing)> 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.Routable || lane.Next.Length == 0)
|
||
continue;
|
||
|
||
// 角度约定与 SpawnDirectAsync 的偏移一致：
|
||
// yaw = facing * PI/180；偏移 = (+cos(yaw), -sin(yaw))
|
||
// ⇒ 方向向量 (dx,dz) 对应 facing = Atan2(-dz, dx)。
|
||
var sdx = lane.Ex - lane.Bx;
|
||
var sdz = lane.Ez - lane.Bz;
|
||
var facing = MathF.Atan2(-sdz, sdx) * (180f / MathF.PI);
|
||
|
||
// ⛑⛑ 采样密度从 5 提到 21：加了横向偏离过滤之后，弯车道只剩端点附近可用，
|
||
// 采样太稀会「明明有合格车道却一个点都挑不到」⇒ 白白退回旧路径。
|
||
const int Samples = 20;
|
||
for (var k = 0; k <= Samples; k++)
|
||
{
|
||
var t = (float)k / Samples;
|
||
|
||
// ⛑⛑ 弦上 t 处偏离真实车道太远 ⇒ 这个点在人行道 / 花坛 / 建筑里，不要。
|
||
if (maxLateralMeters > 0f && LateralDeviationAt(lane.Sagitta, t) > maxLateralMeters)
|
||
continue;
|
||
|
||
var cx = lane.Bx + sdx * t;
|
||
var cy = lane.By + (lane.Ey - lane.By) * t;
|
||
var cz = lane.Bz + sdz * t;
|
||
|
||
var ddx = cx - px;
|
||
var ddz = cz - pz;
|
||
var dSq = ddx * ddx + ddz * ddz;
|
||
if (dSq < minSq || dSq > maxSq)
|
||
continue;
|
||
|
||
pool.Add((cx, cy, cz, facing));
|
||
}
|
||
}
|
||
}
|
||
|
||
if (pool.Count == 0)
|
||
return false;
|
||
|
||
var picked = pool[rng.Next(pool.Count)];
|
||
x = picked.X; y = picked.Y; z = picked.Z; facingDegrees = picked.Facing;
|
||
return true;
|
||
}
|
||
|
||
// ---------------------------------------------------------------------------------------------
|
||
// 路线规划（GameRouter.VehicleNav.cs）用的取值口。数据仍然只有这一份：
|
||
// 只为「最近车道」另建了一份索引（按起点/中点/终点三个关键 cell 挂号），因为 try 9 是长车道
|
||
// 只按起点挂号会漏掉「从路中间上车」的情形，而我们不能动 TryPickSpawn 那张 begin-only 索引
|
||
// （改了会改变 T3-7 车流的随机出生分布）。
|
||
// ---------------------------------------------------------------------------------------------
|
||
|
||
private sealed class NavIndex
|
||
{
|
||
internal Dictionary<(int, int), List<int>> Cells = [];
|
||
}
|
||
|
||
private static readonly Lazy<NavIndex> NavCache = new(BuildNavIndex);
|
||
|
||
private static NavIndex BuildNavIndex()
|
||
{
|
||
var index = new NavIndex();
|
||
foreach (var (handle, lane) in Cache.Value.Lanes)
|
||
{
|
||
Index(handle, lane.Bx, lane.Bz);
|
||
Index(handle, (lane.Bx + lane.Ex) * 0.5f, (lane.Bz + lane.Ez) * 0.5f);
|
||
Index(handle, lane.Ex, lane.Ez);
|
||
}
|
||
|
||
void Index(int h, float x, float z)
|
||
{
|
||
var cell = CellOf(x, z);
|
||
if (!index.Cells.TryGetValue(cell, out var bucket))
|
||
index.Cells[cell] = bucket = [];
|
||
if (!bucket.Contains(h))
|
||
bucket.Add(h);
|
||
}
|
||
|
||
return index;
|
||
}
|
||
|
||
/// <summary>一次 <c>TrySnapLane</c> 的结果。</summary>
|
||
internal readonly record struct LaneSnap(int Handle, float T, float Distance, float X, float Y, float Z);
|
||
|
||
/// <summary>
|
||
/// 找 (px,pz) 半径内最近的车道，并把点投影到它的弦上（<c>T</c> = 归一化里程）。
|
||
/// 只看 XZ 平面：竖直方向(y) 对「哪条路最近」没有意义，而我们本来也只有端点。
|
||
///
|
||
/// ⛑⛑ 2026-09-30：<b>只返回 <see cref="Lane.Routable"/> 的车道</b>（最大强连通分量内的）。
|
||
/// 吸附到孤儿车道上 ⇒ Dijkstra 永远解不出路 ⇒ 按自动驾驶没反应。详见 <see cref="MarkRoutableLanes"/>。
|
||
/// 车流的 <c>TryPickSpawn</c> 不受影响（那条路径不走这里）。
|
||
/// </summary>
|
||
internal static bool TrySnapLane(float px, float pz, float maxRadius, out LaneSnap snap)
|
||
=> TrySnapLane(px, pz, maxRadius, 0f, out snap);
|
||
|
||
/// <summary>
|
||
/// 同 <see cref="TrySnapLane(float, float, float, out LaneSnap)"/>，多一个<b>横向偏离</b>过滤。
|
||
///
|
||
/// ⛑⛑ 召唤车「停在人行道 / 花坛里」的另一半根因：停车点是玩家坐标投影到最近车道<b>弦</b>上
|
||
/// 得到的，弯车道时这个点同样在路面之外。给 <paramref name="maxLateralMeters"/>（如 1.5）
|
||
/// 即可只接受贴近真实路面的投影位置。
|
||
///
|
||
/// ⛑ 传 0（= 上面那个重载）保持旧行为，自动驾驶导航走的就是那条，别动它。
|
||
/// </summary>
|
||
internal static bool TrySnapLane(float px, float pz, float maxRadius, float maxLateralMeters,
|
||
out LaneSnap snap)
|
||
{
|
||
snap = default;
|
||
var table = Cache.Value;
|
||
if (!table.Ok)
|
||
return false;
|
||
|
||
var reach = (int)MathF.Ceiling(maxRadius / CellSize) + 1;
|
||
var baseCell = CellOf(px, pz);
|
||
var best = float.MaxValue;
|
||
var found = false;
|
||
|
||
for (var dx = -reach; dx <= reach; dx++)
|
||
for (var dz = -reach; dz <= reach; dz++)
|
||
{
|
||
if (!NavCache.Value.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;
|
||
|
||
// ⛑⛑ 跳过孤岛车道：它们参与不了任何路径（见 MarkRoutableLanes 的注释）。
|
||
if (!lane.Routable)
|
||
continue;
|
||
|
||
var sx = lane.Ex - lane.Bx;
|
||
var sz = lane.Ez - lane.Bz;
|
||
var denom = sx * sx + sz * sz;
|
||
var t = denom > 1e-6f
|
||
? ((px - lane.Bx) * sx + (pz - lane.Bz) * sz) / denom
|
||
: 0f;
|
||
t = Math.Clamp(t, 0f, 1f);
|
||
|
||
// ⛑⛑ 弦上 t 处偏离真实车道太远 ⇒ 停车点会落在人行道 / 花坛里，换一条。
|
||
if (maxLateralMeters > 0f && LateralDeviationAt(lane.Sagitta, t) > maxLateralMeters)
|
||
continue;
|
||
|
||
var cx = lane.Bx + sx * t;
|
||
var cz = lane.Bz + sz * t;
|
||
var ddx = cx - px;
|
||
var ddz = cz - pz;
|
||
var dist = MathF.Sqrt(ddx * ddx + ddz * ddz);
|
||
if (dist > maxRadius || dist >= best)
|
||
continue;
|
||
|
||
best = dist;
|
||
snap = new LaneSnap(h, t, dist, cx, lane.By + (lane.Ey - lane.By) * t, cz);
|
||
found = true;
|
||
}
|
||
}
|
||
|
||
return found;
|
||
}
|
||
|
||
/// <summary>一条车道的弦与长度。找不到返回 false。</summary>
|
||
internal static bool TryLaneGeometry(int handle, out float bx, out float by, out float bz,
|
||
out float ex, out float ey, out float ez, out float length)
|
||
{
|
||
bx = by = bz = ex = ey = ez = length = 0f;
|
||
if (!Cache.Value.Lanes.TryGetValue(handle, out var lane))
|
||
return false;
|
||
|
||
bx = lane.Bx; by = lane.By; bz = lane.Bz;
|
||
ex = lane.Ex; ey = lane.Ey; ez = lane.Ez;
|
||
length = lane.Length;
|
||
return true;
|
||
}
|
||
|
||
/// <summary>后继车道（多值）。没有后继返回空数组。</summary>
|
||
internal static int[] NextOf(int handle)
|
||
=> Cache.Value.Lanes.TryGetValue(handle, out var lane) ? lane.Next : [];
|
||
|
||
/// <summary>
|
||
/// 规划一条**直行优先**的路径：从起始车道出发，每一步都挑「方向最贴合当前朝向」的后继。
|
||
/// 返回接下来要走的车道序列（不含起始车道本身）。
|
||
///
|
||
/// ★★ 为什么需要它（2026-09-28）：车流原来是每到一条车道就**随机挑后继**（<c>PickNext</c>），
|
||
/// 于是车会乱拐、掉头、钻死路、很快被回收 —— 用户看到的就是「开很短距离就停下/消失」。
|
||
/// 有了预规划路径，车沿道路连续跑 1~2 段甚至更远，观感从"随机乱窜"变成"正常行驶"。
|
||
///
|
||
/// ★ 不直接用导航那套 Dijkstra：Dijkstra 求的是**到指定目标**的最短路，
|
||
/// 而车流没有目的地，要的是"顺着路往前开"。直行优先恰好表达这个意图，
|
||
/// 且天然排除掉头（dot 为负的后继排最后）。
|
||
///
|
||
/// ★ 接缝超过 <paramref name="maxHop"/> 的后继直接跳过 —— 与 <c>Advance</c> 的硬上限一致，
|
||
/// 否则规划出来的路径里会混进几百米的瞬移跳。
|
||
/// </summary>
|
||
internal static List<int> PlanRoute(int startLane, int maxHops, float maxHop)
|
||
{
|
||
var route = new List<int>(Math.Max(0, maxHops));
|
||
var table = Cache.Value;
|
||
if (!table.Ok)
|
||
return route;
|
||
|
||
var cur = startLane;
|
||
for (var k = 0; k < maxHops; k++)
|
||
{
|
||
if (!table.Lanes.TryGetValue(cur, out var lane))
|
||
break;
|
||
|
||
var fdx = lane.Ex - lane.Bx;
|
||
var fdz = lane.Ez - lane.Bz;
|
||
var flen = MathF.Sqrt(fdx * fdx + fdz * fdz);
|
||
if (flen < 1e-3f)
|
||
break;
|
||
fdx /= flen;
|
||
fdz /= flen;
|
||
|
||
var best = -1;
|
||
var bestDot = -2f;
|
||
foreach (var h in lane.Next)
|
||
{
|
||
if (!table.Lanes.TryGetValue(h, out var cand))
|
||
continue;
|
||
var gx = cand.Bx - lane.Ex;
|
||
var gz = cand.Bz - lane.Ez;
|
||
if (gx * gx + gz * gz > maxHop * maxHop)
|
||
continue; // 会瞬移的后继不用
|
||
var cdx = cand.Ex - cand.Bx;
|
||
var cdz = cand.Ez - cand.Bz;
|
||
var clen = MathF.Sqrt(cdx * cdx + cdz * cdz);
|
||
if (clen < 1e-3f)
|
||
continue;
|
||
var dot = fdx * (cdx / clen) + fdz * (cdz / clen);
|
||
if (dot > bestDot)
|
||
{
|
||
bestDot = dot;
|
||
best = h;
|
||
}
|
||
}
|
||
|
||
if (best < 0)
|
||
break; // 没有合格后继 = 到头了
|
||
route.Add(best);
|
||
cur = best;
|
||
}
|
||
return route;
|
||
}
|
||
|
||
/// <summary>一次车道衔接的细节（诊断用）。</summary>
|
||
internal struct LaneHop
|
||
{
|
||
internal int FromLane;
|
||
internal int ToLane;
|
||
|
||
/// <summary>旧车道终点 → 新车道起点的接缝距离（米）。越大越像瞬移。</summary>
|
||
internal float Gap;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 沿车道推进指定距离（米）。换道时把 <paramref name="state"/> 切到下一条并把里程归零。
|
||
/// 返回 false = 走到路网死路，调用方应当回收这辆车。
|
||
///
|
||
/// ★ <paramref name="maxGap"/>：只在接缝 ≤ 该值的后继里挑。捐赠端 next 是多值后继，
|
||
/// 实测 5.4% 的后继接缝 > 12m（最大 340m），直接随机挑会让车「瞬移一段甚至倒退」。
|
||
/// 找不到合格后继时退回「接缝最小」的那条，并如实把 Gap 报给调用方。
|
||
/// </summary>
|
||
/// <param name="maxHop">
|
||
/// 换道接缝的**绝对硬上限**（米）。超过就判定"开到尽头"，返回 false 让调用方回收。
|
||
/// 为什么需要它：<c>PickNext</c> 在所有后继都被"方向反折"过滤掉时会退回 <c>fallback</c>
|
||
/// （全局最小接缝）—— 那条可能远在天边。实测日志里出现过 <c>worstGap=394.1m</c>，
|
||
/// 而车道数据本身 p99 接缝就有 347m、max 2965m（>30m 的占 13.92%）。
|
||
/// 跳过去就是几百米的瞬移，比车在路口消失难看得多 —— 宁可回收。
|
||
/// </param>
|
||
internal static bool Advance(ref DriveState s, float meters, Random rng, float maxGap,
|
||
out LaneHop? hop, float maxHop = float.MaxValue, int preferredNext = -1)
|
||
{
|
||
hop = null;
|
||
var table = Cache.Value;
|
||
if (!table.Ok)
|
||
return false;
|
||
|
||
var guard = 0;
|
||
while (guard++ < 32)
|
||
{
|
||
if (!table.Lanes.TryGetValue(s.Lane, out var lane))
|
||
return false;
|
||
|
||
var remain = lane.Length - s.Distance;
|
||
if (meters < remain)
|
||
{
|
||
s.Distance += meters;
|
||
return true;
|
||
}
|
||
|
||
if (lane.Next.Length == 0)
|
||
{
|
||
s.Distance = lane.Length;
|
||
return false;
|
||
}
|
||
|
||
meters -= remain;
|
||
var from = s.Lane;
|
||
|
||
// ★ 有预规划路径且这一跳就用它；否则退回「最小接缝 + 不反折」的随机挑法。
|
||
int next;
|
||
float gap;
|
||
if (preferredNext >= 0 && Array.IndexOf(lane.Next, preferredNext) >= 0)
|
||
{
|
||
next = preferredNext;
|
||
gap = 0f;
|
||
if (table.Lanes.TryGetValue(next, out var cand))
|
||
{
|
||
var ax = cand.Bx - lane.Ex;
|
||
var az = cand.Bz - lane.Ez;
|
||
gap = MathF.Sqrt(ax * ax + az * az);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
next = PickNext(lane, table, rng, maxGap, out gap);
|
||
}
|
||
|
||
if (next < 0 || gap > maxHop)
|
||
{
|
||
// 接缝太远 ⇒ 不跳，当作开到尽头，交给调用方回收。
|
||
// 硬跳会造成数百米的瞬移（见 maxHop 的注释）。
|
||
s.Distance = lane.Length;
|
||
return false;
|
||
}
|
||
s.Lane = next;
|
||
s.Distance = 0f;
|
||
hop = new LaneHop { FromLane = from, ToLane = next, Gap = gap };
|
||
}
|
||
return true;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 挑下一条车道：在「接缝最小」的基础上放宽 <paramref name="maxGap"/> 米的容差再随机选，
|
||
/// 并排除方向反折的后继（那会让车掉头，视觉上像倒退瞬移）。
|
||
///
|
||
/// ★ 为什么是**相对容差**而不是「接缝 ≤ N 米」的绝对阈值：
|
||
/// 实测 400 起点 × 600 跳，绝对阈值 8m 会让 15.05% 的衔接 > 12m、p99 达 607m；
|
||
/// 而「总选最小缝」只有 2.55% > 12m、p99 50m。原因是绝对阈值一旦命中就随机选，
|
||
/// 容易拐进后继质量差的车道；贪心取最近则倾向留在连贯的主干道上。
|
||
/// 所以 maxGap=0 = 严格取接缝最小（并列最小里随机），是实测最优。
|
||
/// </summary>
|
||
private static int PickNext(Lane from, Table table, Random rng, float maxGap, out float gap)
|
||
{
|
||
var fex = from.Ex;
|
||
var fez = from.Ez;
|
||
var fdx = fex - from.Bx;
|
||
var fdz = fez - from.Bz;
|
||
|
||
List<int> good = [];
|
||
List<int> forward = [];
|
||
var minGap = float.MaxValue;
|
||
var fallback = from.Next[0];
|
||
|
||
foreach (var h in from.Next)
|
||
{
|
||
if (!table.Lanes.TryGetValue(h, out var cand))
|
||
continue;
|
||
|
||
var vx = cand.Bx - fex;
|
||
var vz = cand.Bz - fez;
|
||
var d = MathF.Sqrt(vx * vx + vz * vz);
|
||
if (d < minGap)
|
||
{
|
||
minGap = d;
|
||
fallback = h;
|
||
}
|
||
|
||
// 方向反折：新车道朝向与旧车道相反 → 车会「掉头」，视觉上像倒退瞬移。
|
||
if (fdx * (cand.Ex - cand.Bx) + fdz * (cand.Ez - cand.Bz) < 0f)
|
||
continue;
|
||
|
||
forward.Add(h);
|
||
if (d <= minGap + maxGap)
|
||
good.Add(h);
|
||
}
|
||
|
||
gap = minGap;
|
||
if (good.Count > 0)
|
||
return good[rng.Next(good.Count)];
|
||
if (forward.Count > 0 && maxGap > 0f)
|
||
return forward[rng.Next(forward.Count)];
|
||
return fallback;
|
||
}
|
||
|
||
/// <summary>服务端侧的线性插值的粗略世界坐标（只用于 AOI 判定 / 日志，客户端自己有真实曲线）。</summary>
|
||
internal static void PositionOf(in DriveState s, out float x, out float y, out float z)
|
||
{
|
||
var table = Cache.Value;
|
||
if (!table.Lanes.TryGetValue(s.Lane, out var lane))
|
||
{
|
||
x = y = z = 0f;
|
||
return;
|
||
}
|
||
var t = Math.Clamp(s.Distance / lane.Length, 0f, 1f);
|
||
x = lane.Bx + (lane.Ex - lane.Bx) * t;
|
||
y = lane.By + (lane.Ey - lane.By) * t;
|
||
z = lane.Bz + (lane.Ez - lane.Bz) * t;
|
||
}
|
||
|
||
/// <summary>距当前车道末端还有多少米（用于「接近路口就减速」）。</summary>
|
||
internal static float RemainOf(in DriveState s)
|
||
{
|
||
var table = Cache.Value;
|
||
if (!table.Lanes.TryGetValue(s.Lane, out var lane))
|
||
return 0f;
|
||
return MathF.Max(0f, lane.Length - s.Distance);
|
||
}
|
||
|
||
/// <summary>当前车道的限速（m/s）。车换道后会变，所以每 tick 都要重取。</summary>
|
||
internal static float LimitOf(in DriveState s)
|
||
{
|
||
var table = Cache.Value;
|
||
return table.Lanes.TryGetValue(s.Lane, out var lane) ? lane.Speed : 9.72f;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 朝向 <b>弧度</b>（0 = +Z，顺时针为正），由车道端点方向推出。
|
||
/// ★ 2026-09-21 修正：原先这里返回「度」却被直接填进 ClientVehicleInitData.Facing，
|
||
/// 实测线上包里 Facing = -88.5 —— 那是度数当弧度用了。Facing 是弧度，
|
||
/// EulerAngles 才是度（调用处各自换算）。
|
||
/// </summary>
|
||
internal static float FacingOf(in DriveState s)
|
||
{
|
||
var table = Cache.Value;
|
||
if (!table.Lanes.TryGetValue(s.Lane, out var lane))
|
||
return 0f;
|
||
var dx = lane.Ex - lane.Bx;
|
||
var dz = lane.Ez - lane.Bz;
|
||
if (MathF.Abs(dx) < 1e-4f && MathF.Abs(dz) < 1e-4f)
|
||
return 0f;
|
||
// ★ 单位 = 度，与已生效的 SyncAetherAICrowdAdd 一致（那里 Facing 与 EulerAngles.y
|
||
// 填的是同一个值）。曾误改为弧度，导致下发 -88.5 被当成弧度（-5070°）。
|
||
return MathF.Atan2(dx, dz) * 180f / MathF.PI;
|
||
}
|
||
}
|