private float Calculate(NavGraph graph, int nd1, int nd2) { float distance = new Vector2(graph.GetNode(nd1).Position().x - graph.GetNode(nd2).Position().x, graph.GetNode(nd1).Position().y - graph.GetNode(nd2).Position().y).magnitude; return(distance); }
private bool search(int source, int target) { Stack <GraphEdge> stack = new Stack <GraphEdge>(); foreach (var e in graph.GetAdjoinEdges(source)) { stack.Push(e); } hasVisited[source] = true; while (stack.Count != 0) { var edge = stack.Pop(); DebugController.instance.AddPathLine(graph.GetNode(edge.From).Pos, graph.GetNode(edge.To).Pos); hasVisited[edge.From] = true; pathIndex[edge.To] = edge.From; if (edge.To == target) { return(true); } foreach (var e in graph.GetAdjoinEdges(edge.To)) { if (!hasVisited[e.To]) { stack.Push(e); } } } return(false); }
private bool search(int source, int target) { if (source == target) { pathIndex[target] = source; return(true); } Queue <GraphEdge> queue = new Queue <GraphEdge>(); hasVisited[source] = true; pathIndex[source] = source; foreach (var e in graph.GetAdjoinEdges(source)) { queue.Enqueue(e); hasVisited[e.To] = true; pathIndex[e.To] = e.From; if (e.To == target) { return(true); } } while (queue.Count != 0) { var edge = queue.Dequeue(); DebugController.instance.AddPathLine(graph.GetNode(edge.From).Pos, graph.GetNode(edge.To).Pos); foreach (var e in graph.GetAdjoinEdges(edge.To)) { if (!hasVisited[e.To]) { queue.Enqueue(e); hasVisited[e.To] = true; pathIndex[e.To] = e.From; if (e.To == target) { return(true); } } } } return(false); }
protected override void OnPaint(PaintEventArgs e) { base.OnPaint(e); foreach (NavGraphNode node in graph.nodes) { if (node.Index == -1) { continue; } int left = (int)(node.Position.X - cellWidth / 2.0f); int top = (int)(node.Position.Y - cellHeight / 2.0f); e.Graphics.DrawRectangle(Pens.Black, left, top, cellWidth, cellHeight); } foreach (NavGraphNode node in graph.nodes) { if (node.Index == -1) { continue; } e.Graphics.FillEllipse(Brushes.LightGray, node.Position.X - 2, node.Position.Y - 2, 5, 5); foreach (GraphEdge edge in graph.edges[node.Index]) { Vector2 from = node.Position; Vector2 to = graph.GetNode(edge.To).Position; e.Graphics.DrawLine(Pens.LightGray, from.X, from.Y, to.X, to.Y); } } foreach (GraphEdge edge in subTree) { if (edge != null) { Vector2 from = graph.GetNode(edge.From).Position; Vector2 to = graph.GetNode(edge.To).Position; e.Graphics.DrawLine(Pens.Red, from.X, from.Y, to.X, to.Y); } } }
// // use to add he eight neighboring edges of a graph node that // is positioned in a grid layout public static void AddAllNeighboursToGridNode(NavGraph graph, int row, int col, int numCellsX, int numCellsY) { for (int i = -1; i <= +1; ++i) { for (int j = -1; j <= +1; ++j) { //skip if equal to this node if (i == 0 && j == 0) { continue; } int nodeX = col + j; int nodeY = row + i; //check to see if this is a valid neighbour if (ValidNeighbour(nodeX, nodeY, numCellsX, numCellsY)) { //calculate the distance to this node Vector2 posNode = graph.GetNode(row * numCellsX + col).Position; Vector2 posNeighbour = graph.GetNode(nodeY * numCellsX + nodeX).Position; float dist = Vector2.Length(posNeighbour - posNode); GraphEdge newEdge; newEdge = new GraphEdge(row * numCellsX + col, nodeY * numCellsX + nodeX, dist); graph.AddEdge(newEdge); //if graph is not a diagraph then an edge needs to be added going //in the other direction if (!graph.IsDigraph()) { newEdge = new GraphEdge(nodeY * numCellsX + nodeX, row * numCellsX + col, dist); graph.AddEdge(newEdge); } } } } }
//returns the path as a list of PathEdges override public List <PathEdge> GetPathAsPathEdges() { //just return an empty path if no target or no path found if (targetNodeIndex_ < 0) { return(null); } List <PathEdge> pathEdges = new List <PathEdge>(); int nd = targetNodeIndex_; while ((nd != sourceNodeIndex_) && (shortestPathTree_[nd] != null)) { pathEdges.Insert(0, new PathEdge(navGraph_.GetNode(shortestPathTree_[nd].From()).Position(), navGraph_.GetNode(shortestPathTree_[nd].To()).Position(), shortestPathTree_[nd].GetEdgeType())); nd = shortestPathTree_[nd].From(); } return(pathEdges); }
// Gizmos to provide relevant information in scene view private void OnDrawGizmos() { if (regionCount > 0 && showRegionsInEditor) { CreateRegions(); for (int i = 0; i < regionCount; i++) { for (int j = 0; j < regionCount; j++) { regions[i, j].bounding.DrawGizmos(Color.blue); } } } if (showOctreeInEditor) { // Oct tree if (octree == null) { CreateOctTree(); } DrawOctTree(octree); } if (!showNavGraphInEditor) { return; } if (navGraph == null || width != navGraph.Width || height != navGraph.Depth) { CreateNavGraph(); } for (int x = 0; x < navGraph.Width; x++) { for (int z = 0; z < navGraph.Depth; z++) { Node current = navGraph.GetNode(x, z); foreach (Node neighbour in current.GetNeighbours()) { Gizmos.DrawLine(current.worldPosition, neighbour.worldPosition); } } } }
//returns the path as a list of PathEdges override public List <PathEdge> GetPathAsPathEdges() { List <PathEdge> path = new List <PathEdge>(); //just return an empty path if no target or no path found if (targetIdx_ < 0) { return(path); } int nd = targetIdx_; PathEdge newPathEdge = null; Vector2 source; Vector2 destination; while ((nd != sourceIdx_) && (shortestPathTree_[nd] != null)) { source = graph_.GetNode(shortestPathTree_[nd].From()).Position(); destination = graph_.GetNode(shortestPathTree_[nd].To()).Position(); // change map position to real position source.x *= GameSettings.GetInstance().TILE_SIZE; source.y *= GameSettings.GetInstance().TILE_SIZE; destination.x *= GameSettings.GetInstance().TILE_SIZE; destination.y *= GameSettings.GetInstance().TILE_SIZE; newPathEdge = new PathEdge(source, destination); path.Insert(0, newPathEdge); nd = shortestPathTree_[nd].From(); } return(path); }
private bool search(int source, int target) { //int num = 0; if (source == target) { //当起点和终点相同时 Debug.Log("目标和起点节点相同" + target); return(true); } //Debug.Log("起始点是"+source+"终点是"+target); costToNode[source] = 0; costToEnd[source] = 0; IndexPriorityQueue <double> pq = new IndexPriorityQueue <double>(costToEnd, (i, j) => (float)(i - j), PriorityOrder.MinFirst); searchFrontier[source] = new GraphEdge(source, source); while (!pq.isEmpty()) { //Debug.Log("A*"); int index = pq.DequeueIndex(); spt[index] = searchFrontier[index]; //Debug.Log("最小花费的点是"+index+"花费是"+costToEnd[index]); DebugController.instance.AddPathLine(graph.GetNode(spt[index].From).Pos, graph.GetNode(spt[index].To).Pos); if (index == target) { //Debug.Log("A*总的搜索次数是"+num); return(true); } //Debug.Log("" + graph.GetAdjoinEdges(index).Count); foreach (var e in graph.GetAdjoinEdges(index)) { //num++; double costNode = costToNode[e.From] + e.Cost; double costEnd = costNode + heuristic(e.To, target); //Debug.Log("边的总花费是" + costEnd+"边的花费是"+e.Cost+"上一个节点的花费是"+costToNode[e.From]+"启发因子花费是"+ heuristic(e.To, target)); if (searchFrontier[e.To] == null) { costToNode[e.To] = costNode; costToEnd[e.To] = costEnd; searchFrontier[e.To] = e; pq.ChangePriority(e.To, costEnd); } else { //Debug.Log("哈哈哈哈"+costNode+",,"+costToNode[e.To]); if (costNode < costToNode[e.To] && spt[e.To] == null) { //否则只有当spt上没有这个节点 //Debug.Log("啦啦啦啦"); costToEnd[e.To] = costEnd; costToNode[e.To] = costNode; searchFrontier[e.To] = e; pq.ChangePriority(e.To, costEnd); } } } } return(false); }
private bool search(int source, int target) { int num = 0; if (source == target) { spt[target] = new GraphEdge(target, target); Debug.Log("一样"); return(true); } IndexPriorityQueue <float> queue = new IndexPriorityQueue <float>(costToNode, (i, j) => (i - j), PriorityOrder.MinFirst); searchFrontier[source] = new GraphEdge(source, source); //searchFrontier[] //int t = queue.TopIndex(); //Debug.Log("起点处理完后最小的点是" + t+"花费是"+costToNode[t]); while (!queue.isEmpty()) { var index = queue.DequeueIndex(); spt[index] = searchFrontier[index]; DebugController.instance.AddPathLine(graph.GetNode(spt[index].From).Pos, graph.GetNode(spt[index].To).Pos); if (index == target) { //Debug.Log("DJ总的搜索次数是"+num); return(true); } //queue.ChangePriority(index,float.PositiveInfinity); #region 邻接边的寻找 foreach (var e in graph.GetAdjoinEdges(index)) { num++; float costNode = costToNode[e.From] + e.Cost; //float costEnd = costNode + heuristic(e.To, target); if (searchFrontier[e.To] == null) { costToNode[e.To] = costNode; searchFrontier[e.To] = e; queue.ChangePriority(e.To, costNode); } else if (costNode < costToNode[e.To] && spt[e.To] == null) { //否则只有当spt上没有这个节点 Debug.Log("lalalal"); costToNode[e.To] = costNode; searchFrontier[e.To] = e; queue.ChangePriority(e.To, costNode); } } #endregion } return(false); }
//calculate the straight line distance from node nd1 to node nd2 float Calculate(NavGraph graph, int nd1, int nd2) { return(Vector2.Length(graph.GetNode(nd2).Position - graph.GetNode(nd1).Position)); }
public Path SearchAStar(NavGraph graph, Vector3 source, Vector3 target) { Stopwatch sw = new Stopwatch(); sw.Start(); BinaryHeap <Node> open = new BinaryHeap <Node>(graph.Width * graph.Depth); HashSet <Node> closed = new HashSet <Node>(); Node start = graph.GetNode(source); if (!start.walkable) { start = graph.FindNearestWalkable(start); } Node goal = graph.GetNode(target); if (!goal.walkable) { goal = graph.FindNearestWalkable(goal); } if (start == null || goal == null) { return(Path.Empty); } // F cost is used to prioritize open.Insert(start, (int)start.f); while (open.Count > 0) { Node current = open.Extract(); if (current == goal) { sw.Stop(); UnityEngine.Debug.Log("[A*]: Found path! Time: " + sw.ElapsedMilliseconds + "ms"); return(ReconstructPath(start, goal)); } closed.Add(current); // Evaluate neighbours foreach (Node neighbour in current.GetNeighbours()) { if (closed.Contains(neighbour)) { continue; } // Calculate cost from source to the neighbour float tentative_g = current.g + CalculateHeuristic(current, neighbour); // If neighbour is already in open, and its not a better path, it does not need to be saved if (open.Contains(neighbour) && tentative_g >= neighbour.g) { continue; } // Save this neighbours, as its a better path neighbour.parentNode = current; neighbour.g = tentative_g; neighbour.h = CalculateHeuristic(neighbour, goal); if (!open.Contains(neighbour)) { open.Insert(neighbour, (int)neighbour.f); } // Because f is now changed, it should be resorted in the heap. else { open.HeapifyUp(neighbour); } } } UnityEngine.Debug.LogError("[A*]: Couldnt find a path"); return(Path.Empty); }
/// <summary> /// (八向)洪水填充算法 /// </summary> private void FloodFill(NavGraphNode startPoint) { Queue <NavGraphNode> queue = new Queue <NavGraphNode>(); queue.Enqueue(startPoint); FillState state; int id = 0; while (queue.Count != 0) { var node = queue.Dequeue(); #region 进行八个方向的填充 //上方 if ((state = isFill(node.Pos, Vector2.up, distanceBetweenNode, ref id)) == FillState.Success) { var n = AddPointAndNode(node, node.Pos + up * distanceBetweenNode, 0, lowCost); queue.Enqueue(n); } else if (state == FillState.FailByOtherPoint) { if (id >= 0 && !navGraph.HasEdge(node.ID, id)) { NavGraph.AddEdge(node.ID, id, lowCost); //NavGraph.AddEdge(id, node.ID, lowCost); DebugController.instance.AddLine(node.Pos, navGraph.GetNode(id).Pos); } } //下方 if ((state = isFill(node.Pos, Vector2.down, distanceBetweenNode, ref id)) == FillState.Success) { var n = AddPointAndNode(node, node.Pos + down * distanceBetweenNode, 0, lowCost); queue.Enqueue(n); } else if (state == FillState.FailByOtherPoint) { if (id >= 0 && !navGraph.HasEdge(node.ID, id)) { NavGraph.AddEdge(node.ID, id, lowCost); //NavGraph.AddEdge(id, node.ID, lowCost); DebugController.instance.AddLine(node.Pos, navGraph.GetNode(id).Pos); } } //左边 if ((state = isFill(node.Pos, Vector2.left, distanceBetweenNode, ref id)) == FillState.Success) { var n = AddPointAndNode(node, node.Pos + left * distanceBetweenNode, 0, lowCost); queue.Enqueue(n); } else if (state == FillState.FailByOtherPoint) { if (id >= 0 && !navGraph.HasEdge(node.ID, id)) { NavGraph.AddEdge(node.ID, id, lowCost); //NavGraph.AddEdge(id, node.ID, lowCost); DebugController.instance.AddLine(node.Pos, navGraph.GetNode(id).Pos); } } //右边 if ((state = isFill(node.Pos, Vector2.right, distanceBetweenNode, ref id)) == FillState.Success) { var n = AddPointAndNode(node, node.Pos + right * distanceBetweenNode, 0, lowCost); queue.Enqueue(n); } else if (state == FillState.FailByOtherPoint) { if (id >= 0 && !navGraph.HasEdge(node.ID, id)) { NavGraph.AddEdge(node.ID, id, lowCost); //NavGraph.AddEdge(id, node.ID, lowCost); DebugController.instance.AddLine(node.Pos, navGraph.GetNode(id).Pos); } } //左上角 if ((state = isFill(node.Pos, Vector2.up + Vector2.left, distanceBetweenDiagonal, ref id)) == FillState.Success) { var n = AddPointAndNode(node, node.Pos + leftUp * distanceBetweenDiagonal, 0, heightCost); queue.Enqueue(n); } else if (state == FillState.FailByOtherPoint) { if (id >= 0 && !navGraph.HasEdge(node.ID, id)) { NavGraph.AddEdge(node.ID, id, heightCost); //NavGraph.AddEdge(id, node.ID, heightCost); DebugController.instance.AddLine(node.Pos, navGraph.GetNode(id).Pos); } } //右上角 if ((state = isFill(node.Pos, Vector2.up + Vector2.right, distanceBetweenDiagonal, ref id)) == FillState.Success) { var n = AddPointAndNode(node, node.Pos + rightUp * distanceBetweenDiagonal, 0, heightCost); queue.Enqueue(n); } else if (state == FillState.FailByOtherPoint) { if (id >= 0 && !navGraph.HasEdge(node.ID, id)) { NavGraph.AddEdge(node.ID, id, heightCost); //NavGraph.AddEdge(id, node.ID, heightCost); DebugController.instance.AddLine(node.Pos, navGraph.GetNode(id).Pos); } } //左下角 if ((state = isFill(node.Pos, Vector2.down + Vector2.left, distanceBetweenDiagonal, ref id)) == FillState.Success) { var n = AddPointAndNode(node, node.Pos + leftDown * distanceBetweenDiagonal, 0, heightCost); queue.Enqueue(n); } else if (state == FillState.FailByOtherPoint) { if (id >= 0 && !navGraph.HasEdge(node.ID, id)) { NavGraph.AddEdge(node.ID, id, heightCost); //NavGraph.AddEdge(id, node.ID, heightCost); DebugController.instance.AddLine(node.Pos, navGraph.GetNode(id).Pos); } } //右下角 if ((state = isFill(node.Pos, Vector2.down + Vector2.right, distanceBetweenDiagonal, ref id)) == FillState.Success) { var n = AddPointAndNode(node, node.Pos + rightDown * distanceBetweenDiagonal, 0, heightCost); queue.Enqueue(n); } else if (state == FillState.FailByOtherPoint) { if (id >= 0 && !navGraph.HasEdge(node.ID, id)) { NavGraph.AddEdge(node.ID, id, heightCost); //NavGraph.AddEdge(id, node.ID, heightCost); DebugController.instance.AddLine(node.Pos, navGraph.GetNode(id).Pos); } } #endregion } }
private float Calculate(NavGraph graph, int nd1, int nd2) { float distance = new Vector2( graph.GetNode(nd1).Position().x - graph.GetNode(nd2).Position().x, graph.GetNode(nd1).Position().y - graph.GetNode(nd2).Position().y).magnitude; return distance; }