/*******************************************************/ /* Planning - aux */ /*******************************************************/ //used when trying to recover a plan where the agent is not on a platform and state of the plan //join plans on utils //simple plan recovery - check if agent is on a platform that belongs to the plan and with the same state private void recoverPlan() { //TODO - update debug drawing currentAction = Moves.NO_ACTION; lastMove = Moves.NO_ACTION; controller.jumpReached = false; controller.rollReached = false; //see if there is a point in the original plan that is the same as the one the agent is on and that has the same number or less of diamonds caught bool pointFound = false; Platform currentPlatform = utils.onPlatform(circleInfo.X, circleInfo.Y + circleInfo.Radius, 25, 10); List <Point> pathPoints = pathPlan.getOriginalPoints(); int i; //start from the end for (i = pathPoints.Count - 1; i >= 0; i--) { if (pathPoints[i].getUncaughtDiamonds().Count >= uncaughtCollectibles.Count) { Platform pointPlatform = utils.onPlatform(pathPoints[i].getPosX(), pathPoints[i].getPosY() + circleInfo.Radius, 25, 10); if (utils.samePlatform(currentPlatform, pointPlatform) && !utils.obstacleBetween(circleInfo.X, pathPoints[i].getPosX(), currentPlatform)) { pointFound = true; break; } } } if (pointFound) { //create a new plan from the point we got previously pathPlan = new PathPlan(cutplan, remaining.Count, utils); pathPlan.setTotalCollectibles(originalPlan.getTotalCollectibles()); pathPlan.setCurrentPoint(i); for (int j = i; j < pathPoints.Count; j++) { pathPlan.addPointEnd(pathPoints[j]); } pathPlan.setOriginalPoints(pathPoints); firstAction = true; return; } //if no platform in common was found, then try to find a way to a platform in the plan and replan if this fails else if (pathPlan.getPathPoints().Count != 0) { 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); //TEST RRT.setDiamonds(Diamonds); State initialState = new State(circleInfo.X, circleInfo.Y, circleInfo.VelocityX, circleInfo.VelocityY, circleInfo.Radius, circleInfo.Radius, caughtDiamonds, remainingDiamonds); float[] returnPos = new float[2]; returnPos[0] = pathPlan.getPathPoints()[0].getPosX(); returnPos[1] = pathPlan.getPathPoints()[0].getPosY(); RRT.setReturnPos(returnPos); Tree t = RRT.buildNewMPRRT(initialState, sim, GoalType.Return, iterationsS); if (t.getGoal() != null) { PathPlan shortPlan = RRT.getPlan(t); pathPlan = pathPlan.joinPlans(shortPlan, pathPlan); pathPlan.cleanPlan(obstaclesInfo, remainingDiamonds, area, circleInfo.Radius, true, true); getDebugInfo = true; return; } else { replan(false); } } replan(false); }