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