public ExpandingTree(ExpandingNode root)
{
nodeList = new List <ExpandingNode>();
rootNode = root;
root.AddToParent(null);
nodeList.Add(root);
}
public void AddToParent(ExpandingNode node, ExpandingNode parent)
{
if (parent != null)
{
node.AddToParent(parent);
nodeList.Add(node);
}
}
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;
}
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;
}
}
return(maxNode);
}
public ExpandingNode(PlanningNode node)
{
planningNode = node;
parentNode = null;
childrenNodes = new List <ExpandingNode>();
minVal = 0.0;
maxVal = Double.MaxValue;
level = -1;
instRwd = 0.0;
futrRwd = 0.0;
state = STATE.NEW;
}
public void AddToParent(ExpandingNode parent)
{
parentNode = parent;
if (parent != null)
{
parent.childrenNodes.Add(this);
level = parent.level + 1;
}
else
{
level = 0;
}
}
public HexaPath GetPath(ExpandingNode node)
{
HexaPath path = new HexaPath();
ExpandingNode currentNode = node;
while (currentNode != null)
{
path.InsertFront(currentNode.planningNode.pos);
currentNode = currentNode.parentNode;
}
return(path);
}
public void BackPropagateMinVal(ExpandingNode node, double val)
{
// set self minVal
node.minVal = val;
ExpandingNode parent = node.parentNode;
// stop at the root
while (parent != null && parent.minVal < val)
{
parent.minVal = val;
parent = parent.parentNode;
}
}
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);
}
HexaPath GetMaxPath(ExpandingTree tree)
{
HexaPath path = new HexaPath();
ExpandingNode root = tree.GetRoot();
path.AddPos(root.planningNode.pos);
ExpandingNode nextChild = tree.GetMaxChild(root);
while (nextChild != null)
{
path.AddPos(nextChild.planningNode.pos);
nextChild = tree.GetMaxChild(nextChild);
}
return(path);
}
public ExpandingNode GetMaxChild(ExpandingNode node)
{
ExpandingNode maxChild = null;
double maxVal = -0.1;
List <ExpandingNode> .Enumerator e = node.childrenNodes.GetEnumerator();
while (e.MoveNext())
{
if (e.Current.maxVal > maxVal)
{
maxChild = e.Current;
maxVal = e.Current.maxVal;
}
}
return(maxChild);
}
private void FormatVertex(object sender, FormatVertexEventArgs <ExpandingNode> e)
{
ExpandingNode ent = e.Vertex;
//e.VertexFormatter.Label = ent.GetName();
e.VertexFormatter.Shape = GraphvizVertexShape.Record;
e.VertexFormatter.Style = GraphvizVertexStyle.Filled;
switch (ent.state)
{
case ExpandingNode.STATE.NEW:
e.VertexFormatter.FillColor = Color.Orange;
break;
case ExpandingNode.STATE.EXPANDED:
e.VertexFormatter.FillColor = Color.Red;
break;
case ExpandingNode.STATE.FREEZED:
e.VertexFormatter.FillColor = Color.LightBlue;
break;
}
GraphvizRecord rec = new GraphvizRecord();
GraphvizRecordCell name = new GraphvizRecordCell();
name.Text = ent.GetName();
GraphvizRecordCell maxScore = new GraphvizRecordCell();
maxScore.Text = ent.maxVal.ToString();
GraphvizRecordCell minScore = new GraphvizRecordCell();
minScore.Text = ent.minVal.ToString();
rec.Cells.Add(name);
rec.Cells.Add(maxScore);
rec.Cells.Add(minScore);
e.VertexFormatter.Record = rec;
}
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);
}
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 ExpandingNode GetMaxLeafNode(int stopLevel)
{
List <ExpandingNode> .Enumerator e = nodeList.GetEnumerator();
double maxVal = -0.1;
ExpandingNode maxLeafNode = null;
while (e.MoveNext())
{
// if it is less than stop level and child count = 0
if (e.Current.level < stopLevel && e.Current.childrenNodes.Count == 0)
{
// only expanding NEW node
if (e.Current.state == ExpandingNode.STATE.NEW)
{
if (e.Current.maxVal > maxVal)
{
maxLeafNode = e.Current;
}
}
}
}
return(maxLeafNode);
}