public void RemovePlanningNode(PlanningNode node, int t)
{
if (t > 0)
{
for (int i = _timeLevels[t - 1].mEdges.Count - 1; i >= 0; i--)
{
if (_timeLevels[t - 1].mEdges[i].to == node)
{
_timeLevels[t - 1].mEdges.RemoveAt(i);
}
}
}
for (int i = _timeLevels[t].mEdges.Count - 1; i >= 0; i--)
{
if (_timeLevels[t].mEdges[i].from == node)
{
_timeLevels[t].mEdges.RemoveAt(i);
}
}
for (int i = _timeLevels[t].mNodes.Count - 1; i >= 0; i--)
{
if (_timeLevels[t].mNodes[i] == node)
{
_timeLevels[t].mNodes.RemoveAt(i);
}
}
}
public void Connect(PlanningNode from, PlanningNode to)
{
PlanningEdge edge = new PlanningEdge(from, to);
mEdges.Add(edge);
edge.parent = this;
}
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 int GetIndex(PlanningNode node)
{
for (int i = 0; i < mNodes.Count; i++)
{
if (mNodes[i] == node)
{
return(i);
}
}
return(-1);
}
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);
}
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 bool hasOut(PlanningNode node, int level)
{
if (level < 0 || level >= _planningLength - 1)
{
return(false);
}
List <PlanningEdge> .Enumerator e = _timeLevels[level].mEdges.GetEnumerator();
while (e.MoveNext())
{
if (e.Current.from == node)
{
return(true);
}
}
return(true);
}
public List <PlanningEdge> GetEdges(PlanningNode node)
{
List <PlanningEdge> edges = new List <PlanningEdge>();
List <PlanningEdge> .Enumerator e = mEdges.GetEnumerator();
while (e.MoveNext())
{
if (e.Current.from.pos.X == node.pos.X &&
e.Current.from.pos.Y == node.pos.Y)
{
edges.Add(e.Current);
}
}
return(edges);
}
public bool hasEdge(PlanningNode from, PlanningNode to)
{
if (to.parent.Level != from.parent.Level + 1)
{
return(false);
}
List <PlanningEdge> .Enumerator e = from.parent.mEdges.GetEnumerator();
while (e.MoveNext())
{
if (e.Current.from == from && e.Current.to == to)
{
return(true);
}
}
return(false);
}
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);
}
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);
}
public PlanningEdge(PlanningNode from, PlanningNode to)
{
_from = from;
_to = to;
}
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);
}
public void AddPlanningNode(PlanningNode node, int t)
{
_timeLevels[t].AddPlanningNode(node);
}
public void AddPlanningNode(PlanningNode node)
{
mNodes.Add(node);
node.parent = this;
}
