HOG2
Propagation.cpp
Go to the documentation of this file.
1 /*
2  * Propagation.cpp
3  * hog2
4  *
5  * by Zhifu Zhang
6  *
7  * This code emphasizes on correctness.
8  */
9 
10 #include <sys/time.h>
11 #include <math.h>
12 #include <deque>
13 #include "Propagation.h"
14 #include <cstring>
15 
16 using namespace PropUtil;
17 
18 //#define MAXINT 2147483648
19 
20 #define CLOSEDMODE 0
21 #define OPENMODE 1
22 #define NEWMODE 2
23 //#define WAITMODE 3
24 #define DELAYMODE 3
25 
26 const static bool verbose = false;
27 const static bool drawtext = false;
28 
29 //static unsigned long tickStart;
30 //
31 //static unsigned long tickGen;
32 
33 void Prop::GetPath(GraphEnvironment *_env, Graph *_g, graphState from, graphState to, std::vector<graphState> &thePath) {
34  if (!InitializeSearch(_env,_g,from,to,thePath))
35  return;
36 
37  struct timeval t0,t1;
38 
39  gettimeofday(&t0,0);
40  while(!DoSingleSearchStep(thePath))
41  {}
42  gettimeofday(&t1,0);
43 
44 // double usedtime = t1.tv_sec-t0.tv_sec + (t1.tv_usec-t0.tv_usec)/1000000.0;
45 
46  //if (thePath.size() > 0)
47  // printf("\nNodes expanded=%ld, Nodes touched=%ld, Reopenings=%ld.\n",GetNodesExpanded(),GetNodesTouched(),NodesReopened);
48 
49  //char algname[20];
50 
51 
52  //printf("Algorithm %s, time used=%lf sec, N/sec=%lf, solution cost=%lf, solution edges=%d.\n", algname,usedtime,GetNodesExpanded()/usedtime,solutionCost,(int)thePath.size());
53 }
54 
55 bool Prop::InitializeSearch(GraphEnvironment *_env, Graph *_g, graphState from, graphState to, std::vector<graphState> &thePath) {
56  env = _env;
57  grp = _g;
58  nodesExpanded = 0;
59  nodesTouched = 0;
60  NodesReopened = 0;
61  metaexpanded = 0;
62  closedSize = 0;
63  reopenings = 0;
64  start = from;
65  goal = to;
66  justExpanded = from;
67 
68  closedList.clear();
69  openQueue.reset();
70  FCache.reset();
71  delayCache.reset();
72 
73  thePath.clear();
74 
75  if (verID==PROP_A)
76  strcpy(algname,"A*");
77  else if (verID==PROP_B)
78  strcpy(algname,"B");
79  else if (verID==PROP_BP)
80  strcpy(algname,"B'");
81  else if (verID==PROP_APPROX)
82  strcpy(algname,"Approx");
83  else if (verID==PROP_BFS)
84  strcpy(algname,"BFSRepair");
85  else if (verID==PROP_DELAY)
86  strcpy(algname,"Delay");
87  else if (verID==PROP_DP)
88  strcpy(algname,"Dual Prop");
89  else if (verID==PROP_BPMX)
90  strcpy(algname,"BPMX");
91  else if (verID==PROP_DPMX)
92  strcpy(algname,"DPMX");
93  else if (verID==PROP_BPMXE)
94  strcpy(algname,"BPMXE");
95  else if (verID==PROP_DPDLMX)
96  strcpy(algname,"DPDLMX");
97  else {
98  printf("I don't know what to do.\n");
99  exit(-1);
100  }
101 
102  //tickNewExp = tickReExp = tickBPMX = 0;
103  //nNewExp = nReExp = nBPMX = 0;
104 
105  if ((from == UINT32_MAX) || (to == UINT32_MAX) || (from == to))
106  {
107  thePath.resize(0);
108  return false;
109  }
110 
111  // step (1)
112  SearchNode first(env->HCost(start, goal), 0, start, start);
113  openQueue.Add(first);
114 
115  F = 0;
116 
117  return true;
118 }
119 
120 bool Prop::DoSingleSearchStep(std::vector<graphState> &thePath) {
121  switch (verID)
122  {
123  case PROP_A:
124  return DoSingleStepA(thePath);
125  case PROP_B:
126  return DoSingleStepB(thePath);
127  case PROP_BP:
128  return DoSingleStepBP(thePath);
129  case PROP_APPROX:
130  return DoSingleStepApprox(thePath);
131  case PROP_BFS:
132  return DoSingleStepBFS(thePath);
133  case PROP_DELAY:
134  return DoSingleStepDelay(thePath);
135  case PROP_DP:
136  return DoSingleStepDP(thePath);
137  case PROP_BPMX:
138  return DoSingleStepBPMX(thePath);
139  case PROP_DPMX:
140  return DoSingleStepDPMX(thePath);
141  case PROP_BPMXE:
142  return DoSingleStepBPMXE(thePath);
143  case PROP_DPDLMX:
144  return DoSingleStepDPDLMX(thePath);
145  default:
146  return true;
147  }
148  //if (verID == PROP_A)
149  // return DoSingleStepA(thePath);
150  //if (verID == PROP_B)
151  // return DoSingleStepB(thePath);
152  //else if (verID == PROP_BP)
153  // return DoSingleStepBP(thePath);
154  //else if (verID == PROP_APPROX)
155  // return DoSingleStepApprox(thePath);
156  //else if (verID == PROP_BFS)
157  // return DoSingleStepBFS(thePath);
158  //else if (verID == PROP_DELAY)
159  // return DoSingleStepDelay(thePath);
160  //else // PROP_DP
161  // return DoSingleStepDP(thePath);
162 }
163 
164 void Prop::ComputeNewHMero3a(double &h, double &h_tmp, graphState neighbor, SearchNode& nb, double altH, int mode)
165 { // this is necessary because the modified h is stored in the node, not the environment
166  if (mode == OPENMODE || mode == CLOSEDMODE)//openQueue.IsIn(SearchNode(neighbor)))
167  {
168  //SearchNode nb = openQueue.find(SearchNode(neighbor));
169  h = max( nb.fCost - nb.gCost, altH);
170 
171  h_tmp = nb.fCost - nb.gCost;
172  }
173  //else if (mode == CLOSEDMODE)//closedList.find(neighbor) != closedList.end())
174  //{
175  // //SearchNode nb = closedList.find(neighbor)->second;
176  // h = max( nb.fCost - nb.gCost, altH);
177 
178  // h_tmp = nb.fCost - nb.gCost;
179  //}
180  else
181  {
182  double envH = env->HCost(neighbor,goal);
183  h = max(envH , altH);
184 
185  h_tmp = envH;
186  }
187 }
188 
189 void Prop::RelaxOpenNode(double f, double g, graphState neighbor, SearchNode &neighborNode, graphState topNodeID)
190 {
191 
192  if (f < neighborNode.fCost) // trick: if f equal, g is always less, causes key increased
193  {
194  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
195  openQueue.DecreaseKey(neighborNode); // adjust its position in OPEN
196  }
197  else //if (f > neighborNode.fCost)
198  {
199  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
200  openQueue.IncreaseKey(neighborNode); // adjust its position in OPEN
201  }
202 
203 }
204 
205 void Prop::RelaxDelayNode(double f, double g, graphState neighbor, SearchNode &neighborNode, graphState topNodeID)
206 {
207 
208  if (g < neighborNode.gCost) // trick: if g equal, then f must be greater, which causes key increased
209  {
210  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
211  delayCache.DecreaseKey(neighborNode); // adjust its position in OPEN
212  }
213  else //if (f > neighborNode.fCost)
214  {
215  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
216  delayCache.IncreaseKey(neighborNode); // adjust its position in OPEN
217  }
218 
219 }
220 
221 bool Prop::DoSingleStepA(std::vector<graphState> &thePath) {
222 // return false means the search is not finished, true otherwise
223 
224  /* step (2) */
225  if (openQueue.size() == 0)
226  {
227  thePath.resize(0); // no path found!
228  closedList.clear();
229  openQueue.reset();
230  env = 0;
231  return true;
232  }
233 
234  /* step (3) */
235  nodesExpanded += 1;
236  SearchNode topNode = openQueue.Remove();
237  graphState topNodeID = topNode.currNode;
238  closedList[topNodeID] = topNode;
239 
240  //if (topNode.rxp)
241  // nReExp++;
242  //else
243  // nNewExp++;
244 
245  //tickStart = clock();
246 
247  if (verbose) {
248  printf("Expanding node %ld , g=%lf, h=%lf, f=%lf.\n",topNodeID,topNode.gCost,topNode.fCost-topNode.gCost,topNode.fCost);
249  }
250 
251  justExpanded = topNodeID;
252 
253  /* step (4) */
254  if (env->GoalTest(topNodeID, goal))
255  {
256  ExtractPathToStart(topNodeID, thePath);
257  closedList.clear();
258  openQueue.reset();
259  env = 0;
260  return true;
261  }
262 
263  //tickStart = clock();
264 
265  /* step (5), computing gi is delayed */
266  neighbors.resize(0);
267  env->GetSuccessors(topNodeID, neighbors);
268 
269  //Categorize(neighbors);
270 
271 
272  //while(true)
273  //{
274  // SearchNode neighborNode;
275  // graphState neighbor;
276  // int mode;
277 
278  // if (!NextNeighbor(neighborNode, neighbor, mode))
279  // break;
280  for (unsigned int x = 0; x<neighbors.size(); x++)
281  {
282  nodesTouched++;
283 
284  /* step (5) */
285  graphState neighbor = neighbors[x];
286  double edgeWeight = env->GCost(topNodeID,neighbor);
287  double g = topNode.gCost + edgeWeight;
288  double h = env->HCost(neighbor,goal);
289  double f = g + h;
290 
291  // determine neighbor type
292  SearchNode neighborNode;
293  int mode;
294  neighborNode = openQueue.find(SearchNode(neighbor));
295  if (neighborNode.currNode == neighbor) {
296  mode = OPENMODE;
297  }
298  else {
299  NodeLookupTable::iterator iter = closedList.find(neighbor);
300  if (iter != closedList.end()) {
301  neighborNode = iter->second;
302  mode = CLOSEDMODE;
303  }
304  else {
305  mode = NEWMODE;
306  }
307  }
308 
309  /* step (6), neither in OPEN nor CLOSED */
310  if (mode == NEWMODE)
311  {
312  SearchNode n(f,g,neighbor,topNodeID);
313  n.isGoal = (neighbor==goal);
314  openQueue.Add(n);
315 
316  if (verbose) {
317  printf("Adding node %ld to OPEN, g=%lf, h=%lf, f=%lf.\n",neighbor,g,f-g,f);
318  }
319  }
320 
321  /* step (7) */
322  else
323  {
324  //SearchNode neighborNode;
325  if (mode == OPENMODE)//openQueue.IsIn(SearchNode(neighbor)))
326  {
327  //neighborNode = openQueue.find(SearchNode(neighbor));
328 
329  //if (neighborNode.gCost <= g)
330  if (!fgreater(neighborNode.gCost,g))
331  continue;
332 
333  if (verbose) {
334  printf("Adjusting node %ld in OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
335  }
336 
337  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
338  openQueue.DecreaseKey(neighborNode); // adjust its position in OPEN
339  }
340  else //if (closedList.find(neighbor) != closedList.end())
341  {
342  //neighborNode = closedList.find(neighbor)->second;
343 
344  //if (neighborNode.gCost <= g)
345  if (!fgreater(neighborNode.gCost,g))
346  continue;
347 
348  if (verbose) {
349  printf("Moving node %ld from CLOSED to OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
350  }
351 
352  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
353  closedList.erase(neighbor); // delete from CLOSED
354 
355  //neighborNode.rxp = true;
356 
357  NodesReopened++;
358 
359  openQueue.Add(neighborNode); // add to OPEN
360  }
361 
362  }
363  }
364 
365  neighbors.clear();
366 
367  //if (topNode.rxp)
368  // tickReExp += clock() - tickStart;
369  //else
370  // tickNewExp += clock() - tickStart;
371 
372  return false;
373 }
374 
375 bool Prop::DoSingleStepB(std::vector<graphState> &thePath) {
376 // return false means the search is not finished, true otherwise
377 
378  /* step (2) */
379  if (openQueue.size() == 0)
380  {
381  thePath.resize(0); // no path found!
382  closedList.clear();
383  openQueue.reset();
384  env = 0;
385  return true;
386  }
387 
388  /* step (3) */
389  nodesExpanded += 1;
390 
391 
392  SearchNode topNode;
393  if (fless(openQueue.top().fCost , F))
394  {
395  GetLowestG(topNode);
396  if (verbose)
397  printf("Expanding a node below F.\n");
398  }
399  else
400  {
401  topNode = openQueue.Remove();
402 
403  if (fgreater(topNode.fCost,F))
404  {
405  F = topNode.fCost; // update F
406  if (verbose)
407  {
408  printf("F updated to %lf.\n",F);
409  }
410  }
411  }
412 
413  //if (topNode.rxp)
414  // nReExp++;
415  //else
416  // nNewExp++;
417  //tickStart = clock();
418 
419  graphState topNodeID = topNode.currNode;
420  closedList[topNodeID] = topNode;
421 
422  if (verbose) {
423  printf("Expanding node %ld , g=%lf, h=%lf, f=%lf.\n",topNodeID,topNode.gCost,topNode.fCost-topNode.gCost,topNode.fCost);
424  }
425 
426  justExpanded = topNodeID;
427 
428  /* step (4) */
429  if (env->GoalTest(topNodeID, goal))
430  {
431  ExtractPathToStart(topNodeID, thePath);
432  closedList.clear();
433  openQueue.reset();
434  env = 0;
435  return true;
436  }
437 
438  /* step (5), computing gi is delayed */
439  neighbors.resize(0);
440  env->GetSuccessors(topNodeID, neighbors);
441 
442  //Categorize(neighbors);
443 
444 
445  //while(true)
446  //{
447  // SearchNode neighborNode;
448  // graphState neighbor;
449  // int mode;
450  // //double edgeWeight;
451 
452  // if (!NextNeighbor(neighborNode, neighbor, mode))
453  // break;
454  for (unsigned int x = 0; x<neighbors.size(); x++)
455  {
456  nodesTouched++;
457 
458  /* step (5) */
459  graphState neighbor = neighbors[x];
460  double edgeWeight = env->GCost(topNodeID,neighbor);
461  double g = topNode.gCost + edgeWeight;
462  double h = env->HCost(neighbor,goal);
463  double f = g + h;
464 
465  // determine neighbor type
466  SearchNode neighborNode;
467  int mode;
468  neighborNode = openQueue.find(SearchNode(neighbor));
469  if (neighborNode.currNode == neighbor) {
470  mode = OPENMODE;
471  }
472  else {
473  NodeLookupTable::iterator iter = closedList.find(neighbor);
474  if (iter != closedList.end()) {
475  neighborNode = iter->second;
476  mode = CLOSEDMODE;
477  }
478  else {
479  mode = NEWMODE;
480  }
481  }
482 
483  /* step (6), neither in OPEN nor CLOSED */
484  if (mode == NEWMODE)
485  {
486  SearchNode n(f,g,neighbor,topNodeID);
487  n.isGoal = (neighbor==goal);
488  openQueue.Add(n);
489 
490  if (verbose) {
491  printf("Adding node %ld to OPEN, g=%lf, h=%lf, f=%lf.\n",neighbor,g,f-g,f);
492  }
493  }
494 
495  /* step (7) */
496  else
497  {
498  //SearchNode neighborNode;
499  if (mode == OPENMODE)//openQueue.IsIn(SearchNode(neighbor)))
500  {
501  //neighborNode = openQueue.find(SearchNode(neighbor));
502 
503  //if (neighborNode.gCost <= g)
504  if (!fgreater(neighborNode.gCost,g))
505  continue;
506 
507  if (verbose) {
508  printf("Adjusting node %ld in OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
509  }
510 
511  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
512  openQueue.DecreaseKey(neighborNode); // adjust its position in OPEN
513  }
514  else //if (closedList.find(neighbor) != closedList.end())
515  {
516  //neighborNode = closedList.find(neighbor)->second;
517 
518  //if (neighborNode.gCost <= g)
519  if (!fgreater(neighborNode.gCost,g))
520  continue;
521 
522  if (verbose) {
523  printf("Moving node %ld from CLOSED to OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
524  }
525 
526  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
527  closedList.erase(neighbor); // delete from CLOSED
528 
529  NodesReopened++;
530 
531  //neighborNode.rxp = true;
532 
533  openQueue.Add(neighborNode); // add to OPEN
534  }
535 
536  }
537  }
538 
539  neighbors.clear();
540 
541  //if (topNode.rxp)
542  // tickReExp += clock() - tickStart;
543  //else
544  // tickNewExp += clock() - tickStart;
545 
546  return false;
547 }
548 
549 /* Algorithm B' . step (3a) (3b) are inserted into algorithm B. step (7) is also changed, since we need to store the updated h */
550 bool Prop::DoSingleStepBP(std::vector<graphState> &thePath)
551 {
552  /* step (2) */
553  if (openQueue.size() == 0)
554  {
555  thePath.resize(0); // no path found!
556  closedList.clear();
557  openQueue.reset();
558  FCache.reset();
559  env = 0;
560  return true;
561  }
562 
563  /* step (3) */
564  nodesExpanded += 1;
565 
566  // select the node to expand
567  SearchNode topNode;
568  if (fless(openQueue.top().fCost , F))
569  {
570  GetLowestG(topNode);
571  if (verbose)
572  printf("Expanding a node below F.\n");
573  }
574  //else if (fequal(openQueue.top().fCost, F))
575  //{
576  // GetLowestGF(topNode);
577  //}
578  else
579  {
580  topNode = openQueue.Remove();
581 
582  if (fgreater(topNode.fCost,F))
583  {
584  F = topNode.fCost; // update F
585  if (verbose)
586  {
587  printf("F updated to %lf.\n",F);
588  }
589  }
590  }
591 
592  //if (topNode.rxp)
593  // nReExp++;
594  //else
595  // nNewExp++;
596 
597  graphState topNodeID = topNode.currNode;
598  closedList[topNodeID] = topNode;
599 
600  if (verbose)
601  {
602  printf("Expanding node %ld , g=%lf, h=%lf, f=%lf.\n",topNodeID,topNode.gCost,topNode.fCost-topNode.gCost,topNode.fCost);
603  }
604 
605  justExpanded = topNodeID;
606 
607  /* step (4) */
608  if (env->GoalTest(topNodeID, goal))
609  {
610  ExtractPathToStart(topNodeID, thePath);
611  closedList.clear();
612  openQueue.reset();
613  FCache.reset();
614  env = 0;
615  return true;
616  }
617 
618  //tickStart = clock();
619 
620  /* step (5), computing gi is delayed */
621  neighbors.resize(0);
622  env->GetSuccessors(topNodeID, neighbors);
623 
624  //Categorize(neighbors);
625 
626  double hTop = topNode.fCost - topNode.gCost;
627  double minH2 = DBL_MAX; // min ( edgeWeight(i) + h(neighbor(i)) )
628 
629 
630  //while(true)
631  //{
632  // SearchNode neighborNode;
633  // graphState neighbor;
634  // int mode;
635  // //double edgeWeight;
636 
637  // if (!NextNeighbor(neighborNode, neighbor, mode))
638  // break;
639  for (unsigned int x = 0; x<neighbors.size(); x++)
640  {
641  nodesTouched++;
642 
643  /* step (5) */
644  graphState neighbor = neighbors[x];
645  double edgeWeight = env->GCost(topNodeID,neighbor);
646  double g = topNode.gCost + edgeWeight;
647 
648  /* step Mero (3a) */
649  double h_tmp; // for printing reports only
650  double h;
651 
652  // determine neighbor type
653  SearchNode neighborNode;
654  int mode;
655  neighborNode = openQueue.find(SearchNode(neighbor));
656  if (neighborNode.currNode == neighbor) {
657  mode = OPENMODE;
658  }
659  else {
660  NodeLookupTable::iterator iter = closedList.find(neighbor);
661  if (iter != closedList.end()) {
662  neighborNode = iter->second;
663  mode = CLOSEDMODE;
664  }
665  else {
666  mode = NEWMODE;
667  }
668  }
669 
670  ComputeNewHMero3a(h, h_tmp, neighbor, neighborNode, hTop - edgeWeight, mode);
671 
672  if (verbose)
673  {
674  if (fgreater(h,h_tmp))
675  printf("Improving h of node %ld by Mero rule (a), %lf->%lf\n",neighbor,h_tmp,h);
676  }
677 
678  double f = g + h;
679 
680  /* step Mero (3b) */
681  minH2 = min(minH2, h + edgeWeight);
682 
683  /* step (6), neither in OPEN nor CLOSED */
684  if (mode == NEWMODE)
685  {
686  SearchNode n(f,g,neighbor,topNodeID);
687  n.isGoal = (neighbor==goal);
688  openQueue.Add(n);
689 
690  if (verbose)
691  {
692  printf("Adding node %ld to OPEN, g=%lf, h=%lf, f=%lf.\n",neighbor,g,f-g,f);
693  }
694  }
695 
696  /* step (7) */
697  else
698  {
699  //SearchNode neighborNode;
700  if (mode == OPENMODE) //openQueue.IsIn(SearchNode(neighbor)))
701  {
702  //neighborNode = openQueue.find(SearchNode(neighbor));
703 
704  //if (neighborNode.gCost <= g) {
705  if (!fgreater(neighborNode.gCost,g))
706  {
707  // we may fail to update g, but still update h
708  /* proved to be impossible */
709  if (UpdateHOnly(neighborNode, h))
710  openQueue.IncreaseKey(neighborNode);
711  continue;
712  }
713 
714  if (verbose)
715  {
716  printf("Adjusting node %ld in OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
717  }
718 
719  RelaxOpenNode(f, g, neighbor, neighborNode, topNodeID);
720  }
721  else //if (closedList.find(neighbor) != closedList.end())
722  {
723  //neighborNode = closedList.find(neighbor)->second;
724 
725  //if (neighborNode.gCost <= g) {
726  if (!fgreater(neighborNode.gCost,g))
727  {
728  // we may fail to update g, but still update h
729  if (UpdateHOnly(neighborNode, h))
730  closedList[neighbor] = neighborNode;
731  continue;
732  }
733 
734  if (verbose)
735  {
736  printf("Moving node %ld from CLOSED to OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
737  }
738 
739  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
740  closedList.erase(neighbor); // delete from CLOSED
741  NodesReopened++;
742 
743  //neighborNode.rxp = true;
744 
745  openQueue.Add(neighborNode); // add to OPEN
746  }
747 
748  }
749  }
750 
751  /* step Mero (3b), update h of parent */
752  if (fgreater(minH2 , hTop))
753  {
754  topNode.fCost = minH2 + topNode.gCost; // f = h + g
755  closedList[topNodeID] = topNode;
756 
757  if (verbose)
758  {
759  printf("Improving h of node %ld by Mero rule (b), %lf->%lf\n",topNodeID,hTop,minH2);
760  }
761  }
762 
763  neighbors.clear();
764 
765  //if (topNode.rxp)
766  // tickReExp += clock() - tickStart;
767  //else
768  // tickNewExp += clock() - tickStart;
769 
770  return false;
771 }
772 
773 
774 bool Prop::DoSingleStepApprox(std::vector<graphState> &thePath)
775 {
776  /* step (2) */
777  if (openQueue.size() == 0)
778  {
779  thePath.resize(0); // no path found!
780  closedList.clear();
781  openQueue.reset();
782  FCache.reset();
783  env = 0;
784  return true;
785  }
786 
787  /* step (3) */
788  nodesExpanded += 1;
789 
790 
791  // select the node to expand
792  SearchNode topNode;
793  //if (fless(openQueue.top().fCost , F))
794  //{
795  // GetLowestG(topNode);
796  // if (verbose)
797  // printf("Expanding a node below F.\n");
798  //}
799  //else
800  {
801  topNode = openQueue.Remove();
802 
803  if (fgreater(topNode.fCost,F))
804  {
805  F = topNode.fCost; // update F
806  if (verbose)
807  {
808  printf("F updated to %lf.\n",F);
809  }
810  }
811  }
812 
813  graphState topNodeID = topNode.currNode;
814  closedList[topNodeID] = topNode;
815 
816  if (verbose)
817  {
818  printf("Expanding node %ld , g=%lf, h=%lf, f=%lf.\n",topNodeID,topNode.gCost,topNode.fCost-topNode.gCost,topNode.fCost);
819  }
820 
821  justExpanded = topNodeID;
822 
823  /* step (4) */
824  if (env->GoalTest(topNodeID, goal))
825  {
826  ExtractPathToStart(topNodeID, thePath);
827  closedList.clear();
828  openQueue.reset();
829  FCache.reset();
830  env = 0;
831  return true;
832  }
833 
834  /* step (5), computing gi is delayed */
835  neighbors.resize(0);
836  env->GetSuccessors(topNodeID, neighbors);
837 
838  //Categorize(neighbors);
839 
840 // double hTop = topNode.fCost - topNode.gCost;
841  //double minH2 = DBL_MAX; // min ( edgeWeight(i) + h(neighbor(i)) )
842 
843 
844  //while(true)
845  //{
846  // SearchNode neighborNode;
847  // graphState neighbor;
848  // int mode;
849  // //double edgeWeight;
850 
851  // if (!NextNeighbor(neighborNode, neighbor, mode))
852  // break;
853  for (unsigned int x = 0; x<neighbors.size(); x++)
854  {
855  nodesTouched++;
856 
857  /* step (5) */
858  graphState neighbor = neighbors[x];
859  double edgeWeight = env->GCost(topNodeID,neighbor);
860  double g = topNode.gCost + edgeWeight;
861 
862  /* step Mero (3a) */
863  double h_tmp; // for printing reports only
864  double h = env->HCost(neighbor,goal);
865 
866  // determine neighbor type
867  SearchNode neighborNode;
868  int mode;
869  neighborNode = openQueue.find(SearchNode(neighbor));
870  if (neighborNode.currNode == neighbor) {
871  mode = OPENMODE;
872  }
873  else {
874  NodeLookupTable::iterator iter = closedList.find(neighbor);
875  if (iter != closedList.end()) {
876  neighborNode = iter->second;
877  mode = CLOSEDMODE;
878  }
879  else {
880  mode = NEWMODE;
881  }
882  }
883 
884  //ComputeNewHMero3a(h, h_tmp, neighbor, neighborNode, hTop - edgeWeight,mode);
885 
886  if (verbose)
887  {
888  if (fgreater(h,h_tmp))
889  printf("Improving h of node %ld by Mero rule (a), %lf->%lf\n",neighbor,h_tmp,h);
890  }
891 
892  double f = g + h;
893 
894  /* step Mero (3b) */
895  //minH2 = min(minH2, h + edgeWeight);
896 
897  /* step (6), neither in OPEN nor CLOSED */
898  if (mode == NEWMODE)
899  {
900  SearchNode n(f,g,neighbor,topNodeID);
901  n.isGoal = (neighbor==goal);
902  openQueue.Add(n);
903 
904  if (verbose)
905  {
906  printf("Adding node %ld to OPEN, g=%lf, h=%lf, f=%lf.\n",neighbor,g,f-g,f);
907  }
908  }
909 
910  /* step (7) */
911  else
912  {
913  //SearchNode neighborNode;
914  if (mode == OPENMODE) //openQueue.IsIn(SearchNode(neighbor)))
915  {
916  //neighborNode = openQueue.find(SearchNode(neighbor));
917 
918  //if (neighborNode.gCost <= g) {
919  if (!fgreater(neighborNode.gCost,g))
920  {
921  // we may fail to update g, but still update h
922  /* proved to be impossible */
923  //if (UpdateHOnly(neighborNode, h))
924  // openQueue.IncreaseKey(neighborNode);
925 
926  continue;
927  }
928 
929  if (verbose)
930  {
931  printf("Adjusting node %ld in OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
932  }
933 
934  RelaxOpenNode(f, g, neighbor, neighborNode, topNodeID);
935  }
936  else //if (closedList.find(neighbor) != closedList.end())
937  {
938  //neighborNode = closedList.find(neighbor)->second;
939 
940  //if (neighborNode.gCost <= g) {
941  if (!fgreater(neighborNode.gCost,g))
942  {
943  // we may fail to update g, but still update h
944  //if (UpdateHOnly(neighborNode, h))
945  // closedList[neighbor] = neighborNode;
946 
947  continue;
948  }
949 
950  if (verbose)
951  {
952  printf("Moving node %ld from CLOSED to OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
953  }
954 
955  // careful! gCost will be altered in next operation!
956  double oldG = neighborNode.gCost;
957 
958  // this operation is optional, as appears in the variant version of the alg
959  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
960  closedList[neighbor] = neighborNode; // this line was MISSING !!!
961 
962  if (fgreater(oldG - g , delta)) // if we can reduce g, and not by a small amount, then don't ignore
963  {
964  closedList.erase(neighbor); // delete from CLOSED
965  NodesReopened++;
966 
967  openQueue.Add(neighborNode); // add to OPEN
968  }
969 
970  }
971 
972  }
973  }
974 
975  /* step Mero (3b), update h of parent */
976  //if (fgreater(minH2 , hTop))
977  //{
978  // topNode.fCost = minH2 + topNode.gCost; // f = h + g
979  // closedList[topNodeID] = topNode;
980 
981  // if (verbose)
982  // {
983  // printf("Improving h of node %ld by Mero rule (b), %lf->%lf\n",topNodeID,hTop,minH2);
984  // }
985  //}
986 
987  neighbors.clear();
988 
989  return false;
990 }
991 
992 bool Prop::UpdateHOnly(SearchNode &neighborNode, double h)
993 {
994  if (fgreater(h , neighborNode.fCost - neighborNode.gCost))
995  {
996  double f = h + neighborNode.gCost;
997  neighborNode.fCost = f;
998  return true;
999  }
1000  return false;
1001 }
1002 
1003 /* after using Categorize(), this algorithm is not quite right. to be fixed later ... */
1004 bool Prop::DoSingleStepBFS(std::vector<graphState> &/*thePath*/) {
1005  return true;
1006 }
1007 // get lowest g node from OPEN, with f < F
1008 void Prop::GetLowestG(SearchNode &gNode)
1009 {
1010  gNode = openQueue.FindSpecialMin(F);
1011 
1012  double fCost = gNode.fCost;
1013  gNode.fCost = -1; // set it to min then extract from top
1014 
1015  openQueue.DecreaseKey(gNode);
1016  openQueue.Remove();
1017 
1018  gNode.fCost = fCost;
1019 }
1020 
1021 // get lowest g node from OPEN, with f == F
1022 void Prop::GetLowestGF(SearchNode &gNode)
1023 {
1024  gNode = openQueue.FindTieFMin(F);
1025 
1026  double fCost = gNode.fCost;
1027  gNode.fCost = -1; // set it to min then extract from top
1028 
1029  openQueue.DecreaseKey(gNode);
1030  openQueue.Remove();
1031 
1032  gNode.fCost = fCost;
1033 }
1034 
1035 
1036 // Nathan's alg
1037 bool Prop::DoSingleStepDelay(std::vector<graphState> &/*thePath*/)
1038 {
1039  return true;
1040 }
1041 
1042 // for DP
1043 void Prop::ReverseProp(SearchNode& topNode) {
1044  //tickStart = clock();
1045 
1047  graphState topNodeID = topNode.currNode;
1048  double currentG = topNode.gCost;
1049  for (unsigned int x=0;x<neighbors.size();x++)
1050  {
1051  graphState neighbor = neighbors[x];
1052  SearchNode neighborNode = openQueue.find(SearchNode(neighbor));
1053  if (neighborNode.currNode == neighbor) {
1054  double newG = env->GCost(topNodeID,neighbor) + neighborNode.gCost;
1055  if (currentG > newG) {
1056  currentG = newG;
1057  topNode.copy(topNode.fCost - topNode.gCost + newG, newG, topNodeID, neighbor);
1058  }
1059  }
1060  else {
1061  NodeLookupTable::iterator iter = closedList.find(neighbor);
1062  if (iter != closedList.end()) {
1063  neighborNode = iter->second;
1064  double newG = env->GCost(topNodeID,neighbor) + neighborNode.gCost;
1065  if (currentG > newG) {
1066  currentG = newG;
1067  topNode.copy(topNode.fCost - topNode.gCost + newG, newG, topNodeID, neighbor);
1068  }
1069  }
1070  else {
1071  }
1072  }
1073  }
1074 
1075  //nBPMX++;
1076  //tickBPMX += clock() - tickStart;
1077 
1078  metaexpanded += 1; //
1079  nodesExpanded += 1;
1080  nodesTouched += neighbors.size();
1081 }
1082 
1083 // for BPMX
1084 void Prop::ReversePropX1(SearchNode& topNode)
1085 {
1086  //tickStart = clock();
1087 
1088  double maxh = topNode.fCost - topNode.gCost;
1089  for (unsigned int x=0;x<neighbors.size();x++)
1090  {
1091  graphState neighbor = neighbors[x];
1092  SearchNode neighborNode = openQueue.find(SearchNode(neighbor));
1093  if (neighborNode.currNode == neighbor) {
1094  maxh = max(maxh, (neighborNode.fCost - neighborNode.gCost) - env->GCost(topNode.currNode,neighbor));
1095  }
1096  else {
1097  NodeLookupTable::iterator iter = closedList.find(neighbor);
1098  if (iter != closedList.end()) {
1099  neighborNode = iter->second;
1100  double oh = maxh;
1101  maxh = max(maxh, (neighborNode.fCost - neighborNode.gCost) - env->GCost(topNode.currNode,neighbor));
1102 
1103  if (bpmxLevel > 1 && oh < maxh) {
1104  fifo.push_back(neighbor);
1105  }
1106  }
1107  else {
1108  maxh = max(maxh, env->HCost(neighbor,goal) - env->GCost(topNode.currNode,neighbor));
1109  }
1110  }
1111  }
1112 
1113  //nBPMX++;
1114  //tickBPMX += clock() - tickStart;
1115 
1116  metaexpanded += 1; //
1117  nodesExpanded += 1;
1118  nodesTouched += neighbors.size();
1119 
1120  topNode.fCost = maxh + topNode.gCost;
1121 }
1122 
1123 
1124 // for DPMX
1125 // and DPDLMX
1126 void Prop::ReversePropX2(SearchNode& topNode) {
1127 
1128  //tickStart = clock();
1130  graphState topNodeID = topNode.currNode;
1131  double currentG = topNode.gCost;
1132  double maxh = topNode.fCost - topNode.gCost;
1133  for (unsigned int x=0;x<neighbors.size();x++)
1134  {
1135  graphState neighbor = neighbors[x];
1136  SearchNode neighborNode = openQueue.find(SearchNode(neighbor));
1137  if (neighborNode.currNode == neighbor) {
1138  double edge = env->GCost(topNodeID,neighbor);
1139  double newG = edge + neighborNode.gCost;
1140  if (fgreater(currentG , newG)) {
1141  currentG = newG;
1142  topNode.copy(topNode.fCost - topNode.gCost + newG, newG, topNodeID, neighbor);
1143  }
1144  maxh = max(maxh, (neighborNode.fCost - neighborNode.gCost) - edge);
1145  }
1146  else {
1147  NodeLookupTable::iterator iter = closedList.find(neighbor);
1148  if (iter != closedList.end()) {
1149  neighborNode = iter->second;
1150  double edge = env->GCost(topNodeID,neighbor);
1151  double newG = edge + neighborNode.gCost;
1152  if (fgreater(currentG , newG)) {
1153  currentG = newG;
1154  topNode.copy(topNode.fCost - topNode.gCost + newG, newG, topNodeID, neighbor);
1155  }
1156  maxh = max(maxh, (neighborNode.fCost - neighborNode.gCost) - edge);
1157  }
1158  else { // new node
1159  //neighborNode = delayCache.find(SearchNode(neighbor));
1160  //if (neighborNode.currNode == neighbor) {
1161  // double edge = env->GCost(topNodeID, neighbor);
1162  // double newG = edge + neighborNode.gCost;
1163  // if (fgreater(currentG, newG)) {
1164  // currentG = newG;
1165  // topNode.copy(topNode.fCost - topNode.gCost + newG, newG, topNodeID, neighbor);
1166  // }
1167  // maxh = max(maxh, (neighborNode.fCost - neighborNode.gCost) - edge);
1168  //}
1169  //else
1170  maxh = max(maxh, env->HCost(neighbor,goal) - env->GCost(topNodeID,neighbor));
1171  }
1172  }
1173  }
1174 
1175  topNode.fCost = maxh + topNode.gCost;
1176 
1177  //nBPMX++;
1178  //tickBPMX += clock() - tickStart;
1179 
1180  metaexpanded += 1; //
1181  nodesExpanded += 1;
1182  nodesTouched += neighbors.size();
1183 }
1184 
1185 void Prop::Broadcast(int level, int levelcount)
1186 { // we will only enque the newly updated (neighbor) nodes; or neighbor nodes being able to update others
1187  NodeLookupTable::iterator iter;
1188 
1189  //tickStart = clock();
1190 
1191  for (int i=0;i<levelcount;i++) {
1192  graphState front = fifo.front();
1193  fifo.pop_front();
1194 
1195  iter = closedList.find(front);
1196  if (iter == closedList.end())
1197  continue;
1198 
1199  SearchNode frontNode = iter->second;
1200  double frontH = frontNode.fCost - frontNode.gCost;
1201 
1202  myneighbors.clear();
1203  env->GetSuccessors(front, myneighbors);
1204 
1205  // backward pass
1206  for (unsigned int x = 0; x < myneighbors.size(); x++)
1207  {
1208  graphState neighbor = myneighbors[x];
1209  iter = closedList.find(neighbor);
1210  if (iter != closedList.end()) {
1211  double edgeWeight = env->GCost(front,neighbor);
1212  SearchNode neighborNode = iter->second;
1213 
1214  double neighborH = neighborNode.fCost - neighborNode.gCost;
1215 
1216  if (fgreater(neighborH - edgeWeight, frontH)) {
1217  frontH = neighborH - edgeWeight;
1218  frontNode.fCost = frontNode.gCost + frontH;
1219  fifo.push_back(neighbor);
1220  }
1221  }
1222  else {
1223  SearchNode neighborNode = openQueue.find(SearchNode(neighbor));
1224  if (neighborNode.currNode == neighbor) {
1225  double edgeWeight = env->GCost(front,neighbor);
1226 
1227  double neighborH = neighborNode.fCost - neighborNode.gCost;
1228 
1229  if (fgreater(neighborH - edgeWeight, frontH)) {
1230  frontH = neighborH - edgeWeight;
1231  frontNode.fCost = frontNode.gCost + frontH;
1232  }
1233  }
1234  else {
1235  //neighborNode = delayCache.find(SearchNode(neighbor));
1236  //if (neighborNode.currNode == neighbor) {
1237  // double edgeWeight = env->GCost(front,neighbor);
1238  // double neighborH = neighborNode.fCost - neighborNode.gCost;
1239 
1240  // if (fgreater(neighborH - edgeWeight, frontH)) {
1241  // frontH = neighborH - edgeWeight;
1242  // frontNode.fCost = frontNode.gCost + frontH;
1243  // fifo.push_back(neighbor); // trick: its neighbors guaranteed been generated
1244  // }
1245  //}
1246  }
1247  }
1248  }
1249  // store frontNode
1250  closedList[front] = frontNode;
1251 
1252  // forward pass
1253  for (unsigned int x = 0; x < myneighbors.size(); x++)
1254  {
1255  graphState neighbor = myneighbors[x];
1256  NodeLookupTable::iterator theIter = closedList.find(neighbor);
1257  if (theIter != closedList.end()) {
1258  double edgeWeight = env->GCost(front,neighbor);
1259  SearchNode neighborNode = theIter->second;
1260 
1261  double neighborH = neighborNode.fCost - neighborNode.gCost;
1262 
1263  if (fgreater(frontH - edgeWeight, neighborH)) {
1264  neighborNode.fCost = neighborNode.gCost + frontH - edgeWeight; // store neighborNode
1265  closedList[neighbor] = neighborNode;
1266  fifo.push_back(neighbor);
1267  }
1268  }
1269  else {
1270  SearchNode neighborNode = openQueue.find(SearchNode(neighbor));
1271  if (neighborNode.currNode == neighbor) {
1272  double edgeWeight = env->GCost(front,neighbor);
1273 
1274  double neighborH = neighborNode.fCost - neighborNode.gCost;
1275 
1276  if (fgreater(frontH - edgeWeight, neighborH)) {
1277  neighborNode.fCost = neighborNode.gCost + frontH - edgeWeight;
1278  openQueue.IncreaseKey(neighborNode);
1279  }
1280  }
1281  else {
1282  neighborNode = delayCache.find(SearchNode(neighbor));
1283  double edgeWeight = env->GCost(front,neighbor);
1284  double neighborH = neighborNode.fCost - neighborNode.gCost;
1285 
1286  if (fgreater(frontH - edgeWeight, neighborH)) {
1287  neighborNode.fCost = neighborNode.gCost + frontH - edgeWeight;
1288  delayCache.IncreaseKey(neighborNode);
1289  fifo.push_back(neighbor); // trick: its neighbors guaranteed been generated
1290  }
1291  }
1292  }
1293  }
1294  }
1295 
1296  //nBPMX += 2;
1297  //tickBPMX += clock() - tickStart;
1298 
1299  metaexpanded += 2; //
1300  nodesExpanded += 2;
1301  nodesTouched += 2*levelcount;
1302 
1303  level++;
1304  if (level < bpmxLevel && fifo.size() > 0)
1305  Broadcast(level, fifo.size());
1306 }
1307 
1308 
1309 /* Algorithm Dual Propagation. Only applicable to undirected graphs! */
1310 bool Prop::DoSingleStepDP(std::vector<graphState> &thePath)
1311 {
1312  /* step (2) */
1313  if (openQueue.size() == 0)
1314  {
1315  thePath.resize(0); // no path found!
1316  closedList.clear();
1317  openQueue.reset();
1318  FCache.reset();
1319  env = 0;
1320  return true;
1321  }
1322 
1323  /* step (3) */
1324  nodesExpanded += 1;
1325 
1326  // select the node to expand
1327  SearchNode topNode;
1328  //if (fless(openQueue.top().fCost , F))
1329  //{
1330  // GetLowestG(topNode);
1331  // if (verbose)
1332  // printf("Expanding a node below F.\n");
1333  //}
1334  //else
1335  {
1336  topNode = openQueue.Remove();
1337 
1338  if (fgreater(topNode.fCost,F))
1339  {
1340  F = topNode.fCost; // update F
1341  if (verbose)
1342  {
1343  printf("F updated to %lf.\n",F);
1344  }
1345  }
1346  }
1347 
1348  //if (topNode.rxp)
1349  // nReExp++;
1350  //else
1351  // nNewExp++;
1352 
1353 // unsigned long tickTmp ;//= clock();
1354 
1355  neighbors.resize(0);
1356  env->GetSuccessors(topNode.currNode, neighbors);
1357 
1358  //tickGen = clock() - tickTmp;
1359 
1360  //Categorize(neighbors);
1361 
1362  bool doReverse = false;
1363 
1364  int ichild = 0;
1365 
1366 _REVERSE:
1367  // reverseProp() here, top node will be updated inside, so put topnode into closed afterwards
1368  if (doReverse) {
1369  if (bpmxLevel >= 1)
1370  ReversePropX2(topNode);
1371  else
1372  ReverseProp(topNode);
1373 
1374  // counting supplement
1375  //if (nodesExpanded%2) {
1376  // nodesExpanded += 1;
1377 
1378  // if (topNode.rxp)
1379  // nReExp++;
1380  // else
1381  // nNewExp++;
1382  //}
1383 
1384  //nodesExpanded += 1; //ichild / (double)neighbors.size();
1385  }
1386 
1387  //tickStart = clock();
1388 
1389  graphState topNodeID = topNode.currNode;
1390  closedList[topNodeID] = topNode;
1391 
1392  if (verbose)
1393  {
1394  printf("Expanding node %ld , g=%lf, h=%lf, f=%lf.\n",topNodeID,topNode.gCost,topNode.fCost-topNode.gCost,topNode.fCost);
1395  }
1396 
1397  justExpanded = topNodeID;
1398 
1399  /* step (4) */
1400  if (env->GoalTest(topNodeID, goal))
1401  {
1402  //CleanUpOpen(topNode.gCost); // put nodes in open with f==F but g<g(goal) into closed, since they are likely part of the solution
1403 
1404  ExtractPathToStart(topNodeID, thePath);
1405  closedList.clear();
1406  openQueue.reset();
1407  FCache.reset();
1408  env = 0;
1409  return true;
1410  }
1411 
1412  /* step (5), computing gi is delayed */
1413  //neighbors.resize(0);
1414  //env->GetSuccessors(topNodeID, neighbors);
1415 
1416  double hTop = topNode.fCost - topNode.gCost;
1417  double minH2 = DBL_MAX; // min ( edgeWeight(i) + h(neighbor(i)) )
1418 
1419  //while(true)
1420  //{
1421  // SearchNode neighborNode;
1422  // graphState neighbor;
1423  // int mode;
1424  // //double edgeWeight;
1425 
1426  // if (!NextNeighbor(neighborNode, neighbor, mode))
1427  // break;
1428  //}
1429  unsigned int x;
1430  for ( x = 0,ichild=1; x<neighbors.size(); x++,ichild++)
1431  {
1432  nodesTouched++;
1433 
1434  /* step (5) */
1435  graphState neighbor = neighbors[x];
1436  double edgeWeight = env->GCost(topNodeID,neighbor);
1437  double g = topNode.gCost + edgeWeight;
1438 
1439  /* step Mero (3a) */
1440  double h_tmp; // for printing reports only
1441  double h;
1442 
1443  // determine neighbor type
1444  SearchNode neighborNode;
1445  int mode;
1446  neighborNode = openQueue.find(SearchNode(neighbor));
1447  if (neighborNode.currNode == neighbor) {
1448  mode = OPENMODE;
1449  }
1450  else {
1451  NodeLookupTable::iterator iter = closedList.find(neighbor);
1452  if (iter != closedList.end()) {
1453  neighborNode = iter->second;
1454  mode = CLOSEDMODE;
1455  }
1456  else {
1457  mode = NEWMODE;
1458  }
1459  }
1460 
1461  if (bpmxLevel >= 1)
1462  ComputeNewHMero3a(h, h_tmp, neighbor, neighborNode, hTop - edgeWeight, mode);
1463  else {
1464  if (mode == NEWMODE)
1465  h = env->HCost(neighbor,goal);
1466  else
1467  h = neighborNode.fCost - neighborNode.gCost;
1468  }
1469 
1470  if (verbose)
1471  {
1472  if (fgreater(h,h_tmp))
1473  printf("Improving h of node %ld by Mero rule (a), %lf->%lf\n",neighbor,h_tmp,h);
1474  }
1475 
1476  if (bpmxLevel >= 1)
1477  if (fgreater(h - edgeWeight , hTop)) {
1478  //if (topNode.rxp) {
1479  // tickReExp += clock() - tickStart + tickGen;
1480  // tickGen = 0;
1481  //}
1482  //else {
1483  // tickNewExp += clock() - tickStart + tickGen;
1484  // tickGen = 0;
1485  //}
1486 
1487  topNode.fCost = topNode.gCost + h - edgeWeight; // hack: fix for when heuristic morphing
1488  doReverse = true;
1489  goto _REVERSE;
1490  }
1491 
1492  double f = g + h;
1493 
1494  /* step Mero (3b) */
1495  minH2 = min(minH2, h + edgeWeight);
1496 
1497  /* step (6), neither in OPEN nor CLOSED */
1498  if (mode == NEWMODE)
1499  {
1500  SearchNode n(f,g,neighbor,topNodeID);
1501  n.isGoal = (neighbor==goal);
1502  openQueue.Add(n);
1503 
1504  if (verbose)
1505  {
1506  printf("Adding node %ld to OPEN, g=%lf, h=%lf, f=%lf.\n",neighbor,g,f-g,f);
1507  }
1508  }
1509 
1510  /* step (7) */
1511  else
1512  {
1513  //SearchNode neighborNode;
1514  if (mode == OPENMODE)
1515  {
1516  //neighborNode = openQueue.find(SearchNode(neighbor));
1517 
1518  //if (neighborNode.gCost <= g) {
1519  if (!fgreater(neighborNode.gCost,g))
1520  {
1521  /* covered by backward H propagation check already */
1522  if (fless(neighborNode.gCost + edgeWeight , topNode.gCost)) {
1523  //if (topNode.rxp) {
1524  // tickReExp += clock() - tickStart + tickGen;
1525  // tickGen = 0;
1526  //}
1527  //else {
1528  // tickNewExp += clock() - tickStart + tickGen;
1529  // tickGen = 0;
1530  //}
1531 
1532  doReverse = true;
1533  goto _REVERSE;
1534  }
1535  // we may fail to update g, but still update h
1536  /* proved to be impossible */
1537  //if (UpdateHOnly(neighborNode, h))
1538  // openQueue.IncreaseKey(neighborNode);
1539  continue;
1540  }
1541 
1542  if (verbose)
1543  {
1544  printf("Adjusting node %ld in OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
1545  }
1546 
1547  RelaxOpenNode(f, g, neighbor, neighborNode, topNodeID);
1548  }
1549  else //if (closedList.find(neighbor) != closedList.end())
1550  {
1551  //neighborNode = closedList.find(neighbor)->second;
1552 
1553  //if (neighborNode.gCost <= g) {
1554  if (!fgreater(neighborNode.gCost,g))
1555  {
1556  if (fless(neighborNode.gCost + edgeWeight , topNode.gCost)) {
1557 
1558  //if (topNode.rxp) {
1559  // tickReExp += clock() - tickStart + tickGen;
1560  // tickGen = 0;
1561  //}
1562  //else {
1563  // tickNewExp += clock() - tickStart + tickGen;
1564  // tickGen = 0;
1565  //}
1566 
1567  doReverse = true;
1568  goto _REVERSE;
1569  }
1570  // we may fail to update g, but still update h
1571  if (bpmxLevel >= 1)
1572  if (UpdateHOnly(neighborNode, h))
1573  closedList[neighbor] = neighborNode;
1574  continue;
1575  }
1576 
1577  if (verbose)
1578  {
1579  printf("Moving node %ld from CLOSED to OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
1580  }
1581 
1582  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
1583  closedList.erase(neighbor); // delete from CLOSED
1584  NodesReopened++;
1585 
1586  //neighborNode.rxp = true;
1587 
1588  openQueue.Add(neighborNode); // add to OPEN
1589  }
1590 
1591  }
1592  }
1593 
1594  /* step Mero (3b), update h of parent */
1595  if (bpmxLevel >= 1)
1596  if (fgreater(minH2 , hTop))
1597  {
1598  topNode.fCost = minH2 + topNode.gCost; // f = h + g
1599  closedList[topNodeID] = topNode;
1600 
1601  if (verbose)
1602  {
1603  printf("Improving h of node %ld by Mero rule (b), %lf->%lf\n",topNodeID,hTop,minH2);
1604  }
1605  }
1606 
1607  neighbors.clear();
1608 
1609  //if (topNode.rxp) {
1610  // tickReExp += clock() - tickStart + tickGen;
1611  //}
1612  //else {
1613  // tickNewExp += clock() - tickStart + tickGen;
1614  //}
1615 
1616  return false;
1617 }
1618 
1619 bool Prop::DoSingleStepBPMX(std::vector<graphState> &thePath)
1620 {
1621  /* step (2) */
1622  if (openQueue.size() == 0)
1623  {
1624  thePath.resize(0); // no path found!
1625  closedList.clear();
1626  openQueue.reset();
1627  FCache.reset();
1628  env = 0;
1629  return true;
1630  }
1631 
1632  /* step (3) */
1633  nodesExpanded += 1;
1634 
1635  // select the node to expand
1636  SearchNode topNode;
1637  if (fless(openQueue.top().fCost , F))
1638  {
1639  GetLowestG(topNode);
1640  if (verbose)
1641  printf("Expanding a node below F.\n");
1642  }
1643  else
1644  {
1645  topNode = openQueue.Remove();
1646 
1647  if (fgreater(topNode.fCost,F))
1648  {
1649  F = topNode.fCost; // update F
1650  if (verbose)
1651  {
1652  printf("F updated to %lf.\n",F);
1653  }
1654  }
1655  }
1656 
1657  //if (topNode.rxp)
1658  // nReExp++;
1659  //else
1660  // nNewExp++;
1661 
1662 // unsigned long tickTmp;// = clock();
1663 
1664  /* step (5), computing gi is delayed */
1665  neighbors.resize(0);
1666  env->GetSuccessors(topNode.currNode, neighbors);
1667 
1668  //tickGen = clock() - tickTmp;
1669 
1670  bool doBPMX = false;
1671  //Categorize(neighbors);
1672  int ichild = 0;
1673 
1674 _BPMX:
1675 
1676  if (doBPMX) {
1677  ReversePropX1(topNode);
1678 
1679  // counting supplement
1680  //if (nodesExpanded%2) {
1681  // nodesExpanded += 1;
1682 
1683  // if (topNode.rxp)
1684  // nReExp++;
1685  // else
1686  // nNewExp++;
1687  //}
1688 
1689  //nodesExpanded += 1; //ichild / (double)neighbors.size();
1690  }
1691 
1692  //tickStart = clock();
1693 
1694  // topnode may be updated in ReverseProp, so put to closed afterwards
1695  graphState topNodeID = topNode.currNode;
1696  closedList[topNodeID] = topNode;
1697 
1698  if (verbose)
1699  {
1700  printf("Expanding node %ld , g=%lf, h=%lf, f=%lf.\n",topNodeID,topNode.gCost,topNode.fCost-topNode.gCost,topNode.fCost);
1701  }
1702 
1703  justExpanded = topNodeID;
1704 
1705  /* step (4) */
1706  if (env->GoalTest(topNodeID, goal))
1707  {
1708  ExtractPathToStart(topNodeID, thePath);
1709  closedList.clear();
1710  openQueue.reset();
1711  FCache.reset();
1712  env = 0;
1713  return true;
1714  }
1715 
1716  double hTop = topNode.fCost - topNode.gCost;
1717  double minH2 = DBL_MAX; // min ( edgeWeight(i) + h(neighbor(i)) )
1718 
1719 
1720  //while(true)
1721  //{
1722  //SearchNode neighborNode;
1723  //graphState neighbor;
1724  //int mode;
1726 
1727  //if (!NextNeighbor(neighborNode, neighbor, mode))
1728  // break;
1729  unsigned int x;
1730  for (x = 0,ichild=1; x<neighbors.size(); x++,ichild++)
1731  {
1732  nodesTouched++;
1733 
1734  /* step (5) */
1735  graphState neighbor = neighbors[x];
1736  double edgeWeight = env->GCost(topNodeID,neighbor);
1737  double g = topNode.gCost + edgeWeight;
1738 
1739  /* step Mero (3a) */
1740  double h_tmp; // for printing reports only
1741  double h;
1742 
1743  // determine neighbor type
1744  SearchNode neighborNode;
1745  int mode;
1746  neighborNode = openQueue.find(SearchNode(neighbor));
1747  if (neighborNode.currNode == neighbor) {
1748  mode = OPENMODE;
1749  }
1750  else {
1751  NodeLookupTable::iterator iter = closedList.find(neighbor);
1752  if (iter != closedList.end()) {
1753  neighborNode = iter->second;
1754  mode = CLOSEDMODE;
1755  }
1756  else {
1757  mode = NEWMODE;
1758  }
1759  }
1760 
1761  ComputeNewHMero3a(h, h_tmp, neighbor, neighborNode, hTop - edgeWeight, mode);
1762 
1763  if (verbose)
1764  {
1765  if (fgreater(h,h_tmp))
1766  printf("Improving h of node %ld by Mero rule (a), %lf->%lf\n",neighbor,h_tmp,h);
1767  }
1768 
1769  if (fgreater(h - edgeWeight , hTop)) {
1770  //if (topNode.rxp) {
1771  // tickReExp += clock() - tickStart + tickGen;
1772  // tickGen = 0;
1773  //}
1774  //else {
1775  // tickNewExp += clock() - tickStart + tickGen;
1776  // tickGen = 0;
1777  //}
1778 
1779  topNode.fCost = topNode.gCost + h - edgeWeight; // hack: fix for when heuristic morphing
1780  doBPMX = true;
1781  goto _BPMX;
1782  }
1783 
1784  double f = g + h;
1785 
1786  /* step Mero (3b) */
1787  minH2 = min(minH2, h + edgeWeight);
1788 
1789  /* step (6), neither in OPEN nor CLOSED */
1790  if (mode == NEWMODE)
1791  {
1792  SearchNode n(f,g,neighbor,topNodeID);
1793  n.isGoal = (neighbor==goal);
1794  openQueue.Add(n);
1795 
1796  if (verbose)
1797  {
1798  printf("Adding node %ld to OPEN, g=%lf, h=%lf, f=%lf.\n",neighbor,g,f-g,f);
1799  }
1800  }
1801 
1802  /* step (7) */
1803  else
1804  {
1805  //SearchNode neighborNode;
1806  if (mode == OPENMODE) //openQueue.IsIn(SearchNode(neighbor)))
1807  {
1808  //neighborNode = openQueue.find(SearchNode(neighbor));
1809 
1810  //if (neighborNode.gCost <= g) {
1811  if (!fgreater(neighborNode.gCost,g))
1812  {
1813  // we may fail to update g, but still update h
1814  /* proved to be impossible */
1815  if (UpdateHOnly(neighborNode, h))
1816  openQueue.IncreaseKey(neighborNode);
1817  continue;
1818  }
1819 
1820  if (verbose)
1821  {
1822  printf("Adjusting node %ld in OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
1823  }
1824 
1825  RelaxOpenNode(f, g, neighbor, neighborNode, topNodeID);
1826  }
1827  else //if (closedList.find(neighbor) != closedList.end())
1828  {
1829  //neighborNode = closedList.find(neighbor)->second;
1830 
1831  //if (neighborNode.gCost <= g) {
1832  if (!fgreater(neighborNode.gCost,g))
1833  {
1834  // we may fail to update g, but still update h
1835  if (UpdateHOnly(neighborNode, h)) {
1836  closedList[neighbor] = neighborNode;
1837 
1838  if (bpmxLevel > 1)
1839  fifo.push_back(neighbor);
1840  }
1841  continue;
1842  }
1843 
1844  if (verbose)
1845  {
1846  printf("Moving node %ld from CLOSED to OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
1847  }
1848 
1849  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
1850  closedList.erase(neighbor); // delete from CLOSED
1851  NodesReopened++;
1852 
1853  //neighborNode.rxp = true;
1854 
1855  openQueue.Add(neighborNode); // add to OPEN
1856  }
1857 
1858  }
1859  }
1860 
1861  /* step Mero (3b), update h of parent */
1862  if (fgreater(minH2 , hTop))
1863  {
1864  topNode.fCost = minH2 + topNode.gCost; // f = h + g
1865  closedList[topNodeID] = topNode;
1866 
1867  if (verbose)
1868  {
1869  printf("Improving h of node %ld by Mero rule (b), %lf->%lf\n",topNodeID,hTop,minH2);
1870  }
1871  }
1872 
1873  neighbors.clear();
1874 
1875  if (fifo.size() > 0) {
1876  Broadcast(1,fifo.size()); // (level, levelcount)
1877  }
1878 
1879  //if (topNode.rxp)
1880  // tickReExp += clock() - tickStart + tickGen;
1881  //else
1882  // tickNewExp += clock() - tickStart + tickGen;
1883 
1884  return false;
1885 }
1886 
1887 
1888 bool Prop::DoSingleStepBPMXE(std::vector<graphState> &)
1889 {
1890  return true;
1891 }
1892 
1893 /* DP + Delay + BPMX */
1894 bool Prop::DoSingleStepDPDLMX(std::vector<graphState> &thePath)
1895 {
1896  /* step (2) */
1897 
1898  //if (openQueue.size() == 0)
1899  //{
1900  // thePath.resize(0); // no path found!
1901  // closedList.clear();
1902  // openQueue.reset();
1903  // FCache.reset();
1904  // env = 0;
1905  // return true;
1906  //}
1907 
1908 
1909  /* step (3) */
1910  nodesExpanded += 1;
1911 
1912  // select the node to expand
1913  // this is where delay comes into play
1914  SearchNode topNode;
1915 
1916  if (goal == openQueue.top().currNode)
1917  {
1918  bool found = false;
1919  while(delayCache.size() > 0 && delayCache.top().gCost < openQueue.top().fCost) {
1920  topNode = delayCache.Remove();
1921  if (topNode.fCost < openQueue.top().fCost) {
1922  found = true;
1923  break;
1924  }
1925  }
1926  if (!found) {
1927  topNode = openQueue.Remove();
1928  }
1929  }
1930  else if (delayCache.size() > 0)
1931  {
1932  if (openQueue.size() ==0 || (reopenings < fDelay(nodesExpanded-NodesReopened) && delayCache.top().gCost < openQueue.top().fCost)) {
1933  topNode = delayCache.Remove();
1934  reopenings++;
1935  }
1936  else {
1937  topNode = openQueue.Remove();
1938  reopenings = 0;
1939  }
1940  }
1941  else if (openQueue.size() > 0)
1942  {
1943  topNode = openQueue.Remove();
1944  reopenings = 0;
1945  }
1946  else
1947  {
1948  thePath.resize(0); // no path found!
1949  closedList.clear();
1950  openQueue.reset();
1951  FCache.reset();
1952  delayCache.reset();
1953  env = 0;
1954  return true;
1955  }
1956  //if (fless(openQueue.top().fCost , F))
1957  //{
1958  // GetLowestG(topNode);
1959  // if (verbose)
1960  // printf("Expanding a node below F.\n");
1961  //}
1962  //else
1963  //{
1964  // topNode = openQueue.Remove();
1965 
1966  // if (fgreater(topNode.fCost,F))
1967  // {
1968  // F = topNode.fCost; // update F
1969  // if (verbose)
1970  // {
1971  // printf("F updated to %lf.\n",F);
1972  // }
1973  // }
1974  //}
1975 
1976  neighbors.resize(0);
1977  env->GetSuccessors(topNode.currNode, neighbors);
1978 
1979  //Categorize(neighbors);
1980 
1981  bool doReverse = false;
1982 
1983  int ichild = 0;
1984 
1985 _REVERSE:
1986  // reverseProp() here, top node will be updated inside, so put topnode into closed afterwards
1987  if (doReverse) {
1988  if (bpmxLevel >= 1)
1989  ReversePropX2(topNode);
1990  else
1991  ReverseProp(topNode);
1992 
1993  // counting supplement
1994  //if (nodesExpanded%2)
1995  // nodesExpanded += 1;
1996 
1997  //nodesExpanded += 1; //ichild /(double)neighbors.size();
1998  }
1999 
2000  graphState topNodeID = topNode.currNode;
2001  closedList[topNodeID] = topNode;
2002 
2003  if (verbose)
2004  {
2005  printf("Expanding node %ld , g=%lf, h=%lf, f=%lf.\n",topNodeID,topNode.gCost,topNode.fCost-topNode.gCost,topNode.fCost);
2006  }
2007 
2008  justExpanded = topNodeID;
2009 
2010  /* step (4) */
2011  if (env->GoalTest(topNodeID, goal))
2012  {
2013  //CleanUpOpen(topNode.gCost); // put nodes in open with f==F but g<g(goal) into closed, since they are likely part of the solution
2014 
2015  ExtractPathToStart(topNodeID, thePath);
2016  closedList.clear();
2017  openQueue.reset();
2018  delayCache.reset();
2019  FCache.reset();
2020  env = 0;
2021  return true;
2022  }
2023 
2024  /* step (5), computing gi is delayed */
2025  //neighbors.resize(0);
2026  //env->GetSuccessors(topNodeID, neighbors);
2027 
2028  double hTop = topNode.fCost - topNode.gCost;
2029  double minH2 = DBL_MAX; // min ( edgeWeight(i) + h(neighbor(i)) )
2030 
2031  //while(true)
2032  //{
2033  // SearchNode neighborNode;
2034  // graphState neighbor;
2035  // int mode;
2036  // //double edgeWeight;
2037 
2038  // if (!NextNeighbor(neighborNode, neighbor, mode))
2039  // break;
2040  //}
2041  unsigned int x ;
2042  for ( x = 0,ichild=1; x<neighbors.size(); x++,ichild++)
2043  {
2044  nodesTouched++;
2045 
2046  /* step (5) */
2047  graphState neighbor = neighbors[x];
2048  double edgeWeight = env->GCost(topNodeID,neighbor);
2049  double g = topNode.gCost + edgeWeight;
2050 
2051  /* step Mero (3a) */
2052  double h_tmp; // for printing reports only
2053  double h;
2054 
2055  // determine neighbor type
2056  SearchNode neighborNode;
2057  int mode;
2058  neighborNode = openQueue.find(SearchNode(neighbor));
2059  if (neighborNode.currNode == neighbor) {
2060  mode = OPENMODE;
2061  }
2062  else {
2063  NodeLookupTable::iterator iter = closedList.find(neighbor);
2064  if (iter != closedList.end()) {
2065  neighborNode = iter->second;
2066  mode = CLOSEDMODE;
2067  }
2068  else {
2069  neighborNode = delayCache.find(SearchNode(neighbor));
2070  if (neighborNode.currNode == neighbor)
2071  mode = DELAYMODE;
2072  else
2073  mode = NEWMODE;
2074  }
2075  }
2076 
2077  if (bpmxLevel >= 1)
2078  ComputeNewHMero3a(h, h_tmp, neighbor, neighborNode, hTop - edgeWeight, mode);
2079  else {
2080  if (mode == NEWMODE)
2081  h = env->HCost(neighbor,goal);
2082  else
2083  h = neighborNode.fCost - neighborNode.gCost;
2084  }
2085 
2086  if (verbose)
2087  {
2088  if (fgreater(h,h_tmp))
2089  printf("Improving h of node %ld by Mero rule (a), %lf->%lf\n",neighbor,h_tmp,h);
2090  }
2091 
2092  if (bpmxLevel >= 1)
2093  if (fgreater(h - edgeWeight , hTop)) {
2094  topNode.fCost = topNode.gCost + h - edgeWeight; // hack: fix for when heuristic morphing
2095  doReverse = true;
2096  goto _REVERSE;
2097  }
2098 
2099  double f = g + h;
2100 
2101  /* step Mero (3b) */
2102  minH2 = min(minH2, h + edgeWeight);
2103 
2104  /* step (6), neither in OPEN nor CLOSED */
2105  if (mode == NEWMODE)
2106  {
2107  SearchNode n(f,g,neighbor,topNodeID);
2108  n.isGoal = (neighbor==goal);
2109  openQueue.Add(n);
2110 
2111  if (verbose)
2112  {
2113  printf("Adding node %ld to OPEN, g=%lf, h=%lf, f=%lf.\n",neighbor,g,f-g,f);
2114  }
2115  }
2116 
2117  /* step (7) */
2118  else
2119  {
2120  //SearchNode neighborNode;
2121  if (mode == OPENMODE)
2122  {
2123  //neighborNode = openQueue.find(SearchNode(neighbor));
2124 
2125  //if (neighborNode.gCost <= g) {
2126  if (!fgreater(neighborNode.gCost,g))
2127  {
2128  /* covered by backward H propagation check already */
2129  if (fless(neighborNode.gCost + edgeWeight , topNode.gCost)) {
2130  doReverse = true;
2131  goto _REVERSE;
2132  }
2133  // we may fail to update g, but still update h
2134  /* proved to be impossible */
2135  //if (UpdateHOnly(neighborNode, h))
2136  // openQueue.IncreaseKey(neighborNode);
2137  continue;
2138  }
2139 
2140  if (verbose)
2141  {
2142  printf("Adjusting node %ld in OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
2143  }
2144 
2145  RelaxOpenNode(f, g, neighbor, neighborNode, topNodeID);
2146  }
2147  else if (mode == DELAYMODE)
2148  {
2149  if (!fgreater(neighborNode.gCost,g))
2150  {
2151  if (fless(neighborNode.gCost + edgeWeight, topNode.gCost)) {
2152  doReverse = true;
2153  goto _REVERSE;
2154  }
2155  // fail to update g, but may still update h
2156  if (UpdateHOnly(neighborNode, h))
2157  delayCache.IncreaseKey(neighborNode);
2158  continue;
2159  }
2160 
2161  RelaxDelayNode(f, g, neighbor, neighborNode, topNodeID);
2162  }
2163  else //if (closedList.find(neighbor) != closedList.end())
2164  {
2165  //neighborNode = closedList.find(neighbor)->second;
2166 
2167  //if (neighborNode.gCost <= g) {
2168  if (!fgreater(neighborNode.gCost,g))
2169  {
2170  if (fless(neighborNode.gCost + edgeWeight , topNode.gCost)) {
2171  doReverse = true;
2172  goto _REVERSE;
2173  }
2174  // we may fail to update g, but still update h
2175  if (bpmxLevel >= 1)
2176  if (UpdateHOnly(neighborNode, h))
2177  closedList[neighbor] = neighborNode;
2178  continue;
2179  }
2180 
2181  if (verbose)
2182  {
2183  printf("Moving node %ld from CLOSED to OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
2184  }
2185 
2186  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
2187  closedList.erase(neighbor); // delete from CLOSED
2188  NodesReopened++;
2189 
2190  openQueue.Add(neighborNode); // add to OPEN
2191  }
2192 
2193  }
2194  }
2195 
2196  /* step Mero (3b), update h of parent */
2197  if (bpmxLevel >= 1)
2198  if (fgreater(minH2 , hTop))
2199  {
2200  topNode.fCost = minH2 + topNode.gCost; // f = h + g
2201  closedList[topNodeID] = topNode;
2202 
2203  if (verbose)
2204  {
2205  printf("Improving h of node %ld by Mero rule (b), %lf->%lf\n",topNodeID,hTop,minH2);
2206  }
2207  }
2208 
2209  neighbors.clear();
2210 
2211  return false;
2212 }
2213 
2214 /* not used */
2215 bool Prop::DoSingleStepDPMX(std::vector<graphState> &thePath)
2216 {
2217  /* step (2) */
2218  if (openQueue.size() == 0)
2219  {
2220  thePath.resize(0); // no path found!
2221  closedList.clear();
2222  openQueue.reset();
2223  FCache.reset();
2224  env = 0;
2225  return true;
2226  }
2227 
2228  /* step (3) */
2229  nodesExpanded += 1;
2230 
2231  // select the node to expand
2232  SearchNode topNode;
2233  if (fless(openQueue.top().fCost , F))
2234  {
2235  GetLowestG(topNode);
2236  if (verbose)
2237  printf("Expanding a node below F.\n");
2238  }
2239  else
2240  {
2241  topNode = openQueue.Remove();
2242 
2243  if (fgreater(topNode.fCost,F))
2244  {
2245  F = topNode.fCost; // update F
2246  if (verbose)
2247  {
2248  printf("F updated to %lf.\n",F);
2249  }
2250  }
2251  }
2252 
2253  neighbors.resize(0);
2254  env->GetSuccessors(topNode.currNode, neighbors);
2255 
2256  //Categorize(neighbors);
2257 
2258  // reverseProp() here, top node will be updated inside, so put topnode into closed afterwards
2259  ReversePropX2(topNode);
2260 
2261  //nodesExpanded += 1;
2262 
2263  graphState topNodeID = topNode.currNode;
2264  closedList[topNodeID] = topNode;
2265 
2266  if (verbose)
2267  {
2268  printf("Expanding node %ld , g=%lf, h=%lf, f=%lf.\n",topNodeID,topNode.gCost,topNode.fCost-topNode.gCost,topNode.fCost);
2269  }
2270 
2271  justExpanded = topNodeID;
2272 
2273  /* step (4) */
2274  if (env->GoalTest(topNodeID, goal))
2275  {
2276  //CleanUpOpen(topNode.gCost); // put nodes in open with f==F but g<g(goal) into closed, since they are likely part of the solution
2277 
2278  ExtractPathToStart(topNodeID, thePath);
2279  closedList.clear();
2280  openQueue.reset();
2281  FCache.reset();
2282  env = 0;
2283  return true;
2284  }
2285 
2286  /* step (5), computing gi is delayed */
2287  //neighbors.resize(0);
2288  //env->GetSuccessors(topNodeID, neighbors);
2289 
2290  double hTop = topNode.fCost - topNode.gCost;
2291  double minH2 = DBL_MAX; // min ( edgeWeight(i) + h(neighbor(i)) )
2292 
2293  //while(true)
2294  //{
2295  // SearchNode neighborNode;
2296  // graphState neighbor;
2297  // int mode;
2298  // //double edgeWeight;
2299 
2300  // if (!NextNeighbor(neighborNode, neighbor, mode))
2301  // break;
2302  //}
2303  for (unsigned int x = 0; x<neighbors.size(); x++)
2304  {
2305  nodesTouched++;
2306 
2307  /* step (5) */
2308  graphState neighbor = neighbors[x];
2309  double edgeWeight = env->GCost(topNodeID,neighbor);
2310  double g = topNode.gCost + edgeWeight;
2311 
2312  /* step Mero (3a) */
2313  double h_tmp; // for printing reports only
2314  double h;
2315 
2316  // determine neighbor type
2317  SearchNode neighborNode;
2318  int mode;
2319  neighborNode = openQueue.find(SearchNode(neighbor));
2320  if (neighborNode.currNode == neighbor) {
2321  mode = OPENMODE;
2322  }
2323  else {
2324  NodeLookupTable::iterator iter = closedList.find(neighbor);
2325  if (iter != closedList.end()) {
2326  neighborNode = iter->second;
2327  mode = CLOSEDMODE;
2328  }
2329  else {
2330  mode = NEWMODE;
2331  }
2332  }
2333 
2334  ComputeNewHMero3a(h, h_tmp, neighbor, neighborNode, hTop - edgeWeight, mode);
2335 
2336  if (verbose)
2337  {
2338  if (fgreater(h,h_tmp))
2339  printf("Improving h of node %ld by Mero rule (a), %lf->%lf\n",neighbor,h_tmp,h);
2340  }
2341 
2342  double f = g + h;
2343 
2344  /* step Mero (3b) */
2345  minH2 = min(minH2, h + edgeWeight);
2346 
2347  /* step (6), neither in OPEN nor CLOSED */
2348  if (mode == NEWMODE)
2349  {
2350  SearchNode n(f,g,neighbor,topNodeID);
2351  n.isGoal = (neighbor==goal);
2352  openQueue.Add(n);
2353 
2354  if (verbose)
2355  {
2356  printf("Adding node %ld to OPEN, g=%lf, h=%lf, f=%lf.\n",neighbor,g,f-g,f);
2357  }
2358  }
2359 
2360  /* step (7) */
2361  else
2362  {
2363  //SearchNode neighborNode;
2364  if (mode == OPENMODE)
2365  {
2366  //neighborNode = openQueue.find(SearchNode(neighbor));
2367 
2368  //if (neighborNode.gCost <= g) {
2369  if (!fgreater(neighborNode.gCost,g))
2370  {
2371  // we may fail to update g, but still update h
2372  if (UpdateHOnly(neighborNode, h))
2373  openQueue.IncreaseKey(neighborNode);
2374  continue;
2375  }
2376 
2377  if (verbose)
2378  {
2379  printf("Adjusting node %ld in OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
2380  }
2381 
2382  RelaxOpenNode(f, g, neighbor, neighborNode, topNodeID);
2383  }
2384  else //if (closedList.find(neighbor) != closedList.end())
2385  {
2386  //neighborNode = closedList.find(neighbor)->second;
2387 
2388  //if (neighborNode.gCost <= g) {
2389  if (!fgreater(neighborNode.gCost,g))
2390  {
2391  // we may fail to update g, but still update h
2392  if (UpdateHOnly(neighborNode, h))
2393  closedList[neighbor] = neighborNode;
2394  continue;
2395  }
2396 
2397  if (verbose)
2398  {
2399  printf("Moving node %ld from CLOSED to OPEN, g=%lf, h=%lf, f=%lf; g_old=%lf.\n",neighbor,g,f-g,f, neighborNode.gCost);
2400  }
2401 
2402  neighborNode.copy(f,g,neighbor,topNodeID); // parent may be changed
2403  closedList.erase(neighbor); // delete from CLOSED
2404  NodesReopened++;
2405 
2406  openQueue.Add(neighborNode); // add to OPEN
2407  }
2408 
2409  }
2410  }
2411 
2412  /* step Mero (3b), update h of parent */
2413  if (fgreater(minH2 , hTop))
2414  {
2415  topNode.fCost = minH2 + topNode.gCost; // f = h + g
2416  closedList[topNodeID] = topNode;
2417 
2418  if (verbose)
2419  {
2420  printf("Improving h of node %ld by Mero rule (b), %lf->%lf\n",topNodeID,hTop,minH2);
2421  }
2422  }
2423 
2424  neighbors.clear();
2425 
2426  return false;
2427 }
2428 
2429 void Prop::ExtractPathToStart(graphState goalNode, std::vector<graphState> &thePath)
2430 {
2431  SearchNode n;
2432  NodeLookupTable::iterator iter;
2433 
2434  closedSize = closedList.size();
2435 
2436  if (closedList.find(goalNode) != closedList.end())
2437  {
2438  n = closedList[goalNode];
2439  }
2440  else n = openQueue.find(SearchNode(goalNode));
2441 
2442  solutionCost = n.gCost;
2443  do {
2444  //solutionCost += env->GCost(n.prevNode,n.currNode);
2445 
2446  if (verbose)
2447  printf("%ld<-%ld,",n.currNode,n.prevNode);
2448 
2449  thePath.push_back(n.currNode);
2450  //n = closedList[n.prevNode];
2451  iter = closedList.find(n.prevNode);
2452  if (iter != closedList.end())
2453  n = iter->second;
2454  else
2455  n = openQueue.find(SearchNode(n.prevNode));
2456 
2457  } while (n.currNode != n.prevNode);
2458  //thePath.push_back(n.currNode);
2459  pathSize = thePath.size();
2460 }
2461 
2462 void Prop::OpenGLDraw() const
2463 {
2464 // // node to expand: blue
2465 // // in open: green
2466 // // in closed: red
2467 // // in waitlist: yellow
2468 //
2469 // //float r,gcost,b;
2470 // double x,y,z;
2471 // SearchNode sn;
2472 // graphState nodeID;
2473 // SearchNode topn;
2474 // char buf[100];
2475 //
2476 // // draw nodes
2477 // node_iterator ni = grp->getNodeIter();
2478 // for (node* n = grp->nodeIterNext(ni); n; n = grp->nodeIterNext(ni))
2479 // {
2480 // graphGenerator::GetLoc(n,x,y,z);
2481 //
2482 // nodeID = (graphState) n->GetNum();
2483 // // draw sphere first
2484 //
2485 // // if it's just expanded
2486 // NodeLookupTable::iterator hiter;
2487 // if (nodeID == goal)
2488 // {
2489 // glColor3f(1.0, 0.0, 1.0); // Magenta
2490 // DrawSphere(x,y,z,0.025);
2491 // }
2492 // else if (nodeID == justExpanded)
2493 // {
2494 // sn = closedList.find(nodeID)->second;
2495 // glColor3f(0,0,1); // blue
2496 // DrawSphere(x,y,z,0.025);
2497 //
2498 // memset(buf,0,100);
2499 // sprintf(buf,"%d [%d,%d,%d]",n->GetNum(), (int)sn.gCost, (int)(sn.fCost - sn.gCost), (int)sn.fCost);
2500 // }
2501 // // if in closed
2502 // else if ((hiter = closedList.find(nodeID)) != closedList.end())
2503 // {
2504 // sn = hiter->second;
2505 // glColor3f(1,0,0); // red
2506 // DrawSphere(x,y,z,0.025);
2507 //
2508 // memset(buf,0,100);
2509 // sprintf(buf,"%d [%d,%d,%d]",n->GetNum(), (int)sn.gCost, (int)(sn.fCost - sn.gCost), (int)sn.fCost);
2510 // }
2511 // // if in open
2512 // else if (openQueue.IsIn(SearchNode(nodeID)))
2513 // {
2514 // sn = openQueue.find(SearchNode(nodeID));
2515 //
2516 //
2517 //
2518 // glColor3f(0,1,0); // green
2519 // DrawSphere(x,y,z,0.025);
2520 //
2521 //
2522 // memset(buf,0,100);
2523 // sprintf(buf,"%d [%ld,%ld,%ld]",n->GetNum(), (long)sn.gCost, (long)(sn.fCost - sn.gCost), (long)sn.fCost);
2524 // }
2525 // else if (WaitList.IsIn(SearchNode(nodeID)))
2526 // {
2527 // sn = WaitList.find(SearchNode(nodeID));
2528 //
2529 // glColor3f(1.0, 1.0, 0.0); // yellow
2530 // DrawSphere(x,y,z,0.025);
2531 //
2532 // memset(buf,0,100);
2533 // sprintf(buf,"%d [%ld,%ld,%ld]",n->GetNum(), (long)sn.gCost, (long)(sn.fCost - sn.gCost), (long)sn.fCost);
2534 // }
2535 // // neither, ignore
2536 // else
2537 // {
2538 // continue;
2539 //
2540 // glColor3f(1,1,1); // white
2541 // DrawSphere(x,y,z,0.025);
2542 //
2543 // memset(buf,0,100);
2544 // sprintf(buf,"%d [?,%ld,?]",n->GetNum(), (long)env->HCost(nodeID,goal));
2545 // }
2546 //
2547 // // draw the text info, in black
2548 // if (drawtext)
2549 // DrawText(x,y,z-0.15,0,0,0,buf);
2550 // }
2551 //
2552 // // draw edges
2553 // edge_iterator ei = grp->getEdgeIter();
2554 // for (edge* e = grp->edgeIterNext(ei); e; e = grp->edgeIterNext(ei))
2555 // {
2556 // DrawEdge(e->getFrom(), e->getTo(), e->GetWeight());
2557 // }
2558 }
2559 
2560 void Prop::DrawText(double x, double y, double z, float r, float gg, float b, char* str)
2561 {
2562  //glPushMatrix();
2563  // rotate ?
2564 
2565  glPushMatrix();
2566  glColor3f(r,gg,b);
2567  glTranslatef(x,y,z);
2568  glScalef(1.0/(20*120.0), 1.0/(20*120.0), 1);
2569  glRotatef(180, 0.0, 0.0, 1.0);
2570  glRotatef(180, 0.0, 1.0, 0.0);
2571 
2572  int i=0;
2573  while(str[i])
2574  {
2575 // glutStrokeCharacter(GLUT_STROKE_ROMAN,str[i]);
2576  i++;
2577  }
2578  glPopMatrix();
2579 }
2580 
2581 void Prop::DrawEdge(unsigned int from, unsigned int to, double weight)
2582 {
2583  double x1,y1,z1;
2584  double x2,y2,z2;
2585  char buf[100] = {0};
2586 
2587  node* nfrom = grp->GetNode(from);
2588  node* nto = grp->GetNode(to);
2589 
2590  graphGenerator::GetLoc(nfrom,x1,y1,z1);
2591  graphGenerator::GetLoc(nto,x2,y2,z2);
2592 
2593  // draw line segment
2594  glBegin(GL_LINES);
2595  glColor3f(1,0,0); // red
2596  glVertex3f(x1,y1,z1);
2597  glVertex3f(x2,y2,z2);
2598  glEnd();
2599 
2600  // draw weight info
2601  if (drawtext) {
2602  sprintf(buf,"%ld",(long)weight);
2603  DrawText((x1+x2)/2, (y1+y2)/2, (z1+z2)/2 - 0.15, 1, 0, 0, buf); // in red
2604  }
2605 }
2606 
Prop::DoSingleStepBFS
bool DoSingleStepBFS(std::vector< graphState > &thePath)
Definition: Propagation.cpp:1004
graphMove
Definition: GraphEnvironment.h:34
Prop::RelaxDelayNode
void RelaxDelayNode(double f, double g, graphState neighbor, PropUtil::SearchNode &neighborNode, graphState topNodeID)
Definition: Propagation.cpp:205
min
double min(double a, double b)
Definition: FPUtil.h:35
Prop::DoSingleStepBP
bool DoSingleStepBP(std::vector< graphState > &thePath)
Definition: Propagation.cpp:550
graphState
unsigned long graphState
Definition: GraphEnvironment.h:32
Prop::OpenGLDraw
void OpenGLDraw() const
Definition: Propagation.cpp:2462
Prop::DrawText
void DrawText(double x, double y, double z, float r, float g, float b, char *str)
Definition: Propagation.cpp:2560
PropUtil::SearchNode::prevNode
graphState prevNode
Definition: Propagation.h:65
Prop::ReversePropX2
void ReversePropX2(PropUtil::SearchNode &topNode)
Definition: Propagation.cpp:1126
Prop::DrawEdge
void DrawEdge(unsigned int from, unsigned int to, double weight)
Definition: Propagation.cpp:2581
drawtext
const static bool drawtext
Definition: Propagation.cpp:27
Prop::ReversePropX1
void ReversePropX1(PropUtil::SearchNode &topNode)
Definition: Propagation.cpp:1084
Prop::DoSingleStepDPDLMX
bool DoSingleStepDPDLMX(std::vector< graphState > &thePath)
Definition: Propagation.cpp:1894
CLOSEDMODE
#define CLOSEDMODE
Definition: Propagation.cpp:20
PROP_BP
#define PROP_BP
Definition: Propagation.h:29
PropUtil::SearchNode::fCost
double fCost
Definition: Propagation.h:62
Prop::GetLowestG
void GetLowestG(PropUtil::SearchNode &gNode)
Definition: Propagation.cpp:1008
neighbor
graphMove neighbor
Definition: RoadMap.h:17
Prop::GetLowestGF
void GetLowestGF(PropUtil::SearchNode &gNode)
Definition: Propagation.cpp:1022
Graph
A generic Graph class.
Definition: Graph.h:66
Prop::ComputeNewHMero3a
void ComputeNewHMero3a(double &h, double &h_tmp, graphState neighbor, PropUtil::SearchNode &neighborNode, double altH, int mode)
Definition: Propagation.cpp:164
Prop::DoSingleStepDPMX
bool DoSingleStepDPMX(std::vector< graphState > &thePath)
Definition: Propagation.cpp:2215
PropUtil
Definition: Propagation.h:40
Prop::UpdateHOnly
bool UpdateHOnly(PropUtil::SearchNode &node, double h)
Definition: Propagation.cpp:992
Prop::DoSingleStepBPMX
bool DoSingleStepBPMX(std::vector< graphState > &thePath)
Definition: Propagation.cpp:1619
Prop::DoSingleStepB
bool DoSingleStepB(std::vector< graphState > &thePath)
Definition: Propagation.cpp:375
Prop::InitializeSearch
bool InitializeSearch(GraphEnvironment *env, Graph *_g, graphState from, graphState to, std::vector< graphState > &thePath)
Definition: Propagation.cpp:55
Prop::RelaxOpenNode
void RelaxOpenNode(double f, double g, graphState neighbor, PropUtil::SearchNode &neighborNode, graphState topNodeID)
Definition: Propagation.cpp:189
DELAYMODE
#define DELAYMODE
Definition: Propagation.cpp:24
fless
bool fless(double a, double b)
Definition: FPUtil.h:28
PropUtil::SearchNode::gCost
double gCost
Definition: Propagation.h:63
DrawText
void DrawText(double x, double y, double z, double scale, const char *str)
Definition: GLUtil.cpp:526
PROP_DPDLMX
#define PROP_DPDLMX
Definition: Propagation.h:38
Prop::DoSingleStepDP
bool DoSingleStepDP(std::vector< graphState > &thePath)
Definition: Propagation.cpp:1310
PROP_BPMX
#define PROP_BPMX
Definition: Propagation.h:34
Prop::Broadcast
void Broadcast(int level, int levelcount)
Definition: Propagation.cpp:1185
NEWMODE
#define NEWMODE
Definition: Propagation.cpp:22
PropUtil::graphGenerator::GetLoc
static void GetLoc(node *n, double &x, double &y, double &z)
Definition: Propagation.h:164
PropUtil::SearchNode::currNode
graphState currNode
Definition: Propagation.h:64
Propagation.h
fgreater
bool fgreater(double a, double b)
Definition: FPUtil.h:29
max
#define max(a, b)
Definition: MinimalSectorAbstraction.cpp:40
Prop::ReverseProp
void ReverseProp(PropUtil::SearchNode &topNode)
Definition: Propagation.cpp:1043
Prop::DoSingleStepA
bool DoSingleStepA(std::vector< graphState > &thePath)
Definition: Propagation.cpp:221
PROP_DELAY
#define PROP_DELAY
Definition: Propagation.h:32
Prop::ExtractPathToStart
void ExtractPathToStart(graphState goalNode, std::vector< graphState > &thePath)
Definition: Propagation.cpp:2429
UINT32_MAX
#define UINT32_MAX
Definition: GenericAStar.h:22
PROP_B
#define PROP_B
Definition: Propagation.h:28
PROP_APPROX
#define PROP_APPROX
Definition: Propagation.h:30
PROP_BFS
#define PROP_BFS
Definition: Propagation.h:31
PROP_DPMX
#define PROP_DPMX
Definition: Propagation.h:35
PropUtil::SearchNode::copy
void copy(double f, double g, graphState curr, graphState prev)
Definition: Propagation.h:55
Prop::GetPath
void GetPath(GraphEnvironment *env, Graph *_g, graphState from, graphState to, std::vector< graphState > &thePath)
Definition: Propagation.cpp:33
Prop::DoSingleStepBPMXE
bool DoSingleStepBPMXE(std::vector< graphState > &thePath)
Definition: Propagation.cpp:1888
Prop::DoSingleSearchStep
bool DoSingleSearchStep(std::vector< graphState > &thePath)
Definition: Propagation.cpp:120
Prop::DoSingleStepApprox
bool DoSingleStepApprox(std::vector< graphState > &thePath)
Definition: Propagation.cpp:774
PropUtil::SearchNode
Definition: Propagation.h:42
OPENMODE
#define OPENMODE
Definition: Propagation.cpp:21
PropUtil::SearchNode::isGoal
bool isGoal
Definition: Propagation.h:67
PROP_DP
#define PROP_DP
Definition: Propagation.h:33
GraphEnvironment
Definition: GraphEnvironment.h:291
PROP_BPMXE
#define PROP_BPMXE
Definition: Propagation.h:36
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
verbose
const static bool verbose
Definition: Propagation.cpp:26
Prop::DoSingleStepDelay
bool DoSingleStepDelay(std::vector< graphState > &thePath)
Definition: Propagation.cpp:1037
PROP_A
#define PROP_A
Definition: Propagation.h:27
edge
Edge class for connections between node in a Graph.
Definition: Graph.h:129
Generated by   doxygen 1.8.17