public TW[] Dijkstra(TW maxWeight, int startIndex = 0)
{
Reset();
TW[] weights = new TW[N];
for (int i = 0; i < N; i++)
{
weights[i] = maxWeight;
}
weights[startIndex] = default(TW);
Status(startIndex, VStatus.Discovered);
IPriorityQueue <DijkstraNode> nodes = BuildDijkstraNodes(weights);
while (nodes.Count != 0)
{
DijkstraNode node = nodes.DelMax();
Status(node.Index, VStatus.Visited);
for (int u = FirstNbr(node.Index); -1 < u; u = NextNbr(node.Index, u))
{
if (Status(u) != VStatus.Visited)
{
Status(u, VStatus.Discovered);
if (Sum(weights[node.Index], Weight(node.Index, u)).CompareTo(weights[u]) < 0)
{
weights[u] = Sum(weights[node.Index], Weight(node.Index, u));
}
}
}
nodes = BuildDijkstraNodes(weights);
}
return(weights);
}
private static List <DijkstraNode> DijkstraReccursive(DijkstraNode source, DijkstraNode dest, List <DijkstraNode> currentPath, List <DijkstraNode> finalPath, Graph g)
{
if (source == dest)
{
//ending point
finalPath.Clear();
foreach (var r in currentPath)
{
finalPath.Add(r);
}
return(finalPath);
}
foreach (var r in source.OutgoingEdges)
{
float alt = source.value + r.Weight;
DijkstraNode edgeDest = g.GetNode(r.Destination) as DijkstraNode;
if (alt < edgeDest.value)
{
edgeDest.value = alt;
currentPath.Add(edgeDest);
finalPath = DijkstraReccursive(edgeDest, dest, currentPath, finalPath, g);
currentPath.RemoveAt(currentPath.Count - 1);
}
}
return(finalPath); //useless but whatever
//maybe could throw exception when not found a path
}
public DijkstraNode(Coord position, DijkstraNode parent = null)
{
Position = position;
Parent = parent;
Position = position;
Distance = float.MaxValue;
}
/// 未確定ノードの中で最短経路をもつノードを検索
/// 関数で使う関数
/// return 最短経路をもつノード
private DijkstraNode FindMinNode()
{
double dMinDistance = int.MaxValue; // 最小値を最初無限大とする
DijkstraNode Finder = null;
// 全てのノードをチェック
foreach (DijkstraNode node in _nodes)
{
// 確定したノードは無視
if (node.Status == DijkstraNode.NodeStatus.Completed)
{
continue;
}
// 未確定のノードの中で最短距離のノードを探す
if (node.Distance >= dMinDistance)
{
continue;
}
dMinDistance = node.Distance;
Finder = node;
}
if (Finder == null)
{
return(null);
}
// 最短距離を見つけた。このノードは確定！
Finder.Status = DijkstraNode.NodeStatus.Completed;
return(Finder);
}
public DijkstraLink(string linkid, DijkstraNode node1, DijkstraNode node2)
{
linkID = linkid;
Node1 = node1;
Node2 = node2;
LinkDistance = Mathf.Sqrt(Mathf.Pow(node1.x - node2.x, 2f) + Mathf.Pow(node1.y - node2.y, 2f));
}
/// <summary>
/// simplification: call Dijkstra algorithm from instance of Graph
/// </summary>
/// <param name="graph"> source graph </param>
/// <param name="source"> source node </param>
/// <returns> graph with filled "distances" and "prevs" of every node </returns>
public static Graph FindDijkstraShortestPathsFrom(this Graph graph, DijkstraNode source)
{
var dijkstra = new DijkstraAlgorhitm();
dijkstra.Execute(graph, source);
return(graph);
}
public void Initialize(IFactory <GameObject, Transform, Vector2, Node> _nodeFactory, DijkstraNode _dijkstraNodePrefab, AStarNode _astarNodePrefab, SignalBus _signalBus, GameDataContainer _gameDataContainer)
{
dijkstraNodePrefab = _dijkstraNodePrefab;
gameDataContainer = _gameDataContainer;
astarNodePrefab = _astarNodePrefab;
nodeFactory = _nodeFactory;
signalBus = _signalBus;
}
private static string GetShortestPathsAsString(Graph graph, DijkstraNode source)
{
var nodes = graph.AdjDict.GetVertices();
var distances = nodes.Select(node => $"to {node.Name}= {node.Distance}");
string formattedDistances = string.Join(", ", distances);
return($"source is {source.Name}. {formattedDistances}");
}
public Result FindPath(DijkstraServiceModel model)
{
if (!ValidationUtil.IsObjectValid(model))
{
return(null);
}
this.startNode = model.StartNode;
this.endNode = model.EndNode;
var grid = model.Grid;
var heap = new MinHeap <DijkstraNode>();
var allSteps = new HashSet <INode>();
this.startNode.Distance = 0;
this.startNode.IsVisited = true;
heap.Add(this.startNode);
while (heap.Count != 0)
{
var currentNode = heap.Pop();
if (grid[currentNode.Row, currentNode.Col].NodeType == NodeType.Wall)
{
continue;
}
// Destination target found
if (currentNode.Equals(this.endNode))
{
allSteps.Remove(allSteps.ElementAt(0));
return(new Result(allSteps, this.GetAllNodesInShortestPathOrder(currentNode)));
}
allSteps.Add(currentNode);
var rowDirection = new[] { -1, +1, 0, 0 };
var columnDirection = new[] { 0, 0, +1, -1 };
for (var i = 0; i < 4; i++)
{
var currentRowDirection = currentNode.Row + rowDirection[i];
var currentColDirection = currentNode.Col + columnDirection[i];
if (currentRowDirection < 0 ||
currentColDirection < 0 ||
currentRowDirection >= grid.GetLength(0) ||
currentColDirection >= grid.GetLength(1))
{
continue;
}
var targetNode = grid[currentRowDirection, currentColDirection];
this.AddNodeToHeap(currentNode, targetNode, heap);
}
}
return(new Result(allSteps));
}
/*
* 最短経路探索を実行するメソッド
* return 探索回数
*/
public int Execute(DijkstraNode startNode, DijkstraNode endNode)
{
StartNode = startNode;
EndNode = endNode;
if (startNode == null)
{
return(0);
}
// 全節点で距離を無限大，未確定とする
foreach (DijkstraNode node in ListMaster.dijkstraNodeList)
{
node.Distance = int.MaxValue;
node.Status = DijkstraNode.NodeStatus.NotYet;
node.SourceNode = null;
node.linkList = ListMaster.dijkstraLinkList;
//To Do startNodeとEndNodeがListMasterとは別物になってしまっているため、ListMaster内に組み込む
if (startNode.x == node.x && startNode.y == node.y)
{
startNode = node;
}
if (EndNode.x == node.x && EndNode.y == node.y)
{
EndNode = node;
}
}
// 始点では距離はゼロ，確定とする
startNode.Distance = 0;
startNode.Status = DijkstraNode.NodeStatus.Completed;
nodeCompList.Add(startNode);
startNode.SourceNode = null;
//To Do 先ほどのToDo同様、startNodeとEndNodeをListMaster内に組み込む
foreach (DijkstraNode node in ListMaster.dijkstraNodeList)
{
if (startNode.x == node.x && startNode.y == node.y)
{
node.Distance = 0;
node.Status = DijkstraNode.NodeStatus.Completed;
node.SourceNode = null;
break;
}
}
DijkstraNode scanNode = startNode;
int nCount = 0;
while (scanNode != null && EndNode.Status != DijkstraNode.NodeStatus.Completed)
{
UpdateNodeProp(scanNode); // 隣接点のノードを更新
scanNode = FindMinNode(); // 最短経路をもつノードを検索
nCount++;
}
return(nCount);
}
public DijkstraNode(Node node, Node start, Node end)
{
this.node = node;
visited = false;
startNode = node == start ? true : false;
endNode = node == end ? true : false;
distance = startNode ? 0f : float.MaxValue;
parent = null;
}
private static void Dijkstra(NodeNetCreator net, DijkstraNode start, DijkstraNode end)
{
DijkstraNode currentNode = start;
while (currentNode != end)
{
UpdateDistances(net, currentNode);
currentNode = SetNewCurrentNode(net, currentNode);
}
}
private void Fill()
{
DijkstraNode curNode = this.nodes[target.x, target.y];
while (curNode.tDist < int.MaxValue)
{
this.Propagate(curNode);
curNode = this.NextNode();
}
}
/// <summary>
/// Dijkstra initialization
/// </summary>
/// <param name="graph"> source graph </param>
/// <param name="source"> source node </param>
/// <param name="distances"> empty priority queue </param>
private void Initialize
(Graph graph, DijkstraNode source, DijkstraPriorityQueue distances)
{
source.Distance = 0;
var vertices = graph.AdjDict.GetVertices();
foreach (var v in vertices)
{
distances.Insert(v, v.Distance);
}
}
public Coord GetClosestAvailablePoint(Coord coord)
{
FastPriorityQueue <DijkstraNode> frontier = new FastPriorityQueue <DijkstraNode>(AvailabilityMap.Height * AvailabilityMap.Width);
DijkstraNode[] explored = new DijkstraNode[AvailabilityMap.Height * AvailabilityMap.Width];
AdjacencyRule adjacencyRule = Distance;
DijkstraNode initial = new DijkstraNode(coord, null);
initial.Distance = 0;
frontier.Enqueue(initial, 0);
while (frontier.Count > 0)
{
DijkstraNode current = frontier.Dequeue();
if (AvailabilityMap[current.Position])
{
return(current.Position);
}
foreach (Coord neighbor in adjacencyRule.Neighbors(current.Position))
{
if (!WalkabilityMap[neighbor])
{
continue;
}
int neighborIndex = neighbor.ToIndex(AvailabilityMap.Width);
bool neighborExplored = explored[neighborIndex] != null;
float neighborDistance = (float)Distance.Calculate(current.Position, neighbor) + current.Distance;
DijkstraNode neighborNode =
neighborExplored ? explored[neighborIndex] : new DijkstraNode(neighbor, current);
explored[neighborIndex] = neighborNode;
if (neighborExplored && neighborDistance < neighborNode.Distance)
{
neighborNode.Distance = neighborDistance;
frontier.UpdatePriority(neighborNode, neighborNode.Distance);
}
else
{
neighborNode.Distance = neighborDistance;
frontier.Enqueue(neighborNode, neighborDistance);
}
}
}
return(Coord.NONE);
}
/// <summary>
/// find shortest distance to vertice v
/// between known distance (v.Distance)
/// and from u across edge (u, v)
/// </summary>
/// <param name="u"> source node </param>
/// <param name="v"> adjacent node </param>
/// <param name="weight"> weith of edge between u and v </param>
/// <param name="queue"> pririty queue with Dijkstra nodes </param>
private void Relax(DijkstraNode u, DijkstraNode v, int weight,
DijkstraPriorityQueue queue)
{
var distanceThroughU = u.Distance + weight;
if (distanceThroughU < v.Distance)
{
v.Distance = distanceThroughU;
v.Prev = u;
queue.ChangePriority(v, distanceThroughU);
}
}
private static DijkstraNode[][] InitializeDijkstraNodes(bool[][] fields)
{
int sizeX = fields.Length;
int sizeY = fields[0].Length;
var nodes = new DijkstraNode[sizeX][];
for (int x = 0; x < sizeX; x++)
{
nodes[x] = new DijkstraNode[sizeY];
}
return(nodes);
}
/// <summary>
/// See <see cref="BaseGraphSearchPathfinder{TNode,TMap}.OnPerformAlgorithm"/> for more details.
/// </summary>
protected override void OnPerformAlgorithm(DijkstraNode currentNode, DijkstraNode neighborNode, Point neighborPoint, Point endPoint, StopFunction stopFunction)
{
int neighborScore = HeuristicHelper.FastEuclideanDistance(neighborPoint, endPoint);
if (neighborNode == null)
{
Map.OpenNode(neighborPoint, currentNode, neighborScore);
}
else if (neighborScore < neighborNode.Score)
{
neighborNode.Update(neighborScore, currentNode);
}
}
private string GetPathAsString(Graph graph, DijkstraNode source, DijkstraNode destination)
{
var pathNodes = new Stack <string>();
var currentNode = destination;
while (currentNode != null && !currentNode.Equals(source.Prev)) // it would be null if currentNode is source
{
pathNodes.Push(currentNode.Name);
currentNode = currentNode.Prev;
}
return(string.Join(" -> ", pathNodes));
}
/// コンストラクタ
/// nNodeCount => 全ノード数
/// class Dijkstrの初期化関数
public Dijkstra()
{
int count = 8;
_nodes = new List <DijkstraNode>();
for (int i = 0; i < count * count; i++) //////ここにnodeの個数を入れる
{
var c = new DijkstraNode
{
nodeNumber = i
};
_nodes.Add(c);
}
}
/// <summary>
/// Function which realises Dijkstra algorith.
/// </summary>
/// <param name="sID">source ID</param>
/// <param name="dID">destination ID</param>
/// <param name="g">Graph</param>
/// <returns>the path from source to destination</returns>
public static List <Node> DijkstraAlgorithm(int sID, int dID, Graph g)
{
Graph g2 = PrepareGraphForDijkstra(g);
DijkstraNode source = g2.GetNode(sID) as DijkstraNode;
DijkstraNode destination = g2.GetNode(dID) as DijkstraNode;
source.value = 0;
List <DijkstraNode> finalPath = new List <DijkstraNode>();
List <DijkstraNode> currentPath = new List <DijkstraNode>();
currentPath.Add(source);
finalPath = DijkstraReccursive(source, destination, currentPath, finalPath, g2);
return(DijkstraConvertingLists(finalPath));
}
/*
*指定ノードに隣接するノードの最短距離を計算するメソッド
* 引数 指定ノード
*/
public void UpdateNodeProp(DijkstraNode sourceNode)
{
if (sourceNode == null)
{
return;
}
DijkstraNode destinationNode;
float dTotalDistance;
//リンクリストの中から指定ノードに関連しているものを検索
foreach (DijkstraLink link in ListMaster.dijkstraLinkList)
{
destinationNode = null;
//link内にsourceNodeがあるか判別する処理
if (link.Node1.x == sourceNode.x && link.Node1.y == sourceNode.y)
{
link.flag = 1;
destinationNode = link.Node2;
}
else if (link.Node2.x == sourceNode.x && link.Node2.y == sourceNode.y)
{
link.flag = 2;
destinationNode = link.Node1;
}
else
{
continue;
}
// 隣接ノードを見つけた際の処理
if (destinationNode.Status == DijkstraNode.NodeStatus.Completed)
{
continue;
}
// ノードの現在の距離に現在取り出しているlinkの距離を加える
dTotalDistance = sourceNode.Distance + link.LinkDistance;
if (destinationNode.Distance <= dTotalDistance)
{
continue;
}
// 現在の仮の最短距離よりもっと短いルートを見つけた際の処理
destinationNode.Distance = dTotalDistance; // 仮の最短距離
destinationNode.SourceNode = sourceNode;
destinationNode.Status = DijkstraNode.NodeStatus.Temporary;
} //foreach終わり
}
/// 指定ノードに隣接するノードの最短距離を計算する
/// 関数で使う関数
/// sourceNode => 指定ノード
private void UpdateNodeProp(DijkstraNode sourceNode)
{
if (_branches == null)
{
return;
}
DijkstraNode destinationNode;
double dTotalDistance;
// ブランチリストの中から指定ノードに関連しているものを検索
foreach (DijkstraBranch branch in _branches)
{
destinationNode = null;
if (branch.Node1.Equals(sourceNode) == true)
{
destinationNode = branch.Node2;
}
else if (branch.Node2.Equals(sourceNode) == true)
{
destinationNode = branch.Node1;
}
else
{
continue;
}
// 確定しているノードは無視。
if (destinationNode.Status == DijkstraNode.NodeStatus.Completed)
{
continue;
}
// 隣接ノードを見つけた。
print("検索Node:" + destinationNode.nodeNumber);
// ノードの現在の距離に枝の距離を加える。
dTotalDistance = sourceNode.Distance + branch.Distance;
if (destinationNode.Distance <= dTotalDistance)
{
continue;
}
// 現在の仮の最短距離よりもっと短い行き方を見つけた。
destinationNode.Distance = dTotalDistance; // 仮の最短距離
destinationNode.SourceNode = sourceNode;
destinationNode.Status = DijkstraNode.NodeStatus.Temporary;
}
}
//StartPointとGoalPointをcsvファイルから読み込む処理
void Start()
{
TextAsset StartFile = Resources.Load("CSV/" + START_POINT_DATA_NAME) as TextAsset;
TextAsset GoalFile = Resources.Load("CSV/" + GOAL_POINT_DATA_NAME) as TextAsset;
StringReader S_reader = new StringReader(StartFile.text);
StringReader G_reader = new StringReader(GoalFile.text);
Timer timer = GameObject.Find("Timer").GetComponent <Timer>();
GameObject sp;
string[] S_rows;
string[] G_rows;
//1行目はインデックス
S_reader.ReadLine();
/*
* StartPoint一覧が保存されているファイルを読み込み，インスタンスに保存する処理
*/
while (S_reader.Peek() > -1)
{
S_rows = S_reader.ReadLine().Split(',');
sp = Instantiate(StartPointPrefab, new Vector3(float.Parse(S_rows[1]), float.Parse(S_rows[2]), 0f), transform.rotation);
sp.GetComponent <StartPoint>().StartX = float.Parse(S_rows[1]);
sp.GetComponent <StartPoint>().StartY = float.Parse(S_rows[2]);
sp.GetComponent <StartPoint>().spName = S_rows[0];
sp.GetComponent <StartPoint>().volume = new int[2] {
int.Parse(S_rows[6]), int.Parse(S_rows[7])
};
spList.Add(sp);
}
/*
* GoalPoint一覧が保存されているファイルを読み込み，リストに保存する処理
*/
G_reader.ReadLine();
while (G_reader.Peek() > -1)
{
G_rows = G_reader.ReadLine().Split(',');
GoalPoint = new DijkstraNode(float.Parse(G_rows[1]), float.Parse(G_rows[2]));
GoalPointList.Add(GoalPoint);
}
}
private void Propagate(DijkstraNode node)
{
node.visited = true;
foreach (Vector2Int move in this.moves)
{
Vector2Int newPos = node.pos - move;
if (this.board.InBounds(newPos) && (this.board.IsFreeTile(newPos) || newPos == origin))
{
DijkstraNode newNode = this.nodes[newPos.x, newPos.y];
if (node.tDist + 1 < newNode.tDist)
{
newNode.tDist = node.tDist + 1;
}
}
}
}
// TODO: This can be implemented more efficiently.
private DijkstraNode NextNode()
{
DijkstraNode bestNode = DijkstraNode.WorstNode();
for (int i = 0; i < this.board.size; i++)
{
for (int j = 0; j < this.board.size; j++)
{
if (!this.nodes[i, j].visited && this.nodes[i, j].tDist < bestNode.tDist)
{
bestNode = this.nodes[i, j];
}
}
}
return(bestNode);
}
/*
* 引数で受け取ったスタート地点の座標に直線距離で一番近いゴール地点を検索するメソッド
* 引数 スタート地点の情報
* return 最寄りのゴール地点
*/
public DijkstraNode Calculate_SG_Distance(DijkstraNode StartPoint)
{
NearestGoal = null;
shortest_length = int.MaxValue;
foreach (DijkstraNode g in _StartPointMaster.GoalPointList)
{
length = Mathf.Sqrt(Mathf.Pow(StartPoint.x - g.x, 2f) + Mathf.Pow(StartPoint.y - g.y, 2f));
if (length <= shortest_length)
{
shortest_length = length;
NearestGoal = g;
}
}
return(NearestGoal);
}
private static void UpdateDistances(NodeNetCreator net, DijkstraNode current)
{
foreach (var dn in dNodes)
{
Stretch st = net.GetStretch(current.node, dn.node);
if (st != null && !dn.visited)
{
float possibleNewDistance = current.distance + st.GetLength();
if (possibleNewDistance < dn.distance)
{
dn.distance = possibleNewDistance;
dn.parent = current;
}
}
}
current.visited = true;
}
public bool FindPath(DijkstraNode[] pathfindingNetwork, DijkstraNode targetNode, DijkstraNode startNode, IPathRequest pathRequest)
{
if (targetNode.Clearance < pathRequest.AgentSize)
{
return(false);
}
ResetNetwork(pathfindingNetwork);
var openSet = new MaxHeap <DijkstraNode>(pathfindingNetwork.Length);
var closedSet = new HashSet <DijkstraNode>();
openSet.Add(targetNode);
targetNode.GCost = 0f;
while (openSet.Count > 0)
{
var currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
foreach (var connection in currentNode.DefinitionNode.Connections)
{
var toNode = NodePointer.Dereference(connection.To, pathfindingNetwork);
if ((connection.CollisionCategory & pathRequest.CollisionCategory) != 0 || closedSet.Contains(toNode))
{
continue;
}
if (toNode.Clearance < pathRequest.AgentSize)
{
toNode.GCost = float.NaN;
}
else
{
var newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode.DefinitionNode, toNode.DefinitionNode) * currentNode.DefinitionNode.MovementCostModifier;
if (newMovementCostToNeighbour < toNode.GCost || !openSet.Contains(toNode))
{
toNode.GCost = newMovementCostToNeighbour;
if (!openSet.Contains(toNode))
{
openSet.Add(toNode);
}
}
}
}
}
return(true);
}
/*
* 探索したノードを基に最短経路(リンクの連なり)を持ったリストを作成するメソッド
* return 最短経路を持ったリスト
*/
public List <DijkstraLink> FindRoute()
{
List <DijkstraNode> doneNodeList = new List <DijkstraNode>();
List <DijkstraLink> routeList = new List <DijkstraLink>();
DijkstraNode node = EndNode;
while (node != null && node.SourceNode != null)
{
doneNodeList.Add(node);
node = node.SourceNode;
}
doneNodeList.Add(node);
//EndNode→StartNodeの準でリストに保存されているためReverse()
doneNodeList.Reverse();
DijkstraNode node1, node2;
for (int i = 0; i < doneNodeList.Count - 1; i++)
{
node1 = doneNodeList[i];
node2 = doneNodeList[i + 1];
foreach (DijkstraLink link in ListMaster.dijkstraLinkList)
{
//To Do このif文の書き方をスマートにする（可読性を上げる）
if (node1.x == link.Node1.x && node1.y == link.Node1.y &&
node2.x == link.Node2.x && node2.y == link.Node2.y ||
node2.x == link.Node1.x && node2.y == link.Node1.y &&
node1.x == link.Node2.x && node1.y == link.Node2.y)
{
node2.ID = link.linkID;
}
}
foreach (DijkstraLink route in node1.linkList)
{
if (route.Node2.x == node2.x && route.Node2.y == node2.y && route.linkID == node2.ID)
{
routeList.Add(route);
break;
}
}
}
return(routeList);
}
