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