HOG2
fMM.h
Go to the documentation of this file.
1 //
2 // fMM.h
3 // hog2
4 //
5 // Created by Nathan Sturtevant on 10/27/15.
6 // Copyright © 2015 University of Denver. All rights reserved.
7 //
8 
9 #ifndef FMM_h
10 #define FMM_h
11 
12 #include "AStarOpenClosed.h"
13 #include "FPUtil.h"
14 #include "Timer.h"
15 #include <unordered_map>
16 
17 template<typename state>
18 class FMMOpenClosedData {
19 public:
20  FMMOpenClosedData() {}
21  FMMOpenClosedData(const state &theData, double gCost, double hCost, uint64_t parent, uint64_t openLoc, dataLocation location)
22  :data(theData), g(gCost), h(hCost), parentID(parent), openLocation(openLoc), where(location) { reopened = false; }
23  state data;
24  double g;
25  double h;
26  double frac;
27  uint64_t parentID;
28  uint64_t openLocation;
29  bool reopened;
30  dataLocation where;
31 };
32 
33 
34 template <class state, int epsilon = 0>
35 struct fMMCompare {
36  bool operator()(const FMMOpenClosedData<state> &i1, const FMMOpenClosedData<state> &i2) const
37  {
38  // FIXME: Note that i2 happens (but isn't guaranteed) to be the uninitialized element,
39  // so we can use the fraction from i1 properly. But, this could be broken.
40 // printf("%f - %f\n", i1.frac, i2.frac);
41  double p1 = std::max((i1.g+i1.h), i1.g/i1.frac+epsilon);
42  double p2 = std::max((i2.g+i2.h), i2.g/i1.frac+epsilon);
43  if (fequal(p1, p2))
44  {
45  return (fgreater(i1.g, i2.g)); // low g-cost over high
46  //return (fless(i1.g/i1.frac, i2.g/i1.frac)); // high g-cost over low
47  }
48  return (fgreater(p1, p2)); // low priority over high
49  }
50 };
51 
52 namespace std {
53  template <> struct hash<std::pair<double, double>>
54  {
55  size_t operator()(const std::pair<double, double> & x) const
56  {
57  return std::hash<double>()(x.first)^(std::hash<double>()(x.second)<<16);
58  }
59  };
60 }
61 
62 
63 template <class state, class action, class environment, class priorityQueue = AStarOpenClosed<state, fMMCompare<state>, FMMOpenClosedData<state>> >
64 class fMM {
65 public:
66  fMM(double epsilon = 1.0):epsilon(epsilon) { forwardHeuristic = 0; backwardHeuristic = 0; env = 0; ResetNodeCount(); fraction = 0.5; }
67  void SetFraction(double frac) {fraction = frac; }
68  virtual ~fMM() {}
69  void GetPath(environment *env, const state& from, const state& to,
70  Heuristic<state> *forward, Heuristic<state> *backward, std::vector<state> &thePath);
71  bool InitializeSearch(environment *env, const state& from, const state& to,
72  Heuristic<state> *forward, Heuristic<state> *backward, std::vector<state> &thePath);
73  bool DoSingleSearchStep(std::vector<state> &thePath);
74 
75  virtual const char *GetName() { return "fMM"; }
76 
77  void ResetNodeCount() { nodesExpanded = nodesTouched = uniqueNodesExpanded = 0; }
78 
79  inline const int GetNumForwardItems() { return forwardQueue.size(); }
80  inline const FMMOpenClosedData<state> &GetForwardItem(unsigned int which) { return forwardQueue.Lookat(which); }
81  inline const int GetNumBackwardItems() { return backwardQueue.size(); }
82  inline const FMMOpenClosedData<state> &GetBackwardItem(unsigned int which) { return backwardQueue.Lookat(which); }
83 
84  uint64_t GetUniqueNodesExpanded() const { return uniqueNodesExpanded; }
85  uint64_t GetNodesExpanded() const { return nodesExpanded; }
86  uint64_t GetNodesTouched() const { return nodesTouched; }
87  uint64_t GetNecessaryExpansions() const;
88  //void FullBPMX(uint64_t nodeID, int distance);
89 
90  std::string SVGDraw() const;
91  void OpenGLDraw() const;
92  void Draw(Graphics::Display &display) const;
93  void PrintHDist()
94  {
95  std::vector<uint64_t> d;
96  for (auto i = dist.begin(); i != dist.end(); i++)
97  {
98  if (i->first.first < i->first.second)
99  {
100  int h = (int)i->first.second;
101  if (h >= d.size())
102  d.resize(h+1);
103  d[h] += i->second;
104  }
105  }
106  printf("fMM Dynamic Distribution\n");
107  for (int x = 0; x < d.size(); x++)
108  {
109  if (d[x] != 0)
110  printf("%d\t%" PRId64 "\n", x, d[x]);
111  }
112  }
113  void PrintOpenStats(std::unordered_map<std::pair<double, double>, int> &s)
114  {
115  printf("Search distributions: (%s)\n", ((&s)==(&f))?"forward":"backward");
116  for (auto i = s.begin(); i != s.end(); i++)
117  {
118  if (i->second > 0)
119  {
120  bool ignore = false;
121  ignore = (i->first.first+i->first.second >= currentCost);
122  printf("%c g: %1.1f h: %1.1f count: %d\n", ignore?'*':' ',
123  i->first.first, i->first.second, i->second);
124  }
125  }
126  }
127 
128  // void SetWeight(double w) {weight = w;}
129 private:
130 
131  void ExtractPathToGoal(state &node, std::vector<state> &thePath)
132  { uint64_t theID; backwardQueue.Lookup(env->GetStateHash(node), theID); ExtractPathToGoalFromID(theID, thePath); }
133  void ExtractPathToGoalFromID(uint64_t node, std::vector<state> &thePath)
134  {
135  do {
136  thePath.push_back(backwardQueue.Lookup(node).data);
137  node = backwardQueue.Lookup(node).parentID;
138  } while (backwardQueue.Lookup(node).parentID != node);
139  thePath.push_back(backwardQueue.Lookup(node).data);
140  }
141 
142  void ExtractPathToStart(state &node, std::vector<state> &thePath)
143  { uint64_t theID; forwardQueue.Lookup(env->GetStateHash(node), theID); ExtractPathToStartFromID(theID, thePath); }
144  void ExtractPathToStartFromID(uint64_t node, std::vector<state> &thePath)
145  {
146  do {
147  thePath.push_back(forwardQueue.Lookup(node).data);
148  node = forwardQueue.Lookup(node).parentID;
149  } while (forwardQueue.Lookup(node).parentID != node);
150  thePath.push_back(forwardQueue.Lookup(node).data);
151  }
152 
153  void Draw(Graphics::Display &display, const priorityQueue &queue) const;
154  void OpenGLDraw(const priorityQueue &queue) const;
155  std::string SVGDraw(const priorityQueue &queue) const;
156 
157  void Expand(priorityQueue &current,
158  priorityQueue &opposite,
159  Heuristic<state> *heuristic,
160  const state &target,
161  std::unordered_map<std::pair<double, double>, int> &count);
162  // std::unordered_map<double, int> &ming,
163  // std::unordered_map<double, int> &minf);
164  priorityQueue forwardQueue, backwardQueue;
165  state goal, start;
166  std::unordered_map<std::pair<double, double>, int> dist;
167  std::unordered_map<std::pair<double, double>, int> f, b;
168  uint64_t nodesTouched, nodesExpanded, uniqueNodesExpanded;
169  state middleNode;
170  double currentCost;
171  double lastMinForwardG;
172  double lastMinBackwardG;
173  double epsilon;
174  double fraction;
175 
176  std::vector<state> neighbors;
177  environment *env;
178  Timer t;
179  Heuristic<state> *forwardHeuristic;
180  Heuristic<state> *backwardHeuristic;
181 
182  double oldp1;
183  double oldp2;
184  bool recheckPath;
185 };
186 
187 template <class state, class action, class environment, class priorityQueue>
188 void fMM<state, action, environment, priorityQueue>::GetPath(environment *env, const state& from, const state& to,
189  Heuristic<state> *forward, Heuristic<state> *backward, std::vector<state> &thePath)
190 {
191  if (InitializeSearch(env, from, to, forward, backward, thePath) == false)
192  return;
193  t.StartTimer();
194  while (!DoSingleSearchStep(thePath))
195  { }
196 }
197 
198 template <class state, class action, class environment, class priorityQueue>
199 bool fMM<state, action, environment, priorityQueue>::InitializeSearch(environment *env, const state& from, const state& to,
200  Heuristic<state> *forward, Heuristic<state> *backward,
201  std::vector<state> &thePath)
202 {
203  this->env = env;
204  forwardHeuristic = forward;
205  backwardHeuristic = backward;
206  currentCost = DBL_MAX;
207  forwardQueue.Reset();
208  backwardQueue.Reset();
209  ResetNodeCount();
210  thePath.resize(0);
211  start = from;
212  goal = to;
213  if (start == goal)
214  return false;
215  oldp1 = oldp2 = 0;
216  lastMinForwardG = 0;
217  lastMinBackwardG = 0;
218 // forwardQueue.AddOpenNode(start, env->GetStateHash(start), 0, forwardHeuristic->HCost(start, goal));
219 // backwardQueue.AddOpenNode(goal, env->GetStateHash(goal), 0, backwardHeuristic->HCost(goal, start));
220  uint64_t i;
221  i = forwardQueue.AddOpenNode(start, env->GetStateHash(start), 0, forwardHeuristic->HCost(start, goal));
222  forwardQueue.Lookup(i).frac = fraction;
223  backwardQueue.AddOpenNode(goal, env->GetStateHash(goal), 0, backwardHeuristic->HCost(goal, start));
224  backwardQueue.Lookup(i).frac = 1-fraction;
225 
226  f.clear();
227  b.clear();
228  recheckPath = false;
229  return true;
230 }
231 
232 template <class state, class action, class environment, class priorityQueue>
233 bool fMM<state, action, environment, priorityQueue>::DoSingleSearchStep(std::vector<state> &thePath)
234 {
235  if (forwardQueue.OpenSize() == 0 || backwardQueue.OpenSize() == 0)
236  {
237  return true;
238  }
239 
240  // if (forwardQueue.OpenSize() == 0)
241  // //Expand(backwardQueue, forwardQueue, backwardHeuristic, start, g_b, f_b);
242  // Expand(backwardQueue, forwardQueue, backwardHeuristic, start, b);
243  //
244  // if (backwardQueue.OpenSize() == 0)
245  // //Expand(forwardQueue, backwardQueue, forwardHeuristic, goal, g_f, f_f);
246  // Expand(forwardQueue, backwardQueue, forwardHeuristic, goal, f);
247 
248  uint64_t forward = forwardQueue.Peek();
249  uint64_t backward = backwardQueue.Peek();
250 
251  const FMMOpenClosedData<state> &nextForward = forwardQueue.Lookat(forward);
252  const FMMOpenClosedData<state> &nextBackward = backwardQueue.Lookat(backward);
253 
254  double p1 = std::max(nextForward.g+nextForward.h, nextForward.g/nextForward.frac);
255  if (nextForward.frac == 0)
256  p1 = DBL_MAX;
257  double p2 = std::max(nextBackward.g+nextBackward.h, nextBackward.g/nextBackward.frac);
258  if (nextBackward.frac == 0)
259  p2 = DBL_MAX;
260  if (p1 > oldp1)
261  {
262  // printf("Forward priority to %1.2f [%" PRId64 " expanded - %1.2fs]\n", p1, GetNodesExpanded(), t.EndTimer());
263  oldp1 = p1;
264  //PrintOpenStats(f);
265  }
266  if (p2 > oldp2)
267  {
268  // printf("Backward priority to %1.2f [%" PRId64 " expanded - %1.2fs]\n", p2, GetNodesExpanded(), t.EndTimer());
269  oldp2 = p2;
270  //PrintOpenStats(b);
271  }
272 
273  if (fless(p1, p2))
274  {
275 // printf("Next state priority %f (f)\n", p1);
276  //Expand(forwardQueue, backwardQueue, forwardHeuristic, goal, g_f, f_f);
277  Expand(forwardQueue, backwardQueue, forwardHeuristic, goal, f);
278  }
279  else if (fless(p2, p1))
280  {
281 // printf("Next state priority %f (b)\n", p2);
282  //Expand(backwardQueue, forwardQueue, backwardHeuristic, start, g_b, f_b);
283  Expand(backwardQueue, forwardQueue, backwardHeuristic, start, b);
284  }
285  else { // equal priority
286 // printf("Next state priority %f (t)\n", p1);
287 // printf(" * f: %f/%f=%f ", nextForward.g, nextForward.frac, nextForward.g/nextForward.frac);
288 // std::cout << nextForward.data << "\n";
289 // printf(" * b: %f/%f=%f ", nextBackward.g, nextBackward.frac, nextBackward.g/nextBackward.frac);
290 // std::cout << nextBackward.data << "\n";
291  if (!fequal(nextForward.g/nextForward.frac, p1))
292  {
293  Expand(forwardQueue, backwardQueue, forwardHeuristic, goal, f);
294  }
295  else if (!fequal(nextBackward.g/nextBackward.frac, p2))
296  {
297  Expand(backwardQueue, forwardQueue, backwardHeuristic, start, b);
298  }
299  else {
300  Expand(forwardQueue, backwardQueue, forwardHeuristic, goal, f);
301  //Expand(forwardQueue, backwardQueue, forwardHeuristic, goal, g_f, f_f);
302  }
303  }
304  // check if we can terminate
305  if (recheckPath)
306  {
307  recheckPath = false;
308  // TODO: make this more efficient
309  double minForwardG = DBL_MAX;
310  double minBackwardG = DBL_MAX;
311  double minForwardF = DBL_MAX;
312  double minBackwardF = DBL_MAX;
313  double forwardP;
314  double backwardP;
315 
316  for (auto i = f.begin(); i != f.end(); i++)
317  {
318  if (i->second > 0) // some elements
319  {
320  if ((i->first.first + i->first.second < currentCost) && // termination only stopped by lower f-cost
321  (i->first.first + lastMinBackwardG + 1.0 < currentCost))
322  {
323  minForwardG = std::min(minForwardG, i->first.first);
324  minForwardF = std::min(minForwardF, i->first.first+i->first.second);
325  }
326  }
327  }
328  for (auto i = b.begin(); i != b.end(); i++)
329  {
330  if (i->second > 0) // some elements
331  {
332  if ((i->first.first + i->first.second < currentCost) && // termination only stopped by lower f-cost
333  (i->first.first + lastMinForwardG + 1.0 < currentCost))
334  {
335  minBackwardG = std::min(minBackwardG, i->first.first);
336  minBackwardF = std::min(minBackwardF, i->first.first+i->first.second);
337  }
338  }
339  }
340 
341  {
342  auto iB = backwardQueue.Lookat(backwardQueue.Peek());
343  backwardP = std::max(iB.g+iB.h, iB.g/iB.frac);
344  auto iF = forwardQueue.Lookat(forwardQueue.Peek());
345  forwardP = std::max(iF.g+iF.h, iF.g/iF.frac);
346  }
347  bool done = false;
348  if (minForwardF == DBL_MAX)
349  {
350  minForwardF = minForwardG = currentCost+1;
351  }
352  if (minBackwardF == DBL_MAX)
353  {
354  minBackwardF = minBackwardG = currentCost+1;
355  }
356  if (!fgreater(currentCost, minForwardF))
357  {
358  // printf("Terminated on forwardf (%f >= %f)\n", minForwardF, currentCost);
359  done = true;
360  }
361  if (!fgreater(currentCost, minBackwardF))
362  {
363  // printf("Terminated on backwardf (%f >= %f)\n", minBackwardF, currentCost);
364  done = true;
365  }
366  if (!fgreater(currentCost, minForwardG+minBackwardG+epsilon)) // TODO: epsilon
367  {
368  // printf("Terminated on g+g+epsilon (%f+%f+%f >= %f)\n", minForwardG, minBackwardG, epsilon, currentCost);
369  done = true;
370  }
371  if (!fgreater(currentCost, std::min(forwardP, backwardP)))
372  {
373  // printf("Terminated on forwardP/backwardP (min(%f, %f) >= %f)\n", forwardP, backwardP, currentCost);
374  done = true;
375  }
376  // if (!fgreater(currentCost, backwardP))
377  // {
378  // printf("Terminated on backwardP\n");
379  // done = true;
380  // }
381  // for now, always terminate
382  lastMinBackwardG = minBackwardG;
383  lastMinForwardG = minForwardG;
384  if (done)
385  {
386  // PrintOpenStats(f);
387  // PrintOpenStats(b);
388 
389  std::vector<state> pFor, pBack;
390  ExtractPathToGoal(middleNode, pBack);
391  ExtractPathToStart(middleNode, pFor);
392  reverse(pFor.begin(), pFor.end());
393  thePath = pFor;
394  thePath.insert( thePath.end(), pBack.begin()+1, pBack.end() );
395 
396  return true;
397  }
398  }
399  return false;
400 }
401 
402 template <class state, class action, class environment, class priorityQueue>
403 void fMM<state, action, environment, priorityQueue>::Expand(priorityQueue &current,
404  priorityQueue &opposite,
405  Heuristic<state> *heuristic, const state &target,
406  std::unordered_map<std::pair<double, double>, int> &count)
407 {
408  uint64_t nextID = current.Close();
409  nodesExpanded++;
410  if (current.Lookup(nextID).reopened == false)
411  uniqueNodesExpanded++;
412 
413  if (0)
414  {
415  auto &i = current.Lookup(nextID);
416  std::cout << "Expanding " << i.data << "\n";
417  printf("g: %1.1f, h:%1.1f, pr:%1.1f\n", current.Lookup(nextID).g, current.Lookup(nextID).h, std::max((i.g+i.h), i.g/i.frac));
418  }
419 
420  // decrease count from parent
421  {
422  auto &parentData = current.Lookup(nextID);
423  count[{parentData.g,parentData.h}]--;
424  if (count[{parentData.g,parentData.h}] == 0 && currentCost < DBL_MAX)
425  {
426  recheckPath = true;
427  }
428  }
429 
430  env->GetSuccessors(current.Lookup(nextID).data, neighbors);
431  for (auto &succ : neighbors)
432  {
433  nodesTouched++;
434  uint64_t childID;
435  uint64_t hash = env->GetStateHash(succ);
436  auto loc = current.Lookup(hash, childID);
437  auto &childData = current.Lookup(childID);
438  auto &parentData = current.Lookup(nextID);
439 
440  double edgeCost = env->GCost(parentData.data, succ);
441  switch (loc)
442  {
443  case kClosedList: // ignore
444  if (fless(parentData.g+edgeCost, childData.g))
445  {
446  childData.h = std::max(childData.h, parentData.h-edgeCost);
447  childData.parentID = nextID;
448  childData.g = parentData.g+edgeCost;
449  count[{childData.g,childData.h}]++;
450  dist[{childData.g,childData.h}]++;
451  current.Reopen(childID);
452  }
453  break;
454  case kOpenList: // update cost if needed
455  {
456  // 1-step BPMX
457  parentData.h = std::max(childData.h-edgeCost, parentData.h);
458 
459  if (fgreater(parentData.h-edgeCost, childData.h))
460  {
461  count[{childData.g,childData.h}]--;
462  dist[{childData.g,childData.h}]--;
463  //minf[childData.g+childData.h]--;
464  childData.h = parentData.h-edgeCost;
465  //minf[childData.g+childData.h]++;
466  count[{childData.g,childData.h}]++;
467  dist[{childData.g,childData.h}]++;
468  }
469  if (fless(parentData.g+edgeCost, childData.g))
470  {
471  count[{childData.g,childData.h}]--;
472  dist[{childData.g,childData.h}]--;
473  childData.parentID = nextID;
474  childData.g = parentData.g+edgeCost;
475  current.KeyChanged(childID);
476  count[{childData.g,childData.h}]++;
477  dist[{childData.g,childData.h}]++;
478 
479 
480  // TODO: check if we improved the current solution?
481  uint64_t reverseLoc;
482  auto loc = opposite.Lookup(hash, reverseLoc);
483  if (loc == kOpenList)
484  {
485  if (fless(parentData.g+edgeCost + opposite.Lookup(reverseLoc).g, currentCost))
486  {
487  recheckPath = true;
488  // TODO: store current solution
489  printf("Potential updated solution found, cost: %1.2f + %1.2f = %1.2f\n",
490  parentData.g+edgeCost,
491  opposite.Lookup(reverseLoc).g,
492  parentData.g+edgeCost+opposite.Lookup(reverseLoc).g);
493  currentCost = parentData.g+edgeCost + opposite.Lookup(reverseLoc).g;
494  middleNode = succ;
495  // PrintOpenStats(f);
496  // PrintOpenStats(b);
497  }
498  }
499  }
500  }
501  break;
502  case kNotFound:
503  {
504  double g = parentData.g+edgeCost;
505  double h = std::max(heuristic->HCost(succ, target), parentData.h-edgeCost);
506 
507  // Ignore nodes that don't have lower f-cost than the incumbant solution
508  if (!fless(g+h, currentCost))
509  break;
510  // ming[g]++;
511  // minf[g+h]++;
512  count[{g,h}]++;
513  dist[{g,h}]++;
514  // 1-step BPMX
515  parentData.h = std::max(h-edgeCost, parentData.h);
516 
517  uint64_t i;
518  i = current.AddOpenNode(succ, // This may invalidate our references
519  hash,
520  g,
521  h,
522  nextID);
523  if (&current == &forwardQueue)
524  current.Lookup(i).frac = fraction;
525  else
526  current.Lookup(i).frac = 1-fraction;
527 
528  // check for solution
529  uint64_t reverseLoc;
530  auto loc = opposite.Lookup(hash, reverseLoc);
531  if (loc == kOpenList)
532  {
533  if (fless(current.Lookup(nextID).g+edgeCost + opposite.Lookup(reverseLoc).g, currentCost))
534  {
535  recheckPath = true;
536  // TODO: store current solution
537  printf("Potential solution found, cost: %1.2f + %1.2f = %1.2f\n",
538  current.Lookup(nextID).g+edgeCost,
539  opposite.Lookup(reverseLoc).g,
540  current.Lookup(nextID).g+edgeCost+opposite.Lookup(reverseLoc).g);
541  currentCost = current.Lookup(nextID).g+edgeCost + opposite.Lookup(reverseLoc).g;
542  middleNode = succ;
543  // PrintOpenStats(f);
544  // PrintOpenStats(b);
545  }
546  }
547  }
548  }
549  }
550 }
551 
552 template <class state, class action, class environment, class priorityQueue>
553 uint64_t fMM<state, action, environment, priorityQueue>::GetNecessaryExpansions() const
554 {
555  uint64_t count = 0;
556  for (unsigned int x = 0; x < forwardQueue.size(); x++)
557  {
558  const FMMOpenClosedData<state> &data = forwardQueue.Lookat(x);
559  if ((data.where == kClosedList) && (fless(data.g+data.h, currentCost)) && fless(data.g*2, currentCost))
560  count++;
561  }
562  for (unsigned int x = 0; x < backwardQueue.size(); x++)
563  {
564  const FMMOpenClosedData<state> &data = backwardQueue.Lookat(x);
565  if ((data.where == kClosedList) && (fless(data.g+data.h, currentCost)) && (fless(data.g*2, currentCost)))
566  count++;
567  }
568  return count;
569 }
570 
571 template <class state, class action, class environment, class priorityQueue>
572 void fMM<state, action, environment, priorityQueue>::OpenGLDraw() const
573 {
574  OpenGLDraw(forwardQueue);
575  OpenGLDraw(backwardQueue);
576 }
577 
578 template <class state, class action, class environment, class priorityQueue>
579 void fMM<state, action, environment, priorityQueue>::OpenGLDraw(const priorityQueue &queue) const
580 {
581  double transparency = 0.9;
582  if (queue.size() == 0)
583  return;
584  uint64_t top = -1;
585  // double minf = 1e9, maxf = 0;
586  if (queue.OpenSize() > 0)
587  {
588  top = queue.Peek();
589  }
590  for (unsigned int x = 0; x < queue.size(); x++)
591  {
592  const FMMOpenClosedData<state> &data = queue.Lookat(x);
593  if (x == top)
594  {
595  env->SetColor(1.0, 1.0, 0.0, transparency);
596  env->OpenGLDraw(data.data);
597  }
598  if ((data.where == kOpenList) && (data.reopened))
599  {
600  env->SetColor(0.0, 0.5, 0.5, transparency);
601  env->OpenGLDraw(data.data);
602  }
603  else if (data.where == kOpenList)
604  {
605  env->SetColor(0.0, 1.0, 0.0, transparency);
606  env->OpenGLDraw(data.data);
607  }
608  else if ((data.where == kClosedList) && (data.reopened))
609  {
610  env->SetColor(0.5, 0.0, 0.5, transparency);
611  env->OpenGLDraw(data.data);
612  }
613  else if (data.where == kClosedList)
614  {
615  env->SetColor(1.0, 0.0, 0.0, transparency);
616  env->OpenGLDraw(data.data);
617  }
618  }
619 }
620 
621 template <class state, class action, class environment, class priorityQueue>
622 std::string fMM<state, action, environment, priorityQueue>::SVGDraw() const
623 {
624  std::string s;
625  s += SVGDraw(forwardQueue);
626  s += SVGDraw(backwardQueue);
627  return s;
628 }
629 
630 template <class state, class action, class environment, class priorityQueue>
631 std::string fMM<state, action, environment, priorityQueue>::SVGDraw(const priorityQueue &queue) const
632 {
633  std::string s;
634  double transparency = 1.0;
635  if (queue.size() == 0)
636  return s;
637  uint64_t top = -1;
638 
639  if (queue.OpenSize() > 0)
640  {
641  top = queue.Peek();
642  }
643  for (unsigned int x = 0; x < queue.size(); x++)
644  {
645  const auto &data = queue.Lookat(x);
646 
647  if (x == top)
648  {
649  env->SetColor(1.0, 1.0, 0.0, transparency);
650  s+=env->SVGDraw(data.data);
651  }
652  else if ((data.where == kOpenList) && (data.reopened))
653  {
654  env->SetColor(0.0, 0.5, 0.5, transparency);
655  s+=env->SVGDraw(data.data);
656  }
657  else if (data.where == kOpenList)
658  {
659  env->SetColor(0.0, 1.0, 0.0, transparency);
660  s+=env->SVGDraw(data.data);
661  }
662  else if ((data.where == kClosedList) && (data.reopened))
663  {
664  env->SetColor(0.5, 0.0, 0.5, transparency);
665  s+=env->SVGDraw(data.data);
666  }
667  else if (data.where == kClosedList)
668  {
669  env->SetColor(1.0, 0.0, 0.0, transparency);
670  s+=env->SVGDraw(data.data);
671  }
672  }
673  return s;
674 }
675 
676 
677 template <class state, class action, class environment, class priorityQueue>
678 void fMM<state, action, environment, priorityQueue>::Draw(Graphics::Display &display) const
679 {
680  Draw(display, forwardQueue);
681  Draw(display, backwardQueue);
682 }
683 
684 template <class state, class action, class environment, class priorityQueue>
685 void fMM<state, action, environment, priorityQueue>::Draw(Graphics::Display &display, const priorityQueue &queue) const
686 {
687  double transparency = 0.9;
688  if (queue.size() == 0)
689  return;
690  uint64_t top = -1;
691  // double minf = 1e9, maxf = 0;
692  if (queue.OpenSize() > 0)
693  {
694  top = queue.Peek();
695  }
696  for (unsigned int x = 0; x < queue.size(); x++)
697  {
698  const auto &data = queue.Lookat(x);
699  if (x == top)
700  {
701  env->SetColor(1.0, 1.0, 0.0, transparency);
702  env->Draw(display, data.data);
703  }
704  if ((data.where == kOpenList) && (data.reopened))
705  {
706  env->SetColor(0.0, 0.5, 0.5, transparency);
707  env->Draw(display, data.data);
708  }
709  else if (data.where == kOpenList)
710  {
711  env->SetColor(0.0, 1.0, 0.0, transparency);
712  env->Draw(display,data.data);
713  }
714  else if ((data.where == kClosedList) && (data.reopened))
715  {
716  env->SetColor(0.5, 0.0, 0.5, transparency);
717  env->Draw(display,data.data);
718  }
719  else if (data.where == kClosedList)
720  {
721  env->SetColor(1.0, 0.0, 0.0, transparency);
722  env->Draw(display, data.data);
723  }
724  }
725 }
726 
727 
728 #endif /* FMM_h */
fMM::middleNode
state middleNode
Definition: fMM.h:169
fMM::Draw
void Draw(Graphics::Display &display) const
Definition: fMM.h:678
fMM::t
Timer t
Definition: fMM.h:178
std::hash< std::pair< double, double > >::operator()
size_t operator()(const std::pair< double, double > &x) const
Definition: fMM.h:55
fMM::goal
state goal
Definition: fMM.h:165
fMM::fraction
double fraction
Definition: fMM.h:174
fMM::GetBackwardItem
const FMMOpenClosedData< state > & GetBackwardItem(unsigned int which)
Definition: fMM.h:82
min
double min(double a, double b)
Definition: FPUtil.h:35
fMM::backwardQueue
priorityQueue backwardQueue
Definition: fMM.h:164
fMM::GetUniqueNodesExpanded
uint64_t GetUniqueNodesExpanded() const
Definition: fMM.h:84
Heuristic
Definition: Heuristic.h:30
FMMOpenClosedData::openLocation
uint64_t openLocation
Definition: fMM.h:28
FMMOpenClosedData::where
dataLocation where
Definition: fMM.h:30
fMM::nodesExpanded
uint64_t nodesExpanded
Definition: fMM.h:168
fMM::PrintOpenStats
void PrintOpenStats(std::unordered_map< std::pair< double, double >, int > &s)
Definition: fMM.h:113
fMM::neighbors
std::vector< state > neighbors
Definition: fMM.h:176
if
if(state==GLUT_DOWN)
Definition: GLUThog.cpp:244
AStarOpenClosed.h
d
mcData d[]
Definition: MotionCaptureMovement.cpp:21
FMMOpenClosedData::reopened
bool reopened
Definition: fMM.h:29
fMMCompare::operator()
bool operator()(const FMMOpenClosedData< state > &i1, const FMMOpenClosedData< state > &i2) const
Definition: fMM.h:36
fMM::currentCost
double currentCost
Definition: fMM.h:170
FPUtil.h
fMM::ExtractPathToGoal
void ExtractPathToGoal(state &node, std::vector< state > &thePath)
Definition: fMM.h:131
FMMOpenClosedData::h
double h
Definition: fMM.h:25
fMM::GetName
virtual const char * GetName()
Definition: fMM.h:75
fMM::GetNumForwardItems
const int GetNumForwardItems()
Definition: fMM.h:79
fMM::ExtractPathToStartFromID
void ExtractPathToStartFromID(uint64_t node, std::vector< state > &thePath)
Definition: fMM.h:144
FMMOpenClosedData::frac
double frac
Definition: fMM.h:26
fMM::start
state start
Definition: fMM.h:165
FMMOpenClosedData::FMMOpenClosedData
FMMOpenClosedData(const state &theData, double gCost, double hCost, uint64_t parent, uint64_t openLoc, dataLocation location)
Definition: fMM.h:21
fMM::~fMM
virtual ~fMM()
Definition: fMM.h:68
FMMOpenClosedData::parentID
uint64_t parentID
Definition: fMM.h:27
fMM::oldp2
double oldp2
Definition: fMM.h:183
fMM::epsilon
double epsilon
Definition: fMM.h:173
fMM::forwardQueue
priorityQueue forwardQueue
Definition: fMM.h:164
fMMCompare
Definition: fMM.h:35
Timer.h
fMM::PrintHDist
void PrintHDist()
Definition: fMM.h:93
fMM::oldp1
double oldp1
Definition: fMM.h:182
fMM::SetFraction
void SetFraction(double frac)
Definition: fMM.h:67
fMM::dist
std::unordered_map< std::pair< double, double >, int > dist
Definition: fMM.h:166
fMM::InitializeSearch
bool InitializeSearch(environment *env, const state &from, const state &to, Heuristic< state > *forward, Heuristic< state > *backward, std::vector< state > &thePath)
Definition: fMM.h:199
loc
Definition: MapGenerators.cpp:296
fMM::backwardHeuristic
Heuristic< state > * backwardHeuristic
Definition: fMM.h:180
fMM::GetNodesTouched
uint64_t GetNodesTouched() const
Definition: fMM.h:86
Graphics::Display
Definition: Graphics.h:146
fless
bool fless(double a, double b)
Definition: FPUtil.h:28
fMM::GetNodesExpanded
uint64_t GetNodesExpanded() const
Definition: fMM.h:85
fMM::GetForwardItem
const FMMOpenClosedData< state > & GetForwardItem(unsigned int which)
Definition: fMM.h:80
fMM::GetNumBackwardItems
const int GetNumBackwardItems()
Definition: fMM.h:81
dataLocation
dataLocation
Definition: AStarOpenClosed.h:27
Heuristic::HCost
virtual double HCost(const state &a, const state &b) const
Definition: Heuristic.h:73
fMM::recheckPath
bool recheckPath
Definition: fMM.h:184
fMM::lastMinBackwardG
double lastMinBackwardG
Definition: fMM.h:172
fMM::forwardHeuristic
Heuristic< state > * forwardHeuristic
Definition: fMM.h:179
fMM::SVGDraw
std::string SVGDraw() const
Definition: fMM.h:622
fMM::b
std::unordered_map< std::pair< double, double >, int > b
Definition: fMM.h:167
fgreater
bool fgreater(double a, double b)
Definition: FPUtil.h:29
max
#define max(a, b)
Definition: MinimalSectorAbstraction.cpp:40
FMMOpenClosedData
Definition: fMM.h:18
kOpenList
@ kOpenList
Definition: AStarOpenClosed.h:28
fMM::Expand
void Expand(priorityQueue &current, priorityQueue &opposite, Heuristic< state > *heuristic, const state &target, std::unordered_map< std::pair< double, double >, int > &count)
Definition: fMM.h:403
FMMOpenClosedData::FMMOpenClosedData
FMMOpenClosedData()
Definition: fMM.h:20
fMM::ExtractPathToStart
void ExtractPathToStart(state &node, std::vector< state > &thePath)
Definition: fMM.h:142
fMM::f
std::unordered_map< std::pair< double, double >, int > f
Definition: fMM.h:167
fMM::OpenGLDraw
void OpenGLDraw() const
Definition: fMM.h:572
std
Definition: CanonicalGraphEnvironment.h:26
FMMOpenClosedData::g
double g
Definition: fMM.h:24
fMM::lastMinForwardG
double lastMinForwardG
Definition: fMM.h:171
Timer
Definition: Timer.h:19
kNotFound
@ kNotFound
Definition: AStarOpenClosed.h:30
fMM::fMM
fMM(double epsilon=1.0)
Definition: fMM.h:66
fMM::env
environment * env
Definition: fMM.h:177
fMM::DoSingleSearchStep
bool DoSingleSearchStep(std::vector< state > &thePath)
Definition: fMM.h:233
FMMOpenClosedData::data
state data
Definition: fMM.h:23
fMM::ResetNodeCount
void ResetNodeCount()
Definition: fMM.h:77
fMM::GetPath
void GetPath(environment *env, const state &from, const state &to, Heuristic< state > *forward, Heuristic< state > *backward, std::vector< state > &thePath)
Definition: fMM.h:188
fMM::ExtractPathToGoalFromID
void ExtractPathToGoalFromID(uint64_t node, std::vector< state > &thePath)
Definition: fMM.h:133
fequal
bool fequal(double a, double b, double tolerance=TOLERANCE)
Definition: FPUtil.h:32
fMM
Definition: fMM.h:64
kClosedList
@ kClosedList
Definition: AStarOpenClosed.h:29
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
fMM::GetNecessaryExpansions
uint64_t GetNecessaryExpansions() const
Definition: fMM.h:553
fMM::uniqueNodesExpanded
uint64_t uniqueNodesExpanded
Definition: fMM.h:168
Graphics::Display::data
Definition: Graphics.h:226
fMM::nodesTouched
uint64_t nodesTouched
Definition: fMM.h:168
Generated by   doxygen 1.8.17