/// <summary>
/// Performs the necessary updates after start moved.
/// </summary>
/// <param name='currentState'>
/// Current state.
/// </param>
public void UpdateAfterStartMoved(DefaultState currentState)
{
if (Plan.ContainsState(currentState))
{
float actionCost = Plan.Node(currentState).action.cost;
foreach (ARAstarNode node in Plan.Elements().Values)
{
node.g -= actionCost;
}
Visited.UpdateList(Plan.Node(currentState));
}
else
{
// TODO : what if the current state is not part of the plan ?
// can we treat this like the obstacle movement splitting up the search graph
firstTime = true;
inflationFactor = 2.5f;
Open.Clear();
Close.Clear();
Plan.Clear();
Incons.Clear();
createStartNode(ref currentState, ref goalState);
Open.Insert(startNode);
Visited.insertNode(ref startNode);
Open.startState = startNode.action.state;
}
moved = false;
}
//An edge is just a node using previous state, action cost as the edge cost, and action state
public void InifiniteUpdate(ref Dictionary <DefaultState[], Edge> edges, ref Dictionary <DefaultState, ADAstarNode> nodes, ref Stack <DefaultAction> plan, float maxTime, ref DefaultState startState)
{
float score = 0.0f;
PlanningDomainBase domain = default(PlanningDomainBase);
foreach (PlanningDomainBase d in _planningDomain)
{
if (d.evaluateDomain(ref startState) > score)
{
score = d.evaluateDomain(ref startState);
domain = d;
}
}
/*float maxChange = 0.0f;
* if(changeInEdgeCost || inflationFactor != 1.0f)
* {
*
* foreach(Edge edge in edges.Values)
* {
* if(edge.previousCost != edge.cost)
* {
* maxChange = Mathf.Abs(edge.cost - edge.previousCost);
* edge.previousCost = edge.cost;
* ADAstarNode n = nodes[edge.u];
* UpdateState(ref n);
* }
* }
* }
*
* if(maxChange > edgeChangeThreshold) //Decide either increase inflation factor or replan from scratch
* {
* if(PLANNER_MODE.MODE.Equals(PlannerMode.PLANNING_MODE.IncreaseFactor)){
* inflationFactor += .1f;
* }
* else if(PLANNER_MODE.MODE.Equals(PlannerMode.PLANNING_MODE.FromScratch)){
* ComputeorImprovePath(ref domain, maxTime );
* }
* // inflationFactor += .5; // Search for a good value to increase
* //OR
* //ComputeorImprovePath();
* } */
//else
if (inflationFactor > 1.0f)
{
inflationFactor -= .2f; //Decide decrease amount
}
//Decrease inflationFactor by some amount
//Move states from incons to open
foreach (KeyValuePair <DefaultState, ADAstarNode> keyVal in Incons)
{
Open.Add(keyVal);
}
Incons.Clear();
foreach (KeyValuePair <DefaultState, ADAstarNode> keyval in Open)
{
keyval.Value.key1 = GetKey(keyval.Value)[0];
keyval.Value.key2 = GetKey(keyval.Value)[1];
}
Open.Sort(ADAstartCopareCost.CompareCost);
//Closed.Clear();
//computeimprove path
ComputeorImprovePath(ref domain, maxTime);
//publish solution
}