Exemplo n.º 1
0
        /********************************************************************************************/
        /********************************************************************************************/
        /***                                                                                      ***/
        /***                                     PLANNING                                         ***/
        /***                                                                                      ***/
        /********************************************************************************************/
        /********************************************************************************************/

        private void planSolution()
        {
            //The agent must be still so it starts at the same position as the one in the first point of the plan
            //This is to guarantee that the agent stops before start planning, and keeps still
            if (rectangleInfo.VelocityY < correctVelYMargin && rectangleInfo.VelocityY > -correctVelYMargin &&
                rectangleInfo.VelocityX < correctVelXMargin && rectangleInfo.VelocityX > -correctVelXMargin)
            {
                //make sure there is nothing moving the agent when planning
                currentAction = Moves.NO_ACTION;

                //if the plan is new build a new tree
                if (newPlan)
                {
                    //update the diamond list
                    RRT.setDiamonds(Diamonds);
                    State initialState = new State(rectangleInfo.X, rectangleInfo.Y, rectangleInfo.VelocityX, rectangleInfo.VelocityY, rectangleInfo.Height / 2, 0, caughtCollectibles, uncaughtCollectibles);
                    //run algorithm
                    T = RRT.buildNewRRT(initialState, predictor, iterationsS);
                }
                else //continue the previous tree
                {
                    T = RRT.RRT(T);
                }
                //draw the nodes of the tree
                if (debugTree)
                {
                    debugInfo = RRT.getDebugTreeInfo(T).ToArray();
                }

                newPlan = false;

                //if the argorithm reached a goal state or a semi plan then get the plan
                if (T.getGoal() != null)
                {
                    if (!written)
                    {
                        int exploredNodesOnce  = RRT.getExploredNodesOnce();
                        int exploredNodesTotal = RRT.getExploredNodesTotal();
                        int totalNodes         = T.getNodes().Count;
                        utils.writeTimeToFile(1, 1, searchTime, exploredNodesOnce, exploredNodesTotal, totalNodes, gSpeed);
                        written = true;
                    }

                    pathPlan = RRT.getPlan(T);

                    firstAction = true;

                    //do not plan on the next iteration
                    planRRT      = false;
                    getDebugInfo = true;

                    //save a copy of the original plan
                    originalPlan = pathPlan.clone();
                    pathPlan.saveOriginal();
                }
            }
            else
            {
                keepStill();
            }
        }
        /********************************************************************************************/
        /********************************************************************************************/
        /***                                                                                      ***/
        /***                                     PLANNING                                         ***/
        /***                                                                                      ***/
        /********************************************************************************************/
        /********************************************************************************************/

        private void planSolution()
        {
            //The agent must be still so it starts at the same position as the one in the first point of the plan
            //This is to guarantee that the agent stops before start planning, and keeps still
            if (circleInfo.VelocityY < correctVelYMargin && circleInfo.VelocityY > -correctVelYMargin &&
                circleInfo.VelocityX < correctVelXMargin && circleInfo.VelocityX > -correctVelXMargin &&
                utils.onPlatform(circleInfo.X, circleInfo.Y + circleInfo.Radius, 25, 10) != null)
            {
                //make sure there is nothing moving the agent when planning
                currentAction = Moves.NO_ACTION;

                //if the plan is new build a new tree
                if (newPlan)
                {
                    List <DiamondInfo> remainingDiamonds = new List <DiamondInfo>();
                    List <DiamondInfo> caughtDiamonds    = new List <DiamondInfo>();
                    foreach (DiamondInfo diamond in Diamonds)
                    {
                        if (!diamond.wasCaught())
                        {
                            remainingDiamonds.Add(diamond);
                        }
                        else
                        {
                            caughtDiamonds.Add(diamond);
                        }
                    }

                    //Simulator
                    Simulator sim = new CircleSimulator(Platforms);
                    sim.setSimulator(circleInfo.X, circleInfo.Y, circleInfo.VelocityX, circleInfo.VelocityY, remainingDiamonds);

                    //update the diamond list
                    RRT.setDiamonds(Diamonds);
                    //create initial state
                    State initialState = new State(circleInfo.X, circleInfo.Y, circleInfo.VelocityX, circleInfo.VelocityY, circleInfo.Radius, circleInfo.Radius, caughtDiamonds, remainingDiamonds);
                    //run algorithm
                    T = RRT.buildNewRRT(initialState, sim, iterationsS);
                }
                else //continue the previous tree
                {
                    T = RRT.RRT(T);
                }
                //draw the nodes of the tree
                if (debugTree)
                {
                    debugInfo = RRT.getDebugTreeInfo(T).ToArray();
                }

                newPlan = false;

                //if the argorithm reached a goal state or a semi plan then get the plan
                if (T.getGoal() != null)
                {
                    if (!written)
                    {
                        int exploredNodesOnce  = RRT.getExploredNodesOnce();
                        int exploredNodesTotal = RRT.getExploredNodesTotal();
                        int totalNodes         = T.getNodes().Count;
                        utils.writeTimeToFile(1, 0, searchTime, exploredNodesOnce, exploredNodesTotal, totalNodes, gSpeed);
                        written = true;
                    }

                    pathPlan = RRT.getPlan(T);

                    firstAction = true;
                    lastMove    = Moves.NO_ACTION;


                    //do not plan on the next iteration
                    planRRT      = false;
                    getDebugInfo = true;

                    //save a copy of the original plan
                    if (cutplan)
                    {
                        pathPlan.cleanPlan(obstaclesInfo, Diamonds, area, circleInfo.Radius, true, true);
                    }
                    originalPlan = pathPlan.clone();
                    pathPlan.saveOriginal();
                }
            }
            else
            {
                keepStill();
            }
        }