static WorldState ApplyPostConditions(ActionPlanner ap, int actionnr, WorldState fr)
{
WorldState pst = ap.postConditions[actionnr];
long unaffected = pst.dontcare;
long affected = (unaffected ^ -1L);
fr.values = (fr.values & unaffected) | (pst.values & affected);
fr.dontcare &= pst.dontcare;
return(fr);
}
//!< Internal function to reconstruct the plan by tracing from last node to initial node.
static void reconstruct_plan(ActionPlanner ap, AStarNode goalnode, string[] plan, WorldState[] worldstates, ref int plansize)
{
AStarNode curnode = goalnode;
int idx = plansize - 1;
int numsteps = 0;
while (!string.IsNullOrEmpty(curnode.actionname))
{
if (idx >= 0)
{
plan[idx] = curnode.actionname;
worldstates[idx] = curnode.ws;
int i = idx_in_closed(curnode.parentws);
if (i == -1)
{
--idx;
numsteps++;
break;
}
curnode = closed[i];
}
--idx;
numsteps++;
}
idx++; // point to last filled
if (idx > 0)
{
for (int i = 0; i < numsteps; ++i)
{
plan[i] = plan[i + idx];
worldstates[i] = worldstates[i + idx];
}
}
if (idx < 0)
{
// Console.WriteLine(string.Format("ERROR: Plan of size {0} cannot be returned in buffer of size {1}", numsteps, plansize ));
plansize = numsteps;
}
}
static int numClosed = 0; //!< The nr of nodes in our closed set.
public static int Plan(
ActionPlanner ap,
WorldState start,
WorldState goal,
string[] plan,
WorldState[] worldstates,
ref int plansize
)
{
// put start in opened list
numOpened = 0;
AStarNode n0;
n0.ws = start;
n0.parentws = start;
n0.costSoFar = 0;
n0.heuristicCost = calc_h(start, goal);
n0.costSoFarAndHeurisitcCost = n0.costSoFar + n0.heuristicCost;
n0.actionname = null;
//n0.parent = new WeakReference (null);
n0.depth = 1;
opened[numOpened++] = n0;
// empty closed list
numClosed = 0;
do
{
if (numOpened == 0) // Console.WriteLine( "Did not find a path." );
{
return(-1);
}
// find the node with lowest rank
int lowestIdx = -1;
int lowestVal = int.MaxValue;
for (int i = 0; i < numOpened; i++)
{
if (opened[i].costSoFarAndHeurisitcCost < lowestVal)
{
lowestVal = opened[i].costSoFarAndHeurisitcCost;
lowestIdx = i;
}
}
// remove the node with the lowest rank
AStarNode cur = opened[lowestIdx];
if (numOpened > 0)
{
opened[lowestIdx] = opened[numOpened - 1];
}
numOpened--;
// Console.WriteLine ("--------------------------------------\n");
// Console.WriteLine("CurrentNode: " + cur.ToString());
// Console.WriteLine("CurrentState: " + cur.ws);
// Console.WriteLine(string.Format("Opened: {0} Closed: {1}", numOpened, numClosed));
// if it matches the goal, we are done!
long care = (goal.dontcare ^ -1L);
bool match = ((cur.ws.values & care) == (goal.values & care));
if (match)
{
// Console.WriteLine("Matched goal!");
reconstruct_plan(ap, cur, plan, worldstates, ref plansize);
return(cur.costSoFarAndHeurisitcCost);
}
// add it to closed
closed[numClosed++] = cur;
// Console.WriteLine("CLOSING: " + cur);
if (numClosed == MAXCLOS)
{
// Console.WriteLine("Closed set overflow");
return(-1);
} // ran out of storage for closed set
// iterate over neighbours
var actionnames = new string[ActionPlanner.MAXACTIONS];
var actioncosts = new int[ActionPlanner.MAXACTIONS];
var to = new WorldState[ActionPlanner.MAXACTIONS];
int numtransitions = ap.GetPossibleTransitions(cur.ws, to, actionnames, actioncosts, ActionPlanner.MAXACTIONS);
// Console.WriteLine( "Neighbours: " + numtransitions );
for (int i = 0; i < numtransitions; i++)
{
// Console.WriteLine("Processing {0} -> {1}", cur.actionname, actionnames[i]);
// Console.WriteLine("State: " + to[i]);
AStarNode nb;
int cost = cur.costSoFar + actioncosts[i];
int idx_o = idx_in_opened(to[i]);
int idx_c = idx_in_closed(to[i]);
// Console.WriteLine("Cost: {0} Idx Opened: {1} Idx Closed: {2}", cost, idx_o, idx_c);
// if neighbor in OPEN and cost less than g(neighbor):
if (idx_o >= 0 && cost < opened[idx_o].costSoFar)
{
// Console.WriteLine("OPENED Neighbor: " + opened[ idx_o ].ws);
// Console.WriteLine("neighbor in OPEN and cost less than g(neighbor)");
// remove neighbor from OPEN, because new path is better
if (numOpened > 0)
{
opened[idx_o] = opened[numOpened - 1];
numOpened--;
// Console.WriteLine("Remove neighbor from OPEN, because new path is better");
}
idx_o = -1; // BUGFIX: neighbor is no longer in OPEN, signal this so that we can re-add it.
}
// if neighbor in CLOSED and cost less than g(neighbor):
if (idx_c >= 0 && cost < closed[idx_c].costSoFar)
{
// Console.WriteLine("CLOSED Neighbor: " + closed[ idx_c ].ws);
// Console.WriteLine("neighbor in CLOSED and cost less than g(neighbor)");
// remove neighbor from CLOSED
if (numClosed > 0)
{
// Console.WriteLine("remove neighbor from CLOSED");
closed[idx_c] = closed[numClosed - 1];
}
numClosed--;
}
// if neighbor not in OPEN and neighbor not in CLOSED:
if (idx_c == -1 && idx_o == -1)
{
nb.ws = to[i];
nb.costSoFar = cost;
nb.heuristicCost = calc_h(nb.ws, goal);
nb.costSoFarAndHeurisitcCost = nb.costSoFar + nb.heuristicCost;
nb.actionname = actionnames[i];
nb.parentws = cur.ws;
//nb.parent = new WeakReference(cur);
nb.depth = cur.depth + 1;
opened[numOpened++] = nb;
// Console.WriteLine("NEW OPENED: " + nb.ToString());
}
if (numOpened == MAXOPEN)
{
// Console.WriteLine("Opened set overflow");
return(-1);
} // ran out of storage for opened set
// Console.WriteLine("\n--\n");
}
} while(true);
return(-1);
}
public static void Main(string[] args)
{
ActionPlanner ap = ActionPlanner.Instance;
ap.SetPrecondition("scout", "armedwithgun", true);
ap.SetPostcondition("scout", "enemyvisible", true);
ap.SetPrecondition("approach", "enemyvisible", true);
ap.SetPostcondition("approach", "nearenemy", true);
ap.SetPrecondition("aim", "enemyvisible", true);
ap.SetPrecondition("aim", "weaponloaded", true);
ap.SetPostcondition("aim", "enemylinedup", true);
ap.SetPrecondition("shoot", "enemylinedup", true);
ap.SetPostcondition("shoot", "enemyalive", false);
ap.SetPrecondition("load", "armedwithgun", true);
ap.SetPostcondition("load", "weaponloaded", true);
ap.SetPrecondition("detonatebomb", "armedwithbomb", true);
ap.SetPrecondition("detonatebomb", "nearenemy", true);
ap.SetPostcondition("detonatebomb", "alive", false);
ap.SetPostcondition("detonatebomb", "enemyalive", false);
ap.SetPrecondition("flee", "enemyvisible", true);
ap.SetPostcondition("flee", "nearenemy", false);
Console.WriteLine(ap.Describe());
var startState = new WorldState(0, -1);
startState.Set(ap.FindConditionNameIndex("enemyvisible"), false);
startState.Set(ap.FindConditionNameIndex("armedwithgun"), true);
startState.Set(ap.FindConditionNameIndex("weaponloaded"), false);
startState.Set(ap.FindConditionNameIndex("enemylinedup"), false);
startState.Set(ap.FindConditionNameIndex("enemyalive"), true);
startState.Set(ap.FindConditionNameIndex("armedwithbomb"), true);
startState.Set(ap.FindConditionNameIndex("nearenemy"), false);
startState.Set(ap.FindConditionNameIndex("alive"), true);
ap.SetCost("detonatebomb", 1000); // make suicide more expensive than shooting.
var goal = new WorldState(0, -1);
goal.Set(ap.FindConditionNameIndex("enemyalive"), false);
goal.Set(ap.FindConditionNameIndex("alive"), true);
//goap_worldstate_set( &ap, &goal, "alive", true ); // add this to avoid suicide actions in plan.
Console.WriteLine("Start: " + startState);
Console.WriteLine("Goal: " + goal);
// create and display plan
var plan = AStarSharp.Plan(ap, startState, goal, new ArrayStorage());
for (var j = 0; j < plan.Length; j++)
{
Console.WriteLine(string.Format("{0}: {1}", j, plan [j]));
}
// Stopwatch sw = new Stopwatch ();
// sw.Start ();
//
// for (var i = 0; i < 10000; i++) {
// var plan = AStarSharp.Plan (ap, startState, goal, new ListStorage());
//// for (var j=0; j<plan.Length;j++) {
//// Console.WriteLine (string.Format("{0}: {1}", j, plan[j]));
//// }
// }
// sw.Stop ();
// Console.WriteLine ("C# (ListStorage) Elapsed: " + sw.ElapsedMilliseconds);
// sw.Reset();
//
// sw = new Stopwatch ();
// sw.Start ();
//
// for (var i = 0; i < 10000; i++) {
// var plan = AStarSharp.Plan (ap, startState, goal, new ArrayStorage());
// // for (var j=0; j<plan.Length;j++) {
// // Console.WriteLine (string.Format("{0}: {1}", j, plan[j]));
// // }
// }
// sw.Stop ();
// Console.WriteLine ("C# (ArraySrorage) Elapsed: " + sw.ElapsedMilliseconds);
// sw.Reset();
//
// sw = new Stopwatch ();
// sw.Start ();
//
// for (var i = 0; i < 10000; i++) {
// var states = new WorldState[16];
// var plann = new string[16];
// var plansz = 16;
// int plancost = AStar.Plan(ap, startState, goal, plann, states, ref plansz );
// }
// sw.Stop ();
// Console.WriteLine ("C Elapsed: " + sw.ElapsedMilliseconds);
//
//
////
//// Console.WriteLine ("\n==============================================\n");
//// Console.WriteLine("plancost = " + plancost );
//// Console.WriteLine ("Plan Start: " + startState); // &ap, &fr, desc, sizeof( desc ) );
//// for ( int i=0; i<plansz && i<16; ++i )
//// {
//// Console.WriteLine (string.Format("{0}: {1} - {2}", i, plan[i], states[i]));
//// }
///
Console.ReadKey();
}
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");
}
}
}
