public void SinglePUpdate(TimeSpan elapsedGameTime)
{
//Plan
if (planRRT && predictor != null && predictor.CharactersReady())
{
controlling = false;
planSolution();
}
else if (getDebugInfo)
{
if (cutplan)
{
debugInfo = RRT.getDebugInfo(T, pathPlan.cleanPlan(obstaclesInfo, Diamonds, area, circleInfo.Radius, true, true)).ToArray();
}
else
{
debugInfo = RRT.getDebugInfo(T, pathPlan.debugCleanPlan()).ToArray();
}
//debugInfo = RRT.getDebugInfo(T, null).ToArray();
getDebugInfo = false;
}
else if (!getDebugInfo)
{
debugInfo = null;
}
//Control - if there is a plan then execute it
if (pathPlan.getPathPoints() != null && pathPlan.getPathPoints().Count != 0 && hasStarted)
{
controlling = true;
planExecution();
}
else if (pathPlan.getTotalCollectibles() != 0 && controlling && pathPlan.getTotalCollectibles() == uncaughtCollectibles.Count)
{
//if currently the number of uncaught collectibles is the same as the one supposed to be at the end of the plan, stop the agent and replan
replan(false);
}
//make sure the agent replans when missing the last action
else if (pathPlan.getPathPoints().Count == 0 && controlling)// && lastAction)
{
lastActionReplan();
}
//if it none of the above work and the too much time has already passed, replan
else if (controlling && (gameTime.ElapsedMilliseconds * 0.001f * gSpeed > pointTimeMargin * gSpeed))
{
replan(true);
}
else if (pathPlan.getPathPoints().Count == 0 && !controlling && !planRRT)
{
planRRT = true;
newPlan = true;
}
}
/*******************************************************/
/* 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);
}
/********************************************************************************************/
/********************************************************************************************/
/*** ***/
/*** 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();
}
}