/// <summary>
/// Updates the vertex.
/// </summary>
/// <param name='node'>
/// Node to be updated
/// </param>
void UpdateVertex(ARAstarNode node)
{
//[06]
if (!currentStart.Equals(node.action.state))
{
float minCost = Mathf.Infinity;
//List<DefaultState> neighbors = new List<DefaultState>();
//domain.generateNeighbors(node.action.state, ref neighbors);
List <DefaultAction> transitions = new List <DefaultAction>();
selectedPlanningDomain.generatePredecessors(node.action.state, ref transitions);
foreach (DefaultAction action in transitions)
{
if (Visited.ContainsState(action.state)) //Was visited
{
if (Visited.nodeForState(action.state).g + action.cost < minCost && !Visited.nodeForState(action.state).previousState.Equals(node.action.state))
{
minCost = Visited.nodeForState(action.state).g + action.cost;
node.previousState = action.state;
}
}
}
//node.action = new ARAstarAction(node.previousState, node.action.state);
node.action = selectedPlanningDomain.generateAction(node.previousState, node.action.state);
node.rhs = minCost;
}
if (selectedPlanningDomain.equals(node.action.state, goalState, false))
{
goalPair = new KeyValuePair <DefaultState, ARAstarNode> (node.action.state, node);
}
if (Open.ContainsState(node.action.state))
{
Open.Remove(node.action.state);
}
if (node.g != node.rhs)
{
if (!Close.Contains(node.action.state))
{
Open.Insert(node);
}
else
{
Incons.Insert(node);
}
}
Visited.insertNode(ref node);
}
public void Fill(ref CloseContainer Close, Dictionary <DefaultState, ARAstarNode> Visited, ref DefaultState stateReached, PlanningDomainBase domain, ref DefaultState current, ref KeyValuePair <DefaultState, ARAstarNode> goalPair, float inflationFactor)
{
//DefaultState s = goalPair.Key;
//Close.Insert(goalPair.Value);
plan.Clear();
DefaultState s;
if (Visited.ContainsKey(goalPair.Key))
{
s = stateReached = goalPair.Key;
}
else
{
s = stateReached;
}
DefaultAction a;
bool done = false;
/*foreach(ARAstarNode planNode in plan.Values)
* {
* Close.Insert(planNode);
* }
* plan.Clear();
*
* // TODO : check if we still need this function
* Close.UpdateReferences(inflationFactor, domain);*/
do
{
if (domain.equals(s, current, false))
{
done = true;
}
if (Visited.ContainsKey(s))
{
plan[s] = Visited[s];
s = Visited[s].previousState;
}
else
{
break;
}
} while (!done);
//updatePlanReference(domain);
}
public void Fill(ref CloseContainer Close, Dictionary<DefaultState, ARAstarNode> Visited, ref DefaultState stateReached, PlanningDomainBase domain, ref DefaultState current, ref KeyValuePair<DefaultState, ARAstarNode> goalPair, float inflationFactor)
{
//DefaultState s = goalPair.Key;
//Close.Insert(goalPair.Value);
plan.Clear();
DefaultState s;
if(Visited.ContainsKey(goalPair.Key))
s = stateReached = goalPair.Key;
else
s = stateReached;
DefaultAction a;
bool done = false;
/*foreach(ARAstarNode planNode in plan.Values)
{
Close.Insert(planNode);
}
plan.Clear();
// TODO : check if we still need this function
Close.UpdateReferences(inflationFactor, domain);*/
do {
if (domain.equals (s, current, false))
done = true;
if(Visited.ContainsKey(s)){
plan[s] = Visited[s];
s = Visited[s].previousState;
}
else{
break;
}
} while (!done);
//updatePlanReference(domain);
}
void ComputeorImprovePath(ref PlanningDomainBase domain, float maxTime)
{
int i = 0;
int nodesExpanded = 0;
Debug.Log("BLLLLLLLAAAAAAAAAAA");
float prevTime = Time.realtimeSinceStartup;
Debug.Log("OPEN: " + Open.Count);
//Open is sorted with lowest key first, so first element always has the smallest key
//while((ADAstarPlanner.startFound==false) && (nodesExpanded < maxNodes) && (maxTime > 0))
while ((CompareKey(GetKey(Open.First().Value), GetKey(startNode)) == -1 || startNode.rhs != startNode.g) &&
(nodesExpanded < maxNodes) && (maxTime > 0))
{
i++;
Debug.Log("Planning: " + i);
if (i == 48)
{
i = i;
}
if (domain.equals(Open.First().Key, startNode.action.state, true) || startNode.rhs == startNode.g)
{
ADAstarPlanner.startFound = true; Debug.Log("FOUND"); break;
}
KeyValuePair <DefaultState, ADAstarNode> currentPair = Open.First();
ADAstarNode currentNode = currentPair.Value;
Open.Remove(currentPair);
currentNode.alreadyExpanded = true;
nodesExpanded++;
if (currentNode.g > currentNode.rhs)
{
currentNode.g = currentNode.rhs;
Closed.Add(currentNode.action.state, currentNode);
//For all s in pred(s) updateState
foreach (ADAstarNode predecessor in currentNode.predecessors)
{
ADAstarNode pred = predecessor;
UpdateState(ref pred);
}
}
else
{
currentNode.g = Mathf.Infinity;
//For all s in pred(s) U s updateState
UpdateState(ref currentNode);
foreach (ADAstarNode predecessor in currentNode.predecessors)
{
ADAstarNode pred = predecessor;
UpdateState(ref pred);
}
}
float actualTime = Time.realtimeSinceStartup;
maxTime -= (actualTime - prevTime);
prevTime = actualTime;
}
}
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);
}
}
}
