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