public void TwoEqualVectorsAreEqual()
{
Vec2i a = new Vec2i(-45, 40);
Vec2i b = new Vec2i(-45, 40);
Assert.True(a == b && b == a);
Assert.True(a.Equals(b) && b.Equals(a));
}
public List <Vec2i> GeneratePath(Vec2i start, Vec2i end, bool debug = false)
{
cameFrom.Clear();
costSoFar.Clear();
var frontier = new PriorityQueue <Vec2i>();
Target = end;
this.start = start;
frontier.Enqueue(start, 0f);
cameFrom.Add(start, start); // is set to start, None in example
costSoFar.Add(start, 0f);
while (frontier.Count > 0f)
{
// Get the Location from the frontier that has the lowest
// priority, then remove that Location from the frontier
Vec2i current = frontier.Dequeue();
// If we're at the goal Location, stop looking.
if (current.Equals(Target))
{
break;
}
// Neighbors will return a List of valid tile Locations
// that are next to, diagonal to, above or below current
foreach (var neighbor in Neighbors(current))
{
// If neighbor is diagonal to current, graph.Cost(current,neighbor)
// will return Sqrt(2). Otherwise it will return only the cost of
// the neighbor, which depends on its type, as set in the TileType enum.
// So if this is a normal floor tile (1) and it's neighbor is an
// adjacent (not diagonal) floor tile (1), newCost will be 2,
// or if the neighbor is diagonal, 1+Sqrt(2). And that will be the
// value assigned to costSoFar[neighbor] below.
float newCost = costSoFar[current] + Cost(current, neighbor);
// If there's no cost assigned to the neighbor yet, or if the new
// cost is lower than the assigned one, add newCost for this 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 = newCost + Heuristic(neighbor, Target);
frontier.Enqueue(neighbor, priority);
}
}
}
return(FindPath());
}
private List <Vec2i> FindPath()
{
List <Vec2i> path = new List <Vec2i>(1000);
Vec2i current = Target;
while (!current.Equals(start))
{
if (!cameFrom.ContainsKey(current))
{
return(new List <Vec2i>(100));
}
path.Add(current);
current = cameFrom[current];
}
path.Reverse();
return(path);
}
/// <summary>
/// Method that searches whether the destination cell is reachable from the root cell.
/// It searches the graph using a BFS algorithm.
/// </summary>
/// <param name="maze"></param>
/// <param name="root"></param>
/// <param name="destination"></param>
/// <returns></returns>
public static bool Q_Is_Reachable_BFS(this PM_Maze maze, Vec2i root, Vec2i destination)
{
if (root.Equals(destination))
{
return(true);
}
List <Vec2i> all_nodes = maze.CellsPositions_All_List();
Dictionary <Vec2i, bool> visited_nodes = new Dictionary <Vec2i, bool>();
foreach (var n in all_nodes)
{
visited_nodes.Add(n, false);
}
Queue <Vec2i> searchQueue = new Queue <Vec2i>();
searchQueue.Enqueue(root);
visited_nodes[root] = true;
while (searchQueue.Count > 0)
{
var current = searchQueue.Dequeue();
var neighbors = maze.Cells_Connected_To_Cell__List(current);
List <Vec2i> unvisitedNeighbors = neighbors.FindAll(x => visited_nodes[x] == false);
foreach (var neighbor in unvisitedNeighbors)
{
searchQueue.Enqueue(neighbor);
visited_nodes[neighbor] = true;
if (neighbor.Equals(destination))
{
return(true);
}
}
}
return(false);
}
/// <summary>
/// <para>
/// Finds a path connecting the two nodes specified.
/// The values 'start'and 'end' represent the local coordinates of the
/// nodes to path find between. Returns 'true' if a total path was found</para>
/// <para>
/// The outputted path is held in the out variable 'path'. If no path is found, this will
/// instead contain all checked points.
/// </para>
/// <para>The paramatater 'transform', true as default, causes the result to be scaled and
/// shifted to world coordinates ([x,z]*World.ChunkSize + BaseCoord).
/// For path finding, this should be kept true.</para>
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="path"></param>
/// <param name="transform"></param>
/// <returns></returns>
public bool ConnectNodes(Vec2i start, Vec2i end, out List <Vec2i> path, out float cost, bool transform = true)
{
cameFrom.Clear();
costSoFar.Clear();
var frontier = new PriorityQueue <Vec2i>();
goal = end;
this.start = start;
frontier.Enqueue(start, 0f);
cameFrom.Add(start, start); // is set to start, None in example
costSoFar.Add(start, 0f);
List <Vec2i> allPath = new List <Vec2i>(100);
List <Vec2i> validPath = new List <Vec2i>();
allPath.Add(start);
validPath.Add(start);
while (frontier.Count > 0f)
{
// Get the Location from the frontier that has the lowest
// priority, then remove that Location from the frontier
Vec2i current = frontier.Dequeue();
// float tCost = 0;
// if(costSoFar.TryGetValue(current, out tCost))
//{
//if (tCost >= int.MaxValue)
// break;
//}
// If we're at the goal Location, stop looking.
if (current.Equals(goal))
{
break;
}
// Neighbors will return a List of valid tile Locations
// that are next to, diagonal to, above or below current
foreach (var neighbor in Neighbors(current))
{
// If neighbor is diagonal to current, graph.Cost(current,neighbor)
// will return Sqrt(2). Otherwise it will return only the cost of
// the neighbor, which depends on its type, as set in the TileType enum.
// So if this is a normal floor tile (1) and it's neighbor is an
// adjacent (not diagonal) floor tile (1), newCost will be 2,
// or if the neighbor is diagonal, 1+Sqrt(2). And that will be the
// value assigned to costSoFar[neighbor] below.
float newCost = costSoFar[current] + Cost(current, neighbor);
if (newCost < int.MaxValue)
{
validPath.Add(neighbor);
}
// If there's no cost assigned to the neighbor yet, or if the new
// cost is lower than the assigned one, add newCost for this 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 = newCost + Heuristic(neighbor, goal);
frontier.Enqueue(neighbor, priority);
allPath.Add(neighbor);
}
}
}
if (costSoFar.TryGetValue(end, out cost))
{
if (cost < int.MaxValue)
{
if (GameManager.DEBUG)
{
Debug.Log("[SettlementPathFinding] Path cost of " + cost);
}
if (transform)
{
path = TransformPath(FindPath());
}
else
{
path = FindPath();
}
return(true);
}
else
{
if (GameManager.DEBUG)
{
Debug.Log("[SettlementPathFinding] Path cost of " + cost + " - not valid path");
}
}
}
if (transform)
{
path = TransformPath(FindPath());
}
else
{
path = FindPath();
}
//If we need to transform, we transform the path accordingly
return(false);
//Check if the path has worked (is this valid?)
if (cameFrom.ContainsKey(end) && cameFrom.ContainsKey(start))
{
if (transform)
{
path = TransformPath(FindPath());
}
else
{
path = FindPath();
}
return(true);
}
if (transform)
{
path = TransformPath(allPath);
}
else
{
path = allPath;
}
//If we need to transform, we transform the path accordingly
return(false);
}
/// <summary>
/// <para>
/// Finds a path connecting the two nodes specified.
/// The values 'start'and 'end' represent the local coordinates of the
/// nodes to path find between.</para>
/// <para>
/// The paramatater 'transform', true as default, causes the result to be scaled and
/// shifted to world coordinates ([x,z]*World.ChunkSize + BaseCoord).
/// For path finding, this should be kept true.</para>
/// <para>
/// If 'transform' is false, the returned result will be in local PathNode coordinates.
/// This is used in SettlementBuilder to help remove islands
/// </para>
///
/// </summary>
/// <param name="start"></param>
/// <param name="end"></param>
/// <param name="scale"></param>
/// <returns></returns>
public List <Vec2i> ConnectNodes(Vec2i start, Vec2i end, bool transform = true)
{
cameFrom.Clear();
costSoFar.Clear();
var frontier = new PriorityQueue <Vec2i>();
goal = end;
this.start = start;
frontier.Enqueue(start, 0f);
cameFrom.Add(start, start); // is set to start, None in example
costSoFar.Add(start, 0f);
while (frontier.Count > 0f)
{
// Get the Location from the frontier that has the lowest
// priority, then remove that Location from the frontier
Vec2i current = frontier.Dequeue();
// If we're at the goal Location, stop looking.
if (current.Equals(goal))
{
break;
}
// Neighbors will return a List of valid tile Locations
// that are next to, diagonal to, above or below current
foreach (var neighbor in Neighbors(current))
{
// If neighbor is diagonal to current, graph.Cost(current,neighbor)
// will return Sqrt(2). Otherwise it will return only the cost of
// the neighbor, which depends on its type, as set in the TileType enum.
// So if this is a normal floor tile (1) and it's neighbor is an
// adjacent (not diagonal) floor tile (1), newCost will be 2,
// or if the neighbor is diagonal, 1+Sqrt(2). And that will be the
// value assigned to costSoFar[neighbor] below.
float newCost = costSoFar[current] + Cost(current, neighbor);
if (GameManager.DEBUG)
{
Debug.Log("[SettlementPathFinder] Cost for " + current + " to " + neighbor + " is " + Cost(current, neighbor) + " with total cost " + newCost);
}
// If there's no cost assigned to the neighbor yet, or if the new
// cost is lower than the assigned one, add newCost for this 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 = newCost + Heuristic(neighbor, goal);
frontier.Enqueue(neighbor, priority);
}
}
}
//If we need to transform, we transform the path accordingly
if (transform)
{
return(TransformPath(FindPath()));
}
return(FindPath());
}
public static List <Vec2i> BFS_ShortestPath(
this PM_Maze maze,
Vec2i root,
Vec2i destination
)
{
if (root.Equals(destination))
{
throw new System.ArgumentException("root equals destination");
}
Dictionary <Vec2i, Vec2i> predecessors = new Dictionary <Vec2i, Vec2i>();
var allCells = maze.CellsPositions_All_List();
foreach (var node in allCells)
{
predecessors.Add(node, node);
}
List <Vec2i> visitedNodes = new List <Vec2i>();
Queue <Vec2i> searchQueue = new Queue <Vec2i>();
searchQueue.Enqueue(root);
visitedNodes.Add(root);
bool foundDestination = false;
while (searchQueue.Count > 0 && foundDestination == false)
{
var current = searchQueue.Dequeue();
var neighbors = maze.Cells_Connected_To_Cell__List(current);
List <Vec2i> unvisitedNeighbors = neighbors.FindAll(x => visitedNodes.Contains(x) == false);
foreach (var neighbor in unvisitedNeighbors)
{
predecessors[neighbor] = current;
searchQueue.Enqueue(neighbor);
visitedNodes.Add(neighbor);
if (neighbor.Equals(destination))
{
foundDestination = true;
break;
}
}
}
List <Vec2i> shortestPath = new List <Vec2i>();
bool pathFinished = false;
var currentPathPosition = destination;
shortestPath.Add(currentPathPosition);
while (pathFinished == false)
{
var predecessor = predecessors[currentPathPosition];
if (predecessor.Equals(currentPathPosition) == false)
{
shortestPath.Add(predecessor);
currentPathPosition = predecessor;
}
else
{
pathFinished = true;
}
}
shortestPath.Reverse();
if (
shortestPath.Contains(root)
&&
shortestPath.Contains(destination)
)
{
return(shortestPath);
}
return(new List <Vec2i>());
}
