public void AddNode(GoalNode node)
{
base.AddNode(node);
if (node.Value.EntType.IsGoal())
{
PositionToGoalNode.Add(node.Value.Ent.Pos, node);
}
else if (node.Value.EntType.IsMoveable())
{
PositionToMoveableNode.Add(node.Value.Ent.Pos, node);
}
else
{
throw new Exception("GoalGraph does not support any other entity type than goal, box and agent.");
}
}
private Dictionary <GoalNode, List <GoalNode> > CreateDirectedEdgesToStart(GoalNode startNode)
{
Queue <GoalNode> frontier = new Queue <GoalNode>();
Dictionary <GoalNode, List <GoalNode> > childToParent = new Dictionary <GoalNode, List <GoalNode> >();
Dictionary <GoalNode, int> nodeDepths = new Dictionary <GoalNode, int>();
frontier.Enqueue(startNode);
childToParent.Add(startNode, null);
nodeDepths.Add(startNode, 0);
while (frontier.Count > 0)
{
GoalNode leaf = frontier.Dequeue();
int depth = nodeDepths[leaf];
if (leaf.Value.EntType.IsMoveable())
{
continue;
}
foreach (var child in leaf.Edges)
{
GoalNode goalChild = (GoalNode)child.End;
if (nodeDepths.ContainsKey(goalChild) && nodeDepths[goalChild] <= depth)
{
continue;
}
if (!childToParent.ContainsKey(goalChild))
{
childToParent.Add(goalChild, new List <GoalNode>()
{
leaf
});
frontier.Enqueue(goalChild);
nodeDepths.Add(goalChild, depth + 1);
}
else
{
childToParent[goalChild].Add(leaf);
}
}
}
return(childToParent);
}
private int GetShortestPathsData(GoalNode goal, Dictionary <GoalNode, List <GoalNode> > childToParent, Dictionary <GoalNode, int> shortestPathsVisitedNodes, HashSet <Entity> toIgnore, int multiplier)
{
int shortestPathsCount = 1;
Stack <GoalNode> backtrackPaths = new Stack <GoalNode>();
backtrackPaths.Push(goal);
while (backtrackPaths.Count > 0)
{
GoalNode pathEnd = backtrackPaths.Pop();
List <GoalNode> parents = childToParent[pathEnd];
int validParentsCount = 0;
if (parents != null)
{
foreach (var node in parents)
{
if (!toIgnore.Contains(node.Value.Ent))
{
backtrackPaths.Push(node);
validParentsCount++;
}
}
shortestPathsCount += validParentsCount - 1;
}
else
{
validParentsCount = 1;
}
if (pathEnd.Value.EntType.IsGoal())
{
shortestPathsVisitedNodes.TryAdd(pathEnd, 0);
shortestPathsVisitedNodes[pathEnd] += validParentsCount * multiplier;
}
}
return(shortestPathsCount * multiplier);
}
private void CreateGoalPriority(Level level, GoalGraph goalGraph, CancellationToken cancel)
{
Dictionary <GoalNode, Dictionary <GoalNode, List <GoalNode> > > nodeGraphs = new Dictionary <GoalNode, Dictionary <GoalNode, List <GoalNode> > >();
foreach (var goal in level.Goals)
{
GoalNode node = goalGraph.GetGoalNodeFromPosition(goal.Ent.Pos);
nodeGraphs.Add(node, CreateDirectedEdgesToStart(node));
}
Graph groupedGraph = Graph.CreateSimplifiedGraph <EmptyEdgeInfo>(goalGraph);
//GraphShower.ShowGraph(groupedGraph);
List <List <GoalNode> > boxGroups = new List <List <GoalNode> >();
List <List <GoalNode> > agentGroups = new List <List <GoalNode> >();
foreach (var iNode in groupedGraph.Nodes)
{
var node = (Node <NodeGroup, EmptyEdgeInfo>)iNode;
var boxes = node.Value.Nodes.Cast <GoalNode>().Where(x => x.Value.EntType == EntityType.BOX);
if (boxes.Any())
{
boxGroups.Add(boxes.ToList());
}
var agents = node.Value.Nodes.Cast <GoalNode>().Where(x => x.Value.EntType == EntityType.AGENT);
if (agents.Any())
{
agentGroups.Add(agents.ToList());
}
}
HashSet <Entity> toIgnore = new HashSet <Entity>();
HashSet <GoalNode> toIgnoreNodes = new HashSet <GoalNode>();
Dictionary <GoalNode, (int pathsCount, Dictionary <GoalNode, int> pathNodes)> cachedPathResults = new Dictionary <GoalNode, (int pathsCount, Dictionary <GoalNode, int> pathNodes)>();
while (toIgnore.Count < level.Goals.Length)
{
Dictionary <GoalNode, float> nodeCounter = new Dictionary <GoalNode, float>();
foreach (var inode in goalGraph.Nodes)
{
var node = (GoalNode)inode;
if (node.Value.EntType.IsGoal() && !toIgnore.Contains(node.Value.Ent))
{
nodeCounter.Add((GoalNode)node, 0);
}
}
foreach (Goal goal in level.Goals)
{
cancel.ThrowIfCancellationRequested();
if (toIgnore.Contains(goal.Ent))
{
continue;
}
GoalNode start = goalGraph.GetGoalNodeFromPosition(goal.Ent.Pos);
(int pathsCount, Dictionary <GoalNode, int> pathNodes)pathResult;
if (!cachedPathResults.TryGetValue(start, out pathResult) || toIgnoreNodes.Any(x => pathResult.pathNodes.ContainsKey(x)))
{
Dictionary <GoalNode, int> shortestPathsVisitedNodesCount = new Dictionary <GoalNode, int>();
int pathsCount = 0;
if (goal.EntType == EntityType.AGENT_GOAL)
{
foreach (var agentGroup in agentGroups)
{
cancel.ThrowIfCancellationRequested();
int agentsWithSameType = 0;
foreach (var agentNode in agentGroup)
{
if (agentNode.Value.Ent.Type == goal.Ent.Type)
{
agentsWithSameType++;
}
}
if (agentsWithSameType > 0)
{
pathsCount += GetShortestPathsData(agentGroup.First(), nodeGraphs[start], shortestPathsVisitedNodesCount, toIgnore, agentsWithSameType);
}
}
}
else if (goal.EntType == EntityType.BOX_GOAL)
{
foreach (var boxGroup in boxGroups)
{
cancel.ThrowIfCancellationRequested();
int boxesWithSameType = 0;
foreach (var boxNode in boxGroup)
{
if (boxNode.Value.Ent.Type == goal.Ent.Type)
{
boxesWithSameType++;
}
}
if (boxesWithSameType > 0)
{
pathsCount += GetShortestPathsData(boxGroup.First(), nodeGraphs[start], shortestPathsVisitedNodesCount, toIgnore, boxesWithSameType);
}
}
}
else
{
throw new Exception($"Unknown entity type: {goal.EntType}");
}
pathResult = (pathsCount, shortestPathsVisitedNodesCount);
cachedPathResults[start] = pathResult;
}
foreach (var pathNode in pathResult.pathNodes)
{
nodeCounter[pathNode.Key] += (1f / (pathResult.pathsCount)) * pathNode.Value;
}
}
//foreach (var stuff in nodeCounter)
//{
// Console.WriteLine("Key: " + stuff.Key + " Priotity: " + stuff.Value);
//}
//Console.ReadLine();
GoalNode[] newPriorityGroup = nodeCounter.GroupBy(x => x.Value).OrderBy(x => x.First().Value).First().Select(x => x.Key).ToArray();
PriorityLayers.Add(newPriorityGroup);
foreach (var priorityNode in newPriorityGroup)
{
toIgnore.Add(priorityNode.Value.Ent);
toIgnoreNodes.Add(goalGraph.GetGoalNodeFromPosition(priorityNode.Value.Ent.Pos));
}
}
}
