public AStarTimeSliced(Graph.Vertex start, Graph.Vertex goal)
{
Start = start;
Goal = goal;
NavigationGraph = GameWorld.Instance.NavGraph;
OpenSet = new PriorityQueue <PriorityVertex>();
ClosedSet = new HashSet <Graph.Vertex>();
CameFrom = new Dictionary <Graph.Vertex, Graph.Vertex>();
Costs = new Dictionary <Graph.Vertex, double>();
HCosts = new Dictionary <Graph.Vertex, double>();
// Initialize graph
//foreach (Graph.Vertex vertex in NavigationGraph.vertices)
//{
// if (vertex != null) {
// Costs.Add(vertex, Double.MaxValue);
// HCosts.Add(vertex, Double.MaxValue);
// }
//}
// Add start node to queue
Costs[Start] = 0;
HCosts[Start] = Heuristic(Start, Goal);
OpenSet.Add(new PriorityVertex(Start, Costs[Start]));
}
public bool dfs(Graph.Vertex start, bool[] visited, Graph.Vertex[] vertices)
{
if (vertices[start.index].Equals("Finish"))
{
return(true);
}
visited[start.index] = true; //INDEX HAS TO BE LOCATION IN VERTICES ARRAY
for (int i = 0; i < vertices[start.index].Neighbors.Count; i++) //goes through neighbors List
{
//Debug.Log("test index: " + i);
//Debug.Log("visited[i]: " + visited[i]);
if (!visited[vertices[start.index].Neighbors[i].index])
{
if (vertices[start.index].Neighbors[i].vertex.name.Equals("Finish"))
{
points.Push(vertices[start.index].Neighbors[i].vertex.transform.position);
return(true);
}
if (dfs(vertices[vertices[start.index].Neighbors[i].index], visited, vertices))
{
points.Push(vertices[start.index].Neighbors[i].vertex.transform.position);
return(true);
}
}
}
return(false);
}
/// <summary>
/// Initiates path planning for an agent.
/// </summary>
/// <param name="goal">The goal location the agent is trying to reach.</param>
/// <returns>Succes</returns>
public bool RequestPathToLocation(Location start, Location goal)
{
Goal = goal;
Graph.Vertex startVertex = GameWorld.Instance.NavGraph.NearestVertexFromLocation(start);
Graph.Vertex goalVertex = GameWorld.Instance.NavGraph.NearestVertexFromLocation(goal);
currentSearch = new AStarTimeSliced(startVertex, goalVertex);
RequestSearch();
return(true);
}
// <summary>
/// if a vertex does not exist at location coords, add to the dictionary
/// </summary>
/// <param name="coords"></param>
/// <returns>reference to vertex at location coords</returns>
public Vertex AddVertex(Coordinates coords)
{
var vertex = this.GetVertex(coords);
if (vertex == null)
{
vertex = new Graph.Vertex(coords);
vertexMap.Add(coords, vertex);
}
return(vertex);
}
/// <summary>
/// Efficiency Warpper for the Dictionary access operator.
/// This method assumes that invalid keys weren't initialized and should have a cost of Infinity.
/// This makes it so that during initialization the algorithm doesn't need to initialize the costs of all vertices.
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
private double HCostToReach(Graph.Vertex v)
{
if (HCosts.ContainsKey(v))
{
return(HCosts[v]);
}
else
{
return(Double.MaxValue);
}
}
// Heuristic for A* based on euclidian distance.
private static double Heuristic(Graph.Vertex from, Graph.Vertex to)
{
// Determine which heuristic is to be used based on the fact that diagonal edges are used or not.
if (GameWorld.Instance.NavGraph.DiagonalEdgesCost >= 0)
{
return(Utility.Distance(from.Location, to.Location)); // euclidian distance
}
else
{
return(Math.Abs(from.Location.X - to.Location.X) + Math.Abs(from.Location.Y - to.Location.Y)); // cardinal / manhattan distance
}
}
// Reconstructs a path from graph explored by A*
public static List <Location> reconstructPath(Graph.Vertex goal)
{
List <Location> path = new List <Location>();
Graph nav = GameWorld.Instance.NavGraph;
Graph.Vertex curr = goal;
path.Insert(0, curr.Location);
while ((curr = curr.Previous) != null)
{
path.Insert(0, curr.Location);
}
return(path);
}
public SearchStatus CycleOnce()
{
if (!OpenSet.IsEmpty)
{
Graph.Vertex vertex = OpenSet.Get().Vertex;
// End-case : goal found, tell our handler that the goal has been reached.
if (vertex == Goal)
{
GoalAlreadyReached = true;
return(SearchStatus.TARGET_FOUND);
}
ClosedSet.Add(vertex);
foreach (Graph.Edge edge in vertex.Adjacent)
{
Graph.Vertex neighbour = edge.Dest;
if (ClosedSet.Contains(neighbour))
{
continue; // Already evaluated
}
HCosts[neighbour] = CostToReach(vertex) + edge.Cost + Heuristic(neighbour, Goal);
OpenSet.Add(new PriorityVertex(neighbour, HCosts[neighbour])); // Newly discovered node
// Calculate distance from start till neighbour vertex
double tentative_dist = CostToReach(vertex) + edge.Cost;
// First time for each vertex this should be a check against inifinity, but we already set the score above.
// Cost to reach never set, assumed infinite
if (tentative_dist >= CostToReach(neighbour))
{
continue;
}
// Current best path
CameFrom[neighbour] = vertex;
Costs[neighbour] = tentative_dist;
HCosts[neighbour] = CostToReach(neighbour) + Heuristic(neighbour, Goal);
}
}
if (GoalAlreadyReached)
{
return(SearchStatus.TARGET_FOUND);
}
else
{
return(SearchStatus.TARGET_NOT_FOUND);
}
}
/// <summary>
/// Finds vertex with minimum CostFromStart
/// </summary>
/// <param name="list">List of vertices</param>
/// <returns> distance from start vertex </returns>
protected override Vertex MinDist(List <Vertex> list)
{
Graph.Vertex closest = null;
float minDistance = float.MaxValue;
foreach (var vertex in list) // loop through each vertex in list
{
if (vertex.CostFromStart < minDistance) // compare to minimum distance found
{
minDistance = vertex.CostFromStart; // update minimum distance
closest = vertex; // update closest vertex
}
}
return(closest); // return closest vertex in list
}
private static bool RenderShortestPath()
{
const bool SHOW_SMOOTHED_PATH = true;
Graph.Vertex[,] vertices = GameWorld.Instance.NavGraph.vertices;
try
{
if (GameWorld.Instance.PathAlreadyCalculated == null)
{
Random rand = new Random();
Graph.Vertex start = null, goal = null;
while (goal == null || start == null)
{
// Generate random start and goal for pathfinder example
start = vertices[rand.Next(vertices.GetLength(0)), rand.Next(vertices.GetLength(1))]; //start = vertices[2, 1];
goal = vertices[rand.Next(vertices.GetLength(0)), rand.Next(vertices.GetLength(1))]; //goal = vertices[1, 15];
}
GameWorld.Instance.PathAlreadyCalculated = Movement.Planning.Pathfinding.AStar(start, goal);
}
List <Location> path = GameWorld.Instance.PathAlreadyCalculated;
Pen p = new Pen(Color.MediumPurple, Math.Max(1, (float)GameWorld.Instance.Width / 250));
for (int i = 1; i < path.Count; i++)
{
Location v = path[i - 1];
Location w = path[i];
graphics.DrawLine(p, (float)v.X, (float)v.Y, (float)w.X, (float)w.Y);
}
if (SHOW_SMOOTHED_PATH)
{
List <Location> smoothedPath = Movement.Planning.Pathfinding.FinePathSmoothing(path);
p = new Pen(Color.DeepPink, Math.Max(1, (float)GameWorld.Instance.Width / 250));
for (int i = 1; i < smoothedPath.Count; i++)
{
Location v = smoothedPath[i - 1];
Location w = smoothedPath[i];
graphics.DrawLine(p, (float)v.X, (float)v.Y, (float)w.X, (float)w.Y);
}
}
return(true);
}
catch (NullReferenceException e)
{
Console.WriteLine("Could not render Shortest Path");
return(false);
}
}
/**
* Checks wether this tile belongs to a given face.
*/
private static void ComputeFace(int x, int y, ref Graph.Graph graph)
{
// Check wether this tile is a wall, a vertex, or an empty cell.
Vector2 position = new Vector2(x, y);
Tile.Wall cellWall = graph.GetWallAt(position);
Graph.Vertex cellVtx = graph.GetVertexAt(position);
if (cellWall == null && cellVtx == null)
{
// Get the top and left cells.
Vector2 left = position + new Vector2(-1, 0);
Vector2 top = position + new Vector2(0, -1);
// Get the faces of the cells.
Graph.Face leftFace = graph.GetFaceFromPoint(left);
Graph.Face topFace = graph.GetFaceFromPoint(top);
Graph.Face pointFace;
// If both faces are null.
if (leftFace == null && topFace == null)
{
// Create a new face.
pointFace = new Graph.Face();
// Add the face to the graph.
graph.AddFace(pointFace);
// If both faces are different.
}
else if (leftFace != topFace)
{
{
// Merge both faces together.
pointFace = graph.MergeFaces(leftFace, topFace);
// If both faces are the same.
}
}
else
{
// Select it as the point's face.
pointFace = leftFace;
}
// Add the point to the face.
pointFace.AddPoint(position);
}
}
// Reconstructs a path from graph explored by Time-Sliced A*
private List <Location> reconstructPath(Graph.Vertex goal)
{
List <Location> path = new List <Location>();
Graph nav = GameWorld.Instance.NavGraph;
Graph.Vertex curr = goal;
while (curr != null)
{
path.Insert(0, curr.Location);
if (curr == Start)
{
break;
}
curr = CameFrom[curr];
}
return(path);
}
private static void RenderNavGraph()
{
const bool SHOW_VERTEX_LABEL = false;
const bool SHOW_EDGES = true;
const bool SHOW_VERTICES = true;
var vs = GameWorld.Instance.NavGraph.vertices;
for (int x = 0; x < vs.GetLength(0); x++)
{
for (int y = 0; y < vs.GetLength(1); y++)
{
Graph.Vertex v = vs[x, y];
if (v != null)
{
if (SHOW_EDGES)
{
foreach (Graph.Edge e in v.Adjacent)
{
Graph.Vertex w = e.Dest;
graphics.DrawLine(new Pen(Color.Black, 1), (float)v.Location.X, (float)v.Location.Y, (float)w.Location.X, (float)w.Location.Y);
//if (v.Loc.X > w.Loc.X)
//{
// graphics.DrawLine(new Pen(Color.Black, 1), (float)w.Loc.X, (float)w.Loc.Y, (float)w.Loc.X + 5, (float)w.Loc.Y + 5);
// graphics.DrawLine(new Pen(Color.Black, 1), (float)w.Loc.X, (float)w.Loc.Y, (float)w.Loc.X + 5, (float)w.Loc.Y - 5);
//}
}
}
if (SHOW_VERTICES)
{
float renderedVertexRadius = 1.5f;
graphics.DrawEllipse(new Pen(Color.LawnGreen), (float)v.Location.X - renderedVertexRadius / 2,
(float)v.Location.Y - renderedVertexRadius / 2, renderedVertexRadius, renderedVertexRadius);
}
if (SHOW_VERTEX_LABEL)
{
Brush b = new SolidBrush(Color.DarkBlue);
Font f = new Font(SystemFonts.DefaultFont.Name, 6);
graphics.DrawString(v.Label, f, b, (float)v.Location.X, (float)v.Location.Y);
}
}
}
}
}
public void solve()
{
Graph.Vertex start = null;
graph = new Graph();
for (int i = 0; i < graph.vertices.Length; i++) //finds start node
{
//Debug.Log(graph.vertices[i]);
if (graph.vertices[i].vertex.name.Equals("Start"))
{
start = graph.vertices[i];
}
if (start != null)
{
break;
}
}
visited = new bool[graph.vertices.Length];
if (dfs(start, visited, graph.vertices))
{
Debug.Log("Path found");
canvas.GetComponent <Score>().showAITimeTrue();
if (AI != null)
{
Destroy(AI);
}
if (SceneManager.GetActiveScene().name.Equals("Competitive"))
{
StartCoroutine(wait());
GameObject Player = Instantiate(Resources.Load("Player"), start.vertex.transform.position, Quaternion.identity) as GameObject;
}
AI = Instantiate(Resources.Load("AI"), start.vertex.transform.position, Quaternion.identity) as GameObject;
AI.GetComponent <AIMovement>().receivePoints(points);
}
else
{
Debug.Log("No path");
canvas.GetComponentInChildren <Fade>().appearFade();
}
}
/**
* Check if the given tile is a vertex.
* If it is, add it to the given graph reference.
*/
private static void ComputeVertex(String name, int x, int y, ref Graph.Graph graph)
{
// Prepare the regex matcher.
RegEx rgx = new RegEx();
rgx.Compile("^Wall._[TBLR]{2}$|^(Tower)");
// Get and check the result of the regex.
RegExMatch result = rgx.Search(name);
if (result != null)
{
// Check wether a tower was selected.
bool bIsTower = result.GetString(1).Equals("Tower");
// Create a new Vertex instance.
Graph.Vertex vtx = new Graph.Vertex(x, y, graph.GetWallAt(new Vector2(x, y)));
// Add the instance to the graph.
graph.AddVertex(vtx);
}
}
public static List <Location> AStar(Graph.Vertex start, Graph.Vertex goal)
{
Graph navigationGraph = GameWorld.Instance.NavGraph;
PriorityQueue <Graph.Vertex> priorityQueue = new PriorityQueue <Graph.Vertex>();
// Initialize graph
foreach (Graph.Vertex vertex in navigationGraph.vertices)
{
if (vertex != null)
{
vertex.Distance = Double.MaxValue;
vertex.HeuristicDistance = Double.MaxValue;
vertex.Known = false;
}
}
// Add start node to queue
priorityQueue.Add(start);
start.Distance = 0;
start.HeuristicDistance = Heuristic(start, goal);
start.Previous = null;
while (!priorityQueue.IsEmpty)
{
Graph.Vertex vertex = priorityQueue.Get();
if (vertex.Known)
{
continue;
}
vertex.Known = true;
// End-case : goal found, retrieve path to goal node.
if (vertex == goal)
{
return(reconstructPath(goal));
}
foreach (Graph.Edge edge in vertex.Adjacent)
{
Graph.Vertex w = edge.Dest;
if (w.Known)
{
continue; // Already evaluated
}
w.HeuristicDistance = vertex.Distance + edge.Cost + Heuristic(w, goal);
priorityQueue.Add(w); // Newly discovered node
// Calculate distance from start till current vertex
double tentative_dist = vertex.Distance + edge.Cost;
if (w.Distance > tentative_dist)
{
// Current best path
w.Previous = vertex;
w.Distance = tentative_dist;
}
}
}
return(null); // goal not reached
}
internal PriorityVertex(Graph.Vertex v, double prio)
{
Vertex = v;
Priority = prio;
}
