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