public WeightedEdge(IPathfindingNode <T> start, IPathfindingNode <T> end, float weight, Direction direction)
{
Start = start;
End = end;
Weight = weight;
Direction = direction;
}
public bool RemoveNode(IPathfindingNode n)
{
if (nodes.Remove(n))
{
nodeFrontiers.Remove(n);
return(true);
}
return(false);
}
public bool AddNode(IPathfindingNode n)
{
if (nodes.Contains(n))
{
return(false);
}
nodes.Add(n);
nodeFrontiers.Add(n, new List <GraphEdge>());
return(true);
}
public bool Search(Graph graph, IPathfindingNode source, IPathfindingNode target, IHeuristicCalculator hc)
{
pq.Add(source);
while (pq.count > 0)
{
IPathfindingNode nextClosestNode = pq.PopFirst();
if (searchFrontier.ContainsKey(nextClosestNode))
{
GraphEdge nearestEdge = searchFrontier[nextClosestNode];
if (shortestPahTree.ContainsKey(nextClosestNode))
{
shortestPahTree[nextClosestNode] = nearestEdge;
}
else
{
shortestPahTree.Add(nextClosestNode, nearestEdge);
}
}
if (nextClosestNode == target)
{
ConstructPath(source, target);
return(true);
}
List <GraphEdge> frontier = graph.GetFrontier(nextClosestNode);
for (int i = 0; i < frontier.Count; i++)
{
GraphEdge frontierEdge = frontier[i];
IPathfindingNode neighbour = frontierEdge.to;
float realCost = nextClosestNode.RealNodeCost + frontierEdge.cost;
float heuristicCost = hc.Calculate(neighbour, target);
float totalCost = nextClosestNode.TotalNodeCost + frontierEdge.cost;
if (searchFrontier.ContainsKey(neighbour) == false)
{
neighbour.TotalNodeCost = totalCost;
neighbour.RealNodeCost = realCost;
searchFrontier.Add(neighbour, frontierEdge);
pq.Add(neighbour);
}
else if (realCost < neighbour.RealNodeCost)
{
neighbour.TotalNodeCost = totalCost;
neighbour.RealNodeCost = realCost;
pq.MarkToSort();
searchFrontier[neighbour] = frontierEdge;
}
}
}
return(false);
}
private void ConstructPath(IPathfindingNode source, IPathfindingNode target)
{
GraphEdge edge = shortestPahTree[target];
while (edge.from != source)
{
shortestPath.Add(edge.from);
edge = shortestPahTree[edge.from];
}
shortestPath.Reverse();
}
private GraphEdge FindEdge(IPathfindingNode from, IPathfindingNode to)
{
for (int i = 0; i < edges.Count; i++)
{
GraphEdge e = edges[i];
if (e.from == from && e.to == to)
{
return(e);
}
}
return(null);
}
public static Dictionary <IPathfindingNode, double> GetMovementCostArray(IPathfindingNode startNode, PathfindingType pathfindingType, int maxCost)
{
IPathfindingNode[] neighbourNodes = null;
IPathfindingNode neighbourNode;
cameFromArray.Clear();
costSoFarArray.Clear();
frontier.Clear();
currentNode = null;
frontier.Enqueue(startNode, 0);
cameFromArray[startNode] = null;
costSoFarArray[startNode] = 0.0;
while (frontier.NumItems > 0)
{
currentNode = frontier.Dequeue();
//float startTime = Time.realtimeSinceStartup;
neighbourNodes = currentNode.GetConnectedNodes().ToArray();
for (int i = 0; i < neighbourNodes.Length; i++)
{
neighbourNode = neighbourNodes[i];
if (neighbourNode != null && neighbourNode != startNode &&
(neighbourNode.IsEmpty || pathfindingType == PathfindingType.Air))
{
//stepCost = GetTransportTimeToNextTileInDirection(currentPathfindingNode, i, isDirect);
stepCost = 1;
costSoFarArray.TryGetValue(neighbourNode, out currentLowestCost);
newCost = currentLowestCost + stepCost;
if ((currentLowestCost <= 0 || newCost < currentLowestCost) && newCost <= maxCost)
{
costSoFarArray[neighbourNode] = newCost;
//if (pathfindingType == PathfindingType.Tile)
//{
//priority = 1.0 / (newCost + heuristic(endPathfindingNode, neighbourPathfindingNode));
//} else
//{
priority = 1.0 / newCost;
//}
frontier.Enqueue(neighbourNode, priority);
cameFromArray[neighbourNode] = currentNode;
}
}
}
}
return(costSoFarArray);
}
public List <IPathfindingNode> FindPath(IPathfindingNode source, IPathfindingNode target, IHeuristicCalculator h)
{
var shortestPath = new List <IPathfindingNode>();
AStarGraphSearch graphSearch = new AStarGraphSearch();
bool success = graphSearch.Search(_graph, source, target, h);
if (success)
{
shortestPath.Clear();
shortestPath.Add(source);
shortestPath.AddRange(graphSearch.shortestPath);
shortestPath.Add(target);
}
return(shortestPath);
}
public NodeConnection GetConnectionToNode(IPathfindingNode destinationNode)
{
_nodePaths.TryGetValue((Node)destinationNode, out var returnConnection);
return(returnConnection);
}
public static Path GetPathOfTypeForUnit(IPathfindingNode startNode, IPathfindingNode endNode,
PathfindingType pathfindingType, NodeUnit unit)
{
IPathfindingNode[] neighbourNodes = null;
IPathfindingNode neighbourNode;
cameFromArray.Clear();
costSoFarArray.Clear();
frontier.Clear();
currentNode = null;
frontier.Enqueue(startNode, 0);
cameFromArray[startNode] = null;
costSoFarArray[startNode] = 0.0;
while (frontier.NumItems > 0)
{
currentNode = frontier.Dequeue();
//float startTime = Time.realtimeSinceStartup;
if (currentNode == endNode)
{
//float endTime = Time.realtimeSinceStartup;
//float totalTime = endTime - startTime;
path = new Path(startNode, currentNode, cameFromArray, costSoFarArray);
return(path);
}
neighbourNodes = currentNode.GetConnectedNodes().ToArray();
for (int i = 0; i < neighbourNodes.Length; i++)
{
neighbourNode = neighbourNodes[i];
if (neighbourNode != null && neighbourNode != startNode &&
(!neighbourNode.BlocksUnit(unit) || pathfindingType == PathfindingType.Air))
{
stepCost = currentNode.GetConnectionToNode(neighbourNode)._movementPointCost;
bool hasFoundCost = costSoFarArray.TryGetValue(neighbourNode, out currentLowestCost);
var costSoFar = costSoFarArray[currentNode];
newCost = costSoFar + stepCost;
if (currentLowestCost <= 0 || newCost < currentLowestCost)
{
costSoFarArray[neighbourNode] = newCost;
//if (pathfindingType == PathfindingType.Tile)
//{
//priority = 1.0 / (newCost + heuristic(endPathfindingNode, neighbourPathfindingNode));
//} else
//{
priority = 1.0 / newCost;
//}
frontier.Enqueue(neighbourNode, priority);
cameFromArray[neighbourNode] = currentNode;
}
}
}
}
return(null);
}
private static double Heuristic(IPathfindingNode endNode, IPathfindingNode nextNode)
{
//heading = endPathfindingNode.transform.position - nextPathfindingNode.transform.position;
heading = endNode.PositionForHeuristic - nextNode.PositionForHeuristic;
return(heading.magnitude);
}
public GraphEdge(IPathfindingNode from, IPathfindingNode to, float cost)
{
this.to = to;
this.from = from;
this.cost = cost;
}
public List <GraphEdge> GetFrontier(IPathfindingNode n)
{
return(nodeFrontiers[n]);
}
public void AddNodeToGraph(IPathfindingNode node)
{
_graph.AddNode(node);
}