Exemplo n.º 1
0
        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);
                }
            }
        }
Exemplo n.º 2
0
        public void Connect(PlanningNode from, PlanningNode to)
        {
            PlanningEdge edge = new PlanningEdge(from, to);

            mEdges.Add(edge);
            edge.parent = this;
        }
Exemplo n.º 3
0
        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;
        }
Exemplo n.º 4
0
        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);
        }
Exemplo n.º 5
0
 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;
 }
Exemplo n.º 6
0
        public int GetIndex(PlanningNode node)
        {
            for (int i = 0; i < mNodes.Count; i++)
            {
                if (mNodes[i] == node)
                {
                    return(i);
                }
            }

            return(-1);
        }
Exemplo n.º 7
0
        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);
        }
Exemplo n.º 8
0
        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);
        }
Exemplo n.º 9
0
        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);
        }
Exemplo n.º 10
0
        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);
        }
Exemplo n.º 11
0
        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);
        }
Exemplo n.º 12
0
        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);
        }
Exemplo n.º 13
0
        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);
        }
Exemplo n.º 14
0
 public PlanningEdge(PlanningNode from, PlanningNode to)
 {
     _from = from;
     _to   = to;
 }
Exemplo n.º 15
0
        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);
        }
Exemplo n.º 16
0
 public void AddPlanningNode(PlanningNode node, int t)
 {
     _timeLevels[t].AddPlanningNode(node);
 }
Exemplo n.º 17
0
 public void AddPlanningNode(PlanningNode node)
 {
     mNodes.Add(node);
     node.parent = this;
 }