/// <summary>
/// Initializes the containers.
/// </summary>
void InitializedArrays()
{
if (Close == null)
{
Close = new CloseContainer();
}
if (Open == null)
{
Open = new OpenContainer(this, usingHeap);
}
if (Incons == null)
{
Incons = new Incons();
}
if (Visited == null)
{
Visited = new VisitedContainer(selectedPlanningDomain);
}
}
//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
}
void UpdateState(ref ADAstarNode currentNode)
{
PlanningDomainBase domain = default(PlanningDomainBase);
List <DefaultAction> possibleTransitions = new List <DefaultAction>();
float score = 0;
foreach (PlanningDomainBase d in _planningDomain)
{
if (d.evaluateDomain(ref startNode.action.state) > score)
{
score = d.evaluateDomain(ref startNode.action.state);
domain = d;
}
}
if (!currentNode.alreadyExpanded)
{
currentNode.g = Mathf.Infinity;
}
if (!domain.isAGoalState(ref currentNode.action.state, ref goalNode.action.state))
{
possibleTransitions.Clear();
domain.generateTransitions(ref currentNode.action.state, ref currentNode.previousState, ref goalNode.action.state, ref possibleTransitions);
// Determine min(c(s,s')+g(s')) for rhs for every successor
float min_rhs = Mathf.Infinity;
foreach (DefaultAction action in possibleTransitions)
{
DefaultAction nextAction = action;
float newh = domain.ComputeEstimate(ref startNode.action.state, ref nextAction.state, "h");
float newg = domain.ComputeEstimate(ref nextAction.state, ref goalNode.action.state, "g");
//g is calculated as the distance to the goal, just use a dummy value -1.0 and calculate the distance next
ADAstarNode nextNode = new ADAstarNode(newg, newh, ref currentNode.action.state, ref nextAction);
if ((nextAction.cost + nextNode.g) < min_rhs)
{
min_rhs = nextAction.cost + nextNode.g;
}
}
currentNode.rhs = min_rhs;
float[] keys = GetKey(currentNode);
currentNode.key1 = keys[0]; currentNode.key2 = keys[1];
}
Debug.Log("A");
//If open contains node, remove it.
//foreach(KeyValuePair<DefaultState, ADAstarNode> keyval in Open)
for (int i = 0; i < Open.Count; ++i)
{
if (Open[i].Key != null)
{
if (domain.equals(Open[i].Key, currentNode.action.state, false))
{
Open.RemoveAt(i); currentNode.alreadyExpanded = true;
}
}
}
//Open = BackUp;
//KeyValuePair<DefaultState, ADAstarNode> keyval = new KeyValuePair<DefaultState, ADAstarNode>(currentNode.action.state, currentNode);
//if(Open.Contains(keyval)) {Open.Remove(keyval); currentNode.alreadyExpanded = true;}
if (currentNode.g != currentNode.rhs)
{
bool containsNode = false;
//foreach(DefaultState key in Closed.Keys)
//{
//if(domain.equals(key, currentNode.action.state))
//if(domain.equals(key, currentNode.action.state, false))
//{ containsNode = true; break; }
//}
if (Closed.ContainsKey(currentNode.action.state))
{
containsNode = true;
}
if (!containsNode)
{
//Generate all predecessors to keep expanding the open list
generateNodePredecessors(ref domain, ref currentNode);
Open.Add(new KeyValuePair <DefaultState, ADAstarNode>(currentNode.action.state, currentNode));
//Sort by priority keys
Open.Sort(ADAstartCopareCost.CompareCost);
}
else
{
Incons.Add(currentNode.action.state, currentNode);
}
}
}
