//使用开始的实际距离来优先排列
//public void Dijkstra(Grid start, Grid goal)
//{
// came_from = new Dictionary<Grid, Grid>();
// cost_so_far = new Dictionary<Grid, double>();
// frontier = new PriorityQueue<Grid, double>();
// frontier.Push(start, 0);
// came_from[start] = start;
// cost_so_far[start] = 0;
// while (frontier.Count > 0)
// {
// Grid current = frontier.Pop();
// if (current == goal)
// break;
// Grid[] neighbors = graph.Neighbors(current);
// for (int i = 0; i < neighbors.Length; i++)
// {
// Grid next = neighbors[i];
// double new_cost = cost_so_far[current] + graph.Cost(next);
// if (came_from.ContainsKey(next) || new_cost < cost_so_far[next])
// {
// cost_so_far[next] = new_cost;
// double priority = new_cost;
// frontier.Push(next, priority);
// came_from[next] = current;
// }
// }
// }
//}
//以离终点的距离作为优先级来排列
//public void GreedyBestFirstSearch(Grid start, Grid goal)
//{
// came_from = new Dictionary<Grid, Grid>();
// cost_so_far = new Dictionary<Grid, double>();
// frontier = new PriorityQueue<Grid, double>();
// frontier.Push(start, 0);
// came_from[start] = start;
// while (frontier.Count > 0)
// {
// Grid current = frontier.Pop();
// if (current == goal)
// break;
// Grid[] neighbors = graph.Neighbors(current);
// for (int i = 0; i < neighbors.Length; i++)
// {
// Grid next = neighbors[i];
// if (came_from.ContainsKey(next))
// {
// int priority = Heuristic(goal,next);
// frontier.Push(next, priority);
// came_from[next] = current;
// }
// }
// }
//}
public void AstarSearch(Grid start, Grid goal)
{
came_from = new Dictionary <Grid, Grid>();
cost_so_far = new Dictionary <Grid, double>();
frontier = new PriorityQueue <Grid, double>();
//if (start != null)
//{
// Debug.Log("start " + start.name);
// Debug.Log("end " + goal.name);
//}
frontier.Push(start, 0);
came_from.Add(start, start);
//came_from[start] = start;
cost_so_far.Add(start, 0);
//cost_so_far[start] = 0;
while (frontier.Count > 0)
{
Grid current = frontier.Pop();
if (current == goal)
{
break;
}
Grid[] neighbors = graph.Neighbors(current);
for (int i = 0; i < neighbors.Length; i++)
{
Grid next = neighbors[i];
double new_cost = cost_so_far[current] + graph.Cost(next);
if (!cost_so_far.ContainsKey(next) || new_cost < cost_so_far[next])
{
if (cost_so_far.ContainsKey(next))
{
cost_so_far[next] = new_cost;
}
else
{
cost_so_far.Add(next, new_cost);
}
double priority = new_cost + Heuristic(goal, next);
frontier.Push(next, priority);
if (came_from.ContainsKey(next))
{
came_from[next] = current;
}
else
{
came_from.Add(next, current);
}
}
}
}
}
public AStarSearch3(SquareGrid graph, Location start, Location goal, Dictionary <Location, int> costObject)
{
this.start = start;
this.goal = goal;
var frontier = new PriorityQueue <Location>();
frontier.Enqueue(start, 0f);
cameFrom.Add(start, start);
costSoFar.Add(start, 0f);
while (frontier.Count > 0f)
{
Location current = frontier.Dequeue();
if (current.Equals(goal))
{
break;
}
foreach (var neighbor in graph.Neighbors(current))
{
float newCost = costSoFar[current] + graph.Cost(current, neighbor);
if (!costSoFar.ContainsKey(neighbor) || newCost < costSoFar[neighbor])
{
// If we're replacing the previous cost, remove it
if (costSoFar.ContainsKey(neighbor))
{
costSoFar.Remove(neighbor);
cameFrom.Remove(neighbor);
}
costSoFar.Add(neighbor, newCost);
cameFrom.Add(neighbor, current);
float priority;
int N = (int)Heuristic(start, goal);
if (d <= N)
{
priority = newCost + 1.0f * (1 + 1.01f * (1 - 1.0f * d / N)) * Heuristic(neighbor, goal);
}
else
{
priority = newCost + Heuristic(neighbor, goal);
}
frontier.Enqueue(neighbor, priority);
dem++;
}
}
d++;
}
}
public List <Vector2> Path = new List <Vector2>();//寻路结果
public AStarSearch(SquareGrid map, Location start, Location goal)
{
var frontier = new PriorityQueue <Location>();//open列表
//init
frontier.Enqueue(start, 0);
From[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var currentLocation = frontier.Dequeue();
if (currentLocation == goal)
{
break;
}
foreach (var neighbor in map.Neighbors(currentLocation))//遍历邻近点，选择最佳位置
{
//计算移动成本g = 走出当前点的成本 + 走出邻近点的成本
float newCost = costSoFar[currentLocation] + map.Cost(neighbor);//g
if (!costSoFar.ContainsKey(neighbor) || //排除已走过的位置点，类似于close列表
newCost < costSoFar[neighbor])
{
costSoFar[neighbor] = newCost;
float priority = newCost = Heuristic(neighbor, goal);//f=g+h
frontier.Enqueue(neighbor, priority);
From[neighbor] = currentLocation;
}
} //end search Neighbor
} //end Dequeue
//将寻路路径按正常顺序提取
Location current = goal;
Path.Add(new Vector2(current.x, current.y));
while (current != start)
{
current = From[current];
Path.Add(new Vector2(current.x, current.y));
}
Path.Reverse();//From<下一结点，当前结点>
}//end Search def
public AStarSearch2(SquareGrid graph, Location start, Location goal, Dictionary <Location, int> costObject)
{
this.start = start;
this.goal = goal;
var frontier = new PriorityQueue <Location>();
frontier.Enqueue(start, 0f);
cameFrom.Add(start, start);
costSoFar.Add(start, 0f);
while (frontier.Count > 0f)
{
Location current = frontier.Dequeue();
Location parent = cameFrom[current];
if (current.Equals(goal))
{
break;
}
foreach (var neighbor in graph.Neighbors(current))
{
float newCost = costSoFar[current] + graph.Cost(current, neighbor);
if (!costSoFar.ContainsKey(neighbor) || newCost < costSoFar[neighbor])
{
if (costSoFar.ContainsKey(neighbor))
{
costSoFar.Remove(neighbor);
cameFrom.Remove(neighbor);
}
costSoFar.Add(neighbor, newCost);
cameFrom.Add(neighbor, current);
float priority = newCost + Heuristic(neighbor, goal);
frontier.Enqueue(neighbor, priority);
dem++;
}
}
}
}
public IEnumerable <Vector3Int> GetNeighbors(Vector3Int coords)
{
return(grid.Neighbors(coords));
}
