public List <string> Plan() { bool bAllAgentsEqual = CheckAgentEquality(); int i = 300;// countOfLandmarks; int[] statesNum = new int[agents.Count]; for (int k = 0; k < statesNum.Length; k++) { statesNum[k] = 0; } PdbVertex rootVertex = new PdbVertex(statesNum, new List <Action>(), 0, 0, 0, null, null); rootVertex.publicState = new State((Problem)null); foreach (Agent agent in agents) { foreach (GroundedPredicate pgp in agent.GetPublicInitial()) { if (!rootVertex.publicState.Contains(pgp)) { rootVertex.publicState.AddPredicate(pgp); } } } //DateTime begin = DateTime.Now; int count = 0; PdbVertex curentVertex = null; DateTime dtStart = DateTime.Now; //for (; i < 1000; i++) { HashSet <int[]> lVisited = new HashSet <int[]>(new ComparerArray()); HashSet <PdbVertex> lVisited2 = new HashSet <PdbVertex>(); DateTime begin = DateTime.Now; List <PdbVertex> queue = new List <PdbVertex>(); //PriorityQueue<PdbVertex, double> queue2 = new PriorityQueue<PdbVertex, double>(PdbVertex.ComparerDouble); //PriorityQueue<PdbVertex, double> queue2 = new PriorityQueue<PdbVertex, double>(); //Heap heap = new Heap(1000000); //queue2.Insert(rootVertex, rootVertex.h); //heap.Insert(rootVertex); queue.Add(rootVertex); lVisited.Add(rootVertex.statesNubmber); // double priority = -1 * (rootVertex.h + (double)(rootVertex.g) / 100000 ); //queue2.Enqueue(rootVertex, priority); int temp = 0; List <string> lplan = null;// new List<string>(); bool flag = true; int c = 0; double minh = 99999999; while (queue.Count > 0) { c++; if (c % 1000 == 0) { //Console.Write("\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\bExpanded: " + c + ", open: " + queue.Count + ", h: " + curentVertex.h + ", T: " + (int)(DateTime.Now - dtStart).TotalSeconds); Console.Write("\rExpanded: " + c + ", open: " + queue.Count + ", h: " + curentVertex.h + ", T: " + (int)(DateTime.Now - dtStart).TotalSeconds); if (queue.Count > 200000) { return(null); Console.WriteLine(); Console.WriteLine("Queue too large, breaking."); } } flag = true; //curentVertex = queue[0]; // List<PriorityQueueItem<PdbVertex,double>> v = queue2.ToList(); //curentVertex = queue2.Dequeue().Value; temp++; curentVertex = FindMin(queue);// queue[0];// heap.Remove(); // queue.Remove(curentVertex); // queue.RemoveAt(0); count++; bool bDeadend = curentVertex.IsDeadEnd(); if (bDeadend) { continue; } // if (curentVertex.Parent != null && curentVertex.Parent.Parent != null && curentVertex.h == curentVertex.Parent.h && curentVertex.h == curentVertex.Parent.Parent.h) // Console.WriteLine("stuck!"); // Program.messages += agents.Count; int f = 0; List <PdbVertex> needUpDate = new List <PdbVertex>(); HashSet <CompoundFormula> levelPotential = new HashSet <CompoundFormula>(); int index = 1; bool con = false; if (curentVertex.h == 0) { curentVertex.publicPredicateImplications = new List <HashSet <CompoundFormula> >(); PdbVertex tmp = curentVertex; while (tmp != null) { curentVertex.publicPredicateImplications.Insert(0, tmp.canGetInParallel); tmp = tmp.Parent; } string isGoal = curentVertex.IsGoal(out lplan); if (isGoal.Equals("true")) { double time = ((double)((DateTime.Now.Subtract(begin)).Minutes)) * 60.0; time += ((double)((DateTime.Now.Subtract(begin)).Seconds)); time += ((double)((DateTime.Now.Subtract(begin)).Milliseconds) / 1000); //Console.WriteLine(time); //Console.WriteLine(lplan.Count); // Console.WriteLine(((double)((DateTime.Now.Subtract(begin)).Milliseconds) / 1000)); Program.times.Add(time); Program.countActions.Add(lplan.Count); Program.timeSum += time; Program.actionSum += lplan.Count; /* foreach (Action act in curentVertex.lplan) * { * sw.WriteLine(act.Name); * lplan.Add(act.Name); * } * sw.Close();*/ return(lplan); } if (!isGoal.Equals("false")) { //return null; } } bool stop = false; foreach (Agent agent in agents) { foreach (Action act in agent.publicActions) { f++; Program.messages += agents.Count; PdbVertex newVertex = curentVertex.Apply(agent.name, act); bool bVisited = true; if (newVertex != null) { if (bAllAgentsEqual) { bVisited = lVisited2.Contains(newVertex); } else { bVisited = lVisited.Contains(newVertex.statesNubmber); } } if (!bVisited) { // newVertex.History = new List<string>(curentVertex.History); // newVertex.History.Add(act.Name); // newVertex.g += (double)index / 1000000; queue.Add(newVertex); if (minh > newVertex.h) { minh = newVertex.h; // if(minh==0) Console.WriteLine(minh); // break; } if (newVertex.h < curentVertex.h) { stop = true; break; } index++; //queue2.Enqueue(newVertex, priority); // needUpDate.Add(newVertex); CompoundFormula effect = new CompoundFormula("and"); foreach (GroundedPredicate gp in act.HashEffects) { if (agent.PublicPredicates.Contains(gp)) { effect.AddOperand(gp); } } levelPotential.Add(effect); } } if (stop) { break; } } curentVertex.canGetInParallel = levelPotential; } } return(null); }
// public static int counter=0; public List <string> Plan() { bool bAllAgentsEqual = CheckAgentEquality(); int i = 200; int[] statesNum = new int[agents.Count]; for (int k = 0; k < statesNum.Length; k++) { statesNum[k] = 0; } PdbVertex rootVertexHsp = new PdbVertex(statesNum, new List <Action>(), 0, 0, 0, null, null); //rootVertexHsp.h = rootVertexHsp.ComputeFF_h(new bool[countOfLandmarks]); int count = 0; PdbVertex curentVertex = null; DateTime dtStart = DateTime.Now; DateTime begin = DateTime.Now; int queue1PriorityCounter = 0; int queue2PriorityCounter = 0; List <PdbVertex> queue = new List <PdbVertex>(); List <PdbVertex> queue2 = new List <PdbVertex>(); // List<VertexHsp> queue2 = new List<VertexHsp>(); bool first = true; bool first2 = true; HashSet <int[]> lVisited = new HashSet <int[]>(new ComparerArray()); HashSet <PdbVertex> lVisited2 = new HashSet <PdbVertex>(); queue.Add(rootVertexHsp); // lVisited.Add(rootVertexHsp.statesNubmber); // lVisited2.Add(rootVertexHsp); int temp = 0; List <string> lplan = null; bool flag = true; double minLandmark_h = countOfLandmarks; double minHsp_h = rootVertexHsp.h; int c = 0; int cResets = 0; double minh = 1000; int blindCounter = 0; TimeSpan tsDeadendDetection = new TimeSpan(); try { while (queue.Count > 0 || queue2.Count > 0) { c++; if (c % 30 == 0 && curentVertex != null) { Console.Write("\rExpanded: " + c + ", open: " + queue.Count + ", h: " + curentVertex.h + ", h2: " + curentVertex.h2 + ", T: " + (int)(DateTime.Now - dtStart).TotalSeconds + ", deadend = " + (int)tsDeadendDetection.TotalSeconds); if (queue.Count > 2000000) { return(null); } } flag = true; temp++; if (queue.Count > 0) { curentVertex = FindMin(queue); } if (curentVertex == null) { continue; } // counter++; // Console.WriteLine(counter); bool bVisited = false; if (bAllAgentsEqual) { bVisited = lVisited2.Contains(curentVertex); } else { bVisited = lVisited.Contains(curentVertex.statesNubmber); } if (bVisited) { continue; } if (bAllAgentsEqual) { lVisited2.Add(curentVertex); } else { lVisited.Add(curentVertex.statesNubmber); } if (curentVertex.h < minHsp_h) { minHsp_h = curentVertex.h; } // DateTime dtBefore = DateTime.Now; //tsDeadendDetection += (DateTime.Now - dtBefore); count++; blindCounter++; int f = 0; List <PdbVertex> needUpDate = new List <PdbVertex>(); HashSet <CompoundFormula> levelPotential = new HashSet <CompoundFormula>(); int index = 1; bool con = false; Program.StateExpendCounter++; List <PdbVertex> lExpanded = Expand(curentVertex, levelPotential, needUpDate); /*if (lExpanded.Count == 0) * { * curentVertex.onlyPrefered = false; * lExpanded = Expand(curentVertex, levelPotential, needUpDate); * }*/ foreach (VertexHsp newVertexHsp in lExpanded) { f++; newVertexHsp.g += (double)index / 1000000; //bool bVisited = false; /* if(bAllAgentsEqual) * bVisited = lVisited2.Contains(newVertexHsp); * else * bVisited = lVisited.Contains(newVertexHsp.statesNubmber); */ // if (!bVisited) { if (newVertexHsp.isPrefered) { queue2.Add(newVertexHsp); } else { queue.Add(newVertexHsp); } // queue2.Add(newVertexHsp); // lVisited.Add(newVertexHsp.statesNubmber); //lVisited2.Add(newVertexHsp); } index++; /*if (minh > newVertexHsp.h) * { * minh = newVertexHsp.h; * * }*/ /*if (VertexHsp.Comparer(newVertexHsp, curentVertex) == -1 && !con) * { * blindCounter = 0; * }*/ } curentVertex.canGetInParallel = levelPotential; if (curentVertex.h == 0 || curentVertex.h2 == 0) { curentVertex.publicPredicateImplications = new List <HashSet <CompoundFormula> >(); VertexHsp tmp = curentVertex; while (tmp != null) { curentVertex.publicPredicateImplications.Insert(0, tmp.canGetInParallel); tmp = tmp.Parent; } string isGoal = curentVertex.IsGoal(out lplan); if (isGoal.Equals("true")) { double time = ((double)((DateTime.Now.Subtract(begin)).Minutes)) * 60.0; time += ((double)((DateTime.Now.Subtract(begin)).Seconds)); time += ((double)((DateTime.Now.Subtract(begin)).Milliseconds) / 1000); Program.times.Add(time); Program.countActions.Add(lplan.Count); Program.timeSum += time; Program.actionSum += lplan.Count; return(lplan); } if (!isGoal.Equals("false")) { } } } } catch (Exception ex) { Console.WriteLine(ex.ToString()); return(null); } return(null); }