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); }