HOG2
AStarEpsilon.h
Go to the documentation of this file.
1 //
2 // AStarEpsilon.h
3 // HOG2 Demos
4 //
5 // Created by Nathan Sturtevant on 5/24/16.
6 // Copyright © 2016 NS Software. All rights reserved.
7 //
8 
9 #ifndef AStarEpsilon_h
10 #define AStarEpsilon_h
11 
12 
13 #include <iostream>
14 #include "FPUtil.h"
15 #include <unordered_map>
16 #include "AStarOpenClosed.h"
17 #include "BucketOpenClosed.h"
18 #include "TemplateAStar.h"
19 //#include "SearchEnvironment.h" // for the SearchEnvironment class
20 #include "float.h"
21 #include <algorithm> // for vector reverse
22 #include "GenericSearchAlgorithm.h"
23 
24 template <class state>
25 struct GBFSCompare {
26  bool operator()(const AStarOpenClosedData<state> &i1, const AStarOpenClosedData<state> &i2) const
27  {
28  if (fequal(i1.h, i2.h))
29  {
30  return (fless(i1.g, i2.g));
31  }
32  return (fgreater(i1.h, i2.h));
33  }
34 };
35 
36 
40 template <class state, class action, class environment>
41 class AStarEpsilon : public GenericSearchAlgorithm<state,action,environment> {
42 public:
43  AStarEpsilon(double optimalBound = 2) { ResetNodeCount(); env = 0; bound = optimalBound; theHeuristic = 0;}
44  virtual ~AStarEpsilon() {}
45  void GetPath(environment *env, const state& from, const state& to, std::vector<state> &thePath);
46  void GetPath(environment *, const state& , const state& , std::vector<action> & );
47 
48  // uses admissible heuristic (regular A* search)
49  AStarOpenClosed<state, AStarCompare<state>> f;
50  // uses inadmissible heuristic
51  AStarOpenClosed<state, GBFSCompare<state>> focal;
52  state goal, start;
53 
54  bool InitializeSearch(environment *env, const state& from, const state& to, std::vector<state> &thePath);
55  bool DoSingleSearchStep(std::vector<state> &thePath);
56 
57  void ExtractPathToStart(state &node, std::vector<state> &thePath)
58  { uint64_t theID; focal.Lookup(env->GetStateHash(node), theID); ExtractPathToStartFromID(theID, thePath); }
59  void ExtractPathToStartFromID(uint64_t node, std::vector<state> &thePath);
60  const state &GetParent(const state &s);
61  virtual const char *GetName();
62 
63  void PrintStats();
64  uint64_t GetUniqueNodesExpanded() { return uniqueNodesExpanded; }
65  void ResetNodeCount() { nodesExpanded = nodesTouched = 0; uniqueNodesExpanded = 0; }
66  int GetMemoryUsage();
67 
68  state CheckNextOpenNode();
69  state CheckNextFocalNode();
70  bool GetOpenListGCost(const state &val, double &gCost) const;
71  bool GetFocalListGCost(const state &val, double &gCost) const;
72 
73  bool GetClosedListGCost(const state &val, double &gCost) const;
74  bool GetClosedItem(const state &s, AStarOpenClosedData<state> &);
75  unsigned int GetNumOpenItems() { return f.OpenSize(); }
76  unsigned int GetNumFocalItems() { return focal.OpenSize(); }
77  inline const AStarOpenClosedData<state> &GetOpenItem(unsigned int which) { return f.Lookat(f.GetOpenItem(which)); }
78  inline const AStarOpenClosedData<state> &GetFocalItem(unsigned int which) { return focal.Lookat(focal.GetOpenItem(which)); }
79  inline const int GetNumItems() { return focal.size(); }
80  inline const AStarOpenClosedData<state> &GetItem(unsigned int which) { return focal.Lookat(which); }
81  bool HaveExpandedState(const state &val)
82  { uint64_t key; return focal.Lookup(env->GetStateHash(val), key) != kNotFound; }
83  dataLocation GetStateLocation(const state &val)
84  { uint64_t key; return focal.Lookup(env->GetStateHash(val), key); }
85 
86  void SetHeuristic(Heuristic<state> *h) { theHeuristic = h; }
87 
88  uint64_t GetNodesExpanded() const { return nodesExpanded; }
89  uint64_t GetNodesTouched() const { return nodesTouched; }
90 
91  void LogFinalStats(StatCollection *) {}
92 
93  void OpenGLDraw() const;
94  void Draw(Graphics::Display &d) const;
95 
96  void SetOptimalityBound(double w) {bound = w;}
97  double GetOptimalityBound() { return bound; }
98 private:
99  void DrawOpen(Graphics::Display &d) const;
100  void DrawFocal(Graphics::Display &d) const;
101  void ExpandOpen();
102  void ExpandFocal();
103  uint64_t nodesTouched, nodesExpanded;
104 
105  std::vector<state> neighbors;
106  std::vector<uint64_t> neighborID;
107  std::vector<double> edgeCosts;
108  std::vector<dataLocation> neighborLoc;
109 
110  std::vector<state> solution;
111  environment *env;
112  double bestSolution;
113  double bound;
114  uint64_t uniqueNodesExpanded;
115  Heuristic<state> *theHeuristic;
116 };
117 
118 //static const bool verbose = false;
119 
128 template <class state, class action, class environment>
129 const char *AStarEpsilon<state,action,environment>::GetName()
130 {
131  static char name[32];
132  sprintf(name, "AStarEpsilon[%1.2f]", bound);
133  return name;
134 }
135 
147 template <class state, class action, class environment>
148 void AStarEpsilon<state,action,environment>::GetPath(environment *_env, const state& from, const state& to, std::vector<state> &thePath)
149 {
150  //discardcount=0;
151  if (!InitializeSearch(_env, from, to, thePath))
152  {
153  return;
154  }
155  while (!DoSingleSearchStep(thePath))
156  {
157  // if (0 == nodesExpanded%100000)
158  // printf("%" PRId64 " nodes expanded\n", nodesExpanded);
159  }
160 }
161 
162 template <class state, class action, class environment>
163 void AStarEpsilon<state,action,environment>::GetPath(environment *_env, const state& from, const state& to, std::vector<action> &path)
164 {
165  std::vector<state> thePath;
166  if (!InitializeSearch(_env, from, to, thePath))
167  {
168  return;
169  }
170  path.resize(0);
171  while (!DoSingleSearchStep(thePath))
172  {
173  }
174  for (int x = 0; x < thePath.size()-1; x++)
175  {
176  path.push_back(_env->GetAction(thePath[x], thePath[x+1]));
177  }
178 }
179 
180 
191 template <class state, class action, class environment>
192 bool AStarEpsilon<state,action,environment>::InitializeSearch(environment *_env, const state& from, const state& to, std::vector<state> &thePath)
193 {
194  bestSolution = DBL_MAX;
195 
196  if (theHeuristic == 0)
197  theHeuristic = _env;
198  thePath.resize(0);
199  env = _env;
200  focal.Reset(env->GetMaxHash());
201  f.Reset(env->GetMaxHash());
202  solution.clear();
203  ResetNodeCount();
204  start = from;
205  goal = to;
206 
207  if (env->GoalTest(from, to)) //assumes that from and to are valid states
208  {
209  return false;
210  }
211 
212  focal.AddOpenNode(start, env->GetStateHash(start), 0, theHeuristic->HCost(start, goal));
213  f.AddOpenNode(start, env->GetStateHash(start), 0, theHeuristic->HCost(start, goal));
214 
215  return true;
216 }
217 
219 // * Add additional start state to the search. This should only be called after Initialize Search and before DoSingleSearchStep.
220 // * @author Nathan Sturtevant
221 // * @date 01/06/08
222 // */
223 //template <class state, class action, class environment>
224 //void AStarEpsilon<state,action,environment>::AddAdditionalStartState(state& newState)
225 //{
226 // focal.AddOpenNode(newState, env->GetStateHash(newState), 0, weight*theHeuristic->HCost(start, goal));
227 // f.AddOpenNode(newState, env->GetStateHash(newState), 0, theHeuristic->HCost(start, goal));
228 //}
229 //
231 // * Add additional start state to the search. This should only be called after Initialize Search
232 // * @author Nathan Sturtevant
233 // * @date 09/25/10
234 // */
235 //template <class state, class action, class environment>
236 //void AStarEpsilon<state,action,environment>::AddAdditionalStartState(state& newState, double cost)
237 //{
238 // focal.AddOpenNode(newState, env->GetStateHash(newState), cost, weight*theHeuristic->HCost(start, goal));
239 // f.AddOpenNode(newState, env->GetStateHash(newState), cost, theHeuristic->HCost(start, goal));
240 //}
241 
252 template <class state, class action, class environment>
253 bool AStarEpsilon<state,action,environment>::DoSingleSearchStep(std::vector<state> &thePath)
254 {
255  if (solution.size() > 0)
256  {
257  thePath = solution;
258  return true;
259  }
260  if (focal.OpenSize() == 0)
261  {
262  printf("No path\n");
263  thePath.resize(0); // no path found!
264  return true;
265  }
266 
267  uint64_t nodeOnfocal;
268  uint64_t nodeOnF;
269  // only reopen states taken from f, not fhat
270  bool reopen = true;
271 
272  double regularF = f.Lookat(f.Peek()).g+f.Lookat(f.Peek()).h;
273  double focalF = focal.Lookat(focal.Peek()).g+focal.Lookat(focal.Peek()).h;
274  printf("Min f: %1.2f; next focal f: %1.2f. Max allowable: %1.2f.", regularF, focalF, regularF*bound);
275  if (flesseq(focalF, bound*regularF))
276  {
277  //printf(" Expanding focal\n");
278  ExpandFocal();
279  }
280  else {
281  //printf(" Expanding open\n");
282  ExpandOpen();
283  }
284 
285  if (solution.size() > 0)
286  {
287  thePath = solution;
288  return true;
289  }
290 
291 // if (fless(focal.Lookat(focal.Peek()).g+focal.Lookat(focal.Peek()).h, bestSolution))
292 // {
293 // nodeOnfocal = focal.Close();
294 // reopen = false;
295 // dataLocation d = f.Lookup(env->GetStateHash(focal.Lookup(nodeOnfocal).data), nodeOnF);
296 // assert(d != kNotFound);
298 // }
299 // else {
300 // nodeOnF = f.Close();
301 // reopen = true;
302 // dataLocation d = focal.Lookup(env->GetStateHash(f.Lookup(nodeOnF).data), nodeOnfocal);
303 // assert(d != kNotFound);
304 // if (d == kOpenList)
305 // {
306 // focal.Close(nodeOnfocal);
307 // d = focal.Lookup(env->GetStateHash(f.Lookup(nodeOnF).data), nodeOnfocal);
308 // assert(d == kClosedList);
309 // }
310 // }
311 //
312 // if (!fequal(oldF, f.Lookat(f.Peek()).g+f.Lookat(f.Peek()).h))
313 // {
314 // printf("Best solution %1.2f\n", bestSolution);
315 // printf("Best on open %1.2f - lower bound is %1.2f\n", f.Lookat(f.Peek()).g+f.Lookat(f.Peek()).h,
316 // (f.Lookat(f.Peek()).g+f.Lookat(f.Peek()).h)*bound);
317 // }
318 // if (!focal.Lookup(nodeOnfocal).reopened)
319 // uniqueNodesExpanded++;
320 // nodesExpanded++;
321 //
322 // // if n is a goal then
323 // // incumbent ← n
324 // if (env->GoalTest(focal.Lookup(nodeOnfocal).data, goal))
325 // {
326 // // Actually extract the path, since the cost may not actually be the g-cost
327 // ExtractPathToStartFromID(nodeOnfocal, thePath);
328 // bestSolution = env->GetPathLength(thePath);
329 // thePath.resize(0);
330 // printf("Best solution %1.2f\n", bestSolution);
331 // printf("Best on open %1.2f - lower bound is %1.2f\n", f.Lookat(f.Peek()).g+f.Lookat(f.Peek()).h,
332 // (f.Lookat(f.Peek()).g+f.Lookat(f.Peek()).h)*bound);
333 //
334 // return false; // check on next iteration through the loop
335 // }
336 
337 
338  // std::cout << "Expanding: " << focal.Lookup(nodeOnfocal).data << " with f:";
339  // std::cout << focal.Lookup(nodeid).g+focal.Lookup(nodeOnfocal).h << std::endl;
340 
341  return false;
342 }
343 
349 template <class state, class action, class environment>
350 void AStarEpsilon<state,action,environment>::ExpandOpen()
351 {
352  uint64_t nodeid = f.Close();
353  if (!f.Lookup(nodeid).reopened)
354  uniqueNodesExpanded++;
355  nodesExpanded++;
356 
357  if (env->GoalTest(f.Lookup(nodeid).data, goal))
358  {
359  ExtractPathToStartFromID(nodeid, solution);
360  // Path is backwards - reverse
361  reverse(solution.begin(), solution.end());
362  return;
363  }
364 
365  neighbors.resize(0);
366  edgeCosts.resize(0);
367  neighborID.resize(0);
368  neighborLoc.resize(0);
369 
370  // std::cout << "Expanding: " << f.Lookup(nodeid).data << " with f:";
371  // std::cout << f.Lookup(nodeid).g+f.Lookup(nodeid).h << std::endl;
372 
373  env->GetSuccessors(f.Lookup(nodeid).data, neighbors);
374  double bestH = f.Lookup(nodeid).h;
375  double lowHC = DBL_MAX;
376  // 1. load all the children
377  for (unsigned int x = 0; x < neighbors.size(); x++)
378  {
379  uint64_t theID;
380  neighborLoc.push_back(f.Lookup(env->GetStateHash(neighbors[x]), theID));
381  neighborID.push_back(theID);
382  edgeCosts.push_back(env->GCost(f.Lookup(nodeid).data, neighbors[x]));
383  }
384 
385  // iterate again updating costs and writing out to memory
386  for (int x = 0; x < neighbors.size(); x++)
387  {
388  nodesTouched++;
389 
390  switch (neighborLoc[x])
391  {
392  case kClosedList:
393 // if (reopenNodes)
394 // {
395 // if (fless(f.Lookup(nodeid).g+edgeCosts[x], f.Lookup(neighborID[x]).g))
396 // {
397 // f.Lookup(neighborID[x]).parentID = nodeid;
398 // f.Lookup(neighborID[x]).g = f.Lookup(nodeid).g+edgeCosts[x];
399 // f.Reopen(neighborID[x]);
400 // // This line isn't normally needed, but in some state spaces we might have
401 // // equality but different meta information, so we need to make sure that the
402 // // meta information is also copied, since this is the most generic A* implementation
403 // f.Lookup(neighborID[x]).data = neighbors[x];
404 // }
405 // }
406  break;
407  case kOpenList:
408  //edgeCost = env->GCost(f.Lookup(nodeid).data, neighbors[x]);
409  if (fless(f.Lookup(nodeid).g+edgeCosts[x], f.Lookup(neighborID[x]).g))
410  {
411  f.Lookup(neighborID[x]).parentID = nodeid;
412  f.Lookup(neighborID[x]).g = f.Lookup(nodeid).g+edgeCosts[x];
413  // This line isn't normally needed, but in some state spaces we might have
414  // equality but different meta information, so we need to make sure that the
415  // meta information is also copied, since this is the most generic A* implementation
416  f.Lookup(neighborID[x]).data = neighbors[x];
417  f.KeyChanged(neighborID[x]);
418  // std::cout << " Reducing cost to " << f.Lookup(nodeid).g+edgeCosts[x] << "\n";
419  // TODO: unify the KeyChanged calls.
420  }
421  else {
422  // std::cout << " no cheaper \n";
423  }
424  break;
425  case kNotFound:
426  {
427  f.AddOpenNode(neighbors[x],
428  env->GetStateHash(neighbors[x]),
429  f.Lookup(nodeid).g+edgeCosts[x],
430  theHeuristic->HCost(neighbors[x], goal),
431  nodeid);
432  }
433  }
434  }
435 }
436 
437 template <class state, class action, class environment>
438 void AStarEpsilon<state,action,environment>::ExpandFocal()
439 {
440  uint64_t nodeid = focal.Close();
441  if (!focal.Lookup(nodeid).reopened)
442  uniqueNodesExpanded++;
443  nodesExpanded++;
444 
445  if (env->GoalTest(focal.Lookup(nodeid).data, goal))
446  {
447  ExtractPathToStartFromID(nodeid, solution);
448  // Path is backwards - reverse
449  reverse(solution.begin(), solution.end());
450  return;
451  }
452 
453  neighbors.resize(0);
454  edgeCosts.resize(0);
455  neighborID.resize(0);
456  neighborLoc.resize(0);
457 
458  std::cout << "Expanding: " << focal.Lookup(nodeid).data << " with f:";
459  std::cout << focal.Lookup(nodeid).g+focal.Lookup(nodeid).h << std::endl;
460 
461  env->GetSuccessors(focal.Lookup(nodeid).data, neighbors);
462  double bestH = focal.Lookup(nodeid).h;
463  double lowHC = DBL_MAX;
464  // 1. load all the children
465  for (unsigned int x = 0; x < neighbors.size(); x++)
466  {
467  uint64_t theID;
468  neighborLoc.push_back(focal.Lookup(env->GetStateHash(neighbors[x]), theID));
469  neighborID.push_back(theID);
470  edgeCosts.push_back(env->GCost(focal.Lookup(nodeid).data, neighbors[x]));
471  }
472 
473  // iterate again updating costs and writing out to memory
474  for (int x = 0; x < neighbors.size(); x++)
475  {
476  nodesTouched++;
477 
478  switch (neighborLoc[x])
479  {
480  case kClosedList:
481  break;
482  case kOpenList:
483  //edgeCost = env->GCost(focal.Lookup(nodeid).data, neighbors[x]);
484  if (fless(focal.Lookup(nodeid).g+edgeCosts[x], focal.Lookup(neighborID[x]).g))
485  {
486  focal.Lookup(neighborID[x]).parentID = nodeid;
487  focal.Lookup(neighborID[x]).g = focal.Lookup(nodeid).g+edgeCosts[x];
488  // This line isn't normally needed, but in some state spaces we might have
489  // equality but different meta information, so we need to make sure that the
490  // meta information is also copied, since this is the most generic A* implementation
491  focal.Lookup(neighborID[x]).data = neighbors[x];
492  focal.KeyChanged(neighborID[x]);
493  // std::cout << " Reducing cost to " << focal.Lookup(nodeid).g+edgeCosts[x] << "\n";
494  // TODO: unify the KeyChanged calls.
495  }
496  else {
497  // std::cout << " no cheaper \n";
498  }
499  break;
500  case kNotFound:
501  {
502  focal.AddOpenNode(neighbors[x],
503  env->GetStateHash(neighbors[x]),
504  focal.Lookup(nodeid).g+edgeCosts[x],
505  theHeuristic->HCost(neighbors[x], goal),
506  nodeid);
507  }
508  }
509  }
510 }
511 
519 //template <class state, class action, class environment>
520 //state AStarEpsilon<state, action,environment>::CheckNextNode()
521 //{
522 // uint64_t key = focal.Peek();
523 // return focal.Lookup(key).data;
524 // //assert(false);
525 // //return openQueue.top().currNode;
526 //}
527 
528 
537 template <class state, class action,class environment>
538 void AStarEpsilon<state, action,environment>::ExtractPathToStartFromID(uint64_t node,
539  std::vector<state> &thePath)
540 {
541  do {
542  thePath.push_back(focal.Lookup(node).data);
543  node = focal.Lookup(node).parentID;
544  } while (focal.Lookup(node).parentID != node);
545  thePath.push_back(focal.Lookup(node).data);
546 }
547 
548 template <class state, class action,class environment>
549 const state &AStarEpsilon<state, action,environment>::GetParent(const state &s)
550 {
551  uint64_t theID;
552  focal.Lookup(env->GetStateHash(s), theID);
553  theID = focal.Lookup(theID).parentID;
554  return focal.Lookup(theID).data;
555 }
556 
563 template <class state, class action, class environment>
564 void AStarEpsilon<state, action,environment>::PrintStats()
565 {
566  printf("%u items in closed list\n", (unsigned int)focal.ClosedSize());
567  printf("%u items in open queue\n", (unsigned int)focal.OpenSize());
568 }
569 
577 template <class state, class action, class environment>
578 int AStarEpsilon<state, action,environment>::GetMemoryUsage()
579 {
580  return focal.size();
581 }
582 
593 template <class state, class action, class environment>
594 bool AStarEpsilon<state, action,environment>::GetClosedListGCost(const state &val, double &gCost) const
595 {
596  uint64_t theID;
597  dataLocation loc = focal.Lookup(env->GetStateHash(val), theID);
598  if (loc == kClosedList)
599  {
600  gCost = focal.Lookat(theID).g;
601  return true;
602  }
603  return false;
604 }
605 
606 template <class state, class action, class environment>
607 state AStarEpsilon<state, action,environment>::CheckNextOpenNode()
608 {
609  uint64_t key = f.Peek();
610  return f.Lookup(key).data;
611 }
612 
613 template <class state, class action, class environment>
614 state AStarEpsilon<state, action,environment>::CheckNextFocalNode()
615 {
616  uint64_t key = focal.Peek();
617  return focal.Lookup(key).data;
618 }
619 
620 
621 template <class state, class action, class environment>
622 bool AStarEpsilon<state, action,environment>::GetOpenListGCost(const state &val, double &gCost) const
623 {
624  uint64_t theID;
625  dataLocation loc = f.Lookup(env->GetStateHash(val), theID);
626  if (loc == kOpenList)
627  {
628  gCost = f.Lookat(theID).g;
629  return true;
630  }
631  return false;
632 }
633 
634 template <class state, class action, class environment>
635 bool AStarEpsilon<state, action,environment>::GetFocalListGCost(const state &val, double &gCost) const
636 {
637  uint64_t theID;
638  dataLocation loc = focal.Lookup(env->GetStateHash(val), theID);
639  if (loc == kOpenList)
640  {
641  gCost = focal.Lookat(theID).g;
642  return true;
643  }
644  return false;
645 }
646 
647 
648 template <class state, class action, class environment>
649 bool AStarEpsilon<state, action,environment>::GetClosedItem(const state &s, AStarOpenClosedData<state> &result)
650 {
651  uint64_t theID;
652  dataLocation loc = focal.Lookup(env->GetStateHash(s), theID);
653  if (loc == kClosedList)
654  {
655  result = focal.Lookat(theID);
656  return true;
657  }
658  return false;
659 
660 }
661 
662 
669 template <class state, class action, class environment>
670 void AStarEpsilon<state, action,environment>::OpenGLDraw() const
671 {
672  double transparency = 1.0;
673  if (focal.size() == 0)
674  return;
675  uint64_t top = -1;
676  double bound = DBL_MAX;
677 
678  // double minf = 1e9, maxf = 0;
679  if (focal.OpenSize() > 0)
680  {
681  top = focal.Peek();
682  const auto &i = f.Lookat(f.Peek());
683  bound = i.g+i.h;
684  printf("Lowest f on open: %f\n", bound);
685  }
686  for (unsigned int x = 0; x < focal.size(); x++)
687  {
688  const auto &data = focal.Lookat(x);
689  if (x == top)
690  {
691  env->SetColor(1.0, 1.0, 0.0, transparency);
692  env->OpenGLDraw(data.data);
693  }
694 // if ((data.where == kClosedList && !fgreater(data.g+data.h/weight, bound)))
695 // {
696 // env->SetColor(0.0, 0.0, 1.0, transparency);
697 // env->OpenGLDraw(data.data);
698 // }
699  else if ((data.where == kOpenList) && (data.reopened))
700  {
701  env->SetColor(0.0, 0.5, 0.5, transparency);
702  env->OpenGLDraw(data.data);
703  }
704  else if (data.where == kOpenList)
705  {
706  env->SetColor(0.0, 1.0, 0.0, transparency);
707  env->OpenGLDraw(data.data);
708  }
709  else if ((data.where == kClosedList) && (data.reopened))
710  {
711  env->SetColor(0.5, 0.0, 0.5, transparency);
712  env->OpenGLDraw(data.data);
713  }
714  else if (data.where == kClosedList)
715  {
716  if (data.parentID == x)
717  env->SetColor(1.0, 0.5, 0.5, transparency);
718  else
719  env->SetColor(1.0, 0.0, 0.0, transparency);
720  // }
721  env->OpenGLDraw(data.data);
722  }
723  }
724  env->SetColor(1.0, 0.5, 1.0, 0.5);
725  env->OpenGLDraw(goal);
726 }
727 
728 template <class state, class action, class environment>
729 void AStarEpsilon<state, action,environment>::Draw(Graphics::Display &d) const
730 {
731  DrawFocal(d);
732  DrawOpen(d);
733  env->SetColor(1.0, 0.5, 1.0, 0.5);
734  env->Draw(d, goal);
735 }
736 
737 template <class state, class action, class environment>
738 void AStarEpsilon<state, action,environment>::DrawOpen(Graphics::Display &d) const
739 {
740  double transparency = 1.0;
741  if (f.size() == 0)
742  return;
743  uint64_t top = -1;
744  double bound = DBL_MAX;
745 
746  // double minf = 1e9, maxf = 0;
747  if (f.OpenSize() > 0)
748  {
749  top = f.Peek();
750  const auto &i = f.Lookat(f.Peek());
751  bound = i.g+i.h;
752 // printf("Lowest f on open: %f\n", bound);
753  }
754  for (unsigned int x = 0; x < f.size(); x++)
755  {
756  const auto &data = f.Lookat(x);
757  if (x == top)
758  {
759  env->SetColor(1.0, 1.0, 0.0, transparency);
760  env->Draw(d, data.data);
761  }
762 // if ((data.where == kClosedList && !fgreater(data.g+data.h/weight, bound)))
763 // {
764 // env->SetColor(0.0, 0.0, 1.0, transparency);
765 // env->Draw(d, data.data);
766 // }
767  if ((data.where == kOpenList) && (data.reopened))
768  {
769  env->SetColor(0.0, 0.5, 0.5, transparency);
770  env->Draw(d, data.data);
771  }
772  else if (data.where == kOpenList)
773  {
774  env->SetColor(0.0, 1.0, 0.0, transparency);
775  env->Draw(d, data.data);
776  }
777  else if ((data.where == kClosedList) && (data.reopened))
778  {
779  env->SetColor(0.5, 0.0, 0.5, transparency);
780  env->Draw(d, data.data);
781  }
782  else if (data.where == kClosedList)
783  {
784  if (data.parentID == x)
785  env->SetColor(1.0, 0.5, 0.5, transparency);
786  else
787  env->SetColor(0.0, 0.0, 1.0, transparency);
788  env->Draw(d, data.data);
789  }
790  }
791 }
792 
793 template <class state, class action, class environment>
794 void AStarEpsilon<state, action,environment>::DrawFocal(Graphics::Display &d) const
795 {
796  double transparency = 1.0;
797  if (focal.size() == 0)
798  return;
799  uint64_t top = -1;
800  double bound = DBL_MAX;
801 
802  // double minf = 1e9, maxf = 0;
803  if (focal.OpenSize() > 0)
804  {
805  top = focal.Peek();
806  const auto &i = focal.Lookat(focal.Peek());
807  bound = i.g+i.h;
808  // printf("Lowest f on open: %f\n", bound);
809  }
810  for (unsigned int x = 0; x < focal.size(); x++)
811  {
812  const auto &data = focal.Lookat(x);
813  if (x == top)
814  {
815  env->SetColor(1.0, 1.0, 0.0, transparency);
816  env->Draw(d, data.data);
817  }
818  // if ((data.where == kClosedList && !fgreater(data.g+data.h/weight, bound)))
819  // {
820  // env->SetColor(0.0, 0.0, 1.0, transparency);
821  // env->Draw(d, data.data);
822  // }
823  if ((data.where == kOpenList) && (data.reopened))
824  {
825  env->SetColor(0.0, 0.5, 0.5, transparency);
826  env->Draw(d, data.data);
827  }
828  else if (data.where == kOpenList)
829  {
830  env->SetColor(0.0, 1.0, 0.0, transparency);
831  env->Draw(d, data.data);
832  }
833  else if ((data.where == kClosedList) && (data.reopened))
834  {
835  env->SetColor(0.5, 0.0, 0.5, transparency);
836  env->Draw(d, data.data);
837  }
838  else if (data.where == kClosedList)
839  {
840  if (data.parentID == x)
841  env->SetColor(1.0, 0.5, 0.5, transparency);
842  else
843  env->SetColor(1.0, 0.0, 0.0, transparency);
844  env->Draw(d, data.data);
845  }
846  }
847 }
848 #endif /* AStarEpsilon_h */
AStarEpsilon::ResetNodeCount
void ResetNodeCount()
Definition: AStarEpsilon.h:65
AStarEpsilon::SetHeuristic
void SetHeuristic(Heuristic< state > *h)
Definition: AStarEpsilon.h:86
AStarEpsilon::GetClosedItem
bool GetClosedItem(const state &s, AStarOpenClosedData< state > &)
Definition: AStarEpsilon.h:649
GenericSearchAlgorithm.h
An interface for generic search algorithms.
AStarEpsilon::neighborLoc
std::vector< dataLocation > neighborLoc
Definition: AStarEpsilon.h:108
BucketOpenClosed.h
GBFSCompare
Definition: AStarEpsilon.h:25
AStarEpsilon::PrintStats
void PrintStats()
A function that prints the number of states in the closed list and open queue.
Definition: AStarEpsilon.h:564
AStarEpsilon::nodesTouched
uint64_t nodesTouched
Definition: AStarEpsilon.h:103
AStarEpsilon::InitializeSearch
bool InitializeSearch(environment *env, const state &from, const state &to, std::vector< state > &thePath)
Initialize the A* search.
Definition: AStarEpsilon.h:192
AStarEpsilon::GetPath
void GetPath(environment *env, const state &from, const state &to, std::vector< state > &thePath)
Perform an A* search between two states.
Definition: AStarEpsilon.h:148
AStarEpsilon::LogFinalStats
void LogFinalStats(StatCollection *)
Definition: AStarEpsilon.h:91
Heuristic
Definition: Heuristic.h:30
AStarEpsilon::start
state start
Definition: AStarEpsilon.h:52
AStarOpenClosed.h
d
mcData d[]
Definition: MotionCaptureMovement.cpp:21
AStarEpsilon::~AStarEpsilon
virtual ~AStarEpsilon()
Definition: AStarEpsilon.h:44
AStarEpsilon::GetNumItems
const int GetNumItems()
Definition: AStarEpsilon.h:79
AStarOpenClosed::Lookat
const dataStructure & Lookat(uint64_t objKey) const
Definition: AStarOpenClosed.h:87
AStarEpsilon::nodesExpanded
uint64_t nodesExpanded
Definition: AStarEpsilon.h:103
AStarEpsilon::GetOpenListGCost
bool GetOpenListGCost(const state &val, double &gCost) const
Definition: AStarEpsilon.h:622
AStarEpsilon::uniqueNodesExpanded
uint64_t uniqueNodesExpanded
Definition: AStarEpsilon.h:114
AStarEpsilon::GetClosedListGCost
bool GetClosedListGCost(const state &val, double &gCost) const
Get state from the closed list.
Definition: AStarEpsilon.h:594
AStarEpsilon::theHeuristic
Heuristic< state > * theHeuristic
Definition: AStarEpsilon.h:115
AStarEpsilon::GetStateLocation
dataLocation GetStateLocation(const state &val)
Definition: AStarEpsilon.h:83
AStarEpsilon::GetItem
const AStarOpenClosedData< state > & GetItem(unsigned int which)
Definition: AStarEpsilon.h:80
FPUtil.h
GBFSCompare::operator()
bool operator()(const AStarOpenClosedData< state > &i1, const AStarOpenClosedData< state > &i2) const
Definition: AStarEpsilon.h:26
AStarEpsilon::ExpandFocal
void ExpandFocal()
Definition: AStarEpsilon.h:438
AStarOpenClosed::size
size_t size() const
Definition: AStarOpenClosed.h:103
AStarEpsilon::HaveExpandedState
bool HaveExpandedState(const state &val)
Definition: AStarEpsilon.h:81
AStarEpsilon::DrawOpen
void DrawOpen(Graphics::Display &d) const
Definition: AStarEpsilon.h:738
AStarEpsilon::goal
state goal
Definition: AStarEpsilon.h:52
AStarEpsilon::GetUniqueNodesExpanded
uint64_t GetUniqueNodesExpanded()
Definition: AStarEpsilon.h:64
AStarEpsilon::GetMemoryUsage
int GetMemoryUsage()
Return the amount of memory used by AStarEpsilon.
Definition: AStarEpsilon.h:578
AStarOpenClosedData::g
double g
Definition: AStarOpenClosed.h:64
AStarEpsilon::GetNumFocalItems
unsigned int GetNumFocalItems()
Definition: AStarEpsilon.h:76
GenericSearchAlgorithm
Definition: GenericSearchAlgorithm.h:35
AStarEpsilon::GetNumOpenItems
unsigned int GetNumOpenItems()
Definition: AStarEpsilon.h:75
AStarEpsilon::GetNodesExpanded
uint64_t GetNodesExpanded() const
Definition: AStarEpsilon.h:88
AStarEpsilon::env
environment * env
Definition: AStarEpsilon.h:111
AStarEpsilon::bestSolution
double bestSolution
Definition: AStarEpsilon.h:112
loc
Definition: MapGenerators.cpp:296
AStarEpsilon::GetFocalItem
const AStarOpenClosedData< state > & GetFocalItem(unsigned int which)
Definition: AStarEpsilon.h:78
AStarOpenClosed
Definition: AStarOpenClosed.h:74
AStarEpsilon::OpenGLDraw
void OpenGLDraw() const
Draw the open/closed list.
Definition: AStarEpsilon.h:670
AStarEpsilon::DoSingleSearchStep
bool DoSingleSearchStep(std::vector< state > &thePath)
‍**
Definition: AStarEpsilon.h:253
Graphics::Display
Definition: Graphics.h:146
fless
bool fless(double a, double b)
Definition: FPUtil.h:28
AStarEpsilon::CheckNextOpenNode
state CheckNextOpenNode()
Definition: AStarEpsilon.h:607
AStarEpsilon::ExpandOpen
void ExpandOpen()
Expands a single state from the given open list.
Definition: AStarEpsilon.h:350
AStarOpenClosedData
Definition: AStarOpenClosed.h:52
dataLocation
dataLocation
Definition: AStarOpenClosed.h:27
AStarEpsilon::ExtractPathToStartFromID
void ExtractPathToStartFromID(uint64_t node, std::vector< state > &thePath)
Returns the next state on the open list (but doesn't pop it off the queue).
Definition: AStarEpsilon.h:538
TemplateAStar.h
AStarEpsilon::GetParent
const state & GetParent(const state &s)
Definition: AStarEpsilon.h:549
AStarEpsilon::DrawFocal
void DrawFocal(Graphics::Display &d) const
Definition: AStarEpsilon.h:794
fgreater
bool fgreater(double a, double b)
Definition: FPUtil.h:29
AStarEpsilon
A templated version of A*, based on HOG genericAStar.
Definition: AStarEpsilon.h:41
AStarOpenClosed::OpenSize
size_t OpenSize() const
Definition: AStarOpenClosed.h:101
AStarEpsilon::edgeCosts
std::vector< double > edgeCosts
Definition: AStarEpsilon.h:107
kOpenList
@ kOpenList
Definition: AStarOpenClosed.h:28
AStarEpsilon::neighborID
std::vector< uint64_t > neighborID
Definition: AStarEpsilon.h:106
StatCollection
The StatCollection class is for collecting stats across different parts of the simulation.
Definition: StatCollection.h:34
AStarOpenClosed::GetOpenItem
uint64_t GetOpenItem(unsigned int which)
Definition: AStarOpenClosed.h:100
AStarEpsilon::ExtractPathToStart
void ExtractPathToStart(state &node, std::vector< state > &thePath)
Definition: AStarEpsilon.h:57
AStarEpsilon::neighbors
std::vector< state > neighbors
Definition: AStarEpsilon.h:105
AStarEpsilon::SetOptimalityBound
void SetOptimalityBound(double w)
Definition: AStarEpsilon.h:96
AStarEpsilon::GetNodesTouched
uint64_t GetNodesTouched() const
Definition: AStarEpsilon.h:89
AStarEpsilon::CheckNextFocalNode
state CheckNextFocalNode()
Definition: AStarEpsilon.h:614
AStarEpsilon::solution
std::vector< state > solution
Definition: AStarEpsilon.h:110
AStarEpsilon::GetOptimalityBound
double GetOptimalityBound()
Definition: AStarEpsilon.h:97
AStarOpenClosed::Lookup
dataLocation Lookup(uint64_t hashKey, uint64_t &objKey) const
Returns location of object as well as object key.
Definition: AStarOpenClosed.h:263
AStarOpenClosedData::h
double h
Definition: AStarOpenClosed.h:65
AStarEpsilon::GetName
virtual const char * GetName()
Return the name of the algorithm.
Definition: AStarEpsilon.h:129
AStarEpsilon::AStarEpsilon
AStarEpsilon(double optimalBound=2)
Definition: AStarEpsilon.h:43
kNotFound
@ kNotFound
Definition: AStarOpenClosed.h:30
AStarEpsilon::Draw
void Draw(Graphics::Display &d) const
Definition: AStarEpsilon.h:729
path
A linked list of nodes which form a continuous path.
Definition: Path.h:20
AStarEpsilon::GetFocalListGCost
bool GetFocalListGCost(const state &val, double &gCost) const
Definition: AStarEpsilon.h:635
AStarEpsilon::GetOpenItem
const AStarOpenClosedData< state > & GetOpenItem(unsigned int which)
Definition: AStarEpsilon.h:77
AStarEpsilon::bound
double bound
Definition: AStarEpsilon.h:113
fequal
bool fequal(double a, double b, double tolerance=TOLERANCE)
Definition: FPUtil.h:32
AStarEpsilon::focal
AStarOpenClosed< state, GBFSCompare< state > > focal
Definition: AStarEpsilon.h:51
flesseq
bool flesseq(double a, double b)
Definition: FPUtil.h:30
kClosedList
@ kClosedList
Definition: AStarOpenClosed.h:29
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
AStarEpsilon::f
AStarOpenClosed< state, AStarCompare< state > > f
Definition: AStarEpsilon.h:49
Generated by   doxygen 1.8.17