HOG2
SFIDAStar.h
Go to the documentation of this file.
1 /*
2  * SFIDAStar.h
3  * hog2
4  *
5  * Created by Nathan Sturtevant on 1/16/10.
6  * Copyright 2010 NS Software. All rights reserved.
7  *
8  */
9 
10 #ifndef SFIDASTAR_H
11 #define SFIDASTAR_H
12 
13 #include <iostream>
14 #include "SearchEnvironment.h"
15 #include <unordered_map>
16 #include "FPUtil.h"
17 
18 typedef std::unordered_map<uint64_t, double> NodeHashTable;
19 
20 template <class state, class action>
21 class SFIDAStar {
22 public:
23  SFIDAStar() { }
24  virtual ~SFIDAStar() {}
25  void GetPath(SearchEnvironment<state, action> *env, state from, state to,
26  std::vector<state> &thePath);
27  void GetPath(SearchEnvironment<state, action> *env, state from, state to,
28  std::vector<action> &thePath);
29 
30  uint64_t GetNodesExpanded() { return nodesExpanded; }
31  uint64_t GetNodesTouched() { return nodesTouched; }
32  void ResetNodeCount() { nodesExpanded = nodesTouched = 0; }
33 private:
34  unsigned long nodesExpanded, nodesTouched;
35 
36  double DoIteration(SearchEnvironment<state, action> *env,
37  state &currState, state &goalState,
38  action forbiddenForwardAction, bool validForward,
39  action forbiddenBackAction, bool validBack,
40  std::vector<action> &thePath, double bound, double g);
41  bool ShouldSearchForward1(SearchEnvironment<state, action> *env, state &currState, state &goalState,
42  double gCost, double bound);
43  bool ShouldSearchForward2(SearchEnvironment<state, action> *env, state &currState, state &goalState,
44  double gCost, double bound);
45 
46  void UpdateNextBound(double currBound, double fCost);
47  double nextBound;
48 };
49 
50 template <class state, class action>
51 void SFIDAStar<state, action>::GetPath(SearchEnvironment<state, action> *env,
52  state from, state to,
53  std::vector<state> &thePath)
54 {
55  assert(!"Not implemented -- too expensive for large state reps.");
56 }
57 
58 template <class state, class action>
59 void SFIDAStar<state, action>::GetPath(SearchEnvironment<state, action> *env,
60  state from, state to,
61  std::vector<action> &thePath)
62 {
63  nextBound = 0;
64  nodesExpanded = nodesTouched = 0;
65  thePath.resize(0);
66  UpdateNextBound(0, env->HCost(from, to));
67  std::vector<action> act;
68  env->GetActions(from, act);
69  while (thePath.size() == 0)
70  {
71 // printf("Starting iteration with bound %f\n", nextBound);
72  DoIteration(env, from, to, act[0], false, act[0], false, thePath, nextBound, 0);
73  }
74 }
75 
76 template <class state, class action>
77 double SFIDAStar<state, action>::DoIteration(SearchEnvironment<state, action> *env,
78  state &currState, state &goalState,
79  action forbiddenForwardAction, bool validForward,
80  action forbiddenBackAction, bool validBack,
81  std::vector<action> &thePath, double bound, double g)
82 {
83  nodesExpanded++;
84  double h = env->HCost(currState, goalState);
85 
86  if (fgreater(g+h, bound))
87  {
88  UpdateNextBound(bound, g+h);
89  //printf("Stopping at (%d, %d). g=%f h=%f\n", currState>>16, currState&0xFFFF, g, h);
90  return h;
91  }
92  if (env->GoalTest(currState, goalState))
93  return -1; // found goal
94 
95  if (ShouldSearchForward1(env, currState, goalState, g, bound))
96  {
97  std::vector<action> actions;
98  env->GetActions(currState, actions);
99  nodesTouched += actions.size();
100 
101  for (unsigned int x = 0; x < actions.size(); x++)
102  {
103  if (validForward && (actions[x] == forbiddenForwardAction))
104  continue;
105 
106  thePath.push_back(actions[x]);
107 
108  double edgeCost = env->GCost(currState, actions[x]);
109  env->ApplyAction(currState, actions[x]);
110  env->InvertAction(actions[x]);
111  double childH = DoIteration(env, currState, goalState,
112  actions[x], true,
113  forbiddenBackAction, validBack,
114  thePath,
115  bound, g+edgeCost);
116  env->ApplyAction(currState, actions[x]);
117  if (fequal(childH, -1)) // found goal
118  return -1;
119 
120  thePath.pop_back();
121  }
122  }
123  else {
124  std::vector<action> actions;
125  env->GetActions(goalState, actions);
126  nodesTouched += actions.size();
127 
128  for (unsigned int x = 0; x < actions.size(); x++)
129  {
130  if (validBack && (actions[x] == forbiddenBackAction))
131  continue;
132 
133  thePath.push_back(actions[x]);
134 
135  double edgeCost = env->GCost(goalState, actions[x]);
136  env->ApplyAction(goalState, actions[x]);
137  env->InvertAction(actions[x]);
138  double childH = DoIteration(env, currState, goalState,
139  forbiddenForwardAction, validForward,
140  actions[x], true,
141  thePath,
142  bound, g+edgeCost);
143  env->ApplyAction(goalState, actions[x]);
144  if (fequal(childH, -1)) // found goal
145  return -1;
146 
147  thePath.pop_back();
148  }
149  }
150  return h;
151 }
152 
153 template <class state, class action>
154 bool SFIDAStar<state, action>::ShouldSearchForward1(SearchEnvironment<state, action> *env, state &currState, state &goalState,
155  double gCost, double bound)
156 {
157  std::vector<action> actions;
158 // std::vector<action> actions2;
159 // double base = env->HCost(currState, goalState);
160  //double limit = bound-(gCost+base);
161  int forward = 0;
162  int backward = 0;
163 
164  forward = env->GetNumSuccessors(currState);
165  // env->GetActions(currState, actions);
166 // forward+=actions.size();
167 // for (unsigned int x = 0; x < actions.size(); x++)
168 // {
169 // env->ApplyAction(currState, actions[x]);
170 // env->InvertAction(actions[x]);
171 //
172 // if (gCost + env->GCost(currState, actions[x]) + env->HCost(currState, goalState) > bound)
173 // forward+=2;
174 // if (gCost + env->GCost(currState, actions[x]) + env->HCost(currState, goalState) == bound)
175 // forward++;
176 // env->ApplyAction(currState, actions[x]);
177 // }
178 
179 // env->GetActions(goalState, actions);
180 // backward += actions.size();
181  backward = env->GetNumSuccessors(goalState);
182 // for (unsigned int x = 0; x < actions.size(); x++)
183 // {
184 // env->ApplyAction(goalState, actions[x]);
185 // env->InvertAction(actions[x]);
186 //
187 // if (gCost + env->GCost(goalState, actions[x]) + env->HCost(goalState, currState) > bound)
188 // backward+=2;
189 // if (gCost + env->GCost(goalState, actions[x]) + env->HCost(goalState, currState) == bound)
190 // backward+=1;
191 // env->ApplyAction(goalState, actions[x]);
192 // }
193  if (forward < backward)
194  return true;
195  return false;
196 }
197 
198 template <class state, class action>
199 bool SFIDAStar<state, action>::ShouldSearchForward2(SearchEnvironment<state, action> *env, state &currState, state &goalState,
200  double gCost, double bound)
201 {
202  std::vector<action> actions;
203  std::vector<action> actions2;
204 // double base = env->HCost(currState, goalState);
205  //double limit = bound-(gCost+base);
206  int forward = 0;
207  int backward = 0;
208 
209  env->GetActions(currState, actions);
210  for (unsigned int x = 0; x < actions.size(); x++)
211  {
212  env->ApplyAction(currState, actions[x]);
213  env->InvertAction(actions[x]);
214 
215  double firstCost = env->GCost(currState, actions[x]);
216  env->GetActions(currState, actions2);
217  for (unsigned int y = 0; y < actions2.size(); y++)
218  {
219  if (actions[x] == actions2[y]) continue;
220 
221  env->ApplyAction(currState, actions2[y]);
222  if (firstCost + gCost + env->GCost(currState, actions[y]) + env->HCost(currState, goalState) > bound)
223  forward+=2;
224  if (firstCost + gCost + env->GCost(currState, actions[y]) + env->HCost(currState, goalState) == bound)
225  forward++;
226  env->InvertAction(actions2[y]);
227  env->ApplyAction(currState, actions2[y]);
228  }
229  env->ApplyAction(currState, actions[x]);
230  }
231 
232  env->GetActions(goalState, actions);
233  for (unsigned int x = 0; x < actions.size(); x++)
234  {
235  env->ApplyAction(goalState, actions[x]);
236  env->InvertAction(actions[x]);
237 
238  // env->GetActions(goalState, actions2);
239  // for (unsigned int y = 0; y < actions2.size(); y++)
240  // {
241  // if (actions[x] == actions2[y]) continue;
242  //
243  // env->ApplyAction(goalState, actions2[y]);
244  if (gCost + env->GCost(goalState, actions[x]) + env->HCost(goalState, currState) > bound)
245  backward+=2;
246  if (gCost + env->GCost(goalState, actions[x]) + env->HCost(goalState, currState) == bound)
247  backward+=1;
248  // env->InvertAction(actions2[y]);
249  // env->ApplyAction(goalState, actions2[y]);
250  // }
251  env->ApplyAction(goalState, actions[x]);
252  }
253  if (forward > backward)
254  return true;
255  return false;
256 }
257 
258 
259 template <class state, class action>
260 void SFIDAStar<state, action>::UpdateNextBound(double currBound, double fCost)
261 {
262  if (!fgreater(nextBound, currBound))
263  {
264  nextBound = fCost;
265  //printf("Updating next bound to %f\n", nextBound);
266  }
267  else if (fgreater(fCost, currBound) && fless(fCost, nextBound))
268  {
269  nextBound = fCost;
270  //printf("Updating next bound to %f\n", nextBound);
271  }
272 }
273 
274 
275 #endif
276 
277 //template <class state, class action>
278 //class SearchEnvironment {
279 //public:
280 // virtual ~SearchEnvironment() {}
281 // virtual void GetSuccessors(const state &nodeID, std::vector<state> &neighbors) = 0;
282 // virtual void GetActions(const state &nodeID, std::vector<action> &actions) = 0;
283 // virtual action GetAction(state &s1, state &s2) = 0;
284 // virtual void ApplyAction(state &s, action a) = 0;
285 //
286 // virtual double HCost(state &node1, state &node2) = 0;
287 // virtual double GCost(state &node1, state &node2) = 0;
288 // virtual bool GoalTest(state &node, state &goal) = 0;
289 //
290 // virtual uint64_t GetStateHash(const state &node) = 0;
291 // virtual uint64_t GetActionHash(action act) = 0;
292 //};
SFIDAStar::nodesTouched
unsigned long nodesTouched
Definition: SFIDAStar.h:34
SFIDAStar::UpdateNextBound
void UpdateNextBound(double currBound, double fCost)
Definition: SFIDAStar.h:260
SearchEnvironment::InvertAction
virtual bool InvertAction(action &a) const =0
SFIDAStar
Definition: SFIDAStar.h:21
SFIDAStar::GetNodesTouched
uint64_t GetNodesTouched()
Definition: SFIDAStar.h:31
FPUtil.h
SearchEnvironment::GCost
virtual double GCost(const state &node1, const state &node2) const =0
SearchEnvironment::GetNumSuccessors
virtual int GetNumSuccessors(const state &stateID) const
Definition: SearchEnvironment.h:35
SearchEnvironment::GetActions
virtual void GetActions(const state &nodeID, std::vector< action > &actions) const =0
SFIDAStar::GetPath
void GetPath(SearchEnvironment< state, action > *env, state from, state to, std::vector< state > &thePath)
Definition: SFIDAStar.h:51
SFIDAStar::~SFIDAStar
virtual ~SFIDAStar()
Definition: SFIDAStar.h:24
SFIDAStar::nextBound
double nextBound
Definition: SFIDAStar.h:47
NodeHashTable
std::unordered_map< uint64_t, double > NodeHashTable
Definition: SFIDAStar.h:18
fless
bool fless(double a, double b)
Definition: FPUtil.h:28
SFIDAStar::DoIteration
double DoIteration(SearchEnvironment< state, action > *env, state &currState, state &goalState, action forbiddenForwardAction, bool validForward, action forbiddenBackAction, bool validBack, std::vector< action > &thePath, double bound, double g)
Definition: SFIDAStar.h:77
SFIDAStar::GetNodesExpanded
uint64_t GetNodesExpanded()
Definition: SFIDAStar.h:30
SFIDAStar::ResetNodeCount
void ResetNodeCount()
Definition: SFIDAStar.h:32
fgreater
bool fgreater(double a, double b)
Definition: FPUtil.h:29
SFIDAStar::ShouldSearchForward2
bool ShouldSearchForward2(SearchEnvironment< state, action > *env, state &currState, state &goalState, double gCost, double bound)
Definition: SFIDAStar.h:199
SFIDAStar::nodesExpanded
unsigned long nodesExpanded
Definition: SFIDAStar.h:34
SearchEnvironment::HCost
virtual double HCost(const state &node1, const state &node2) const =0
Heuristic value between two arbitrary nodes.
SearchEnvironment::ApplyAction
virtual void ApplyAction(state &s, action a) const =0
SFIDAStar::SFIDAStar
SFIDAStar()
Definition: SFIDAStar.h:23
SearchEnvironment::GoalTest
virtual bool GoalTest(const state &node, const state &goal) const =0
fequal
bool fequal(double a, double b, double tolerance=TOLERANCE)
Definition: FPUtil.h:32
SearchEnvironment
Definition: SearchEnvironment.h:30
SFIDAStar::ShouldSearchForward1
bool ShouldSearchForward1(SearchEnvironment< state, action > *env, state &currState, state &goalState, double gCost, double bound)
Definition: SFIDAStar.h:154
SearchEnvironment.h
Generated by   doxygen 1.8.17