/*******************************************************/
/* 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);
}