public static void Fill(Node position, int range, SuperNode supernode)
{
var openQueue = new Utils.PriorityQueue <VisitedNode>();
var pathNodeMap = new Dictionary <Node, VisitedNode>();
pathNodeMap[position] = new VisitedNode(position, null, 0);
openQueue.Enqueue(pathNodeMap[position], 0);
while (!openQueue.IsEmpty())
{
var current = openQueue.Dequeue();
current.GridNode.ConnectSuperNode(current.Prev != null ? current.Prev.GridNode : null, supernode, current.GScore);
foreach (var neighbour in current.GridNode.GetNeighbours())
{
if (!neighbour.To.SuperNodes.ContainsKey(supernode) && neighbour.Length + current.GScore <= range)
{
var newNode = new VisitedNode(neighbour.To, current, neighbour.Length);
if (pathNodeMap.ContainsKey(neighbour.To))
{
if (openQueue.Update(pathNodeMap[neighbour.To], pathNodeMap[neighbour.To].GScore, newNode, newNode.GScore))
{
pathNodeMap[neighbour.To] = newNode;
}
}
else
{
openQueue.Enqueue(newNode, newNode.GScore);
pathNodeMap[neighbour.To] = newNode;
}
}
}
}
}
public static Path GetPath(Dictionary <Node, float> from, List <Node> to, Path path)
{
var openSet = new Utils.PriorityQueue <VisitedNode>();
var pathNodeMap = new Dictionary <Node, VisitedNode>();
var nodesTo = new HashSet <VisitedNode>();
foreach (var node in to)
{
nodesTo.Add(new VisitedNode(node, null, 0));
}
if (nodesTo.Count == 0)
{
return(null);
}
foreach (var f in from)
{
var nodeFrom = new VisitedNode(f.Key, null, f.Value);
openSet.Enqueue(nodeFrom, nodeFrom.GetCost(nodesTo));
}
while (!openSet.IsEmpty())
{
var curNode = openSet.Dequeue();
if (nodesTo.Contains(curNode))
{
path = ReconstructPath(curNode, path);
//Debug.Log("Found path between " + nodeFrom.GridNode.Position + " and " + nodeTo.GridNode.Position + " of length: " + path.Length + " in " + (DateTime.Now-start).TotalMilliseconds + "ms.");
return(path);
}
curNode.Status = NodeStatus.Closed;
foreach (var neighbour in curNode.GridNode.GetNeighbours())
{
if (pathNodeMap.ContainsKey(neighbour.To))
{
var pathNode = pathNodeMap[neighbour.To];
if (pathNode.Status == NodeStatus.Closed)
{
continue;
}
var node = new VisitedNode(neighbour.To, curNode, neighbour.Length);
if (openSet.Update(pathNode, pathNode.GetCost(nodesTo), node, node.GetCost(nodesTo)))
{
pathNodeMap[neighbour.To] = node;
}
}
else
{
var node = new VisitedNode(neighbour.To, curNode, neighbour.Length);
openSet.Enqueue(node, node.GetCost(nodesTo));
pathNodeMap[neighbour.To] = node;
}
}
}
//Debug.Log("Couldn't find path between " + nodeFrom.GridNode + " and " + nodeTo.GridNode + " in " + (DateTime.Now - start).TotalMilliseconds + "ms.");
return(path);
}
public static void FillNeigbours(SuperNode superNode, int gridSize)
{
var nodesToFill = superNode.GetNeighbours().Select(n => n.To).ToArray();
var openQueue = new Utils.PriorityQueue <VisitedNode>();
var pathNodeMap = new Dictionary <Node, VisitedNode>();
foreach (var childNode in superNode.ChildNodes)
{
if (childNode.SuperNodes[superNode].Length > gridSize * 0.8)
{
pathNodeMap[childNode] = new VisitedNode(childNode, null, childNode.SuperNodes[superNode].Length);
openQueue.Enqueue(pathNodeMap[childNode], pathNodeMap[childNode].GScore);
}
}
while (!openQueue.IsEmpty())
{
var current = openQueue.Dequeue();
if (!current.GridNode.SuperNodes.ContainsKey(superNode))
{
current.GridNode.ConnectSuperNode(current.Prev != null ? (current.Prev.GridNode ?? superNode) : superNode, superNode, current.GScore);
}
foreach (var neighbour in current.GridNode.GetNeighbours())
{
if (!neighbour.To.SuperNodes.ContainsKey(superNode) && neighbour.To.SuperNodes.Any(k => k.Value.Length <= gridSize && nodesToFill.Contains(k.Key)))
{
var newNode = new VisitedNode(neighbour.To, current, neighbour.Length);
if (pathNodeMap.ContainsKey(neighbour.To))
{
if (openQueue.Update(pathNodeMap[neighbour.To], pathNodeMap[neighbour.To].GScore, newNode, newNode.GScore))
{
pathNodeMap[neighbour.To] = newNode;
}
}
else
{
openQueue.Enqueue(newNode, newNode.GScore);
pathNodeMap[neighbour.To] = newNode;
}
}
}
}
}
private PathState CalculateAStartPath(int startX, int startY, int startZ, int targetX, int targetY, int targetZ, int searchLimit, List <int> steps)
{
MiniMapSector[] sectors = new MiniMapSector[4];
sectors[0] = AcquireSector(startX - Constants.PathMatrixCenter, startY - Constants.PathMatrixCenter, startZ, false);
sectors[1] = AcquireSector(startX - Constants.PathMatrixCenter, startY + Constants.PathMatrixCenter, startZ, false);
sectors[2] = AcquireSector(startX + Constants.PathMatrixCenter, startY + Constants.PathMatrixCenter, startZ, false);
sectors[3] = AcquireSector(startX + Constants.PathMatrixCenter, startY - Constants.PathMatrixCenter, startZ, false);
// initial sector position (start position in minimap storage)
var sectorMaxX = sectors[0].SectorX + Constants.MiniMapSectorSize;
var sextorMaxY = sectors[0].SectorY + Constants.MiniMapSectorSize;
var closedList = new Dictionary <int, PathItem>();
var openList = new Utils.PriorityQueue <KeyValuePair <PathItem, int> >(Comparer <KeyValuePair <PathItem, int> > .Create(ComparePathNodes));
PathItem currentNode = new PathItem(startX, startY);
closedList.Add(Hash2DPosition(startX, startY), currentNode);
PathItem foundNode = null;
PathState ret = PathState.PathErrorInternal;
while (currentNode != null)
{
if (closedList.Count > searchLimit)
{
ret = PathState.PathErrorTooFar;
break;
}
if (currentNode.X == targetX && currentNode.Y == targetY && (foundNode == null || currentNode.PathCost < foundNode.PathCost))
{
foundNode = currentNode;
}
if (foundNode != null && (currentNode.PathHeuristic >= foundNode.PathCost))
{
break;
}
for (int i = -1; i <= 1; i++)
{
for (int j = -1; j <= 1; j++)
{
if (i == 0 && j == 0)
{
continue;
}
int currentPosX = currentNode.X + i;
int currentPosY = currentNode.Y + j;
int sectorIndex;
if (currentPosX < sectorMaxX)
{
sectorIndex = currentPosY < sextorMaxY ? 0 : 1;
}
else
{
sectorIndex = currentPosY < sextorMaxY ? 3 : 2;
}
int cost = sectors[sectorIndex].GetCost(currentPosX, currentPosY, startZ);
if (cost >= Constants.PathCostObstacle)
{
continue;
}
int modifier = 1;
if ((i * j) != 0)
{
modifier = 3;
}
int pathCost = currentNode.PathCost + modifier * cost;
var direction = DirectionFromPosToPos(currentNode.X, currentNode.Y, currentPosX, currentPosY);
PathItem neighborNode;
if (closedList.TryGetValue(Hash2DPosition(currentPosX, currentPosY), out PathItem handledNode))
{
neighborNode = handledNode;
if (neighborNode.PathCost <= pathCost)
{
continue;
}
}
else
{
neighborNode = new PathItem(currentPosX, currentPosY);
closedList.Add(Hash2DPosition(currentPosX, currentPosY), neighborNode);
}
neighborNode.Predecessor = currentNode;
neighborNode.Cost = cost;
neighborNode.PathCost = pathCost;
neighborNode.PathHeuristic = neighborNode.PathCost + Distance(currentPosX, currentPosY, targetX, targetY);
neighborNode.Direction = direction;
openList.Push(new KeyValuePair <PathItem, int>(neighborNode, neighborNode.PathHeuristic));
}
}
if (openList.Count > 0)
{
currentNode = openList.Pop().Key;
}
else
{
currentNode = null;
}
}
if (foundNode != null)
{
currentNode = foundNode;
while (currentNode != null)
{
steps.Add((int)currentNode.Direction | currentNode.Cost << 16);
if (steps.Count + 1 >= Constants.PathMaxSteps)
{
break;
}
currentNode = currentNode.Predecessor;
}
steps.RemoveAt(steps.Count - 1);
steps.Reverse();
ret = PathState.PathExists;
}
return(ret);
}
