public void RemoveClosed(AStarSharpNode node) { if (_numClosed > 0) { _closed [_lastFoundClosed] = _closed [_numClosed - 1]; } _numClosed--; }
public void RemoveOpened(AStarSharpNode node) { if (_numOpened > 0) { _opened [_lastFoundOpened] = _opened [_numOpened - 1]; } _numOpened--; }
public AStarSharpNode FindClosed(AStarSharpNode node) { for (var i = 0; i < _numClosed; i++) { long care = node.ws.dontcare ^ -1L; if ((node.ws.values & care) == (_closed[i].ws.values & care)) { _lastFoundClosed = i; return(_closed [i]); } } return(null); }
public AStarSharpNode FindClosed(AStarSharpNode node) { for (var i = 0; i < _closed.Count; i++) { // long care = node.ws.dontcare ^ -1L; // if ((node.ws.values & care) == (_closed[i].ws.values & care)) { // return _closed [i]; // } if (node.Equals(_closed [i])) { return(_closed [i]); } } return(null); }
public List <AStarSharpNode> GetPossibleTransitions1(WorldState fr) { var result = new List <AStarSharpNode> (10); for (int i = 0; i < this.numactions; ++i) { // see if precondition is met WorldState pre = this.preConditions[i]; long care = (pre.dontcare ^ -1L); bool met = ((pre.values & care) == (fr.values & care)); if (met) { AStarSharpNode node = new AStarSharpNode(); node.actionname = this.actionNames [i]; node.costSoFar = this.act_costs [i]; node.ws = ApplyPostConditions(this, i, fr); result.Add(node); } } return(result); }
//!< Internal function to reconstruct the plan by tracing from last node to initial node. static AStarSharpNode[] ReconstructPlan(AStarSharpNode goalnode) { var plan = new AStarSharpNode[goalnode.depth - 1]; AStarSharpNode curnode = goalnode; for (var i = 0; i < goalnode.depth - 1; i++) { plan [i] = curnode; curnode = curnode.parent; } // while (!string.IsNullOrEmpty(curnode.actionname)) // { // // plan.Add (curnode); // if (curnode.parent.Target != null) { // curnode = (AStarNode)curnode.parent.Target; // } else { // break; // } // } return(plan); }
public void AddToClosedList(AStarSharpNode node) { _closed[_numClosed++] = (node); }
public void AddToOpenList(AStarSharpNode node) { _opened[_numOpened++] = (node); }
public bool IsClosed(AStarSharpNode node) { return(_closed.Contains(node)); }
public bool IsOpen(AStarSharpNode node) { return(_opened.Contains(node)); }
public static AStarSharpNode[] Plan(ActionPlanner ap, WorldState start, WorldState goal, IStorage storage) { AStarSharpNode currentNode = new AStarSharpNode(); currentNode.ws = start; currentNode.parentws = start; currentNode.costSoFar = 0; // g currentNode.heuristicCost = CalculateHeuristic(start, goal); // h currentNode.costSoFarAndHeurisitcCost = currentNode.costSoFar + currentNode.heuristicCost; // f currentNode.actionname = null; currentNode.parent = null; currentNode.depth = 1; storage.AddToOpenList(currentNode); while (true) { if (!storage.HasOpened()) // Console.WriteLine( "Did not find a path." ); { return(null); } currentNode = storage.RemoveCheapestOpenNode(); // Console.WriteLine ("--------------------------------------\n"); // Console.WriteLine("CurrentNode: " + currentNode); // Console.WriteLine("CurrentState: " + currentNode.ws); // Console.WriteLine(string.Format("Opened: {0} Closed: {1}", storage._opened.Count, storage._closed.Count)); storage.AddToClosedList(currentNode); // Console.WriteLine("CLOSING: " + currentNode); if (goal.Equals(currentNode.ws)) { // Console.WriteLine ("Finished with plan"); return(ReconstructPlan(currentNode)); return(null); } // var actionnames = new string[ActionPlanner.MAXACTIONS ]; // var actioncosts = new int[ ActionPlanner.MAXACTIONS ]; // var to = new WorldState[ ActionPlanner.MAXACTIONS ]; // int numtransitions = ap.GetPossibleTransitions(currentNode.ws, to, actionnames, actioncosts, ActionPlanner.MAXACTIONS ); var neighbours = ap.GetPossibleTransitions1(currentNode.ws); for (var i = 0; i < neighbours.Count; i++) { var cur = neighbours [i]; // Console.WriteLine("Processing {0} -> {1}", currentNode.actionname, cur.actionname); // Console.WriteLine("State: " + cur.ws); var opened = storage.FindOpened(cur); var closed = storage.FindClosed(cur); int cost = currentNode.costSoFar + cur.costSoFar; // Console.WriteLine("Cost: {0} Idx Opened: {1} Idx Closed: {2}", cost, opened, closed); // if neighbor in OPEN and cost less than g(neighbor): if (opened != null && cost < opened.costSoFar) { // Console.WriteLine("OPENED Neighbor: " + opened.Value.ws); // Console.WriteLine("neighbor in OPEN and cost less than g(neighbor)"); // remove neighbor from OPEN, because new path is better storage.RemoveOpened(opened); opened = null; } // if neighbor in CLOSED and cost less than g(neighbor): if (closed != null && cost < closed.costSoFar) { // Console.WriteLine("CLOSED Neighbor: " + closed.Value.ws); // Console.WriteLine("neighbor in CLOSED and cost less than g(neighbor)"); // remove neighbor from CLOSED storage.RemoveClosed(closed); } // if neighbor not in OPEN and neighbor not in CLOSED: if (opened == null && closed == null) { AStarSharpNode nb = new AStarSharpNode(); nb.ws = cur.ws; nb.costSoFar = cost; nb.heuristicCost = CalculateHeuristic(cur.ws, goal); nb.costSoFarAndHeurisitcCost = nb.costSoFar + nb.heuristicCost; nb.actionname = cur.actionname; nb.parentws = currentNode.ws; nb.parent = currentNode; nb.depth = currentNode.depth + 1; storage.AddToOpenList(nb); // Console.WriteLine("NEW OPENED: " + nb.ToString()); } // Console.WriteLine("\n--\n"); } } }
public void AddToClosedList(AStarSharpNode node) { _closed.Add(node); }
public void AddToOpenList(AStarSharpNode node) { _opened.Add(node); }
public void RemoveClosed(AStarSharpNode node) { _closed.Remove(node); }
public void RemoveOpened(AStarSharpNode node) { _opened.Remove(node); }