HOG2
CanonicalGraphEnvironment.cpp
Go to the documentation of this file.
1 //
2 // CanonicalGraphEnvironment.cpp
3 // hog2 glut
4 //
5 // Created by Nathan Sturtevant on 4/11/16.
6 // Copyright © 2016 University of Denver. All rights reserved.
7 //
8 
9 #include "CanonicalGraphEnvironment.h"
10 
11 
12 bool operator==(const canGraphState &s1, const canGraphState &s2)
13 {
14  //return (s1.parent == s2.parent) && (s1.s == s2.s);
15  return (s1.s == s2.s);
16 }
17 
18 std::ostream &operator<<(std::ostream &out, const canGraphState &s1)
19 {
20  out << s1.s << "(p: " << s1.parent << ")";
21 
22  return out;
23 }
24 
25 
26 CanonicalGraphEnvironment::CanonicalGraphEnvironment(Graph *g)
27 :g(g)
28 {
29  directed = false;
30  drawEdgeCosts = false;
31  drawNodeLabels = false;
32  nodeScale = 1.0;
33  ComputeOrdering();
34 }
35 
36 CanonicalGraphEnvironment::~CanonicalGraphEnvironment()
37 {
38 
39 }
40 
41 void CanonicalGraphEnvironment::GetSuccessors(const canGraphState &stateID, std::vector<canGraphState> &neighbors) const
42 {
43  neighbors.clear();
44  //std::cout << "Successors of " << stateID << " are ";
45 
46  if (stateID.parent == stateID.s) // initial state
47  {
48  node *n = g->GetNode(stateID.parent);
49  for (int e = 0; e < n->GetNumEdges(); e++)
50  {
51  //n->getEdge(e)->setMarked(true);
52  if (n->getEdge(e)->getFrom() == stateID.parent)
53  {
54  neighbors.push_back({stateID.parent, n->getEdge(e)->getTo()});
55  //std::cout << neighbors.back() << " ";
56  }
57  else {
58  neighbors.push_back({stateID.parent, n->getEdge(e)->getFrom()});
59  //std::cout << neighbors.back() << " ";
60  }
61  }
62  }
63  else {
64  auto i = canonicalOrdering.find(stateID);
65  if (i != canonicalOrdering.end())
66  {
67  for (int x = 0; x < 8; x++)
68  {
69  if (i->second&(1<<x))
70  {
71  // add {i->first.second, x} to edges
72  node *n = g->GetNode(i->first.s);
73  edge *e = n->getEdge(x);
74 
75  if (e->getFrom() == n->GetNum())
76  {
77  neighbors.push_back({i->first.s, e->getTo()});
78  //std::cout << neighbors.back() << " ";
79  }
80  else {
81  neighbors.push_back({i->first.s, e->getFrom()});
82  //std::cout << neighbors.back() << " ";
83  }
84  }
85  }
86  }
87  else {
88  //printf(" NOT FOUND ");
89  }
90  }
91  //std::cout << "\n";
92 }
93 
94 //int CanonicalGraphEnvironment::GetNumSuccessors(const canGraphState &stateID) const
95 //{
96 //
97 //}
98 
99 void CanonicalGraphEnvironment::GetActions(const canGraphState &stateID, std::vector<graphMove> &actions) const
100 {
101  assert(false);
102 }
103 
104 graphMove CanonicalGraphEnvironment::GetAction(const canGraphState &s1, const canGraphState &s2) const
105 {
106  assert(false);
107  return {0,0};
108 }
109 
110 void CanonicalGraphEnvironment::ApplyAction(canGraphState &s, graphMove a) const
111 {
112  // cannot apply canonical action
113  assert(false);
114 }
115 
116 bool CanonicalGraphEnvironment::InvertAction(graphMove &a) const
117 {
118  uint32_t tmp = a.from;
119  a.from = a.to;
120  a.to = tmp;
121  if (g->findDirectedEdge(a.from, a.to))
122  return true;
123  return false;
124 }
125 
126 
127 double CanonicalGraphEnvironment::HCost(const canGraphState &state1, const canGraphState &state2) const
128 {
129  return 0;
130 }
131 
132 double CanonicalGraphEnvironment::GCost(const canGraphState &state1, const canGraphState &state2) const
133 {
134  edge *e = g->FindEdge(state1.s, state2.s);
135  assert(e);
136  return e->GetWeight();
137 }
138 
139 double CanonicalGraphEnvironment::GCost(const canGraphState &state1, const graphMove &state2) const
140 {
141  edge *e = g->FindEdge(state2.from, state2.to);
142  assert(e);
143  return e->GetWeight();
144 }
145 
146 bool CanonicalGraphEnvironment::GoalTest(const canGraphState &state, const canGraphState &goal) const
147 {
148  return state.s == goal.s;
149 }
150 
151 uint64_t CanonicalGraphEnvironment::GetMaxHash() const
152 {
153  return g->GetNumNodes();
154 }
155 
156 uint64_t CanonicalGraphEnvironment::GetStateHash(const canGraphState &state) const
157 {
158  return g->GetNode(state.s)->GetNum();
159 }
160 
161 uint64_t CanonicalGraphEnvironment::GetActionHash(graphMove act) const
162 {
163  return (g->GetNode(act.from)->getUniqueID()<<16)|
164  (g->GetNode(act.to)->getUniqueID());
165 }
166 
167 void CanonicalGraphEnvironment::OpenGLDraw() const
168 {
169  // Hittable background behind graph
170  glBegin(GL_TRIANGLE_FAN);
171  glColor4f(0, 0, 0, 0.01);
172  glVertex3f(-1, -1, 0.01);
173  glVertex3f(1, -1, 0.01);
174  glVertex3f(1, 1, 0.01);
175  glVertex3f(-1, 1, 0.01);
176  glEnd();
177  glBegin(GL_LINES);
178  glNormal3f(0, 1, 0);
179 
180  GLdouble off = 0;
181  edge_iterator ei = g->getEdgeIter();
182  for (edge *e = g->edgeIterNext(ei); e; e = g->edgeIterNext(ei))
183  {
184  //int x, y;
185  //double offsetx, offsety;
186  node *n;
187  n = g->GetNode(e->getFrom());
188 
189  {
190  GLfloat r, g, b, t;
191  GetColor(r, g, b, t);
192  glColor4f(r, g, b, t);
193  }
194  if (e->getMarked())
195  {
196  glColor3f(1.0, 0.0, 0.0);
197  off = -0.01;
198  }
199  else {
200  off = 0;
201  }
202 
203  GLdouble x, y, z;
204  x = n->GetLabelF(GraphSearchConstants::kXCoordinate);
205  y = n->GetLabelF(GraphSearchConstants::kYCoordinate);
206  z = n->GetLabelF(GraphSearchConstants::kZCoordinate);
207  glVertex3f(x, y, z+off);
208 
209  n = g->GetNode(e->getTo());
210  x = n->GetLabelF(GraphSearchConstants::kXCoordinate);
211  y = n->GetLabelF(GraphSearchConstants::kYCoordinate);
212  z = n->GetLabelF(GraphSearchConstants::kZCoordinate);
213 
214  glVertex3f(x, y, z+off);
215  }
216  glEnd();
217 
218  char label[5];
219  if (drawEdgeCosts)
220  {
221  glLineWidth(3.0);
222  glColor4f(0.0, 0.0, 0.0, 1.0);
223  edge_iterator ei = g->getEdgeIter();
224  for (edge *e = g->edgeIterNext(ei); e; e = g->edgeIterNext(ei))
225  {
226  sprintf(label, "%1.2f", e->GetWeight());
227  node *n;
228  n = g->GetNode(e->getFrom());
229 
230  GLdouble x, y, z;
231  x = n->GetLabelF(GraphSearchConstants::kXCoordinate);
232  y = n->GetLabelF(GraphSearchConstants::kYCoordinate);
233  z = n->GetLabelF(GraphSearchConstants::kZCoordinate);
234 
235  n = g->GetNode(e->getTo());
236  x += n->GetLabelF(GraphSearchConstants::kXCoordinate);
237  y += n->GetLabelF(GraphSearchConstants::kYCoordinate);
238  z += n->GetLabelF(GraphSearchConstants::kZCoordinate);
239 
240  DrawText(x/2, y/2, z/2-0.003, 0.2, label);
241  }
242  glLineWidth(1.0);
243  }
244  if (drawNodeLabels)
245  {
246  glLineWidth(3.0);
247  glColor4f(0.0, 0.0, 0.0, 1.0);
248  for (int x = 0; x < g->GetNumNodes(); x++)
249  {
250  node *n = g->GetNode(x);
251  DrawTextCentered(n->GetLabelF(GraphSearchConstants::kXCoordinate),
252  n->GetLabelF(GraphSearchConstants::kYCoordinate),
253  n->GetLabelF(GraphSearchConstants::kZCoordinate)-0.003,
254  0.2, n->GetName());
255  }
256  glLineWidth(1.0);
257  }
258 }
259 
260 void CanonicalGraphEnvironment::OpenGLDraw(const canGraphState &cs) const
261 {
262  graphState s = cs.s;
263 
264  GLfloat r, gr, b, t;
265  GetColor(r, gr, b, t);
266  glColor4f(r, gr, b, t);
267 
268 
269  node *n = g->GetNode(s);
270  GLdouble x, y, z, rad;
271  x = (GLdouble)n->GetLabelF(GraphSearchConstants::kXCoordinate);
272  y = (GLdouble)n->GetLabelF(GraphSearchConstants::kYCoordinate);
273  z = (GLdouble)n->GetLabelF(GraphSearchConstants::kZCoordinate);
274  //rad = 20*(GLdouble)0.4/(g->GetNumNodes());
275  rad = nodeScale*(GLdouble)0.4/(g->GetNumNodes());
276  DrawSquare(x, y, z-0.002, rad);
277  glLineWidth(2.0);
278  // if (r+gr+b < 1.5)
279  // glColor4f(1, 1, 1, t);
280  // else
281  glColor4f(0, 0, 0, t);
282  OutlineRect(x-rad, y-rad, x+rad, y+rad, z-0.0021);
283  glLineWidth(1.0);
284 }
285 
286 void CanonicalGraphEnvironment::OpenGLDraw(const canGraphState &s, const graphMove &gm) const
287 {
288 
289 }
290 
291 void CanonicalGraphEnvironment::GLDrawLine(const canGraphState &f, const canGraphState &t) const
292 {
293  graphState from = f.s;
294  graphState to = f.s;
295  {
296  GLfloat r, g, b, t;
297  GetColor(r, g, b, t);
298  glColor4f(r, g, b, t);
299  }
300 
301  glBegin(GL_LINES);
302 
303  GLdouble x, y, z;
304 
305  node *n = g->GetNode(from);
306  x = n->GetLabelF(GraphSearchConstants::kXCoordinate);
307  y = n->GetLabelF(GraphSearchConstants::kYCoordinate);
308  z = n->GetLabelF(GraphSearchConstants::kZCoordinate);
309  glVertex3f(x, y, z);
310 
311  n = g->GetNode(to);
312  x = n->GetLabelF(GraphSearchConstants::kXCoordinate);
313  y = n->GetLabelF(GraphSearchConstants::kYCoordinate);
314  z = n->GetLabelF(GraphSearchConstants::kZCoordinate);
315  glVertex3f(x, y, z);
316 
317  glEnd();
318 }
319 
320 void CanonicalGraphEnvironment::ComputeOrdering()
321 {
322  GraphEnvironment ge(g);
323  TemplateAStar<graphState, graphMove, GraphEnvironment> astar;
324  std::vector<graphState> thePath;
325  canonicalOrdering.clear();
326  double maxEdge = 0;
327  edge_iterator ei = g->getEdgeIter();
328  while (1)
329  {
330  edge *e = (edge *)g->edgeIterNext(ei);
331  if (!e)
332  break;
333  if (e->GetWeight() > maxEdge)
334  maxEdge = e->GetWeight();
335  }
336 
337  for (graphState searchStart = 0; searchStart < g->GetNumNodes(); searchStart++)
338  {
339 // printf("Analyzing from node %s\n", g->GetNode(searchStart)->GetName());
340  double currentBound;
341  astar.SetStopAfterGoal(false);
342  astar.InitializeSearch(&ge, searchStart, searchStart, thePath);
343  while (astar.GetNumOpenItems() > 0)
344  {
345  graphState next = astar.CheckNextNode();
346  astar.GetOpenListGCost(next, currentBound);
347  if (fgreater(currentBound, 2*maxEdge))
348  {
349  break;
350  }
351  else {
352  astar.DoSingleSearchStep(thePath);
353  }
354  }
355  node *searchStartNode = g->GetNode(searchStart);
356  for (int startEdge = 0; startEdge < searchStartNode->GetNumEdges(); startEdge++)
357  {
358  // for all neighbors, get the outgoing moves that are on optimal paths.
359  node *canonicalNode;
360  edge *e = searchStartNode->getEdge(startEdge);
361  if (e->getFrom() == searchStart)
362  canonicalNode = g->GetNode(e->getTo());
363  else
364  canonicalNode = g->GetNode(e->getFrom());
365  if (astar.GetParent(canonicalNode->GetNum()) != searchStart)
366  continue;
367 
368 
369  AStarOpenClosedDataWithF<graphState> parent;
370  astar.GetClosedItem(canonicalNode->GetNum(), parent);
371 
372 // printf(" Checking neighbor %s cost %f\n", canonicalNode->GetName(), parent.g);
373  for (int z = 0; z < canonicalNode->GetNumEdges(); z++)
374  {
375  node *finalNode;
376  edge *e = canonicalNode->getEdge(z);
377  if (e->getFrom() == canonicalNode->GetNum())
378  finalNode = g->GetNode(e->getTo());
379  else
380  finalNode = g->GetNode(e->getFrom());
381  AStarOpenClosedDataWithF<graphState> child;
382  astar.GetClosedItem(finalNode->GetNum(), child);
383  graphState destNum = finalNode->GetNum();
384 // printf(" --Parent of %s is %s ", finalNode->GetName(), g->GetNode(astar.GetParent(destNum))->GetName());
385  if (astar.GetParent(destNum) == canonicalNode->GetNum())
386  {
387 // printf("(adding)\n");
388  canGraphState p = {searchStart, canonicalNode->GetNum()};
389  canonicalOrdering[p] |= 1<<z;
390  }
391  else {
392 // printf("(not adding)\n");
393  }
394  }
395  }
396  }
397  printf("%lu edges stored (of %d max)\n", canonicalOrdering.size(), g->GetNumEdges()*2);
398  if (0)
399  {
400  for (auto &p : canonicalOrdering)
401  {
402  printf("Going from %s to %s, successors: ", g->GetNode(p.first.parent)->GetName(), g->GetNode(p.first.s)->GetName());
403  //std::cout << p.first;
404  for (int x = 0; x < 8; x++)
405  {
406  if (p.second&(1<<x))
407  {
408  edge *e = g->GetNode(p.first.s)->getEdge(x);
409  node *n;
410  if (e->getFrom() == p.first.s)
411  n = g->GetNode(e->getTo());
412  else
413  n = g->GetNode(e->getFrom());
414  printf("%s ", n->GetName());
415  }
416  }
417  printf("\n");
418  }
419  }
420 }
edge::GetWeight
double GetWeight()
Definition: Graph.h:140
OutlineRect
void OutlineRect(GLdouble left, GLdouble top, GLdouble right, GLdouble bottom, double zz)
Definition: GLUtil.cpp:384
DrawSquare
void DrawSquare(GLdouble xx, GLdouble yy, GLdouble zz, GLdouble rad)
Definition: GLUtil.cpp:396
graphMove
Definition: GraphEnvironment.h:34
edge::getFrom
unsigned int getFrom() const
Definition: Graph.h:146
CanonicalGraphEnvironment::GetActionHash
uint64_t GetActionHash(graphMove act) const
Definition: CanonicalGraphEnvironment.cpp:161
edge::getTo
unsigned int getTo() const
Definition: Graph.h:147
Graph::FindEdge
edge * FindEdge(unsigned int from, unsigned int to)
Finds an edge between nodes with ids from and to, no matter which direction.
Definition: Graph.cpp:230
TemplateAStar::SetStopAfterGoal
void SetStopAfterGoal(bool val)
Definition: TemplateAStar.h:139
graphState
unsigned long graphState
Definition: GraphEnvironment.h:32
CanonicalGraphEnvironment::GetStateHash
uint64_t GetStateHash(const canGraphState &state) const
Definition: CanonicalGraphEnvironment.cpp:156
TemplateAStar::GetOpenListGCost
bool GetOpenListGCost(const state &val, double &gCost) const
Definition: TemplateAStar.h:647
graphMove::from
uint32_t from
Definition: GraphEnvironment.h:38
CanonicalGraphEnvironment::InvertAction
bool InvertAction(graphMove &a) const
Definition: CanonicalGraphEnvironment.cpp:116
AStarOpenClosedDataWithF
Definition: AStarOpenClosed.h:36
CanonicalGraphEnvironment::GCost
double GCost(const canGraphState &state1, const canGraphState &state2) const
Definition: CanonicalGraphEnvironment.cpp:132
edge_iterator
std::vector< edge * >::const_iterator edge_iterator
Definition: Graph.h:30
TemplateAStar::CheckNextNode
state CheckNextNode()
Returns the next state on the open list (but doesn't pop it off the queue).
Definition: TemplateAStar.h:497
GraphSearchConstants::kXCoordinate
@ kXCoordinate
Definition: GraphEnvironment.h:51
Graph
A generic Graph class.
Definition: Graph.h:66
canGraphState::s
graphState s
Definition: CanonicalGraphEnvironment.h:20
TemplateAStar::GetNumOpenItems
unsigned int GetNumOpenItems()
Definition: TemplateAStar.h:113
Graph::GetNode
node * GetNode(unsigned long num)
Definition: Graph.cpp:152
node::getEdge
edge * getEdge(unsigned int which)
Definition: Graph.cpp:794
Graph::edgeIterNext
edge * edgeIterNext(edge_iterator &) const
Definition: Graph.cpp:320
CanonicalGraphEnvironment::drawNodeLabels
bool drawNodeLabels
Definition: CanonicalGraphEnvironment.h:94
CanonicalGraphEnvironment::GetActions
void GetActions(const canGraphState &stateID, std::vector< graphMove > &actions) const
Definition: CanonicalGraphEnvironment.cpp:99
CanonicalGraphEnvironment::ApplyAction
void ApplyAction(canGraphState &s, graphMove a) const
Definition: CanonicalGraphEnvironment.cpp:110
canGraphState::parent
graphState parent
Definition: CanonicalGraphEnvironment.h:19
TemplateAStar::GetParent
const state & GetParent(const state &s)
Definition: TemplateAStar.h:575
Graph::getEdgeIter
edge_iterator getEdgeIter() const
Definition: Graph.cpp:315
CanonicalGraphEnvironment.h
CanonicalGraphEnvironment::directed
bool directed
Definition: CanonicalGraphEnvironment.h:91
DrawTextCentered
void DrawTextCentered(double x, double y, double z, double scale, const char *str)
Definition: GLUtil.cpp:542
graphMove::to
uint32_t to
Definition: GraphEnvironment.h:38
operator==
bool operator==(const canGraphState &s1, const canGraphState &s2)
Definition: CanonicalGraphEnvironment.cpp:12
CanonicalGraphEnvironment::drawEdgeCosts
bool drawEdgeCosts
Definition: CanonicalGraphEnvironment.h:93
Graph::GetNumEdges
int GetNumEdges()
Definition: Graph.cpp:397
DrawText
void DrawText(double x, double y, double z, double scale, const char *str)
Definition: GLUtil.cpp:526
TemplateAStar
A templated version of A*, based on HOG genericAStar.
Definition: TemplateAStar.h:73
CanonicalGraphEnvironment::GetAction
graphMove GetAction(const canGraphState &s1, const canGraphState &s2) const
Definition: CanonicalGraphEnvironment.cpp:104
node::getUniqueID
int getUniqueID() const
Definition: Graph.h:177
SearchEnvironment< canGraphState, graphMove >::GetColor
virtual rgbColor GetColor() const
Definition: SearchEnvironment.h:105
CanonicalGraphEnvironment::GetSuccessors
void GetSuccessors(const canGraphState &stateID, std::vector< canGraphState > &neighbors) const
Definition: CanonicalGraphEnvironment.cpp:41
CanonicalGraphEnvironment::GoalTest
bool GoalTest(const canGraphState &state, const canGraphState &goal) const
Definition: CanonicalGraphEnvironment.cpp:146
CanonicalGraphEnvironment::ComputeOrdering
void ComputeOrdering()
Definition: CanonicalGraphEnvironment.cpp:320
CanonicalGraphEnvironment::GLDrawLine
void GLDrawLine(const canGraphState &x, const canGraphState &y) const
Definition: CanonicalGraphEnvironment.cpp:291
GraphSearchConstants::kYCoordinate
@ kYCoordinate
Definition: GraphEnvironment.h:52
fgreater
bool fgreater(double a, double b)
Definition: FPUtil.h:29
Graph::GetNumNodes
int GetNumNodes()
Definition: Graph.cpp:403
CanonicalGraphEnvironment::CanonicalGraphEnvironment
CanonicalGraphEnvironment(Graph *g)
Definition: CanonicalGraphEnvironment.cpp:26
CanonicalGraphEnvironment::g
Graph * g
Definition: CanonicalGraphEnvironment.h:92
node::GetLabelF
double GetLabelF(unsigned int index) const
Definition: Graph.h:215
CanonicalGraphEnvironment::OpenGLDraw
void OpenGLDraw() const
Definition: CanonicalGraphEnvironment.cpp:167
TemplateAStar::InitializeSearch
bool InitializeSearch(environment *env, const state &from, const state &to, std::vector< state > &thePath)
Initialize the A* search.
Definition: TemplateAStar.h:257
node::GetNum
unsigned int GetNum() const
Definition: Graph.h:176
TemplateAStar::GetClosedItem
bool GetClosedItem(const state &s, AStarOpenClosedDataWithF< state > &)
Definition: TemplateAStar.h:673
CanonicalGraphEnvironment::HCost
double HCost(const canGraphState &state1, const canGraphState &state2) const
Heuristic value between two arbitrary nodes.
Definition: CanonicalGraphEnvironment.cpp:127
canGraphState
Definition: CanonicalGraphEnvironment.h:18
CanonicalGraphEnvironment::canonicalOrdering
std::unordered_map< canGraphState, uint8_t > canonicalOrdering
Definition: CanonicalGraphEnvironment.h:90
operator<<
std::ostream & operator<<(std::ostream &out, const canGraphState &s1)
Definition: CanonicalGraphEnvironment.cpp:18
CanonicalGraphEnvironment::~CanonicalGraphEnvironment
~CanonicalGraphEnvironment()
Definition: CanonicalGraphEnvironment.cpp:36
CanonicalGraphEnvironment::GetMaxHash
uint64_t GetMaxHash() const
Definition: CanonicalGraphEnvironment.cpp:151
GraphSearchConstants::kZCoordinate
@ kZCoordinate
Definition: GraphEnvironment.h:53
Graph::findDirectedEdge
edge * findDirectedEdge(unsigned int from, unsigned int to)
Definition: Graph.cpp:189
CanonicalGraphEnvironment::nodeScale
double nodeScale
Definition: CanonicalGraphEnvironment.h:95
GraphEnvironment
Definition: GraphEnvironment.h:291
TemplateAStar::DoSingleSearchStep
bool DoSingleSearchStep(std::vector< state > &thePath)
Expand a single node.
Definition: TemplateAStar.h:314
node::GetNumEdges
int GetNumEdges()
Definition: Graph.h:204
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
edge
Edge class for connections between node in a Graph.
Definition: Graph.h:129
node::GetName
const char * GetName() const
Definition: Graph.h:175
Generated by   doxygen 1.8.17