磁导航1.0内部交管和信号交互
This commit is contained in:
ykkokluo
@@ -0,0 +1,83 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using Newtonsoft.Json.Linq;
|
||||
|
||||
namespace StandardScene.MagCarSimulator
|
||||
{
|
||||
/// <summary>读取 mag-control-areas.json，收集触发点与管控区站点。</summary>
|
||||
public static class MagControlAreaFile
|
||||
{
|
||||
private static readonly char[] SiteSeparators = { ',', ';', '|', ' ', '\t' };
|
||||
|
||||
public static HashSet<int> LoadSiteIds(string path)
|
||||
{
|
||||
var ids = new HashSet<int>();
|
||||
if (string.IsNullOrWhiteSpace(path) || !File.Exists(path))
|
||||
{
|
||||
return ids;
|
||||
}
|
||||
|
||||
var root = JToken.Parse(File.ReadAllText(path));
|
||||
if (root is not JArray rows)
|
||||
{
|
||||
return ids;
|
||||
}
|
||||
|
||||
foreach (var row in rows)
|
||||
{
|
||||
if (row is not JObject obj)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (obj.Value<bool?>("IsUse") == false)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
AddIfPositive(ids, obj.Value<int?>("TriggerSit") ?? 0);
|
||||
foreach (var part in (obj.Value<string>("ControlArea") ?? "").Split(SiteSeparators, StringSplitOptions.RemoveEmptyEntries))
|
||||
{
|
||||
if (int.TryParse(part.Trim(), NumberStyles.Integer, CultureInfo.InvariantCulture, out var id))
|
||||
{
|
||||
AddIfPositive(ids, id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ids;
|
||||
}
|
||||
|
||||
public static string GuessPath(string mapPath, string configuredPath)
|
||||
{
|
||||
if (!string.IsNullOrWhiteSpace(configuredPath) && File.Exists(configuredPath))
|
||||
{
|
||||
return Path.GetFullPath(configuredPath);
|
||||
}
|
||||
|
||||
if (string.IsNullOrWhiteSpace(mapPath))
|
||||
{
|
||||
return configuredPath;
|
||||
}
|
||||
|
||||
var mapDir = Path.GetDirectoryName(Path.GetFullPath(mapPath));
|
||||
if (string.IsNullOrWhiteSpace(mapDir))
|
||||
{
|
||||
return configuredPath;
|
||||
}
|
||||
|
||||
var guessed = Path.GetFullPath(Path.Combine(mapDir, "..", "config", "Signal", "mag-control-areas.json"));
|
||||
return File.Exists(guessed) ? guessed : configuredPath;
|
||||
}
|
||||
|
||||
private static void AddIfPositive(HashSet<int> ids, int id)
|
||||
{
|
||||
if (id > 0)
|
||||
{
|
||||
ids.Add(id);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,213 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace StandardScene.MagCarSimulator
|
||||
{
|
||||
public static class MapPathFinder
|
||||
{
|
||||
/// <summary>
|
||||
/// 生成往返循环：起点→终点→起点（终点不重复，回到起点靠下标回绕）。
|
||||
/// </summary>
|
||||
public static List<int> BuildLoop(SimpleLiteMap map, int startSiteId, int endSiteId)
|
||||
{
|
||||
if (map == null)
|
||||
{
|
||||
throw new System.InvalidOperationException("尚未加载地图");
|
||||
}
|
||||
|
||||
if (startSiteId == endSiteId)
|
||||
{
|
||||
throw new System.InvalidOperationException($"起点和终点不能相同（站点 {startSiteId}）");
|
||||
}
|
||||
|
||||
if (!map.Sites.ContainsKey(startSiteId))
|
||||
{
|
||||
throw new System.InvalidOperationException($"地图中没有起点 {startSiteId}");
|
||||
}
|
||||
|
||||
if (!map.Sites.ContainsKey(endSiteId))
|
||||
{
|
||||
throw new System.InvalidOperationException($"地图中没有终点 {endSiteId}");
|
||||
}
|
||||
|
||||
var forward = FindPath(map, startSiteId, endSiteId);
|
||||
if (forward == null)
|
||||
{
|
||||
throw new System.InvalidOperationException($"找不到路径 {startSiteId} → {endSiteId}");
|
||||
}
|
||||
|
||||
var back = FindPath(map, endSiteId, startSiteId);
|
||||
if (back == null)
|
||||
{
|
||||
throw new System.InvalidOperationException($"找不到返回路径 {endSiteId} → {startSiteId}");
|
||||
}
|
||||
|
||||
var loop = new List<int>(forward.Count + back.Count);
|
||||
loop.AddRange(forward);
|
||||
for (var i = 1; i < back.Count - 1; i++)
|
||||
{
|
||||
loop.Add(back[i]);
|
||||
}
|
||||
|
||||
if (loop.Count < 2)
|
||||
{
|
||||
throw new System.InvalidOperationException("循环路径至少需要两个站点");
|
||||
}
|
||||
|
||||
return loop;
|
||||
}
|
||||
|
||||
public static List<int> NormalizeLoop(IEnumerable<int> siteIds)
|
||||
{
|
||||
var loop = new List<int>();
|
||||
if (siteIds == null)
|
||||
{
|
||||
return loop;
|
||||
}
|
||||
|
||||
foreach (var id in siteIds)
|
||||
{
|
||||
if (id <= 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (loop.Count > 0 && loop[loop.Count - 1] == id)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
loop.Add(id);
|
||||
}
|
||||
|
||||
if (loop.Count >= 2 && loop[0] == loop[loop.Count - 1])
|
||||
{
|
||||
loop.RemoveAt(loop.Count - 1);
|
||||
}
|
||||
|
||||
return loop;
|
||||
}
|
||||
|
||||
public static List<int> ResolveLoop(SimpleLiteMap map, MagCarSimVehicleConfig config)
|
||||
{
|
||||
if (map == null)
|
||||
{
|
||||
throw new System.InvalidOperationException("尚未加载地图");
|
||||
}
|
||||
|
||||
if (config?.LoopSiteIds != null && config.LoopSiteIds.Count >= 3)
|
||||
{
|
||||
var loop = NormalizeLoop(config.LoopSiteIds);
|
||||
if (loop.Count < 3)
|
||||
{
|
||||
throw new System.InvalidOperationException($"{config.Name} 环线站点不足");
|
||||
}
|
||||
|
||||
foreach (var id in loop)
|
||||
{
|
||||
if (!map.Sites.ContainsKey(id))
|
||||
{
|
||||
throw new System.InvalidOperationException($"{config.Name} 环线站点 {id} 不在地图中");
|
||||
}
|
||||
}
|
||||
|
||||
return loop;
|
||||
}
|
||||
|
||||
return BuildLoop(map, config.StartSiteId, config.EndSiteId);
|
||||
}
|
||||
|
||||
public static string FormatPath(IReadOnlyList<int> siteIds, int maxShow = 0)
|
||||
{
|
||||
if (siteIds == null || siteIds.Count == 0)
|
||||
{
|
||||
return "";
|
||||
}
|
||||
|
||||
var show = siteIds.Count;
|
||||
var truncated = false;
|
||||
if (maxShow > 0 && siteIds.Count > maxShow)
|
||||
{
|
||||
show = maxShow;
|
||||
truncated = true;
|
||||
}
|
||||
|
||||
var sb = new StringBuilder();
|
||||
for (var i = 0; i < show; i++)
|
||||
{
|
||||
if (i > 0)
|
||||
{
|
||||
sb.Append("→");
|
||||
}
|
||||
|
||||
sb.Append(siteIds[i]);
|
||||
}
|
||||
|
||||
if (truncated)
|
||||
{
|
||||
sb.Append("→…共").Append(siteIds.Count).Append("站");
|
||||
}
|
||||
else
|
||||
{
|
||||
sb.Append("→").Append(siteIds[0]);
|
||||
}
|
||||
|
||||
return sb.ToString();
|
||||
}
|
||||
|
||||
public static List<int> FindPath(SimpleLiteMap map, int fromSiteId, int toSiteId)
|
||||
{
|
||||
if (fromSiteId == toSiteId)
|
||||
{
|
||||
return new List<int> { fromSiteId };
|
||||
}
|
||||
|
||||
var queue = new Queue<int>();
|
||||
var prev = new Dictionary<int, int>();
|
||||
var visited = new HashSet<int> { fromSiteId };
|
||||
queue.Enqueue(fromSiteId);
|
||||
|
||||
while (queue.Count > 0)
|
||||
{
|
||||
var current = queue.Dequeue();
|
||||
if (!map.Adjacency.TryGetValue(current, out var nexts))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
foreach (var next in nexts)
|
||||
{
|
||||
if (!visited.Add(next))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
prev[next] = current;
|
||||
if (next == toSiteId)
|
||||
{
|
||||
return Reconstruct(prev, fromSiteId, toSiteId);
|
||||
}
|
||||
|
||||
queue.Enqueue(next);
|
||||
}
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
private static List<int> Reconstruct(Dictionary<int, int> prev, int fromSiteId, int toSiteId)
|
||||
{
|
||||
var path = new List<int>();
|
||||
var current = toSiteId;
|
||||
path.Add(current);
|
||||
while (current != fromSiteId)
|
||||
{
|
||||
current = prev[current];
|
||||
path.Add(current);
|
||||
}
|
||||
|
||||
path.Reverse();
|
||||
return path;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,324 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using Newtonsoft.Json.Linq;
|
||||
|
||||
namespace StandardScene.MagCarSimulator
|
||||
{
|
||||
public sealed class SimpleLiteSite
|
||||
{
|
||||
public int Id { get; set; }
|
||||
public string Name { get; set; }
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public int? TagValue { get; set; }
|
||||
public bool NeedStop { get; set; }
|
||||
public IReadOnlyDictionary<string, string> Fields { get; set; }
|
||||
}
|
||||
|
||||
public sealed class SimpleLiteTrack
|
||||
{
|
||||
public int Id { get; set; }
|
||||
public int SiteA { get; set; }
|
||||
public int SiteB { get; set; }
|
||||
public int Direction { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>解析 SimpleLite <c>maps/*.json</c> 的 Sites / Tracks。</summary>
|
||||
public sealed class SimpleLiteMap
|
||||
{
|
||||
public const string NeedStopField = "Mag_NeedStop";
|
||||
|
||||
public string FilePath { get; private set; }
|
||||
public string FileName { get; private set; }
|
||||
public IReadOnlyDictionary<int, SimpleLiteSite> Sites { get; private set; }
|
||||
public IReadOnlyList<SimpleLiteTrack> Tracks { get; private set; }
|
||||
public IReadOnlyDictionary<int, IReadOnlyList<int>> Adjacency { get; private set; }
|
||||
private readonly HashSet<int> _singleOccupancySites = new HashSet<int>();
|
||||
|
||||
public static SimpleLiteMap Load(string path, bool useTagValueAsNode)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(path) || !File.Exists(path))
|
||||
{
|
||||
throw new FileNotFoundException("找不到 SimpleLite 地图文件", path);
|
||||
}
|
||||
|
||||
var root = JObject.Parse(File.ReadAllText(path));
|
||||
var sites = ParseSites(root["Sites"] as JObject);
|
||||
var tracks = ParseTracks(root["Tracks"] as JObject);
|
||||
var adjacency = BuildAdjacency(sites, tracks);
|
||||
|
||||
var map = new SimpleLiteMap
|
||||
{
|
||||
FilePath = Path.GetFullPath(path),
|
||||
FileName = Path.GetFileName(path),
|
||||
Sites = sites,
|
||||
Tracks = tracks,
|
||||
Adjacency = adjacency,
|
||||
UseTagValueAsNode = useTagValueAsNode
|
||||
};
|
||||
map.ResetSingleOccupancyFromStops();
|
||||
return map;
|
||||
}
|
||||
|
||||
public bool UseTagValueAsNode { get; private set; }
|
||||
|
||||
public bool TryGetSite(int siteId, out SimpleLiteSite site)
|
||||
{
|
||||
return Sites.TryGetValue(siteId, out site);
|
||||
}
|
||||
|
||||
public ushort ResolveNode(int siteId)
|
||||
{
|
||||
if (!Sites.TryGetValue(siteId, out var site))
|
||||
{
|
||||
return (ushort)Math.Clamp(siteId, 0, ushort.MaxValue);
|
||||
}
|
||||
|
||||
if (UseTagValueAsNode && site.TagValue.HasValue && site.TagValue.Value >= 0)
|
||||
{
|
||||
return (ushort)Math.Clamp(site.TagValue.Value, 0, ushort.MaxValue);
|
||||
}
|
||||
|
||||
return (ushort)Math.Clamp(site.Id, 0, ushort.MaxValue);
|
||||
}
|
||||
|
||||
public bool NeedStop(int siteId)
|
||||
{
|
||||
return Sites.TryGetValue(siteId, out var site) && site.NeedStop;
|
||||
}
|
||||
|
||||
/// <summary>停止点或交管点：同时只允许一辆。普通站可叠车。</summary>
|
||||
public bool SingleOccupancy(int siteId)
|
||||
{
|
||||
return _singleOccupancySites.Contains(siteId);
|
||||
}
|
||||
|
||||
public IReadOnlyList<int> ListSingleOccupancySiteIds()
|
||||
{
|
||||
var list = new List<int>(_singleOccupancySites);
|
||||
list.Sort();
|
||||
return list;
|
||||
}
|
||||
|
||||
public void ResetSingleOccupancyFromStops()
|
||||
{
|
||||
_singleOccupancySites.Clear();
|
||||
foreach (var site in Sites.Values)
|
||||
{
|
||||
if (site.NeedStop)
|
||||
{
|
||||
_singleOccupancySites.Add(site.Id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void AddSingleOccupancySites(IEnumerable<int> siteIds)
|
||||
{
|
||||
if (siteIds == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
foreach (var id in siteIds)
|
||||
{
|
||||
if (id > 0 && Sites.ContainsKey(id))
|
||||
{
|
||||
_singleOccupancySites.Add(id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public IReadOnlyList<int> ListNeedStopSiteIds()
|
||||
{
|
||||
var list = new List<int>();
|
||||
foreach (var site in Sites.Values)
|
||||
{
|
||||
if (site.NeedStop)
|
||||
{
|
||||
list.Add(site.Id);
|
||||
}
|
||||
}
|
||||
|
||||
list.Sort();
|
||||
return list;
|
||||
}
|
||||
|
||||
private static Dictionary<int, SimpleLiteSite> ParseSites(JObject sitesNode)
|
||||
{
|
||||
var result = new Dictionary<int, SimpleLiteSite>();
|
||||
if (sitesNode == null)
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
foreach (var property in sitesNode.Properties())
|
||||
{
|
||||
if (property.Value is not JObject obj)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var id = obj.Value<int?>("id") ?? ParseInt(property.Name);
|
||||
if (id <= 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var fields = ReadFields(obj["fields"] as JObject);
|
||||
var site = new SimpleLiteSite
|
||||
{
|
||||
Id = id,
|
||||
Name = obj.Value<string>("name") ?? "",
|
||||
X = obj.Value<double?>("x") ?? 0,
|
||||
Y = obj.Value<double?>("y") ?? 0,
|
||||
TagValue = TryReadInt(fields, "TagValue"),
|
||||
NeedStop = IsNeedStop(fields),
|
||||
Fields = fields
|
||||
};
|
||||
result[id] = site;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
private static List<SimpleLiteTrack> ParseTracks(JObject tracksNode)
|
||||
{
|
||||
var result = new List<SimpleLiteTrack>();
|
||||
if (tracksNode == null)
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
foreach (var property in tracksNode.Properties())
|
||||
{
|
||||
if (property.Value is not JObject obj)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var siteA = obj.Value<int?>("siteA") ?? obj.Value<int?>("_siteA") ?? 0;
|
||||
var siteB = obj.Value<int?>("siteB") ?? obj.Value<int?>("_siteB") ?? 0;
|
||||
if (siteA <= 0 || siteB <= 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
result.Add(new SimpleLiteTrack
|
||||
{
|
||||
Id = obj.Value<int?>("id") ?? ParseInt(property.Name),
|
||||
SiteA = siteA,
|
||||
SiteB = siteB,
|
||||
Direction = obj.Value<int?>("direction") ?? 0
|
||||
});
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
private static Dictionary<int, IReadOnlyList<int>> BuildAdjacency(
|
||||
Dictionary<int, SimpleLiteSite> sites,
|
||||
List<SimpleLiteTrack> tracks)
|
||||
{
|
||||
var mutable = new Dictionary<int, List<int>>();
|
||||
foreach (var id in sites.Keys)
|
||||
{
|
||||
mutable[id] = new List<int>();
|
||||
}
|
||||
|
||||
foreach (var track in tracks)
|
||||
{
|
||||
AddEdge(mutable, track.SiteA, track.SiteB, track.Direction);
|
||||
}
|
||||
|
||||
var result = new Dictionary<int, IReadOnlyList<int>>();
|
||||
foreach (var pair in mutable)
|
||||
{
|
||||
result[pair.Key] = pair.Value;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
private static void AddEdge(Dictionary<int, List<int>> graph, int from, int to, int direction)
|
||||
{
|
||||
// 0 双向；1 仅 A→B；2 仅 B→A。其它值按双向处理。
|
||||
var aToB = direction != 2;
|
||||
var bToA = direction != 1;
|
||||
if (aToB)
|
||||
{
|
||||
AddUnique(graph, from, to);
|
||||
}
|
||||
|
||||
if (bToA)
|
||||
{
|
||||
AddUnique(graph, to, from);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AddUnique(Dictionary<int, List<int>> graph, int from, int to)
|
||||
{
|
||||
if (!graph.TryGetValue(from, out var list))
|
||||
{
|
||||
list = new List<int>();
|
||||
graph[from] = list;
|
||||
}
|
||||
|
||||
if (!list.Contains(to))
|
||||
{
|
||||
list.Add(to);
|
||||
}
|
||||
}
|
||||
|
||||
private static Dictionary<string, string> ReadFields(JObject fields)
|
||||
{
|
||||
var result = new Dictionary<string, string>(StringComparer.OrdinalIgnoreCase);
|
||||
if (fields == null)
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
foreach (var property in fields.Properties())
|
||||
{
|
||||
result[property.Name] = property.Value?.Type == JTokenType.Null
|
||||
? ""
|
||||
: property.Value.ToString();
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
private static bool IsNeedStop(Dictionary<string, string> fields)
|
||||
{
|
||||
if (!fields.TryGetValue(NeedStopField, out var raw))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
raw = (raw ?? "").Trim();
|
||||
return raw.Equals("true", StringComparison.OrdinalIgnoreCase)
|
||||
|| raw.Equals("1", StringComparison.OrdinalIgnoreCase)
|
||||
|| raw.Equals("yes", StringComparison.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
private static int? TryReadInt(Dictionary<string, string> fields, string key)
|
||||
{
|
||||
if (!fields.TryGetValue(key, out var raw) || string.IsNullOrWhiteSpace(raw))
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
return int.TryParse(raw, NumberStyles.Integer, CultureInfo.InvariantCulture, out var value)
|
||||
? value
|
||||
: (int?)null;
|
||||
}
|
||||
|
||||
private static int ParseInt(string text)
|
||||
{
|
||||
return int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out var value)
|
||||
? value
|
||||
: 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user