HOG2
TemplateAStar.h
Go to the documentation of this file.
1 
11 #ifndef TemplateAStar_H
12 #define TemplateAStar_H
13 
14 #define __STDC_CONSTANT_MACROS
15 #include <stdint.h>
16 // this is defined in stdint.h, but it doesn't always get defined correctly
17 // even when __STDC_CONSTANT_MACROS is defined before including stdint.h
18 // because stdint might be included elsewhere first...
19 #ifndef UINT32_MAX
20 #define UINT32_MAX 4294967295U
21 #endif
22 
23 
24 #include <iostream>
25 #include "Constraint.h"
26 #include "FPUtil.h"
27 #include <unordered_map>
28 #include "Graphics.h"
29 #include "AStarOpenClosed.h"
30 #include "BucketOpenClosed.h"
31 //#include "SearchEnvironment.h" // for the SearchEnvironment class
32 #include "float.h"
33 
34 #include <algorithm> // for vector reverse
35 
36 #include "GenericSearchAlgorithm.h"
37 //static double lastF = 0;
38 
39 //typedef double (*phi)(double, double);
40 // note, this should be consteval when C++20 is widespread
41 //constexpr auto phi_astar = [](double h, double g) -> double { return g+h; };
42 
43 template <class state>
44 struct AStarCompareWithF {
45  // returns true if i2 is preferred over i1
46  bool operator()(const AStarOpenClosedDataWithF<state> &i1, const AStarOpenClosedDataWithF<state> &i2) const
47  {
48  if (fequal(i1.f, i2.f))
49  {
50  return (fless(i1.g, i2.g));
51  }
52  return fgreater(i1.f, i2.f);
53  }
54 };
55 
56 template <class state>
57 struct AStarCompare {
58  // returns true if i2 is preferred over i1
59  bool operator()(const AStarOpenClosedData<state> &i1, const AStarOpenClosedData<state> &i2) const
60  {
61  if (fequal(i1.g+i1.h, i2.g+i2.h))
62  {
63  return (fless(i1.g, i2.g));
64  }
65  return fgreater(i1.g+i1.h, i2.g+i2.h);
66  }
67 };
68 
72 template <class state, class action, class environment, class openList = AStarOpenClosed<state, AStarCompareWithF<state>, AStarOpenClosedDataWithF<state>> >
73 class TemplateAStar : public GenericSearchAlgorithm<state,action,environment> {
74 public:
75  TemplateAStar() {
76  ResetNodeCount(); env = 0; useBPMX = 0; stopAfterGoal = true; weight=1; reopenNodes = false; theHeuristic = 0; directed = false;
77  theConstraint = 0;
78  phi = [](double h, double g){ return g+h; };
79  }
80  virtual ~TemplateAStar() {}
81  void GetPath(environment *env, const state& from, const state& to, std::vector<state> &thePath);
82  void GetPath(environment *, const state&, const state&, std::vector<action> & );
83 
84  openList openClosedList;
85  state goal, start;
86 
87  bool InitializeSearch(environment *env, const state& from, const state& to, std::vector<state> &thePath);
88  bool DoSingleSearchStep(std::vector<state> &thePath);
89  void AddAdditionalStartState(const state& newState);
90  void AddAdditionalStartState(const state& newState, double cost);
91 
92  state CheckNextNode();
93  void ExtractPathToStart(state &node, std::vector<state> &thePath)
94  {
95  thePath.clear();
96  uint64_t theID;
97  if (openClosedList.Lookup(env->GetStateHash(node), theID) != kNotFound)
98  ExtractPathToStartFromID(theID, thePath);
99  }
100  void ExtractPathToStartFromID(uint64_t node, std::vector<state> &thePath);
101  const state &GetParent(const state &s);
102  virtual const char *GetName();
103 
104  void PrintStats();
105  uint64_t GetUniqueNodesExpanded() { return uniqueNodesExpanded; }
106  void ResetNodeCount() { nodesExpanded = nodesTouched = 0; uniqueNodesExpanded = 0; }
107  int GetMemoryUsage();
108 
109  bool GetClosedListGCost(const state &val, double &gCost) const;
110  bool GetOpenListGCost(const state &val, double &gCost) const;
111  bool GetHCost(const state &val, double &hCost) const;
112  bool GetClosedItem(const state &s, AStarOpenClosedDataWithF<state> &);
113  unsigned int GetNumOpenItems() { return openClosedList.OpenSize(); }
114  inline const AStarOpenClosedDataWithF<state> &GetOpenItem(unsigned int which) { return openClosedList.Lookat(openClosedList.GetOpenItem(which)); }
115  inline const int GetNumItems() { return openClosedList.size(); }
116  inline const AStarOpenClosedDataWithF<state> &GetItem(unsigned int which) { return openClosedList.Lookat(which); }
117  bool HaveExpandedState(const state &val)
118  { uint64_t key; return openClosedList.Lookup(env->GetStateHash(val), key) != kNotFound; }
119  dataLocation GetStateLocation(const state &val)
120  { uint64_t key; return openClosedList.Lookup(env->GetStateHash(val), key); }
121 
122  void SetUseBPMX(int depth) { useBPMX = depth; if (depth) reopenNodes = true; }
123  int GetUsingBPMX() { return useBPMX; }
124 
125  void SetReopenNodes(bool re) { reopenNodes = re; }
126  bool GetReopenNodes() { return reopenNodes; }
127 
128  // Only necessary for BPMX computation
129  void SetDirected(bool d) { directed = d; }
130 
131  void SetHeuristic(Heuristic<state> *h) { theHeuristic = h; }
132  void SetConstraint(Constraint<state> *c) { theConstraint = c; }
133 
134  uint64_t GetNodesExpanded() const { return nodesExpanded; }
135  uint64_t GetNodesTouched() const { return nodesTouched; }
136  uint64_t GetNecessaryExpansions() const;
137  void LogFinalStats(StatCollection *) {}
138 
139  void SetStopAfterGoal(bool val) { stopAfterGoal = val; }
140  bool GetStopAfterGoal() { return stopAfterGoal; }
141 
142  void FullBPMX(uint64_t nodeID, int distance);
143 
144  void OpenGLDraw() const;
145  void Draw(Graphics::Display &disp) const;
146  std::string SVGDraw() const;
147  std::string SVGDrawDetailed() const;
148 
150  void SetPhi(std::function<double(double, double)> p)
151  {
152  phi = p;
153  }
154  double Phi(double h, double g)
155  {
156  return phi(h, g);
157  }
158  void SetWeight(double w)
159  {
160  weight = w;
161  phi = [=](double h, double g){ return g+weight*h; };
162  }
163  double GetWeight() { return weight; }
164 private:
165  uint64_t nodesTouched, nodesExpanded;
166 
167  std::vector<state> neighbors;
168  std::vector<uint64_t> neighborID;
169  std::vector<double> edgeCosts;
170  std::vector<dataLocation> neighborLoc;
171  environment *env;
172  bool stopAfterGoal;
173 
174  double goalFCost;
175  double weight;
176  std::function<double(double, double)> phi;
177  bool directed;
178  int useBPMX;
179  bool reopenNodes;
180  uint64_t uniqueNodesExpanded;
181  environment *radEnv;
182  Heuristic<state> *theHeuristic;
183  Constraint<state> *theConstraint;
184 };
185 
194 template <class state, class action, class environment, class openList>
195 const char *TemplateAStar<state,action,environment,openList>::GetName()
196 {
197  static char name[32];
198  sprintf(name, "TemplateAStar[]");
199  return name;
200 }
201 
213 template <class state, class action, class environment, class openList>
214 void TemplateAStar<state,action,environment,openList>::GetPath(environment *_env, const state& from, const state& to, std::vector<state> &thePath)
215 {
216  if (!InitializeSearch(_env, from, to, thePath))
217  {
218  return;
219  }
220  while (!DoSingleSearchStep(thePath))
221  {
222 // if (0 == nodesExpanded%100000)
223 // printf("%" PRId64 " nodes expanded, %" PRId64 " generated\n", nodesExpanded, nodesTouched);
224  }
225 }
226 
227 template <class state, class action, class environment, class openList>
228 void TemplateAStar<state,action,environment,openList>::GetPath(environment *_env, const state& from, const state& to, std::vector<action> &path)
229 {
230  std::vector<state> thePath;
231  if (!InitializeSearch(_env, from, to, thePath))
232  {
233  return;
234  }
235  path.resize(0);
236  while (!DoSingleSearchStep(thePath))
237  {
238  }
239  for (size_t x = 0; x < thePath.size()-1; x++)
240  {
241  path.push_back(_env->GetAction(thePath[x], thePath[x+1]));
242  }
243 }
244 
245 
256 template <class state, class action, class environment, class openList>
257 bool TemplateAStar<state,action,environment,openList>::InitializeSearch(environment *_env, const state& from, const state& to, std::vector<state> &thePath)
258 {
259  if (theHeuristic == 0)
260  theHeuristic = _env;
261  thePath.resize(0);
262  env = _env;
263  openClosedList.Reset(env->GetMaxHash());
264  ResetNodeCount();
265  start = from;
266  goal = to;
267 
268  if (env->GoalTest(from, to) && (stopAfterGoal)) //assumes that from and to are valid states
269  {
270  return false;
271  }
272 
273  double h = theHeuristic->HCost(start, goal);
274  openClosedList.AddOpenNode(start, env->GetStateHash(start), phi(h, 0), 0, h);
275 
276  return true;
277 }
278 
284 template <class state, class action, class environment, class openList>
285 void TemplateAStar<state,action,environment,openList>::AddAdditionalStartState(const state& newState)
286 {
287  double h = theHeuristic->HCost(newState, goal);
288  openClosedList.AddOpenNode(newState, env->GetStateHash(newState), phi(h, 0), 0, h);
289 }
290 
296 template <class state, class action, class environment, class openList>
297 void TemplateAStar<state,action,environment,openList>::AddAdditionalStartState(const state& newState, double cost)
298 {
299  double h = theHeuristic->HCost(newState, goal);
300  openClosedList.AddOpenNode(newState, env->GetStateHash(newState), phi(h, cost), cost, h);
301 }
302 
313 template <class state, class action, class environment, class openList>
314 bool TemplateAStar<state,action,environment,openList>::DoSingleSearchStep(std::vector<state> &thePath)
315 {
316  if (openClosedList.OpenSize() == 0)
317  {
318  thePath.resize(0); // no path found!
319  //closedList.clear();
320  return true;
321  }
322  uint64_t nodeid = openClosedList.Close();
323 // if (openClosedList.Lookup(nodeid).g+openClosedList.Lookup(nodeid).h > lastF)
324 // { lastF = openClosedList.Lookup(nodeid).g+openClosedList.Lookup(nodeid).h;
325 // //printf("Updated limit to %f\n", lastF);
326 // }
327  if (!openClosedList.Lookup(nodeid).reopened)
328  uniqueNodesExpanded++;
329  nodesExpanded++;
330 
331  if ((stopAfterGoal) && (env->GoalTest(openClosedList.Lookup(nodeid).data, goal)))
332  {
333  ExtractPathToStartFromID(nodeid, thePath);
334  // Path is backwards - reverse
335  reverse(thePath.begin(), thePath.end());
336  goalFCost = openClosedList.Lookup(nodeid).f;// + openClosedList.Lookup(nodeid).h;
337  return true;
338  }
339 
340  neighbors.resize(0);
341  edgeCosts.resize(0);
342  neighborID.resize(0);
343  neighborLoc.resize(0);
344 
345  //std::cout << "Expanding: " << env->GetStateHash(openClosedList.Lookup(nodeid).data) << " with f:";
346  //std::cout << openClosedList.Lookup(nodeid).g+openClosedList.Lookup(nodeid).h << std::endl;
347 
348  env->GetSuccessors(openClosedList.Lookup(nodeid).data, neighbors);
349  double bestH = openClosedList.Lookup(nodeid).h;
350  double lowHC = DBL_MAX;
351  // 1. load all the children
352  for (unsigned int x = 0; x < neighbors.size(); x++)
353  {
354  uint64_t theID;
355  neighborLoc.push_back(openClosedList.Lookup(env->GetStateHash(neighbors[x]), theID));
356  neighborID.push_back(theID);
357  edgeCosts.push_back(env->GCost(openClosedList.Lookup(nodeid).data, neighbors[x]));
358  if (useBPMX)
359  {
360  if (neighborLoc.back() != kNotFound)
361  {
362  if (!directed)
363  bestH = std::max(bestH, openClosedList.Lookup(theID).h-edgeCosts.back());
364  lowHC = std::min(lowHC, openClosedList.Lookup(theID).h+edgeCosts.back());
365  }
366  else {
367  double tmpH = theHeuristic->HCost(neighbors[x], goal);
368  if (!directed)
369  bestH = std::max(bestH, tmpH-edgeCosts.back());
370  lowHC = std::min(lowHC, tmpH+edgeCosts.back());
371  }
372  }
373  }
374 
375  if (useBPMX) // propagate best child to parent
376  {
377  if (!directed)
378  openClosedList.Lookup(nodeid).h = std::max(openClosedList.Lookup(nodeid).h, bestH);
379  openClosedList.Lookup(nodeid).h = std::max(openClosedList.Lookup(nodeid).h, lowHC);
380  openClosedList.Lookup(nodeid).f = phi(openClosedList.Lookup(nodeid).h, openClosedList.Lookup(nodeid).g);
381  }
382 
383  // iterate again updating costs and writing out to memory
384  for (size_t x = 0; x < neighbors.size(); x++)
385  {
386  nodesTouched++;
387  //std::cout << "looking at child with hash : " << env->GetStateHash(neighbors[x]) << "and g-cost"<<openClosedList.Lookup(nodeid).g+edgeCosts[x]<<std::endl;
388  if (theConstraint &&
389  theConstraint->ShouldNotGenerate(start, openClosedList.Lookup(nodeid).data, neighbors[x],
390  openClosedList.Lookup(nodeid).g+edgeCosts[x], goal))
391  continue;
392 
393  switch (neighborLoc[x])
394  {
395  case kClosedList:
396  if (useBPMX) // propagate parent to child - do this before potentially re-opening
397  {
398  if (fless(openClosedList.Lookup(neighborID[x]).h, bestH-edgeCosts[x]))
399  {
400  auto &i = openClosedList.Lookup(neighborID[x]);
401  i.h = bestH-edgeCosts[x];
402  i.f = phi(i.h, i.g);
403  if (useBPMX > 1) FullBPMX(neighborID[x], useBPMX-1);
404  }
405  }
406  if (reopenNodes)
407  {
408  if (fless(openClosedList.Lookup(nodeid).g+edgeCosts[x], openClosedList.Lookup(neighborID[x]).g))
409  {
410  auto &i = openClosedList.Lookup(neighborID[x]);
411  i.parentID = nodeid;
412  i.g = openClosedList.Lookup(nodeid).g+edgeCosts[x];
413  i.f = phi(i.h, i.g);
414  openClosedList.Reopen(neighborID[x]);
415  // This line isn't normally needed, but in some state spaces we might have
416  // equality but different meta information, so we need to make sure that the
417  // meta information is also copied, since this is the most generic A* implementation
418  i.data = neighbors[x];
419  }
420  }
421  break;
422  case kOpenList:
423  //edgeCost = env->GCost(openClosedList.Lookup(nodeid).data, neighbors[x]);
424  if (fless(openClosedList.Lookup(nodeid).g+edgeCosts[x], openClosedList.Lookup(neighborID[x]).g))
425  {
426  auto &i = openClosedList.Lookup(neighborID[x]);
427  i.parentID = nodeid;
428  i.g = openClosedList.Lookup(nodeid).g+edgeCosts[x];
429  i.f = phi(i.h, i.g);
430  // This line isn't normally needed, but in some state spaces we might have
431  // equality but different meta information, so we need to make sure that the
432  // meta information is also copied, since this is the most generic A* implementation
433  i.data = neighbors[x];
434  openClosedList.KeyChanged(neighborID[x]);
435 // std::cout << " Reducing cost to " << openClosedList.Lookup(nodeid).g+edgeCosts[x] << "\n";
436  // TODO: unify the KeyChanged calls.
437  }
438  else {
439 // std::cout << " no cheaper \n";
440  }
441  if (useBPMX) // propagate best child to parent
442  {
443  if (fgreater(bestH-edgeCosts[x], openClosedList.Lookup(neighborID[x]).h))
444  {
445  auto &i = openClosedList.Lookup(neighborID[x]);
446  i.h = std::max(i.h, bestH-edgeCosts[x]);
447  i.f = phi(i.h, i.g);
448  openClosedList.KeyChanged(neighborID[x]);
449  }
450  }
451  break;
452  case kNotFound:
453  { // add node to open list
454  //double edgeCost = env->GCost(openClosedList.Lookup(nodeid).data, neighbors[x]);
455 // std::cout << " adding to open ";
456 // std::cout << double(theHeuristic->HCost(neighbors[x], goal)+openClosedList.Lookup(nodeid).g+edgeCosts[x]);
457 // std::cout << " \n";
458  double h = theHeuristic->HCost(neighbors[x], goal);
459  if (useBPMX)
460  {
461  h = std::max(h, openClosedList.Lookup(nodeid).h-edgeCosts[x]);
462  openClosedList.AddOpenNode(neighbors[x],
463  env->GetStateHash(neighbors[x]),
464  phi(std::max(h, openClosedList.Lookup(nodeid).h-edgeCosts[x]), openClosedList.Lookup(nodeid).g+edgeCosts[x]),
465  openClosedList.Lookup(nodeid).g+edgeCosts[x],
466  h,
467  nodeid);
468  }
469  else {
470  openClosedList.AddOpenNode(neighbors[x],
471  env->GetStateHash(neighbors[x]),
472  phi(h, openClosedList.Lookup(nodeid).g+edgeCosts[x]),
473  openClosedList.Lookup(nodeid).g+edgeCosts[x],
474  h,
475  nodeid);
476  }
477 // if (loc == -1)
478 // { // duplicate edges
479 // neighborLoc[x] = kOpenList;
480 // x--;
481 // }
482  }
483  }
484  }
485 
486  return false;
487 }
488 
496 template <class state, class action, class environment, class openList>
497 state TemplateAStar<state, action,environment,openList>::CheckNextNode()
498 {
499  uint64_t key = openClosedList.Peek();
500  return openClosedList.Lookup(key).data;
501  //assert(false);
502  //return openQueue.top().currNode;
503 }
504 
512 template <class state, class action, class environment, class openList>
513 void TemplateAStar<state, action,environment,openList>::FullBPMX(uint64_t nodeID, int distance)
514 {
515  if (distance <= 0)
516  return;
517 
518  nodesExpanded++;
519  std::vector<state> succ;
520  env->GetSuccessors(openClosedList.Lookup(nodeID).data, succ);
521  double parentH = openClosedList.Lookup(nodeID).h;
522 
523  // load all the children and push parent heuristic value to children
524  for (unsigned int x = 0; x < succ.size(); x++)
525  {
526  uint64_t theID;
527  dataLocation loc = openClosedList.Lookup(env->GetStateHash(succ[x]), theID);
528  double edgeCost = env->GCost(openClosedList.Lookup(nodeID).data, succ[x]);
529  double newHCost = parentH-edgeCost;
530 
531  switch (loc)
532  {
533  case kClosedList:
534  {
535  if (fgreater(newHCost, openClosedList.Lookup(theID).h))
536  {
537  openClosedList.Lookup(theID).h = newHCost;
538  FullBPMX(theID, distance-1);
539  }
540  }
541  case kOpenList:
542  {
543  if (fgreater(newHCost, openClosedList.Lookup(theID).h))
544  {
545  openClosedList.Lookup(theID).h = newHCost;
546  openClosedList.KeyChanged(theID);
547  }
548  }
549  case kNotFound: break;
550  }
551  }
552 }
553 
554 
563 template <class state, class action,class environment,class openList>
564 void TemplateAStar<state, action,environment,openList>::ExtractPathToStartFromID(uint64_t node,
565  std::vector<state> &thePath)
566 {
567  do {
568  thePath.push_back(openClosedList.Lookup(node).data);
569  node = openClosedList.Lookup(node).parentID;
570  } while (openClosedList.Lookup(node).parentID != node);
571  thePath.push_back(openClosedList.Lookup(node).data);
572 }
573 
574 template <class state, class action,class environment,class openList>
575 const state &TemplateAStar<state, action,environment,openList>::GetParent(const state &s)
576 {
577  uint64_t theID;
578  openClosedList.Lookup(env->GetStateHash(s), theID);
579  theID = openClosedList.Lookup(theID).parentID;
580  return openClosedList.Lookup(theID).data;
581 }
582 
583 template <class state, class action, class environment, class openList>
584 uint64_t TemplateAStar<state, action,environment,openList>::GetNecessaryExpansions() const
585 {
586  uint64_t n = 0;
587  for (unsigned int x = 0; x < openClosedList.size(); x++)
588  {
589  const auto &data = openClosedList.Lookat(x);
590  if (fless(data.g + data.h, goalFCost))
591  n++;
592  }
593  return n;
594 }
595 
596 
603 template <class state, class action, class environment, class openList>
604 void TemplateAStar<state, action,environment,openList>::PrintStats()
605 {
606  printf("%u items in closed list\n", (unsigned int)openClosedList.ClosedSize());
607  printf("%u items in open queue\n", (unsigned int)openClosedList.OpenSize());
608 }
609 
617 template <class state, class action, class environment, class openList>
618 int TemplateAStar<state, action,environment,openList>::GetMemoryUsage()
619 {
620  return openClosedList.size();
621 }
622 
633 template <class state, class action, class environment, class openList>
634 bool TemplateAStar<state, action,environment,openList>::GetClosedListGCost(const state &val, double &gCost) const
635 {
636  uint64_t theID;
637  dataLocation loc = openClosedList.Lookup(env->GetStateHash(val), theID);
638  if (loc == kClosedList)
639  {
640  gCost = openClosedList.Lookat(theID).g;
641  return true;
642  }
643  return false;
644 }
645 
646 template <class state, class action, class environment, class openList>
647 bool TemplateAStar<state, action,environment,openList>::GetOpenListGCost(const state &val, double &gCost) const
648 {
649  uint64_t theID;
650  dataLocation loc = openClosedList.Lookup(env->GetStateHash(val), theID);
651  if (loc == kOpenList)
652  {
653  gCost = openClosedList.Lookat(theID).g;
654  return true;
655  }
656  return false;
657 }
658 
659 template <class state, class action, class environment, class openList>
660 bool TemplateAStar<state, action,environment,openList>::GetHCost(const state &val, double &hCost) const
661 {
662  uint64_t theID;
663  dataLocation loc = openClosedList.Lookup(env->GetStateHash(val), theID);
664  if (loc != kNotFound)
665  {
666  hCost = openClosedList.Lookat(theID).h;
667  return true;
668  }
669  return false;
670 }
671 
672 template <class state, class action, class environment, class openList>
673 bool TemplateAStar<state, action,environment,openList>::GetClosedItem(const state &s, AStarOpenClosedDataWithF<state> &result)
674 {
675  uint64_t theID;
676  dataLocation loc = openClosedList.Lookup(env->GetStateHash(s), theID);
677  if (loc == kClosedList)
678  {
679  result = openClosedList.Lookat(theID);
680  return true;
681  }
682  return false;
683 
684 }
685 
686 
693 template <class state, class action, class environment, class openList>
694 void TemplateAStar<state, action,environment,openList>::OpenGLDraw() const
695 {
696  double transparency = 1.0;
697  if (openClosedList.size() == 0)
698  return;
699  uint64_t top = -1;
700 // double minf = 1e9, maxf = 0;
701  if (openClosedList.OpenSize() > 0)
702  {
703  top = openClosedList.Peek();
704  }
705 // for (unsigned int x = 0; x < openClosedList.size(); x++)
706 // {
707 // const AStarOpenClosedData<state> &data = openClosedList.Lookat(x);
708 // double f = data.g+data.h;
709 // if (f > maxf)
710 // maxf = f;
711 // if (f < minf)
712 // minf = f;
713 // }
714  for (unsigned int x = 0; x < openClosedList.size(); x++)
715  {
716  const auto &data = openClosedList.Lookat(x);
717  if (x == top)
718  {
719  env->SetColor(1.0, 1.0, 0.0, transparency);
720  env->OpenGLDraw(data.data);
721  }
722  if ((data.where == kOpenList) && (data.reopened))
723  {
724  env->SetColor(0.0, 0.5, 0.5, transparency);
725  env->OpenGLDraw(data.data);
726  }
727  else if (data.where == kOpenList)
728  {
729  env->SetColor(0.0, 1.0, 0.0, transparency);
730  env->OpenGLDraw(data.data);
731  }
732  else if ((data.where == kClosedList) && (data.reopened))
733  {
734  env->SetColor(0.5, 0.0, 0.5, transparency);
735  env->OpenGLDraw(data.data);
736  }
737  else if (data.where == kClosedList)
738  {
739 // if (top != -1)
740 // {
741 // env->SetColor((data.g+data.h-minf)/(maxf-minf), 0.0, 0.0, transparency);
742 // }
743 // else {
744  if (data.parentID == x)
745  env->SetColor(1.0, 0.5, 0.5, transparency);
746  else
747  env->SetColor(1.0, 0.0, 0.0, transparency);
748 // }
749  env->OpenGLDraw(data.data);
750  }
751  }
752  env->SetColor(1.0, 0.5, 1.0, 0.5);
753  env->OpenGLDraw(goal);
754 }
755 
762 template <class state, class action, class environment, class openList>
763 void TemplateAStar<state, action,environment,openList>::Draw(Graphics::Display &disp) const
764 {
765  double transparency = 1.0;
766  if (openClosedList.size() == 0)
767  return;
768  uint64_t top = -1;
769  // double minf = 1e9, maxf = 0;
770  if (openClosedList.OpenSize() > 0)
771  {
772  top = openClosedList.Peek();
773  }
774  for (unsigned int x = 0; x < openClosedList.size(); x++)
775  {
776  const auto &data = openClosedList.Lookat(x);
777  if (x == top)
778  {
779  env->SetColor(1.0, 1.0, 0.0, transparency);
780  env->Draw(disp, data.data);
781  }
782  else if ((data.where == kOpenList) && (data.reopened))
783  {
784  env->SetColor(0.0, 0.5, 0.5, transparency);
785  env->Draw(disp, data.data);
786  }
787  else if (data.where == kOpenList)
788  {
789  env->SetColor(0.0, 1.0, 0.0, transparency);
790  env->Draw(disp, data.data);
791  }
792  else if ((data.where == kClosedList) && (data.reopened))
793  {
794  env->SetColor(0.5, 0.0, 0.5, transparency);
795  env->Draw(disp, data.data);
796  }
797  else if (data.where == kClosedList)
798  {
799  // if (top != -1)
800  // {
801  // env->SetColor((data.g+data.h-minf)/(maxf-minf), 0.0, 0.0, transparency);
802  // }
803  // else {
804  if (data.parentID == x)
805  env->SetColor(1.0, 0.5, 0.5, transparency);
806  else
807  env->SetColor(1.0, 0.0, 0.0, transparency);
808  // }
809  env->Draw(disp, data.data);
810  }
811  }
812  env->SetColor(1.0, 0.5, 1.0, 0.5);
813  env->Draw(disp, goal);
814 }
815 
816 template <class state, class action, class environment, class openList>
817 std::string TemplateAStar<state, action,environment,openList>::SVGDraw() const
818 {
819  std::string s;
820  double transparency = 1.0;
821  if (openClosedList.size() == 0)
822  return s;
823  uint64_t top = -1;
824 
825  if (openClosedList.OpenSize() > 0)
826  {
827  top = openClosedList.Peek();
828  }
829  for (unsigned int x = 0; x < openClosedList.size(); x++)
830  {
831  const auto &data = openClosedList.Lookat(x);
832 
833  if (x == top)
834  {
835  env->SetColor(1.0, 1.0, 0.0, transparency);
836  s+=env->SVGDraw(data.data);
837  }
838  else if ((data.where == kOpenList) && (data.reopened))
839  {
840  env->SetColor(0.0, 0.5, 0.5, transparency);
841  s+=env->SVGDraw(data.data);
842  }
843  else if (data.where == kOpenList)
844  {
845  env->SetColor(0.0, 1.0, 0.0, transparency);
846  s+=env->SVGDraw(data.data);
847  }
848  else if ((data.where == kClosedList) && (data.reopened))
849  {
850  env->SetColor(0.5, 0.0, 0.5, transparency);
851  s+=env->SVGDraw(data.data);
852  }
853  else if (data.where == kClosedList)
854  {
855  env->SetColor(1.0, 0.0, 0.0, transparency);
856  s+=env->SVGDraw(data.data);
857  }
858  }
859  return s;
860 }
861 
862 template <class state, class action, class environment, class openList>
863 std::string TemplateAStar<state, action,environment,openList>::SVGDrawDetailed() const
864 {
865  std::string s;
866  //double transparency = 1.0;
867  if (openClosedList.size() == 0)
868  return s;
869  uint64_t top = -1;
870 
871  if (openClosedList.OpenSize() > 0)
872  {
873  top = openClosedList.Peek();
874  }
875  for (unsigned int x = 0; x < openClosedList.size(); x++)
876  {
877  const auto &data = openClosedList.Lookat(x);
878 
879 // if (x == top)
880 // {
881 // env->SetColor(1.0, 1.0, 0.0, transparency);
882 // s+=env->SVGDraw(data.data);
883 // }
884 // else if ((data.where == kOpenList) && (data.reopened))
885 // {
886 // env->SetColor(0.0, 0.5, 0.5, transparency);
887 // s+=env->SVGDraw(data.data);
888 // }
889 // else if (data.where == kOpenList)
890 // {
891 // env->SetColor(0.0, 1.0, 0.0, transparency);
892 // s+=env->SVGDraw(data.data);
893 // }
894 // else if ((data.where == kClosedList) && (data.reopened))
895 // {
896 // env->SetColor(0.5, 0.0, 0.5, transparency);
897 // s+=env->SVGDraw(data.data);
898 // }
899 // else if (data.where == kClosedList)
900 // {
901 // env->SetColor(1.0, 0.0, 0.0, transparency);
902 // s+=env->SVGDraw(data.data);
903 // }
904  env->SetColor(0.0, 0.0, 0.0);
905  char d[32];
906  sprintf(d, "%1.1f", data.g+data.h);
907  s+=env->SVGLabelState(data.data, d, 0.35, -0.6, -1.1);
908  sprintf(d, "g:%1.1f", data.g);
909  s+=env->SVGLabelState(data.data, d, 0.25, -0.6, -0.75);
910  sprintf(d, "h:%1.1f", data.h);
911  s+=env->SVGLabelState(data.data, d, 0.25, -0.6, -0.48);
912  }
913  return s;
914 }
915 
916 
917 #endif
TemplateAStar::theConstraint
Constraint< state > * theConstraint
Definition: TemplateAStar.h:183
TemplateAStar::phi
std::function< double(double, double)> phi
Definition: TemplateAStar.h:176
TemplateAStar::SetWeight
void SetWeight(double w)
Definition: TemplateAStar.h:158
GenericSearchAlgorithm.h
An interface for generic search algorithms.
BucketOpenClosed.h
TemplateAStar::SetStopAfterGoal
void SetStopAfterGoal(bool val)
Definition: TemplateAStar.h:139
min
double min(double a, double b)
Definition: FPUtil.h:35
TemplateAStar::SetConstraint
void SetConstraint(Constraint< state > *c)
Definition: TemplateAStar.h:132
Constraint
Definition: Constraint.h:13
TemplateAStar::GetNecessaryExpansions
uint64_t GetNecessaryExpansions() const
Definition: TemplateAStar.h:584
TemplateAStar::SetHeuristic
void SetHeuristic(Heuristic< state > *h)
Definition: TemplateAStar.h:131
TemplateAStar::GetWeight
double GetWeight()
Definition: TemplateAStar.h:163
AStarCompare
Definition: TemplateAStar.h:57
Heuristic
Definition: Heuristic.h:30
TemplateAStar::weight
double weight
Definition: TemplateAStar.h:175
TemplateAStar::GetNodesExpanded
uint64_t GetNodesExpanded() const
Definition: TemplateAStar.h:134
TemplateAStar::ExtractPathToStart
void ExtractPathToStart(state &node, std::vector< state > &thePath)
Definition: TemplateAStar.h:93
Constraint.h
TemplateAStar::GetOpenListGCost
bool GetOpenListGCost(const state &val, double &gCost) const
Definition: TemplateAStar.h:647
AStarOpenClosed.h
d
mcData d[]
Definition: MotionCaptureMovement.cpp:21
AStarOpenClosedDataWithF
Definition: AStarOpenClosed.h:36
TemplateAStar::LogFinalStats
void LogFinalStats(StatCollection *)
Definition: TemplateAStar.h:137
TemplateAStar::neighborID
std::vector< uint64_t > neighborID
Definition: TemplateAStar.h:168
TemplateAStar::ResetNodeCount
void ResetNodeCount()
Definition: TemplateAStar.h:106
FPUtil.h
TemplateAStar::edgeCosts
std::vector< double > edgeCosts
Definition: TemplateAStar.h:169
TemplateAStar::GetHCost
bool GetHCost(const state &val, double &hCost) const
Definition: TemplateAStar.h:660
TemplateAStar::SetReopenNodes
void SetReopenNodes(bool re)
Definition: TemplateAStar.h:125
TemplateAStar::ExtractPathToStartFromID
void ExtractPathToStartFromID(uint64_t node, std::vector< state > &thePath)
Get the path from a goal state to the start state.
Definition: TemplateAStar.h:564
TemplateAStar::GetUsingBPMX
int GetUsingBPMX()
Definition: TemplateAStar.h:123
TemplateAStar::CheckNextNode
state CheckNextNode()
Returns the next state on the open list (but doesn't pop it off the queue).
Definition: TemplateAStar.h:497
TemplateAStar::openClosedList
openList openClosedList
Definition: TemplateAStar.h:84
TemplateAStar::GetStopAfterGoal
bool GetStopAfterGoal()
Definition: TemplateAStar.h:140
TemplateAStar::GetNumOpenItems
unsigned int GetNumOpenItems()
Definition: TemplateAStar.h:113
TemplateAStar::nodesTouched
uint64_t nodesTouched
Definition: TemplateAStar.h:165
TemplateAStar::GetPath
void GetPath(environment *env, const state &from, const state &to, std::vector< state > &thePath)
Perform an A* search between two states.
Definition: TemplateAStar.h:214
TemplateAStar::OpenGLDraw
void OpenGLDraw() const
Draw the open/closed list.
Definition: TemplateAStar.h:694
AStarOpenClosedData::g
double g
Definition: AStarOpenClosed.h:64
TemplateAStar::nodesExpanded
uint64_t nodesExpanded
Definition: TemplateAStar.h:165
TemplateAStar::GetReopenNodes
bool GetReopenNodes()
Definition: TemplateAStar.h:126
GenericSearchAlgorithm
Definition: GenericSearchAlgorithm.h:35
TemplateAStar::SVGDraw
std::string SVGDraw() const
Definition: TemplateAStar.h:817
TemplateAStar::GetParent
const state & GetParent(const state &s)
Definition: TemplateAStar.h:575
loc
Definition: MapGenerators.cpp:296
TemplateAStar::GetStateLocation
dataLocation GetStateLocation(const state &val)
Definition: TemplateAStar.h:119
TemplateAStar::goal
state goal
Definition: TemplateAStar.h:85
TemplateAStar::TemplateAStar
TemplateAStar()
Definition: TemplateAStar.h:75
TemplateAStar::GetNodesTouched
uint64_t GetNodesTouched() const
Definition: TemplateAStar.h:135
TemplateAStar::SVGDrawDetailed
std::string SVGDrawDetailed() const
Definition: TemplateAStar.h:863
Graphics::Display
Definition: Graphics.h:146
TemplateAStar::SetPhi
void SetPhi(std::function< double(double, double)> p)
Setting this function.
Definition: TemplateAStar.h:150
TemplateAStar::AddAdditionalStartState
void AddAdditionalStartState(const state &newState)
Add additional start state to the search.
Definition: TemplateAStar.h:285
fless
bool fless(double a, double b)
Definition: FPUtil.h:28
TemplateAStar::stopAfterGoal
bool stopAfterGoal
Definition: TemplateAStar.h:172
AStarOpenClosedData
Definition: AStarOpenClosed.h:52
dataLocation
dataLocation
Definition: AStarOpenClosed.h:27
TemplateAStar
A templated version of A*, based on HOG genericAStar.
Definition: TemplateAStar.h:73
TemplateAStar::reopenNodes
bool reopenNodes
Definition: TemplateAStar.h:179
TemplateAStar::env
environment * env
Definition: TemplateAStar.h:171
TemplateAStar::uniqueNodesExpanded
uint64_t uniqueNodesExpanded
Definition: TemplateAStar.h:180
AStarCompareWithF
Definition: TemplateAStar.h:44
fgreater
bool fgreater(double a, double b)
Definition: FPUtil.h:29
TemplateAStar::directed
bool directed
Definition: TemplateAStar.h:177
TemplateAStar::GetItem
const AStarOpenClosedDataWithF< state > & GetItem(unsigned int which)
Definition: TemplateAStar.h:116
max
#define max(a, b)
Definition: MinimalSectorAbstraction.cpp:40
TemplateAStar::~TemplateAStar
virtual ~TemplateAStar()
Definition: TemplateAStar.h:80
TemplateAStar::Draw
void Draw(Graphics::Display &disp) const
Draw the open/closed list.
Definition: TemplateAStar.h:763
Graphics.h
TemplateAStar::start
state start
Definition: TemplateAStar.h:85
TemplateAStar::GetUniqueNodesExpanded
uint64_t GetUniqueNodesExpanded()
Definition: TemplateAStar.h:105
kOpenList
@ kOpenList
Definition: AStarOpenClosed.h:28
StatCollection
The StatCollection class is for collecting stats across different parts of the simulation.
Definition: StatCollection.h:34
TemplateAStar::neighbors
std::vector< state > neighbors
Definition: TemplateAStar.h:167
TemplateAStar::PrintStats
void PrintStats()
A function that prints the number of states in the closed list and open queue.
Definition: TemplateAStar.h:604
TemplateAStar::GetMemoryUsage
int GetMemoryUsage()
Return the amount of memory used by TemplateAStar.
Definition: TemplateAStar.h:618
TemplateAStar::GetNumItems
const int GetNumItems()
Definition: TemplateAStar.h:115
TemplateAStar::InitializeSearch
bool InitializeSearch(environment *env, const state &from, const state &to, std::vector< state > &thePath)
Initialize the A* search.
Definition: TemplateAStar.h:257
TemplateAStar::GetOpenItem
const AStarOpenClosedDataWithF< state > & GetOpenItem(unsigned int which)
Definition: TemplateAStar.h:114
AStarOpenClosedDataWithF::g
double g
Definition: AStarOpenClosed.h:43
TemplateAStar::goalFCost
double goalFCost
Definition: TemplateAStar.h:174
TemplateAStar::GetClosedItem
bool GetClosedItem(const state &s, AStarOpenClosedDataWithF< state > &)
Definition: TemplateAStar.h:673
TemplateAStar::SetUseBPMX
void SetUseBPMX(int depth)
Definition: TemplateAStar.h:122
AStarOpenClosedData::h
double h
Definition: AStarOpenClosed.h:65
TemplateAStar::GetClosedListGCost
bool GetClosedListGCost(const state &val, double &gCost) const
Get state from the closed list.
Definition: TemplateAStar.h:634
AStarOpenClosedDataWithF::f
double f
Definition: AStarOpenClosed.h:42
TemplateAStar::HaveExpandedState
bool HaveExpandedState(const state &val)
Definition: TemplateAStar.h:117
AStarCompareWithF::operator()
bool operator()(const AStarOpenClosedDataWithF< state > &i1, const AStarOpenClosedDataWithF< state > &i2) const
Definition: TemplateAStar.h:46
TemplateAStar::useBPMX
int useBPMX
Definition: TemplateAStar.h:178
kNotFound
@ kNotFound
Definition: AStarOpenClosed.h:30
path
A linked list of nodes which form a continuous path.
Definition: Path.h:20
AStarCompare::operator()
bool operator()(const AStarOpenClosedData< state > &i1, const AStarOpenClosedData< state > &i2) const
Definition: TemplateAStar.h:59
TemplateAStar::SetDirected
void SetDirected(bool d)
Definition: TemplateAStar.h:129
fequal
bool fequal(double a, double b, double tolerance=TOLERANCE)
Definition: FPUtil.h:32
TemplateAStar::FullBPMX
void FullBPMX(uint64_t nodeID, int distance)
Perform a full bpmx propagation.
Definition: TemplateAStar.h:513
TemplateAStar::GetName
virtual const char * GetName()
Return the name of the algorithm.
Definition: TemplateAStar.h:195
TemplateAStar::radEnv
environment * radEnv
Definition: TemplateAStar.h:181
kClosedList
@ kClosedList
Definition: AStarOpenClosed.h:29
TemplateAStar::DoSingleSearchStep
bool DoSingleSearchStep(std::vector< state > &thePath)
Expand a single node.
Definition: TemplateAStar.h:314
TemplateAStar::Phi
double Phi(double h, double g)
Definition: TemplateAStar.h:154
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
TemplateAStar::theHeuristic
Heuristic< state > * theHeuristic
Definition: TemplateAStar.h:182
Graphics::Display::data
Definition: Graphics.h:226
TemplateAStar::neighborLoc
std::vector< dataLocation > neighborLoc
Definition: TemplateAStar.h:170
Generated by   doxygen 1.8.17