public int GetExclusiveExpandingNodeNum(PathPlanningGraph graph, HexaPos start)
{
ExpandingTree tree = GetExclusiveExpandingTree(graph, start);
tree.Draw("EXCLUSIVE-EXPANDING-TREE");
return(tree.nodeNum);
}
void UpdateNodeReward(ExpandingNode node, HexaPath path, double[,] entropy, PathPlanningGraph graph)
{
PlanningNode planNode = node.planningNode;
double[,] localEntropy = (double[, ])entropy.Clone();
node.instRwd = GetInstantReward(path, localEntropy, graph);
node.futrRwd = GetEstimatedMaxFutureReward(planNode, path, localEntropy, graph);
// update max val
node.maxVal = node.instRwd + node.futrRwd;
}
public PathPlanningGraph GetPathPlanningGraph(HexaPath path, int radius, HexaPos startPos)
{
int planningLength = path.Length;
PathPlanningGraph planningGraph = new PathPlanningGraph(planningLength);
// create vertex
for (int t = 0; t < planningLength; t++)
{
HexaPos pivot = path[t];
List <HexaPos> hexes = _topologicalGraph.GetMap().GetHexes(pivot.X, pivot.Y, radius, true);
List <HexaPos> .Enumerator e = hexes.GetEnumerator();
while (e.MoveNext())
{
Hex currentHex = _topologicalGraph.GetMap().GetHex(e.Current.X, e.Current.Y);
if (false == _topologicalGraph.GetMap().MapState.IsObstacle(currentHex))
{
PlanningNode node = new PlanningNode(e.Current);
planningGraph.AddPlanningNode(node, t);
}
}
}
// create edge
for (int t = 0; t < planningLength - 1; t++)
{
LevelPartite currentPartite = planningGraph[t];
LevelPartite nextPartite = planningGraph[t + 1];
List <PlanningNode> .Enumerator e1 = currentPartite.mNodes.GetEnumerator();
List <PlanningNode> .Enumerator e2 = nextPartite.mNodes.GetEnumerator();
while (e1.MoveNext())
{
while (e2.MoveNext())
{
if (_topologicalGraph.IsConnected(e1.Current.pos, e2.Current.pos))
{
currentPartite.Connect(e1.Current, e2.Current);
}
}
e2 = nextPartite.mNodes.GetEnumerator();
}
}
return(planningGraph);
}
public HexaPath FindPath(PathPlanningGraph graph, HexaPos start)
{
HexaPath path = null;
double currentScore = 0.0;
PlanningNode startNode = graph[0].GetNode(start);
ExpandingNode root = new ExpandingNode(startNode);
ExpandingTree expandingTree = new ExpandingTree(root);
Console.WriteLine("The number of complete expanding node is " + graph.GetExpandingNodeNumber());
bool exhaustivelyEnumerated = false;
bool stopCritera = false;
int counter = 0;
HexaPath maxPath = null;
double maxScore = 0.0;
do
{
path = ExpandToFindPath(expandingTree, graph, _localEntropy);
if (path == null)
{
stopCritera = true;
}
else
{
currentScore = ScorePath(_agent, _localEntropy, path);
if (currentScore > maxScore)
{
maxScore = currentScore;
maxPath = path;
}
}
//expandingTree.Draw("Expanding-Tree-" + counter.ToString());
counter++;
Console.WriteLine(counter + ", " + maxScore + ", " + expandingTree.nodeNum);
}while((iteratingOnce == false || exhaustivelyEnumerated == true) && (stopCritera == false));
//expandingTree.Draw("Expanding-Tree-N");
Console.WriteLine("The number of node expanded is " + expandingTree.nodeNum);
return(maxPath);
}
double GetInstantReward(HexaPath path, double[,] entropy, PathPlanningGraph graph)
{
double instantReward = 0.0;
// apply path to local entropy and calc instant reward
for (int t = 0; t < path.Length; t++)
{
instantReward += GetEstimation(_agent, entropy, path[t], _map);
HexaPath tempPath = new HexaPath();
tempPath.AddPos(path[t]);
// update entropy
_agent.Update(tempPath, entropy);
}
return(instantReward);
}
public PathPlanningGraph GetPlanningGraph(PathPlanningGraph graph, HexaPos start)
{
PathPlanningGraph newGraph = new PathPlanningGraph(graph);
if (newGraph.planningLength > 0)
{
for (int i = newGraph[0].mNodes.Count - 1; i >= 0; i--)
{
if (newGraph[0].mNodes[i].pos.X != start.X
||
newGraph[0].mNodes[i].pos.Y != start.Y)
{
newGraph.RemovePlanningNode(newGraph[0].mNodes[i], 0);
}
}
}
return(newGraph);
}
ExpandingTree GetExclusiveExpandingTree(PathPlanningGraph graph, HexaPos start)
{
PlanningNode startNode = graph[0].GetNode(start);
ExpandingNode root = new ExpandingNode(startNode);
ExpandingTree expandingTree = new ExpandingTree(root);
bool quit = false;
for (int l = 0; l < graph.planningLength - 1; l++)
{
while (expandingTree.GetNewNodeCountByLevel(l) > 0)
{
quit = false;
for (int i = 0; i < expandingTree.nodeList.Count && quit == false; i++)
{
ExpandingNode currentNode = expandingTree.nodeList[i];
if (currentNode.level == l && currentNode.state == ExpandingNode.STATE.NEW)
{
PlanningNode planNode = currentNode.planningNode;
List <PlanningEdge> edges = graph[l].GetEdges(planNode);
List <PlanningEdge> .Enumerator e2 = edges.GetEnumerator();
while (e2.MoveNext())
{
ExpandingNode newNode = new ExpandingNode(e2.Current.to);
expandingTree.AddToParent(newNode, currentNode);
if (l == graph.planningLength - 2)
{
newNode.state = ExpandingNode.STATE.EXPANDED;
}
}
currentNode.state = ExpandingNode.STATE.EXPANDED;
quit = true;
}
}
}
}
return(expandingTree);
}
public PathPlanningGraph GetPlanningGraph(PathPlanningGraph graph, HexaPos start, HexaPos end)
{
PathPlanningGraph newGraph = GetPlanningGraph(graph, start);
if (newGraph.planningLength > 0)
{
for (int i = newGraph[newGraph.planningLength - 1].mNodes.Count - 1; i >= 0; i--)
{
if (newGraph[newGraph.planningLength - 1].mNodes[i].pos.X != end.X
||
newGraph[newGraph.planningLength - 1].mNodes[i].pos.Y != end.Y)
{
newGraph.RemovePlanningNode(newGraph[newGraph.planningLength - 1].mNodes[i], newGraph.planningLength - 1);
}
}
}
return(newGraph);
}
HexaPath ExpandToFindPath(ExpandingTree tree, PathPlanningGraph graph, double[,] entropy)
{
HexaPath path = null;
int stopLevel = graph.planningLength - 1;
double[,] localEntropy = (double[, ])entropy.Clone();
ExpandingNode start = tree.GetMaxLeafNode(stopLevel);
if (start == null)
{
return(path);
}
ExpandingNode expandingNode = start;
// Get subpath
path = tree.GetPath(start);
UpdateNodeReward(start, path, localEntropy, graph);
// apply path
//_agent.Update(path, localEntropy);
// Expand node till reaching end level
for (int cl = path.Length; cl <= stopLevel; cl++)
{
expandingNode = NodeSpanning(tree, expandingNode, path, entropy, graph, _map);
path.AddPos(expandingNode.planningNode.pos);
}
// score the path and back propagate minVal
double currentScore = ScorePath(_agent, entropy, path);
expandingNode.maxVal = currentScore;
tree.BackPropagateMinVal(expandingNode, currentScore);
tree.Freeze(currentScore);
return(path);
}
public PathPlanningGraph BackwardPrune(PathPlanningGraph graph)
{
PathPlanningGraph newGraph = new PathPlanningGraph(graph);
for (int t = newGraph.planningLength - 1; t >= 0; t--)
{
for (int i = newGraph[t].mNodes.Count - 1; i >= 0; i--)
{
if (t == newGraph.planningLength - 1)
{
if (!newGraph.hasIn(newGraph[t].mNodes[i], t))
{
newGraph.RemovePlanningNode(newGraph[t].mNodes[i], t);
}
}
else if (t == 0)
{
if (!newGraph.hasOut(newGraph[t].mNodes[i], t))
{
newGraph.RemovePlanningNode(newGraph[t].mNodes[i], t);
}
}
else
{
if (!newGraph.hasIn(newGraph[t].mNodes[i], t)
||
!newGraph.hasOut(newGraph[t].mNodes[i], t))
{
newGraph.RemovePlanningNode(newGraph[t].mNodes[i], t);
}
}
}
}
return(newGraph);
}
double GetEstimatedMaxFutureReward(PlanningNode node, HexaPath path, double[,] entropy, PathPlanningGraph graph)
{
int endLevel = graph.planningLength - 1;
int currentLevel = path.Length - 1;
if (endLevel == currentLevel)
{
return(0.0);
}
double maxFutureScore = 0.0;
// backtrack
//start from end level, init future score as 0
int nodeNum = 0;
double[] futureScore = null;
double[] instantScore = null;
double[] totalScore = null;
for (int l = endLevel; l > currentLevel; l--)
{
nodeNum = graph[l].mNodes.Count;
futureScore = new double[nodeNum];
instantScore = new double[nodeNum];
for (int i = 0; i < nodeNum; i++)
{
PlanningNode tempNode = graph[l].mNodes[i];
instantScore[i] = GetEstimation(_agent, entropy, tempNode.pos, _map);
if (l < endLevel)
{
List <PlanningEdge> edges = graph[l].GetEdges(tempNode);
List <PlanningEdge> .Enumerator e = edges.GetEnumerator();
while (e.MoveNext())
{
int j = graph[l + 1].GetIndex(e.Current.to);
if (totalScore[j] > futureScore[i])
{
futureScore[i] = totalScore[j];
}
}
}
else
{
futureScore[i] = 0.0;
}
}
totalScore = new double[nodeNum];
for (int i = 0; i < nodeNum; i++)
{
totalScore[i] = instantScore[i] + futureScore[i];
}
}
// estimate future reward
HexaPos currentPos = node.pos;
List <PlanningEdge> nextEdges = graph[currentLevel].GetEdges(node);
List <PlanningEdge> .Enumerator enumEdge = nextEdges.GetEnumerator();
while (enumEdge.MoveNext())
{
int j = graph[currentLevel + 1].GetIndex(enumEdge.Current.to);
if (totalScore[j] > maxFutureScore)
{
maxFutureScore = totalScore[j];
}
}
return(maxFutureScore);
}
ExpandingNode NodeSpanning(ExpandingTree tree, ExpandingNode node, HexaPath path, double[,] entropy, PathPlanningGraph graph, HexagonalMap map)
{
PlanningNode planNode = node.planningNode;
List <ExpandingNode> newGenerated = new List <ExpandingNode>();
// find all child nodes
int curLevel = path.Length - 1;
if (curLevel < graph.planningLength - 1)
{
List <PlanningEdge> nextEdges = graph[curLevel].GetEdges(planNode);
List <PlanningEdge> .Enumerator enumEd = nextEdges.GetEnumerator();
while (enumEd.MoveNext())
{
ExpandingNode newNode = new ExpandingNode(enumEd.Current.to);
tree.AddToParent(newNode, node);
newGenerated.Add(newNode);
// if new node is already end level,
// set it as EXPANDED
if (curLevel == graph.planningLength - 2)
{
newNode.state = ExpandingNode.STATE.EXPANDED;
}
}
}
// set node to EXPANDED
node.state = ExpandingNode.STATE.EXPANDED;
//update the new generated node
List <ExpandingNode> .Enumerator e2 = newGenerated.GetEnumerator();
while (e2.MoveNext())
{
HexaPath tempPath = tree.GetPath(e2.Current);
double[,] tempEntropy = (double[, ])entropy.Clone();
UpdateNodeReward(e2.Current, tempPath, tempEntropy, graph);
}
//find max node
double maxNodeVal = 0.0;
ExpandingNode maxNode = null;
List <ExpandingNode> .Enumerator e3 = newGenerated.GetEnumerator();
while (e3.MoveNext())
{
if (e3.Current.maxVal > maxNodeVal)
{
maxNode = e3.Current;
maxNodeVal = e3.Current.maxVal;
}
else
{
if (maxNode == null)
{
maxNode = e3.Current;
}
}
}
return(maxNode);
}
public PathPlanningGraph(PathPlanningGraph prevGraph)
{
_planningLength = prevGraph._planningLength;
_timeLevels = (LevelPartite[])prevGraph._timeLevels.Clone();
}
public PathPlanningGraph GetPathPlanningGraph(HexaPos startPos, int planningLength)
{
PathPlanningGraph planningGraph = new PathPlanningGraph(planningLength);
List <HexaPos> currentSet = new List <HexaPos>();
List <HexaPos> nextSet = new List <HexaPos>();
currentSet.Add(startPos);
PlanningNode start_node = new PlanningNode(startPos);
planningGraph.AddPlanningNode(start_node, 0);
// create vertex
for (int t = 1; t < planningLength; t++)
{
List <HexaPos> .Enumerator e1 = currentSet.GetEnumerator();
nextSet.Clear();
while (e1.MoveNext())
{
HexaPos currentPos = e1.Current;
List <HexaPos> hexes = _topologicalGraph.GetMap().GetHexes(currentPos.X, currentPos.Y, 1, true);
List <HexaPos> .Enumerator e = hexes.GetEnumerator();
while (e.MoveNext())
{
Hex currentHex = _topologicalGraph.GetMap().GetHex(e.Current.X, e.Current.Y);
if (false == _topologicalGraph.GetMap().MapState.IsObstacle(currentHex))
{
if (false == planningGraph[t].hasNode(e.Current.X, e.Current.Y))
{
PlanningNode node = new PlanningNode(e.Current);
planningGraph.AddPlanningNode(node, t);
nextSet.Add(new HexaPos(e.Current.X, e.Current.Y));
}
}
}
}
currentSet.Clear();
foreach (HexaPos pos in nextSet)
{
currentSet.Add(pos);
}
}
// create edge
for (int t = 0; t < planningLength - 1; t++)
{
LevelPartite currentPartite = planningGraph[t];
LevelPartite nextPartite = planningGraph[t + 1];
List <PlanningNode> .Enumerator e1 = currentPartite.mNodes.GetEnumerator();
List <PlanningNode> .Enumerator e2 = nextPartite.mNodes.GetEnumerator();
while (e1.MoveNext())
{
while (e2.MoveNext())
{
if (_topologicalGraph.IsConnected(e1.Current.pos, e2.Current.pos))
{
currentPartite.Connect(e1.Current, e2.Current);
}
}
e2 = nextPartite.mNodes.GetEnumerator();
}
}
return(planningGraph);
}
