HOG2
Map2DHeading.cpp
Go to the documentation of this file.
1 //
2 // Map2DHeading.cpp
3 // hog2 glut
4 //
5 // Created by Nathan Sturtevant on 11/12/12.
6 // Copyright (c) 2012 University of Denver. All rights reserved.
7 //
8 
9 #include "Map2DHeading.h"
10 
11 //const double P = 1.0;
12 using std::cout;
13 using std::endl;
14 
15 Map2DHeading::Map2DHeading(Map *m)
16 {
17  map = m;
18  BuildAngleTables();
19  drawWeights = false;
20  DIAGONAL_COST = 1.5;
21 }
22 
23 Map2DHeading::~Map2DHeading()
24 {
25 
26 }
27 
28 void Map2DHeading::GetSuccessors(const xyhLoc &nodeID, std::vector<xyhLoc> &neighbors) const
29 {
30  neighbors.resize(0);
31  xyhLoc tmp;
32  xyhAct act;
33  act.oldHeading = nodeID.h;
34  for (int x = 0; x < 8; x++)
35  {
36  tmp = nodeID;
37  act.newHeading = x;
38  ApplyAction(tmp, act);
39  if (x != nodeID.h || map->CanStep(nodeID.x, nodeID.y, tmp.x, tmp.y))
40  neighbors.push_back(tmp);
41  }
42 }
43 
44 void Map2DHeading::GetActions(const xyhLoc &nodeID, std::vector<xyhAct> &actions) const
45 {
46  actions.resize(0);
47  xyhAct act;
48  act.oldHeading = nodeID.h;
49  for (int x = 0; x < 8; x++)
50  {
51  act.newHeading = x;
52  if (x != nodeID.h)
53  actions.push_back(act);
54  else {
55  xyhLoc tmp;
56  GetNextState(nodeID, act, tmp);
57  if (map->CanStep(nodeID.x, nodeID.y, tmp.x, tmp.y))
58  actions.push_back(act);
59  }
60  }
61 }
62 
63 xyhAct Map2DHeading::GetAction(const xyhLoc &s1, const xyhLoc &s2) const
64 {
65  xyhAct a;
66  a.oldHeading = s1.h;
67  a.newHeading = s2.h;
68  return a;
69 }
70 
71 void Map2DHeading::ApplyAction(xyhLoc &s, xyhAct dir) const
72 {
73  if (dir.oldHeading != dir.newHeading)
74  {
75  s.h = dir.newHeading;
76  return;
77  }
78 
79  switch (dir.newHeading)
80  {
81  case 6: s.y -= 1; break;
82  case 0: s.x += 1; break;
83  case 2: s.y += 1; break;
84  case 4: s.x -= 1; break;
85  case 7: s.y -= 1; s.x += 1; break;
86  case 1: s.y += 1; s.x += 1; break;
87  case 3: s.y += 1; s.x -= 1; break;
88  case 5: s.y -= 1; s.x -= 1; break;
89  }
90 }
91 
92 bool Map2DHeading::InvertAction(xyhAct &a) const
93 {
94  return false;
95 }
96 
97 double Map2DHeading::HCost(const xyhLoc &l1, const xyhLoc &l2) const
98 {
99  double h1;
100  double a = ((l1.x>l2.x)?(l1.x-l2.x):(l2.x-l1.x));
101  double b = ((l1.y>l2.y)?(l1.y-l2.y):(l2.y-l1.y));
102  //return sqrt(a*a+b*b);
103  h1 = (a>b)?(b*DIAGONAL_COST+a-b):(a*DIAGONAL_COST+b-a);
104  if (l1.h != l2.h)
105  h1+=1;
106  return h1;
107 }
108 
109 double Map2DHeading::GetCost(const xyhLoc &a, const xyhLoc &b, double P, double D) const
110 {
111  if (a.x == b.x && a.y == b.y)
112  {
113  //cout << "gcost of " << a << " relative to " << b << " is " << 100.0 << endl;
114  return 100;
115  }
116  float heading = atan2(a.x-b.x, a.y-b.y);
117  int conv = (360.0*heading/(2*3.1415)+180+(b.h+2)*45); // -90 for face left
118  conv = conv%360;
119  if (conv > 180)
120  conv = 360-conv;
121  //printf("Relative heading: %d\n", conv);
122 
123 
124  // for (float P = -1; P <= 1; P += 0.5)
125  // {
126  // float cost = (P<0)?((1.0-(float)conv/180.0)*(-P)):(((float)conv/180.0)*(P));
127  // printf("P : %1.2f C : %1.2f\n", P, cost);
128  // }
129  float cost1 = max(-P*cos(1.0*2*3.1415*conv/360.0), 0);
130  //float cost = (P<0)?((1.0-(float)conv/90.0)*(-P)):(((float)conv/90.0-1.0)*(P));
131  //float cost = (P<0)?((1.0-(float)conv/180.0)*(-P)):(((float)conv/180.0)*(P));
132  float dist = sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
133  // target distance is 2.0
134  float cost2 = min(fabs(D-dist)/D, 1.0);
135 // dist *= dist;
136  //cout << "gcost of " << a << " relative to " << b << " is " << 10*cost1/dist+cost2 << endl;
137  return 10*cost1/dist+cost2;//+((dist<16)?fabs(8.0-dist):0);
138 }
139 
140 double Map2DHeading::GCost(const xyhLoc &node1, const xyhLoc &node2) const
141 {
142  if (node1.h != node2.h) // turn
143  return 1.0;
144 
145  double costModifier = 1.0;
146 
147  //for (CostTable::iterator it = costs.begin(); it != costs.end(); it++)
148  for (const auto &it : costs)
149  {
150  xyhLoc tmp;
151  //GetStateFromHash(it->first, tmp);
152  GetStateFromHash(it.first, tmp);
153  costModifier += GetCost(node1, tmp, it.second.seen, it.second.dist);
154  }
155 // CostTable::iterator iter = costs.find(GetStateHash(node1));
156 // if (iter != costs.end())
157 // {
158 // std::cout << "Found cost " << iter->second << " at state " << node1 << std::endl;
159 // costModifier = iter->second;
160 // }
161 
162  if (0 == (node1.h%2))
163  return costModifier;
164  return DIAGONAL_COST*costModifier;
165 }
166 
167 double Map2DHeading::GCost(const xyhLoc &node1, const xyhAct &act) const
168 {
169  if (act.oldHeading != act.newHeading)
170  return 1.0;
171 
172  double costModifier = 1.0;
173 
174  for (const auto &it : costs)
175  //for (CostTable::iterator it = costs.begin(); it != costs.end(); it++)
176  {
177  xyhLoc tmp;
178  GetStateFromHash(it.first, tmp);
179  costModifier += GetCost(node1, tmp, it.second.seen, it.second.dist);
180  }
181 // CostTable::iterator iter = costs.find(GetStateHash(node1));
182 // if (iter != costs.end())
183 // {
184 // std::cout << "Found cost* " << iter->second << " at state " << node1 << std::endl;
185 // costModifier = iter->second;
186 // }
187 
188  if (0 == act.newHeading%2)
189  return costModifier;
190  return DIAGONAL_COST*costModifier;
191 }
192 
193 bool Map2DHeading::LegalState(const xyhLoc &s)
194 {
195  if ((map->GetTerrainType(s.x, s.y)&kGround) != 0)
196  return true;
197  return false;
198 }
199 
200 bool Map2DHeading::GoalTest(const xyhLoc &node, const xyhLoc &goal) const
201 {
202  return (node == goal);
203 }
204 
205 uint64_t Map2DHeading::GetStateHash(const xyhLoc &node) const
206 {
207  uint64_t res = node.x;
208  res = (res<<16)|node.y;
209  res = (res<<16)|node.h;
210 
211  return res;
212 }
213 
214 void Map2DHeading::GetStateFromHash(uint64_t hash, xyhLoc &node) const
215 {
216  node.h = hash&0xFFFF;
217  hash>>=16;
218  node.y = hash&0xFFFF;
219  hash>>=16;
220  node.x = hash&0xFFFF;
221 }
222 
223 uint64_t Map2DHeading::GetActionHash(xyhAct act) const
224 {
225  return (act.newHeading<<8)+act.oldHeading;
226 }
227 
228 void Map2DHeading::OpenGLDraw() const
229 {
230  map->OpenGLDraw();
231 
232  SetColor(0.0, 1.0, 0.0);
233  if (!drawWeights)
234  return;
235  xyhLoc l;
236  for (CostTable::const_iterator it = costs.begin(); it != costs.end(); it++)
237  {
238  GetStateFromHash(it->first, l);
239  OpenGLDraw(l);
240  }
241 
242 }
243 
244 void Map2DHeading::OpenGLDraw(const xyhLoc &l) const
245 {
246  GLdouble xx, yy, zz, rad;
247  GLfloat r, g, b, t;
248  GetColor(r, g, b, t);
249  map->GetOpenGLCoord(l.x, l.y, xx, yy, zz, rad);
250 
251  GLdouble yoffset = mySin(l.h)*rad;//sin(TWOPI*rot/16)*rad;
252  GLdouble xoffset = myCos(l.h)*rad;//cos(TWOPI*rot/16)*rad;
253 
254  glBegin(GL_TRIANGLES);
255  recVec surfaceNormal;
256  surfaceNormal.x = (((-0.5*xoffset) * (-rad)) - ((+rad) - (-2*yoffset)));
257  surfaceNormal.y = (((rad) * (-2*xoffset)) - ((0.5*yoffset) - (rad)));
258  surfaceNormal.z = (((0.5*yoffset) * (-2*yoffset)) - ((-0.5*xoffset) - (-2*xoffset)));
259  surfaceNormal.normalise();
260  glNormal3f(surfaceNormal.x, surfaceNormal.y, surfaceNormal.z);
261  glColor4f(r, g, b/2, t);
262  glVertex3f(xx+xoffset, yy+yoffset, zz);
263  glColor4f(r, g/2, b, t);
264  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
265  glColor4f(r, g, b/2, t);
266  glVertex3f(xx-xoffset+0.5*yoffset, yy-yoffset-0.5*xoffset, zz);
267 
268  surfaceNormal.x = (((+0.5*xoffset) * (-rad)) - ((+rad) - (-2*yoffset)));
269  surfaceNormal.y = (((rad) * (-2*xoffset)) - ((-0.5*yoffset) - (rad)));
270  surfaceNormal.z = (((-0.5*yoffset) * (-2*yoffset)) - ((+0.5*xoffset) - (-2*xoffset)));
271  surfaceNormal.normalise();
272  glNormal3f(surfaceNormal.x, surfaceNormal.y, surfaceNormal.z);
273  glColor4f(r, g/2, b, t);
274  glVertex3f(xx+xoffset, yy+yoffset, zz);
275  glColor4f(r, g, b/2, t);
276  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
277  glColor4f(r, g/2, b, t);
278  glVertex3f(xx-xoffset-0.5*yoffset, yy-yoffset+0.5*xoffset, zz);
279  glEnd();
280 
281 }
282 
283 void Map2DHeading::OpenGLDraw(const xyhLoc &oldState, const xyhLoc &newState, float perc) const
284 {
285  int DEG = 8;
286  GLfloat r, g, b, t;
287  GetColor(r, g, b, t);
288  //printf("Drawing %f percent\n", perc);
289  // std::cout << oldState << std::endl;
290  // std::cout << newState << std::endl;
291 
292  GLdouble xx, yy, zz, rad;
293 
294  map->GetOpenGLCoord(perc*newState.x + (1-perc)*oldState.x, perc*newState.y + (1-perc)*oldState.y, xx, yy, zz, rad);
295 
296  float rot = (1-perc)*oldState.h+perc*newState.h;
297 
298  if ((oldState.h >= DEG-2) && (newState.h <= 2))
299  {
300  rot = (1-perc)*oldState.h+perc*(newState.h+DEG);
301  if (rot >= DEG)
302  rot -= DEG;
303  }
304  else if ((newState.h >= DEG-2) && (oldState.h <= 2))
305  {
306  rot = (1-perc)*(oldState.h+DEG)+perc*(newState.h);
307  if (rot >= DEG)
308  rot -= DEG;
309  }
310 
311  GLdouble yoffset = sin(TWOPI*rot/8.0)*rad;
312  GLdouble xoffset = cos(TWOPI*rot/8.0)*rad;
313 
314  glBegin(GL_TRIANGLES);
315  recVec surfaceNormal;
316  surfaceNormal.x = (((-0.5*xoffset) * (-rad)) - ((+rad) - (-2*yoffset)));
317  surfaceNormal.y = (((rad) * (-2*xoffset)) - ((0.5*yoffset) - (rad)));
318  surfaceNormal.z = (((0.5*yoffset) * (-2*yoffset)) - ((-0.5*xoffset) - (-2*xoffset)));
319  surfaceNormal.normalise();
320  glNormal3f(surfaceNormal.x, surfaceNormal.y, surfaceNormal.z);
321  glColor4f(r, g, b/2, t);
322  glVertex3f(xx+xoffset, yy+yoffset, zz);
323  glColor4f(r, g/2, b, t);
324  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
325  glColor4f(r, g, b/2, t);
326  glVertex3f(xx-xoffset+0.5*yoffset, yy-yoffset-0.5*xoffset, zz);
327 
328  surfaceNormal.x = (((+0.5*xoffset) * (-rad)) - ((+rad) - (-2*yoffset)));
329  surfaceNormal.y = (((rad) * (-2*xoffset)) - ((-0.5*yoffset) - (rad)));
330  surfaceNormal.z = (((-0.5*yoffset) * (-2*yoffset)) - ((+0.5*xoffset) - (-2*xoffset)));
331  surfaceNormal.normalise();
332  glNormal3f(surfaceNormal.x, surfaceNormal.y, surfaceNormal.z);
333  glColor4f(r, g/2, b, t);
334  glVertex3f(xx+xoffset, yy+yoffset, zz);
335  glColor4f(r, g, b/2, t);
336  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
337  glColor4f(r, g/2, b, t);
338  glVertex3f(xx-xoffset-0.5*yoffset, yy-yoffset+0.5*xoffset, zz);
339  glEnd();
340 }
341 
342 void Map2DHeading::OpenGLDraw(const xyhLoc &, const xyhAct &) const
343 {
344 
345 }
346 
347 void Map2DHeading::GLLabelState(const xyhLoc &, const char *) const
348 {
349 
350 }
351 
352 void Map2DHeading::GLDrawLine(const xyhLoc &x, const xyhLoc &y) const
353 {
354 
355 }
356 
357 
358 void Map2DHeading::GetNextState(const xyhLoc &currents, xyhAct dir, xyhLoc &news) const
359 {
360  news = currents;
361  ApplyAction(news, dir);
362 }
363 
364 void Map2DHeading::BuildAngleTables()
365 {
366  float DEG = 8;
367  for (float x = 0; x < DEG; x++)
368  {
369  sinTable.push_back(sin(TWOPI*(x)/DEG));
370  //printf("sin(%d) = %f\n", x, sinTable.back());
371  cosTable.push_back(cos(TWOPI*(x)/DEG));
372  //printf("cos(%d) = %f\n", x, cosTable.back());
373  }
374 }
375 
376 float Map2DHeading::mySin(int dir) const
377 {
378  return sinTable[dir];
379 }
380 
381 float Map2DHeading::myCos(int dir) const
382 {
383  return cosTable[dir];
384 }
385 
386 void Map2DHeading::SetCost(const xyhLoc &l, double seen, double dist)
387 {
388  hdData d = {seen, dist};
389  costs[GetStateHash(l)] = d;
390 }
391 
392 void Map2DHeading::ClearCost(const xyhLoc &l)
393 {
394  costs.erase(GetStateHash(l));
395 }
396 
397 void Map2DHeading::ClearAllCosts()
398 {
399  costs.clear();
400 }
Map2DHeading::GetActionHash
uint64_t GetActionHash(xyhAct act) const
Definition: Map2DHeading.cpp:223
Map2DHeading::HCost
virtual double HCost(const xyhLoc &) const
Heuristic value between node and the stored goal.
Definition: Map2DHeading.h:61
recVec
A generic vector (essentially the same as a point, but offers normalization)
Definition: GLUtil.h:78
min
double min(double a, double b)
Definition: FPUtil.h:35
xyhLoc
Definition: Map2DHeading.h:21
Map::OpenGLDraw
void OpenGLDraw(tDisplay how=kPolygons) const
Does actual OpenGL drawing of the map.
Definition: Map.cpp:1777
Map2DHeading::GetActions
void GetActions(const xyhLoc &nodeID, std::vector< xyhAct > &actions) const
Definition: Map2DHeading.cpp:44
recVec::z
GLdouble z
Definition: GLUtil.h:98
Map2DHeading::GetStateHash
uint64_t GetStateHash(const xyhLoc &node) const
Definition: Map2DHeading.cpp:205
Map2DHeading::cosTable
std::vector< float > cosTable
Definition: Map2DHeading.h:106
Map2DHeading::drawWeights
bool drawWeights
Definition: Map2DHeading.h:101
if
if(state==GLUT_DOWN)
Definition: GLUThog.cpp:244
d
mcData d[]
Definition: MotionCaptureMovement.cpp:21
Map2DHeading::SetCost
void SetCost(const xyhLoc &, double seen, double dist)
Definition: Map2DHeading.cpp:386
xyhLoc::y
uint16_t y
Definition: Map2DHeading.h:26
Map2DHeading::InvertAction
virtual bool InvertAction(xyhAct &a) const
Definition: Map2DHeading.cpp:92
Map2DHeading::OpenGLDraw
virtual void OpenGLDraw() const
Definition: Map2DHeading.cpp:228
Map2DHeading::ApplyAction
virtual void ApplyAction(xyhLoc &s, xyhAct dir) const
Definition: Map2DHeading.cpp:71
Map2DHeading::~Map2DHeading
virtual ~Map2DHeading()
Definition: Map2DHeading.cpp:23
Map2DHeading::map
Map * map
Definition: Map2DHeading.h:103
Map2DHeading::DIAGONAL_COST
double DIAGONAL_COST
Definition: Map2DHeading.h:104
kGround
@ kGround
Definition: Map.h:55
SearchEnvironment< xyhLoc, xyhAct >::SetColor
virtual void SetColor(const rgbColor &r) const
Definition: SearchEnvironment.h:102
Map2DHeading.h
costs
Definition: FringeSearch.h:15
xyhAct::oldHeading
uint8_t oldHeading
Definition: Map2DHeading.h:42
Map2DHeading::GetSuccessors
virtual void GetSuccessors(const xyhLoc &nodeID, std::vector< xyhLoc > &neighbors) const
Definition: Map2DHeading.cpp:28
Map2DHeading::hdData
Definition: Map2DHeading.h:112
xyhAct::newHeading
uint8_t newHeading
Definition: Map2DHeading.h:43
xyhLoc::x
uint16_t x
Definition: Map2DHeading.h:25
SearchEnvironment< xyhLoc, xyhAct >::GetColor
virtual rgbColor GetColor() const
Definition: SearchEnvironment.h:105
Map2DHeading::mySin
float mySin(int dir) const
Definition: Map2DHeading.cpp:376
Map::GetTerrainType
long GetTerrainType(long x, long y, tSplitSide split=kWholeTile) const
Get the terrain type of the (split) tile at x, y.
Definition: Map.cpp:1028
Map2DHeading::LegalState
bool LegalState(const xyhLoc &s)
Definition: Map2DHeading.cpp:193
Map2DHeading::GCost
virtual double GCost(const xyhLoc &node1, const xyhLoc &node2) const
Definition: Map2DHeading.cpp:140
Map2DHeading::ClearAllCosts
void ClearAllCosts()
Definition: Map2DHeading.cpp:397
Map2DHeading::GLLabelState
virtual void GLLabelState(const xyhLoc &, const char *) const
Definition: Map2DHeading.cpp:347
Map2DHeading::Map2DHeading
Map2DHeading(Map *m)
Definition: Map2DHeading.cpp:15
max
#define max(a, b)
Definition: MinimalSectorAbstraction.cpp:40
Map2DHeading::sinTable
std::vector< float > sinTable
Definition: Map2DHeading.h:107
xyhLoc::h
uint8_t h
Definition: Map2DHeading.h:27
Map::CanStep
bool CanStep(long x1, long y1, long x2, long y2) const
Definition: Map.cpp:1694
Map2DHeading::GetCost
double GetCost(const xyhLoc &a, const xyhLoc &b, double P, double d) const
Definition: Map2DHeading.cpp:109
TWOPI
static const double TWOPI
Definition: GLUtil.h:65
Map2DHeading::GetStateFromHash
void GetStateFromHash(uint64_t hash, xyhLoc &node) const
Definition: Map2DHeading.cpp:214
Map::GetOpenGLCoord
bool GetOpenGLCoord(int _x, int _y, GLdouble &x, GLdouble &y, GLdouble &z, GLdouble &radius) const
Get the openGL coordinates of a given tile.
Definition: Map.cpp:1826
Map2DHeading::BuildAngleTables
void BuildAngleTables()
Definition: Map2DHeading.cpp:364
recVec::y
GLdouble y
Definition: GLUtil.h:98
Map2DHeading::GetNextState
virtual void GetNextState(const xyhLoc &currents, xyhAct dir, xyhLoc &news) const
Definition: Map2DHeading.cpp:358
Map2DHeading::ClearCost
void ClearCost(const xyhLoc &)
Definition: Map2DHeading.cpp:392
xyhAct
Definition: Map2DHeading.h:41
recVec::x
GLdouble x
Definition: GLUtil.h:98
Map2DHeading::GLDrawLine
virtual void GLDrawLine(const xyhLoc &x, const xyhLoc &y) const
Definition: Map2DHeading.cpp:352
recVec::normalise
void normalise()
Normalize a vector.
Definition: GLUtil.cpp:39
Map2DHeading::myCos
float myCos(int dir) const
Definition: Map2DHeading.cpp:381
Map2DHeading::GoalTest
bool GoalTest(const xyhLoc &node, const xyhLoc &goal) const
Definition: Map2DHeading.cpp:200
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
Map
A tile-based representation of the world.
Definition: Map.h:142
Map2DHeading::GetAction
xyhAct GetAction(const xyhLoc &s1, const xyhLoc &s2) const
Definition: Map2DHeading.cpp:63
Generated by   doxygen 1.8.17