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); } } }