GeneralPathfinder.cs
4.11 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
using HaHRCS.Core.Models;
using Rcs.Domain.Entities;
namespace Rcs.Infrastructure.Shared.Algorithms
{
public class GeneralPathfinder
{
private readonly Dictionary<Guid, MapNode> _MapNodes;
private readonly Dictionary<Guid, List<Guid>> _graph;
// 新增存储边信息的字典
private readonly Dictionary<(Guid, Guid), MapEdge> _MapEdges;
public GeneralPathfinder(List<MapNode> MapNodes, List<MapEdge> MapEdges)
{
_MapNodes = MapNodes.ToDictionary(n => n.NodeId, n => n);
_graph = new Dictionary<Guid, List<Guid>>();
_MapEdges = new Dictionary<(Guid, Guid), MapEdge>();
foreach (var MapEdge in MapEdges)
{
if (!_graph.ContainsKey(MapEdge.FromNode))
_graph[MapEdge.FromNode] = new List<Guid>();
_graph[MapEdge.FromNode].Add(MapEdge.ToNode);
// 存储边信息，仅存储单向
_MapEdges[(MapEdge.FromNode, MapEdge.ToNode)] = MapEdge;
}
}
private double Heuristic(MapNode a, MapNode b)
{
// 使用欧几里得距离
return Math.Sqrt(Math.Pow(a.X - b.X, 2) +
Math.Pow(a.Y - b.Y, 2));
}
public AgvPathResult FindPath(Guid startId, Guid goalId)
{
var openSet = new HashSet<Guid> { startId };
var cameFrom = new Dictionary<Guid, Guid>();
var gScore = _MapNodes.Keys.ToDictionary(k => k, k => double.PositiveInfinity);
gScore[startId] = 0;
var fScore = _MapNodes.Keys.ToDictionary(k => k, k => double.PositiveInfinity);
fScore[startId] = Heuristic(_MapNodes[startId], _MapNodes[goalId]);
while (openSet.Count > 0)
{
var current = openSet.OrderBy(n => fScore[n]).First();
if (current == goalId)
{
var MapNodePath = ReconstructMapNodePath(cameFrom, current);
var MapEdgePath = ReconstructMapEdgePath(MapNodePath);
return new AgvPathResult() {
Success = true,
Edges = MapEdgePath,
Nodes = MapNodePath
};
}
openSet.Remove(current);
foreach (var neighbor in _graph.GetValueOrDefault(current, new List<Guid>()))
{
double tentativeGScore = gScore[current] + Heuristic(_MapNodes[current], _MapNodes[neighbor]);
if (tentativeGScore < gScore[neighbor])
{
cameFrom[neighbor] = current;
gScore[neighbor] = tentativeGScore;
fScore[neighbor] = gScore[neighbor] + Heuristic(_MapNodes[neighbor], _MapNodes[goalId]);
if (!openSet.Contains(neighbor))
openSet.Add(neighbor);
}
}
}
return new AgvPathResult()
{
Success = false,
FailReason = "无路径到达目标点"
};
}
private List<MapNode> ReconstructMapNodePath(Dictionary<Guid, Guid> cameFrom, Guid current)
{
var totalPath = new List<Guid> { current };
while (cameFrom.ContainsKey(current))
{
current = cameFrom[current];
totalPath.Insert(0, current);
}
return totalPath.Select(id => _MapNodes[id]).ToList();
}
private List<MapEdge> ReconstructMapEdgePath(List<MapNode> MapNodePath)
{
var MapEdgePath = new List<MapEdge>();
for (int i = 0; i < MapNodePath.Count - 1; i++)
{
var startId = MapNodePath[i].NodeId;
var endId = MapNodePath[i + 1].NodeId;
if (_MapEdges.TryGetValue((startId, endId), out var MapEdge))
{
MapEdgePath.Add(MapEdge);
}
}
return MapEdgePath;
}
}
}