HOG2
Directional2DEnvironment.cpp
Go to the documentation of this file.
1 /*
2  * Directional2DEnvironment.cpp
3  * hog2
4  *
5  * Created by Nathan Sturtevant on 2/20/08.
6  * Copyright 2008 __MyCompanyName__. All rights reserved.
7  *
8  */
9 
10 #include "Directional2DEnvironment.h"
11 #include "FPUtil.h"
12 #include "TemplateAStar.h"
13 
14 #define TABLE_SIZE 15
15 #define ROW_SIZE (2*TABLE_SIZE+1)
16 
17 #pragma message("ABS functions might have been removed; need to verify to make sure they weren't needed")
18 Directional2DEnvironment::Directional2DEnvironment(Map *_m, model envType, heuristicType heuristic)
19 {
20  motionModel = envType;//kHumanoid;
21  //hType = kExtendedPerimeterHeuristic;
22  //hType = kPerimeterHeuristic;
23  //hType = kOctileHeuristic
24 
25  hType = heuristic;
26  BuildAngleTables();
27 // heuristics.resize(4);
28 // for (int x = 0; x < 4; x++)
29 // {
30 // heuristics[x].speed = 0;
31 // heuristics[x].rotation = x;
32 // BuildHTable(heuristics[x]);
33 // }
34  SetColor(1.0, 0.25, 0.25, 1.0);
35  map = _m;
36  checkLegal = true;
37  test = 0;
38 }
39 
40 Directional2DEnvironment::~Directional2DEnvironment()
41 {
42  delete map;
43 }
44 
45 int Directional2DEnvironment::GetNumAngles()
46 {
47  switch (motionModel)
48  {
49  case kVehicle: return 16;
50  case kTank: return 24;
51  case kBetterTank: return 24;
52  case kHumanoid: return 16;
53  }
54  assert(!"Unknown motion model");
55  return 0;
56 }
57 
58 void Directional2DEnvironment::GetSuccessors(const xySpeedHeading &loc, std::vector<xySpeedHeading> &neighbors) const
59 {
60  neighbors.resize(0);
61  std::vector<deltaSpeedHeading> acts;
62  GetActions(loc, acts);
63  for (unsigned int x = 0; x < acts.size(); x++)
64  {
65  xySpeedHeading newLoc(loc);
66  ApplyAction(newLoc, acts[x]);
67  neighbors.push_back(newLoc);
68  }
69 }
70 
71 void Directional2DEnvironment::GetActions(const xySpeedHeading &loc, std::vector<deltaSpeedHeading> &actions) const
72 {
73  if (motionModel == kVehicle)
74  {
75  deltaSpeedHeading sh;
76  if (loc.speed <= 1)
77  {
78  // stop move
79  sh.speed = 0-loc.speed;
80  sh.turn = 0;
81  actions.push_back(sh);
82 //TODO: Backwards moves disabled!
83 // // reverse moves
84 // sh.speed = -1-loc.speed;
85 // sh.turn = 0;
86 // if (Legal(loc, sh))
87 // actions.push_back(sh);
88 // sh.turn = 1;
89 // if (Legal(loc, sh))
90 // actions.push_back(sh);
91 // sh.turn = -1;
92 // if (Legal(loc, sh))
93 // actions.push_back(sh);
94 // sh.turn = 2;
95 // if (Legal(loc, sh))
96 // actions.push_back(sh);
97 // sh.turn = -2;
98 // if (Legal(loc, sh))
99 // actions.push_back(sh);
100  }
101 
102  if (loc.speed >= 1) // medium moves
103  {
104  sh.speed = 2-loc.speed;
105  sh.turn = 0;
106  if (Legal(loc, sh))
107  actions.push_back(sh);
108  sh.turn = 1;
109  if (Legal(loc, sh))
110  actions.push_back(sh);
111  sh.turn = -1;
112  if (Legal(loc, sh))
113  actions.push_back(sh);
114  }
115  if (loc.speed > 1) // fast moves
116  {
117  sh.speed = 3-loc.speed;
118  sh.turn = 0;
119  if (Legal(loc, sh))
120  actions.push_back(sh);
121 // sh.turn = 1;
122 // if (Legal(loc, sh))
123 // actions.push_back(sh);
124 // sh.turn = -1;
125 // if (Legal(loc, sh))
126 // actions.push_back(sh);
127  }
128  // slow moves
129  if (loc.speed < 3)
130  {
131  sh.speed = 1-loc.speed;
132  sh.turn = -3;
133  if (Legal(loc, sh))
134  actions.push_back(sh);
135  sh.turn = -2;
136  if (Legal(loc, sh))
137  actions.push_back(sh);
138  sh.turn = -1;
139  if (Legal(loc, sh))
140  actions.push_back(sh);
141  sh.turn = 0;
142  if (Legal(loc, sh))
143  actions.push_back(sh);
144  sh.turn = 1;
145  if (Legal(loc, sh))
146  actions.push_back(sh);
147  sh.turn = 2;
148  if (Legal(loc, sh))
149  actions.push_back(sh);
150  sh.turn = 3;
151  if (Legal(loc, sh))
152  actions.push_back(sh);
153  }
154  }
155  else if (motionModel == kHumanoid)
156  {
157  deltaSpeedHeading sh;
158 
159  for (int x = 0; x <= 2; x++)
160  {
161  sh.speed = x-loc.speed;
162  sh.turn = 0;
163  if (Legal(loc, sh))
164  actions.push_back(sh);
165 
166  for (int y = 1; y <= 3; y++)
167  {
168  sh.turn = y;
169  if (Legal(loc, sh))
170  actions.push_back(sh);
171  sh.turn = -y;
172  if (Legal(loc, sh))
173  actions.push_back(sh);
174  }
175  }
176  }
177  else if (motionModel == kTank)
178  {
179  deltaSpeedHeading sh;
180 
181  for (int x = -1; x <= 1; x++)
182  {
183  sh.speed = x-loc.speed;
184  sh.turn = 0;
185  if (Legal(loc, sh))
186  actions.push_back(sh);
187 
188  for (int y = 1; y <= 3; y++)
189  {
190  sh.turn = y;
191  if (Legal(loc, sh))
192  actions.push_back(sh);
193  sh.turn = -y;
194  if (Legal(loc, sh))
195  actions.push_back(sh);
196  }
197  }
198  }
199  else if (motionModel == kBetterTank)
200  {
201  deltaSpeedHeading sh;
202 
203  for (int x = -1; x <= 1; x++)
204  {
205  sh.speed = x;
206  sh.turn = 0;
207  if (Legal(loc, sh) && (x != 0))
208  actions.push_back(sh);
209 
210  for (int y = 1; y <= 3; y++)
211  {
212  sh.turn = y;
213  if (Legal(loc, sh))
214  actions.push_back(sh);
215  sh.turn = -y;
216  if (Legal(loc, sh))
217  actions.push_back(sh);
218  }
219  }
220  }
221  else { assert(false); }
222  if (actions.size() == 0) // always have 1 action(?)
223  {
224  deltaSpeedHeading act;
225  act.speed = -loc.speed;
226  act.turn = 0;
227  actions.push_back(act);
228  }
229 }
230 
231 deltaSpeedHeading Directional2DEnvironment::GetAction(const xySpeedHeading &one, const xySpeedHeading &two) const
232 {
233  std::vector<deltaSpeedHeading> acts;
234  xySpeedHeading modified;
235  GetActions(one, acts);
236  for (unsigned int x = 0; x < acts.size(); x++)
237  {
238  modified = one;
239  ApplyAction(modified, acts[x]);
240  //if (two == modified)
241  if (fequal(two.x, modified.x) && fequal(two.y, modified.y) &&
242  (two.rotation == modified.rotation) && (two.speed == modified.speed))
243  {
244  return acts[x];
245  }
246  }
247  assert(false);
248  return acts[0];
249 }
250 
251 bool Directional2DEnvironment::InvertAction(deltaSpeedHeading &act) const
252 {
253  if (motionModel == kBetterTank)
254  {
255  act.turn = -act.turn;
256  act.speed = -act.speed;
257  return true;
258  }
259  assert(false);
260  return false;
261 }
262 
263 void Directional2DEnvironment::RotateCCW(xySpeedHeading &s, unsigned int rotation) const
264 {
265 // GLdouble yoffset = mySin(l.rotation)*s.x;//sin(TWOPI*rot/16)*rad;
266 // GLdouble xoffset = myCos(l.rotation)*s.;//cos(TWOPI*rot/16)*rad;
267  float x1 = s.x;
268  s.x = roundf(s.x*myCos(rotation) + s.y*mySin(rotation));
269  s.y = roundf(s.y*myCos(rotation) - x1*mySin(rotation));
270  s.rotation = ((((motionModel==kTank)||(motionModel==kBetterTank))?24:16)+s.rotation-rotation)%(((motionModel==kTank)||(motionModel==kBetterTank))?24:16);
271 }
272 
273 void Directional2DEnvironment::ApplyAction(xySpeedHeading &s, deltaSpeedHeading dir) const
274 {
275  if ((motionModel != kTank) && (motionModel != kBetterTank))
276  {
277  float xoffset[16] = {1.0, 2.0, 1.0, 1.0,
278  0.0, -1.0, -1.0, -2.0,
279  -1.0, -2.0, -1.0, -1.0,
280  0.0, 1.0, 1.0, 2.0 };
281  float yoffset[16] = {0.0, 1.0, 1.0, 2.0,
282  1.0, 2.0, 1.0, 1.0,
283  0.0, -1.0, -1.0, -2.0,
284  -1.0, -2.0, -1.0, -1.0};
285  s.speed += dir.speed;
286  s.rotation = (16+s.rotation+dir.turn)%16;
287 
288  if (s.speed > 0)
289  {
290  s.x += xoffset[s.rotation];
291  s.y += yoffset[s.rotation];
292  }
293  else if (s.speed < 0)
294  {
295  s.x -= xoffset[s.rotation];
296  s.y -= yoffset[s.rotation];
297  // float tmp = myCos(s.rotation);//*s.speed;
298  // s.x += tmp;
299  // tmp = mySin(s.rotation);//*s.speed;
300  // s.y += tmp;
301  }
302  assert(s.speed >= -1 && s.speed <= 3 && s.rotation >= 0 && s.rotation < 16);
303  }
304  else if (motionModel == kTank)
305  {
306  float xoffset[24] = {1.0, 3.0, 3.0, 1.0, 2.0, 1.0,
307  0.0, -1.0, -2.0,-1.0, -3.0,-3.0,
308  -1.0, -3.0, -3.0,-1.0, -2.0,-1.0,
309  0.0, 1.0, 2.0, 1.0, 3.0, 3.0 };
310  float yoffset[24] = {0.0, 1.0, 2.0, 1.0, 3.0, 3.0,
311  1.0, 3.0, 3.0, 1.0, 2.0, 1.0,
312  0.0, -1.0, -2.0,-1.0, -3.0,-3.0,
313  -1.0, -3.0, -3.0,-1.0, -2.0,-1.0};
314  s.speed += dir.speed;
315  s.rotation = (24+s.rotation+dir.turn)%24;
316 
317  if (s.speed > 0)
318  {
319  s.x += xoffset[s.rotation];
320  s.y += yoffset[s.rotation];
321  }
322  else if (s.speed < 0)
323  {
324  s.x -= xoffset[s.rotation];
325  s.y -= yoffset[s.rotation];
326  // float tmp = myCos(s.rotation);//*s.speed;
327  // s.x += tmp;
328  // tmp = mySin(s.rotation);//*s.speed;
329  // s.y += tmp;
330  }
331  assert(s.speed >= -1 && s.speed <= 1 && s.rotation >= 0 && s.rotation < 24);
332  }
333  else if (motionModel == kBetterTank)
334  {
335  float xoffset[24] = {1.0, 3.0, 3.0, 1.0, 2.0, 1.0,
336  0.0, -1.0, -2.0,-1.0, -3.0,-3.0,
337  -1.0, -3.0, -3.0,-1.0, -2.0,-1.0,
338  0.0, 1.0, 2.0, 1.0, 3.0, 3.0 };
339  float yoffset[24] = {0.0, 1.0, 2.0, 1.0, 3.0, 3.0,
340  1.0, 3.0, 3.0, 1.0, 2.0, 1.0,
341  0.0, -1.0, -2.0,-1.0, -3.0,-3.0,
342  -1.0, -3.0, -3.0,-1.0, -2.0,-1.0};
343  s.speed = 0;
344 
345  if (dir.speed > 0)
346  {
347  s.rotation = (24+s.rotation+dir.turn)%24;
348  s.x += xoffset[s.rotation];
349  s.y += yoffset[s.rotation];
350  }
351  else if (dir.speed < 0)
352  {
353  s.x -= xoffset[s.rotation];
354  s.y -= yoffset[s.rotation];
355  s.rotation = (24+s.rotation+dir.turn)%24;
356  // float tmp = myCos(s.rotation);//*s.speed;
357  // s.x += tmp;
358  // tmp = mySin(s.rotation);//*s.speed;
359  // s.y += tmp;
360  }
361  else {
362  s.rotation = (24+s.rotation+dir.turn)%24;
363  }
364  assert(s.speed >= -1 && s.speed <= 1 && s.rotation >= 0 && s.rotation < 24);
365  }
366  else { // old tank code
367  assert(false);
368  s.speed += dir.speed;
369  s.rotation = (24+s.rotation+dir.turn)%24;
370  if (s.speed != 0)
371  {
372  float tmp = myCos(s.rotation)*1.4;//*s.speed;
373  s.x += tmp;
374  tmp = mySin(s.rotation)*1.4;//*s.speed;
375  s.y += tmp;
376  }
377  }
378 }
379 
380 void Directional2DEnvironment::UndoAction(xySpeedHeading &s, deltaSpeedHeading dir) const
381 {
382  if ((motionModel != kTank) && (motionModel != kBetterTank))
383  {
384  float xoffset[16] = {1.0, 2.0, 1.0, 1.0,
385  0.0, -1.0, -1.0, -2.0,
386  -1.0, -2.0, -1.0, -1.0,
387  0.0, 1.0, 1.0, 2.0 };
388  float yoffset[16] = {0.0, 1.0, 1.0, 2.0,
389  1.0, 2.0, 1.0, 1.0,
390  0.0, -1.0, -1.0, -2.0,
391  -1.0, -2.0, -1.0, -1.0};
392  if (s.speed > 0)
393  {
394  s.x -= xoffset[s.rotation];
395  s.y -= yoffset[s.rotation];
396  }
397  else if (s.speed < 0)
398  {
399  s.x += xoffset[s.rotation];
400  s.y += yoffset[s.rotation];
401 
402  // float tmp = myCos(s.rotation);//*s.speed;
403  // s.x += tmp;
404  // tmp = mySin(s.rotation);//*s.speed;
405  // s.y += tmp;
406  }
407  s.rotation = (16+s.rotation-dir.turn)%16;
408  s.speed -= dir.speed;
409  }
410  else if (motionModel == kTank) {
411  float xoffset[24] = {1.0, 3.0, 3.0, 1.0, 2.0, 1.0,
412  0.0, -1.0, -2.0,-1.0, -3.0,-3.0,
413  -1.0, -3.0, -3.0,-1.0, -2.0,-1.0,
414  0.0, 1.0, 2.0, 1.0, 3.0, 3.0 };
415  float yoffset[24] = {0.0, 1.0, 2.0, 1.0, 3.0, 3.0,
416  1.0, 3.0, 3.0, 1.0, 2.0, 1.0,
417  0.0, -1.0, -2.0,-1.0, -3.0,-3.0,
418  -1.0, -3.0, -3.0,-1.0, -2.0,-1.0};
419 
420  if (s.speed > 0)
421  {
422  s.x -= xoffset[s.rotation];
423  s.y -= yoffset[s.rotation];
424  }
425  else if (s.speed < 0)
426  {
427  s.x += xoffset[s.rotation];
428  s.y += yoffset[s.rotation];
429 
430  // float tmp = myCos(s.rotation);//*s.speed;
431  // s.x += tmp;
432  // tmp = mySin(s.rotation);//*s.speed;
433  // s.y += tmp;
434  }
435  s.rotation = (24+s.rotation-dir.turn)%24;
436  s.speed -= dir.speed;
437  assert(s.speed >= -1 && s.speed <= 1 && s.rotation >= 0 && s.rotation < 24);
438  }
439  else if (motionModel == kBetterTank) {
440  float xoffset[24] = {1.0, 3.0, 3.0, 1.0, 2.0, 1.0,
441  0.0, -1.0, -2.0,-1.0, -3.0,-3.0,
442  -1.0, -3.0, -3.0,-1.0, -2.0,-1.0,
443  0.0, 1.0, 2.0, 1.0, 3.0, 3.0 };
444  float yoffset[24] = {0.0, 1.0, 2.0, 1.0, 3.0, 3.0,
445  1.0, 3.0, 3.0, 1.0, 2.0, 1.0,
446  0.0, -1.0, -2.0,-1.0, -3.0,-3.0,
447  -1.0, -3.0, -3.0,-1.0, -2.0,-1.0};
448 
449  if (dir.speed > 0)
450  {
451  s.x -= xoffset[s.rotation];
452  s.y -= yoffset[s.rotation];
453  s.rotation = (24+s.rotation-dir.turn)%24;
454  }
455  else if (dir.speed < 0)
456  {
457  s.rotation = (24+s.rotation-dir.turn)%24;
458  s.x += xoffset[s.rotation];
459  s.y += yoffset[s.rotation];
460 
461  // float tmp = myCos(s.rotation);//*s.speed;
462  // s.x += tmp;
463  // tmp = mySin(s.rotation);//*s.speed;
464  // s.y += tmp;
465  }
466  else {
467  s.rotation = (24+s.rotation-dir.turn)%24;
468  }
469  s.speed = 0;
470  assert(s.speed >= -1 && s.speed <= 1 && s.rotation >= 0 && s.rotation < 24);
471  }
472  else { // tank!
473  assert(false);
474  if (s.speed != 0)
475  {
476  float tmp = myCos(s.rotation)*1.4;//*s.speed;
477  s.x -= tmp;
478  tmp = mySin(s.rotation)*1.4;//*s.speed;
479  s.y -= tmp;
480  }
481  s.rotation = (24+s.rotation-dir.turn)%24;
482  s.speed -= dir.speed;
483  }
484 }
485 
486 double Directional2DEnvironment::HCost(const xySpeedHeading &l1, const xySpeedHeading &l2) const
487 {
488  float dist = sqrt((l1.x-l2.x)*(l1.x-l2.x)+(l1.y-l2.y)*(l1.y-l2.y));
489  if (motionModel == kHumanoid)
490  dist = ceil(dist/2.0);
491  else if (motionModel == kVehicle)
492  dist = ceil(dist/3.0);
493 
494  return max(dist, LookupStateHeuristic(l1, l2));
495 // double val = ceil(dist/3.0)+(3-l1.speed)+(3-l2.speed);
496 // int angle = (16+l1.rotation-l2.rotation)%16;
497 // return val+angle;
498 }
499 
500 bool Directional2DEnvironment::Legal(const xySpeedHeading &node1, const deltaSpeedHeading &act) const
501 {
502  if (!checkLegal)
503  return true;
504 // int newDir = node1.rotation+act.turn;
505 // int x1 = floor(node1.x);
506 // int y1 = floor(node1.y);
507  xySpeedHeading next=node1;
508  ApplyAction(next, act);
509  if ((motionModel != kTank) && (motionModel != kBetterTank))
510  {
511  return ((map->GetTerrainType(node1.x, node1.y) == kGround) &&
512  (map->GetTerrainType(next.x, next.y) == kGround) &&
513  (map->GetTerrainType((node1.x+next.x)/2, (node1.y+next.y)/2) == kGround));
514  }
515  else {
516  return ((map->GetTerrainType(node1.x, node1.y) == kGround) &&
517  (map->GetTerrainType(next.x, next.y) == kGround) &&
518  (map->GetTerrainType(node1.x+(-node1.x+next.x)/3, node1.y+(-node1.y+next.y)/3) == kGround) &&
519  (map->GetTerrainType(node1.x+2*(-node1.x+next.x)/3, node1.y+2*(-node1.y+next.y)/3) == kGround));
520  }
521 }
522 
523 double Directional2DEnvironment::GCost(const xySpeedHeading &a, const deltaSpeedHeading &b) const
524 {
525  if ((motionModel != kTank) && (motionModel != kBetterTank))
526  {
527  // return 1.0;
528  float val = a.speed+b.speed;
529  if (val == 0)
530  return 1.0;
531 
532  double dist[4] = {1.0, 2.24, 1.42, 2.24};
533  // double dist[4] = {1.0, 1.5, 1.42, 1.5};
534  return dist[(16+a.rotation+b.turn)%4]/fabs(val);
535  }
536  else if (motionModel == kTank) {
537  float val = a.speed+b.speed;
538  if (val == 0)
539  return 1.0;
540  double dist[6] = {1.0, 3.16, 3.61, 1.42, 3.61, 3.16};
541  return dist[(24+a.rotation+b.turn)%6]/fabs(val);
542  }
543  else if (motionModel == kBetterTank) {
544  float val = b.speed;
545  double result;
546  if (val == 0)
547  return 1.0;
548  double dist[6] = {1.0, 3.25, 3.75, 1.50, 3.75, 3.25};
549  if (val > 0)
550  {
551  result = dist[(24+a.rotation+b.turn)%6];
552  }
553  else {
554  result = dist[(a.rotation)%6];
555  }
556 // std::cout << "GCost between " << a << " given action " << b << " is " << result << std::endl;
557  return result;
558  }
559 
560  if (fequal(a.speed+b.speed, 0))
561  return 1.0;
562  return 1.4/std::abs(a.speed+b.speed);
563 // return 1.0/fabs(val);
564 }
565 
566 double Directional2DEnvironment::GCost(const xySpeedHeading &a, const xySpeedHeading &b) const
567 {
568  if ((motionModel != kTank) && (motionModel != kBetterTank))
569  {
570  if (b.speed == 0)
571  return 1.0;
572 
573  // float dist[4] = {1.0f, 2.24f, 1.42f, 2.24f};
574  double dist[4] = {1.0, 2.24, 1.42, 2.24};
575  // double dist[4] = {1.0, 1.5, 1.42, 1.5};
576  double ret = dist[(b.rotation)%4]/std::abs(b.speed);
577  // printf("rot: %d; speed: %d; val: %f\n", b.rotation, b.speed, ret);
578  return ret;
579  }
580  else if (motionModel == kTank)
581  {
582  if (b.speed == 0)
583  return 1.0;
584 
585  double dist[6] = {1.0, 3.16, 3.61, 1.42, 3.61, 3.16};
586  double ret;
587  // if (b.speed > 0)
588  ret = dist[(b.rotation)%6]/std::abs(b.speed);
589  // else
590  // ret = dist[(b.rotation)%6]/(fabs(b.speed)*0.9);
591  // printf("rot: %d; speed: %d; val: %f\n", b.rotation, b.speed, ret);
592  return ret;
593  }
594  else if (motionModel == kBetterTank)
595  {
596  deltaSpeedHeading s = GetAction(a, b);
597  return GCost(a, s);
598 // double dist[6] = {1.0, 3.16, 3.61, 1.42, 3.61, 3.16};
599 // double ret;
600 // if (b.speed-a.speed > 0)
601 // ret = dist[(b.rotation)%6];///fabs(b.speed-a.speed);
602 // else
603 // ret = dist[(a.rotation)%6];///fabs(b.speed-a.speed);
604 // // printf("rot: %d; speed: %d; val: %f\n", b.rotation, b.speed, ret);
605 // return ret;
606  }
607  assert(false);
608  return 0;
609 }
610 
611 bool Directional2DEnvironment::GoalTest(const xySpeedHeading &node, const xySpeedHeading &goal) const
612 {
613  if (test && test->goalTest(node))
614  return true;
615  return (node == goal);
616 // return (((floor(node.x) == floor(goal.x)) && (floor(node.y) == floor(goal.y))) &&
617 // (node.speed == 0) && (node.rotation == goal.rotation));
618 }
619 
620 uint64_t Directional2DEnvironment::GetStateHash(const xySpeedHeading &node) const
621 {
622  // rotation is 0..15
623  // speed is 0..3
624  // float x; float y; uint8_t speed; uint8_t rotation;
625  if (motionModel == kBetterTank)
626  {
627  return (uint64_t)((uint64_t)node.x<<32)|((uint64_t)node.y<<16)|((uint64_t)node.rotation);
628  }
629  uint64_t hval = (((int)node.x*4)<<16)|((int)node.y*4);
630  hval = (uint64_t)((uint64_t)hval<<32)|((uint64_t)node.rotation<<8)|(uint64_t)(node.speed+4);
631  return hval;
632 }
633 
634 uint64_t Directional2DEnvironment::GetActionHash(deltaSpeedHeading act) const
635 {
636  return ((act.turn+4)<<8)+(act.speed+4);
637 }
638 
639 void Directional2DEnvironment::OpenGLDraw() const
640 {
641 // std::cout<<"drawing\n";
642  map->OpenGLDraw();
643 }
644 
645 
646 void Directional2DEnvironment::OpenGLDraw(const xySpeedHeading& oldState, const xySpeedHeading &newState, float perc) const
647 {
648  int DEG = 16;
649  if ((motionModel == kTank) || (motionModel == kBetterTank))
650  DEG = 24;
651  GLfloat r, g, b, t;
652  GetColor(r, g, b, t);
653 
654  GLdouble xx, yy, zz, rad;
655  map->GetOpenGLCoord(perc*newState.x + (1-perc)*oldState.x, perc*newState.y + (1-perc)*oldState.y, xx, yy, zz, rad);
656 
657  float rot = (1-perc)*oldState.rotation+perc*newState.rotation;
658  if ((oldState.rotation >= DEG-4) && (newState.rotation <= 5))
659  {
660  rot = (1-perc)*oldState.rotation+perc*(newState.rotation+DEG);
661  if (rot >= DEG)
662  rot -= DEG;
663  }
664  else if ((newState.rotation >= DEG-4) && (oldState.rotation <= 5))
665  {
666  rot = (1-perc)*(oldState.rotation+DEG)+perc*(newState.rotation);
667  if (rot >= DEG)
668  rot -= DEG;
669  }
670  GLdouble yoffset = sin(TWOPI*rot/DEG)*rad;
671  GLdouble xoffset = cos(TWOPI*rot/DEG)*rad;
672 
673  glBegin(GL_TRIANGLES);
674  glColor4f(r, g, b/2, t);
675  glVertex3f(xx+xoffset, yy+yoffset, zz);
676  glColor4f(r, g/2, b, t);
677  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
678  glColor4f(r, g, b/2, t);
679  glVertex3f(xx-xoffset+0.5*yoffset, yy-yoffset-0.5*xoffset, zz);
680 
681  glColor4f(r, g/2, b, t);
682  glVertex3f(xx+xoffset, yy+yoffset, zz);
683  glColor4f(r, g, b/2, t);
684  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
685  glColor4f(r, g/2, b, t);
686  glVertex3f(xx-xoffset-0.5*yoffset, yy-yoffset+0.5*xoffset, zz);
687  glEnd();
688 }
689 
690 
691 void Directional2DEnvironment::OpenGLDraw(const xySpeedHeading &l) const
692 {
693  GLdouble xx, yy, zz, rad;
694  GLfloat r, g, b, t;
695  GetColor(r, g, b, t);
696  map->GetOpenGLCoord(l.x, l.y, xx, yy, zz, rad);
697 
698  GLdouble yoffset = mySin(l.rotation)*rad;//sin(TWOPI*rot/16)*rad;
699  GLdouble xoffset = myCos(l.rotation)*rad;//cos(TWOPI*rot/16)*rad;
700 
701 // glColor3f(0, 0, 1.0);
702 // glBegin(GL_LINE_STRIP);
703 // glVertex3f(xx-rad, yy-rad, zz-rad);
704 // glVertex3f(xx-rad, yy-rad, zz);
705 // glEnd();
706 
707  glBegin(GL_TRIANGLES);
708  recVec surfaceNormal;
709  surfaceNormal.x = (((-0.5*xoffset) * (-rad)) - ((+rad) - (-2*yoffset)));
710  surfaceNormal.y = (((rad) * (-2*xoffset)) - ((0.5*yoffset) - (rad)));
711  surfaceNormal.z = (((0.5*yoffset) * (-2*yoffset)) - ((-0.5*xoffset) - (-2*xoffset)));
712  surfaceNormal.normalise();
713  glNormal3f(surfaceNormal.x, surfaceNormal.y, surfaceNormal.z);
714  glColor4f(r, g, b/2, t);
715  glVertex3f(xx+xoffset, yy+yoffset, zz);
716  glColor4f(r, g/2, b, t);
717  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
718  glColor4f(r, g, b/2, t);
719  glVertex3f(xx-xoffset+0.5*yoffset, yy-yoffset-0.5*xoffset, zz);
720 
721  surfaceNormal.x = (((+0.5*xoffset) * (-rad)) - ((+rad) - (-2*yoffset)));
722  surfaceNormal.y = (((rad) * (-2*xoffset)) - ((-0.5*yoffset) - (rad)));
723  surfaceNormal.z = (((-0.5*yoffset) * (-2*yoffset)) - ((+0.5*xoffset) - (-2*xoffset)));
724  surfaceNormal.normalise();
725  glNormal3f(surfaceNormal.x, surfaceNormal.y, surfaceNormal.z);
726  glColor4f(r, g/2, b, t);
727  glVertex3f(xx+xoffset, yy+yoffset, zz);
728  glColor4f(r, g, b/2, t);
729  glVertex3f(xx-xoffset, yy-yoffset, zz-rad);
730  glColor4f(r, g/2, b, t);
731  glVertex3f(xx-xoffset-0.5*yoffset, yy-yoffset+0.5*xoffset, zz);
732  glEnd();
733 }
734 
735 
736 void Directional2DEnvironment::OpenGLDraw(const xySpeedHeading& l, const deltaSpeedHeading &) const
737 {
738  GLdouble xx, yy, zz, rad;
739  map->GetOpenGLCoord(l.x, l.y, xx, yy, zz, rad);
740  glColor3f(0.5f, 0.5f, 0.5);
741  DrawSphere(xx-rad+l.x, yy-rad+l.y, zz, rad);
742 }
743 
744 
745 void Directional2DEnvironment::GLDrawLine(const xySpeedHeading &a, const xySpeedHeading &b) const
746 {
747  GLdouble xx, yy, zz, rad;
748  map->GetOpenGLCoord(a.x, a.y, xx, yy, zz, rad);
749 
750  GLfloat rr, gg, bb, t;
751  GetColor(rr, gg, bb, t);
752  glColor4f(rr, gg, bb, t);
753 
754  glBegin(GL_LINES);
755  glVertex3f(xx, yy, zz-rad/2);
756  map->GetOpenGLCoord(b.x, b.y, xx, yy, zz, rad);
757  glVertex3f(xx, yy, zz-rad/2);
758  glEnd();
759 }
760 
761 void Directional2DEnvironment::GetNextState(const xySpeedHeading &, deltaSpeedHeading , xySpeedHeading &) const
762 {
763  assert(false);
764 }
765 
766 //int Directional2DEnvironment::GetModelUniqueAngles()
767 //{
768 // const int angles[3] = {3, 3, 4};
769 // return angles[motionModel];
770 //}
771 
772 void Directional2DEnvironment::BuildHTable(dirHeuristicTable &t)
773 {
774  const int speedCnt[3] = {3, 5, 3}; // number of possible speeds
775  const int speeds[3] = {0, 1, 1}; // offset to make all speeds positive
776  const int angles[3] = {16, 16, 24};
777  checkLegal = false;
778  // rotation is 0..15
779  // speed is 0..3
780 
781  // 9x9
782  //std::vector<std::vector<int> > hTable;
783 // hTable.resize(GetModelUniqueAngles());
784  t.hTable.resize(ROW_SIZE*ROW_SIZE);
785  for (unsigned int x = 0; x < t.hTable.size(); x++)
786  {
787  t.hTable[x].resize(speedCnt[motionModel]*angles[motionModel]); // 64 = 4 speeds * 16 angles
788  for (unsigned int y = 0; y < t.hTable[x].size(); y++)
789  {
790  t.hTable[x][y] = -1;
791  }
792  }
793  // goal is in the middle
794 // hTable[(hTable.size()-1)/2][0] = 0;
795  xySpeedHeading base(0, 0);
796  base.speed = t.speed;
797  base.rotation = t.rotation;
798  int i1, i2;
799  if (LookupStateHashIndex(base, i1, i2))
800  {
801  t.hTable[i1][i2] = 0;
802  }
803  else {
804  assert(!"Directional2DEnvironment::BuildHTable - Illegal base state");
805  }
806 
807 // int i2 = 0+((0+speeds[motionModel])*angles[motionModel]);
808 // t.hTable[(t.hTable.size()-1)/2][i2] = 0;
809 
810 // if (motionModel == kTank)
811 // {
812 // BuildAStarTable();
813 // checkLegal = true;
814 // return;
815 // }
816 
817  //for (int t = 0; t <= TABLE_SIZE*TABLE_SIZE; t++)
818  int updates = 1;
819  while (updates != 0)
820  {
821 // printf("%d updates\n", updates);
822  updates = 0;
823  for (unsigned int x = 0; x < t.hTable.size(); x++)
824  {
825  for (unsigned int y = 0; y < t.hTable[x].size(); y++)
826  {
827  xySpeedHeading s;
828  s.x = (float)(x%ROW_SIZE)-(TABLE_SIZE-0.5);//3.5f;
829  s.y = (float)(x/ROW_SIZE)-(TABLE_SIZE-0.5);//3.5f;
830  s.rotation = y%angles[motionModel];
831  s.speed = y/angles[motionModel]-speeds[motionModel];
832  //printf("Unhashing rot: %d speed: %d from %d\n", s.rotation, s.speed, y);
833 
834 // std::cout << "Getting successors of " << s << ":\n";
835 
836  std::vector<xySpeedHeading> succ;
837  GetSuccessors(s, succ);
838  for (unsigned int z = 0; z < succ.size(); z++)
839  {
840  float val = LookupStateHash(succ[z], t);
841 // std::cout << succ[z] << " has val " << val << "\n";
842  if (val != -1)
843  {
844  if (t.hTable[x][y] == -1)
845  {
846 // int x1 = floorf(s.x);
847 // int y1 = floorf(s.y);
848 // int index1 = (x1+TABLE_SIZE)+(y1+TABLE_SIZE)*(ROW_SIZE);
849 // int index2 = (s.rotation&0xF)|((s.speed&0x3)<<4);
850 // std::cout << "{" << index1 << ", " << index2 << "} ";
851 
852  t.hTable[x][y] = val+GCost(s, succ[z]);
853  updates++;
854 // std::cout << "Assigning " << val+GCost(s, succ[z]) << " to " << s <<
855 // " (" << x << ", " << y << ") from " << succ[z] << std::endl;
856  }
857  else {
858  if (fless(val+GCost(s, succ[z]), t.hTable[x][y]))
859  {
860 // std::cout << "Updating " << val+GCost(s, succ[z]) << " to " << s <<
861 // " (" << x << ", " << y << ") from " << succ[z] << std::endl;
862  t.hTable[x][y] = std::min(t.hTable[x][y], (float)(val+GCost(s, succ[z])));
863  updates++;
864  }
865  }
866  }
867  }
868 // if ((t == 15) && (hTable[x][y] == -1))
869 // hTable[x][y] = 16;
870  }
871  }
872  }
873 
874 // for (unsigned int x = 0; x < hTable.size(); x++)
875 // {
876 // for (unsigned int y = 0; y < hTable[x].size(); y++)
877 // {
878 // xySpeedHeading s;
879 // s.x = (float)(x%ROW_SIZE)-(TABLE_SIZE-0.5);//3.5f;
880 // s.y = (float)(x/ROW_SIZE)-(TABLE_SIZE-0.5);//3.5f;
881 // s.rotation = y%angles[motionModel]; ////y&0xF;
882 // s.speed = y/angles[motionModel]-speeds[motionModel]; ////(y>>4)&0x3;
883 // std::cout << s << " has value of: " << hTable[x][y] << std::endl;
884 // }
885 // std::cout << "---------------" << std::endl;
886 // }
887  checkLegal = true;
888 }
889 
890 bool Directional2DEnvironment::LookupStateHashIndex(const xySpeedHeading &s,
891  int &index1, int &index2) const
892 {
893  const int speeds[4] = {0, 1, 1, 1}; // offset to make all speeds positive
894  const int angles[4] = {16, 16, 24, 24};
895 
896  int x = floorf(s.x);
897  int y = floorf(s.y);
898 
899  if ((x < -TABLE_SIZE) || (x > TABLE_SIZE) || (y < -TABLE_SIZE) || (y > TABLE_SIZE))
900  {
901  return false;
902  }
903  index1 = (x+TABLE_SIZE)+(y+TABLE_SIZE)*(ROW_SIZE);
904  index2 = (s.rotation)+((s.speed+speeds[motionModel])*angles[motionModel]);
905  return true;
906 }
907 
908 float Directional2DEnvironment::LookupStateHash(const xySpeedHeading &s, const dirHeuristicTable &t) const
909 {
910  const int speeds[4] = {0, 1, 1, 1}; // offset to make all speeds positive
911  const int angles[4] = {16, 16, 24, 24};
912 
913  int x = floorf(s.x);
914  int y = floorf(s.y);
915 
916  if ((x < -TABLE_SIZE) || (x > TABLE_SIZE) || (y < -TABLE_SIZE) || (y > TABLE_SIZE))
917  {
918  return -1;
919  }
920  int index1 = (x+TABLE_SIZE)+(y+TABLE_SIZE)*(ROW_SIZE);
921  int index2 = (s.rotation)+((s.speed+speeds[motionModel])*angles[motionModel]);
922  //printf("Hashing rot: %d speed: %d to %d\n", s.rotation, s.speed, index2);
924  return t.hTable[index1][index2];
925 }
926 
927 float Directional2DEnvironment::LookupStateHeuristic(const xySpeedHeading &s1, const xySpeedHeading &s2) const
928 {
929  const int angles[4] = {16, 16, 24, 24};
930  const int angles90[4] = {4, 4, 6, 6};
931 
932  if (hType == kOctileHeuristic)
933  return 0;
934 
935 // if (s2.speed != 0)
936 // return 0;
937 
938  xySpeedHeading s3;
939  s3 = s1;
940  s3.x = floor(s3.x) - floor(s2.x);
941  s3.y = floor(s3.y) - floor(s2.y);
942 
943  int whichHeuristic = -1;
944  for (unsigned int x = 0; x < heuristics.size(); x++)
945  {
946  if (heuristics[x].rotation == s2.rotation)
947  {
948  //printf("Found exact match\n");
949  whichHeuristic = x;
950  break;
951  }
952  else if ((heuristics[x].rotation%angles90[motionModel]) == s2.rotation%angles90[motionModel])
953  {
954  //printf("Found 90 degree match [%d] Our goal: %d\n", heuristics[x].rotation, s2.rotation);
955 
956  // rotate so they match
957  int rotation = -heuristics[x].rotation+s2.rotation;
958  if (rotation < 0)
959  rotation += angles[motionModel];
960  //std::cout << "Before rotation: " << s3 << std::endl;
961  RotateCCW(s3, rotation);
962  //std::cout << "After rotation: " << s3 << std::endl;
963  //assert(heuristics[x].rotation == s3.rotation);
964  whichHeuristic = x;
965  break;
966  }
967  }
968  if (whichHeuristic == -1)
969  {
970  assert(!"No heuristic built");
971  return 0;
972 // // old code that breaks const correctness
973 // int which = heuristics.size();
974 // heuristics.resize(which+1);
975 // // build new heuristic
976 // heuristics[which].speed = 0;
977 // heuristics[which].rotation = s2.rotation;
978 // BuildHTable(heuristics[which]);
979 // whichHeuristic = which;
980 
981  }
982 
983 // std::cout << "Heuristic looking up: " << s3 << std::endl;
984  float val = LookupStateHash(s3, heuristics[whichHeuristic]);
985  if (val != -1)
986  {
987 // printf("Looked up %d\n", val);
988  return val;
989  }
990  if (hType == kPerimeterHeuristic)
991  return 0;
992 
993  s3.x/=2;
994  s3.y/=2;
995  val = LookupStateHash(s3, heuristics[whichHeuristic]);
996  if (val != -1)
997  {
998  if (motionModel != kVehicle)
999  return 2.0*val-2;
1000  else
1001  return 1.5*val;
1002  }
1003 
1004  return 0;
1005 }
1006 
1007 //void Directional2DEnvironment::BuildAStarTable()
1008 //{
1009 // //const int speedCnt[3] = {3, 5, 3}; // offset to make all speeds positive
1010 // const int speeds[3] = {0, 1, 1}; // offset to make all speeds positive
1011 // const int angles[3] = {16, 16, 24};
1012 //
1013 // heuristicType h = hType;
1014 // hType = kOctileHeuristic;
1015 //
1016 // TemplateAStar<xySpeedHeading, deltaSpeedHeading, Directional2DEnvironment> a;
1017 // std::vector<xySpeedHeading> path;
1018 // xySpeedHeading goal;
1019 // xySpeedHeading start;
1020 // goal.x = 0.5;
1021 // goal.y = 0.5;
1022 // goal.rotation = angles[motionModel]/2;
1023 // goal.speed = 0;
1024 // start.x = 0.5;
1025 // start.y = 0.5;
1026 // start.rotation = 0;
1027 // start.speed = 5;
1028 // a.SetStopAfterGoal(false);
1029 // a.InitializeSearch(this, goal, start, path);
1030 // while (!a.DoSingleSearchStep(path))
1031 // { static int cnt = 0; if ((++cnt)%1000 == 0) printf("%d\n", cnt); if (cnt > 100000) break; }
1032 //
1033 // for (unsigned int x = 0; x < hTable.size(); x++)
1034 // {
1035 // printf("Entry %d of %d\n", x, (int)hTable.size());
1036 // for (unsigned int y = 0; y < hTable[x].size(); y++)
1037 // {
1038 // //printf("Sub-entry %d of %d\n", y, (int)hTable[x].size());
1039 // xySpeedHeading s;
1040 // s.x = (float)(x%ROW_SIZE)-(TABLE_SIZE-0.5);//3.5f;
1041 // s.y = (float)(x/ROW_SIZE)-(TABLE_SIZE-0.5);//3.5f;
1042 // // reverse rotation for reverse search
1043 // s.rotation = (angles[motionModel]/2 + y%angles[motionModel])%angles[motionModel]; ////y&0xF;
1044 // s.speed = y/angles[motionModel]-speeds[motionModel]; ////(y>>4)&0x3;
1045 // double cost;
1046 // a.GetClosedListGCost(s, cost);
1047 //
1048 // //a.GetPath(this, s, goal, path);
1049 //
1050 // hTable[x][y] = cost; //GetPathLength(path);
1051 // }
1052 // }
1053 //
1054 //
1055 // hType = h;
1056 //}
1057 
1058 void Directional2DEnvironment::BuildAngleTables()
1059 {
1060  int DEG = 16;
1061  if ((motionModel == kTank) || (motionModel == kBetterTank))
1062  DEG = 24;
1063  for (int x = 0; x < DEG; x++)
1064  {
1065  sinTable.push_back(sin(TWOPI*((float)x)/(float)DEG));
1066  //printf("sin(%d) = %f\n", x, sinTable.back());
1067  cosTable.push_back(cos(TWOPI*((float)x)/(float)DEG));
1068  //printf("cos(%d) = %f\n", x, cosTable.back());
1069  }
1070 }
1071 
1072 float Directional2DEnvironment::mySin(int dir) const
1073 {
1074  return sinTable[dir];
1075 }
1076 
1077 float Directional2DEnvironment::myCos(int dir) const
1078 {
1079  return cosTable[dir];
1080 }
1081 
dirHeuristicTable::speed
int8_t speed
Definition: Directional2DEnvironment.h:25
deltaSpeedHeading::turn
int8_t turn
Definition: Directional2DEnvironment.h:77
dirHeuristicTable
Definition: Directional2DEnvironment.h:23
Directional2DEnvironment::hType
heuristicType hType
Definition: Directional2DEnvironment.h:163
kHumanoid
@ kHumanoid
Definition: Directional2DEnvironment.h:93
xySpeedHeading::speed
int8_t speed
Definition: Directional2DEnvironment.h:88
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
Map::OpenGLDraw
void OpenGLDraw(tDisplay how=kPolygons) const
Does actual OpenGL drawing of the map.
Definition: Map.cpp:1777
recVec::z
GLdouble z
Definition: GLUtil.h:98
Directional2DEnvironment::cosTable
std::vector< float > cosTable
Definition: Directional2DEnvironment.h:168
Directional2DEnvironment::GetStateHash
uint64_t GetStateHash(const xySpeedHeading &node) const
Definition: Directional2DEnvironment.cpp:620
dirHeuristicTable::hTable
std::vector< std::vector< float > > hTable
Definition: Directional2DEnvironment.h:27
Directional2DEnvironment::GetNumAngles
int GetNumAngles()
Definition: Directional2DEnvironment.cpp:45
if
if(state==GLUT_DOWN)
Definition: GLUThog.cpp:244
kVehicle
@ kVehicle
Definition: Directional2DEnvironment.h:94
kBetterTank
@ kBetterTank
Definition: Directional2DEnvironment.h:98
Directional2DEnvironment::GetActionHash
uint64_t GetActionHash(deltaSpeedHeading act) const
Definition: Directional2DEnvironment.cpp:634
model
model
Definition: Directional2DEnvironment.h:92
Directional2DEnvironment::GetActions
void GetActions(const xySpeedHeading &nodeID, std::vector< deltaSpeedHeading > &actions) const
Definition: Directional2DEnvironment.cpp:71
Directional2DEnvironment::BuildAngleTables
void BuildAngleTables()
Definition: Directional2DEnvironment.cpp:1058
FPUtil.h
DrawSphere
void DrawSphere(GLdouble _x, GLdouble _y, GLdouble _z, GLdouble tRadius)
Definition: GLUtil.cpp:433
rotation
float rotation[3][3]
Definition: RC.cpp:21
Directional2DEnvironment::Legal
bool Legal(const xySpeedHeading &node1, const deltaSpeedHeading &act) const
Definition: Directional2DEnvironment.cpp:500
kTank
@ kTank
Definition: Directional2DEnvironment.h:95
Directional2DEnvironment::GoalTest
bool GoalTest(const xySpeedHeading &node, const xySpeedHeading &goal) const
Definition: Directional2DEnvironment.cpp:611
Directional2DEnvironment::InvertAction
virtual bool InvertAction(deltaSpeedHeading &a) const
Definition: Directional2DEnvironment.cpp:251
xySpeedHeading
Definition: Directional2DEnvironment.h:82
xySpeedHeading::x
float x
Definition: Directional2DEnvironment.h:86
kGround
@ kGround
Definition: Map.h:55
Directional2DEnvironment::UndoAction
virtual void UndoAction(xySpeedHeading &s, deltaSpeedHeading dir) const
Definition: Directional2DEnvironment.cpp:380
kOctileHeuristic
@ kOctileHeuristic
Definition: Directional2DEnvironment.h:102
SearchEnvironment< xySpeedHeading, deltaSpeedHeading >::SetColor
virtual void SetColor(const rgbColor &r) const
Definition: SearchEnvironment.h:102
Directional2DEnvironment::map
Map * map
Definition: Directional2DEnvironment.h:170
loc
Definition: MapGenerators.cpp:296
Directional2DEnvironment::HCost
virtual double HCost(const xySpeedHeading &node1, const xySpeedHeading &node2) const
Heuristic value between two arbitrary nodes.
Definition: Directional2DEnvironment.cpp:486
Directional2DEnvironment::GCost
virtual double GCost(const xySpeedHeading &node1, const xySpeedHeading &node2) const
Definition: Directional2DEnvironment.cpp:566
Directional2DEnvironment::heuristics
std::vector< dirHeuristicTable > heuristics
Definition: Directional2DEnvironment.h:165
Directional2DEnvironment::~Directional2DEnvironment
virtual ~Directional2DEnvironment()
Definition: Directional2DEnvironment.cpp:40
fless
bool fless(double a, double b)
Definition: FPUtil.h:28
Directional2DEnvironment::GLDrawLine
void GLDrawLine(const xySpeedHeading &a, const xySpeedHeading &b) const
Definition: Directional2DEnvironment.cpp:745
xySpeedHeading::y
float y
Definition: Directional2DEnvironment.h:87
dirHeuristicTable::rotation
int8_t rotation
Definition: Directional2DEnvironment.h:26
Directional2DEnvironment::motionModel
model motionModel
Definition: Directional2DEnvironment.h:162
Directional2DEnvironment::OpenGLDraw
virtual void OpenGLDraw() const
Definition: Directional2DEnvironment.cpp:639
Directional2DEnvironment::Directional2DEnvironment
Directional2DEnvironment(Map *m, model envType=kVehicle, heuristicType heuristic=kExtendedPerimeterHeuristic)
Definition: Directional2DEnvironment.cpp:18
Directional2DEnvironment::GetNextState
virtual void GetNextState(const xySpeedHeading &currents, deltaSpeedHeading dir, xySpeedHeading &news) const
Definition: Directional2DEnvironment.cpp:761
Directional2DEnvironment::myCos
float myCos(int dir) const
Definition: Directional2DEnvironment.cpp:1077
deltaSpeedHeading::speed
int8_t speed
Definition: Directional2DEnvironment.h:78
TemplateAStar.h
SearchEnvironment< xySpeedHeading, deltaSpeedHeading >::GetColor
virtual rgbColor GetColor() const
Definition: SearchEnvironment.h:105
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
heuristicType
heuristicType
Definition: Directional2DEnvironment.h:101
Directional2DEnvironment::RotateCCW
void RotateCCW(xySpeedHeading &s, unsigned int rotation) const
Definition: Directional2DEnvironment.cpp:263
Directional2DEnvironment::LookupStateHashIndex
bool LookupStateHashIndex(const xySpeedHeading &s, int &index1, int &index2) const
Definition: Directional2DEnvironment.cpp:890
Directional2DEnvironment::ApplyAction
virtual void ApplyAction(xySpeedHeading &s, deltaSpeedHeading dir) const
Definition: Directional2DEnvironment.cpp:273
max
#define max(a, b)
Definition: MinimalSectorAbstraction.cpp:40
Directional2DEnvironment.h
GoalTester::goalTest
virtual bool goalTest(const xySpeedHeading &i1) const =0
Directional2DEnvironment::mySin
float mySin(int dir) const
Definition: Directional2DEnvironment.cpp:1072
deltaSpeedHeading
Definition: Directional2DEnvironment.h:75
xySpeedHeading::rotation
int8_t rotation
Definition: Directional2DEnvironment.h:89
TABLE_SIZE
#define TABLE_SIZE
Definition: Directional2DEnvironment.cpp:14
TWOPI
static const double TWOPI
Definition: GLUtil.h:65
kPerimeterHeuristic
@ kPerimeterHeuristic
Definition: Directional2DEnvironment.h:103
Directional2DEnvironment::BuildHTable
void BuildHTable(dirHeuristicTable &t)
Definition: Directional2DEnvironment.cpp:772
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
Directional2DEnvironment::test
GoalTester * test
Definition: Directional2DEnvironment.h:161
Directional2DEnvironment::LookupStateHash
float LookupStateHash(const xySpeedHeading &s, const dirHeuristicTable &t) const
Definition: Directional2DEnvironment.cpp:908
recVec::y
GLdouble y
Definition: GLUtil.h:98
ROW_SIZE
#define ROW_SIZE
Definition: Directional2DEnvironment.cpp:15
fequal
bool fequal(double a, double b, double tolerance=TOLERANCE)
Definition: FPUtil.h:32
Directional2DEnvironment::checkLegal
bool checkLegal
Definition: Directional2DEnvironment.h:160
recVec::x
GLdouble x
Definition: GLUtil.h:98
recVec::normalise
void normalise()
Normalize a vector.
Definition: GLUtil.cpp:39
Directional2DEnvironment::sinTable
std::vector< float > sinTable
Definition: Directional2DEnvironment.h:169
Directional2DEnvironment::GetSuccessors
void GetSuccessors(const xySpeedHeading &nodeID, std::vector< xySpeedHeading > &neighbors) const
Definition: Directional2DEnvironment.cpp:58
Directional2DEnvironment::GetAction
deltaSpeedHeading GetAction(const xySpeedHeading &s1, const xySpeedHeading &s2) const
Definition: Directional2DEnvironment.cpp:231
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
Map
A tile-based representation of the world.
Definition: Map.h:142
Directional2DEnvironment::LookupStateHeuristic
float LookupStateHeuristic(const xySpeedHeading &s1, const xySpeedHeading &s2) const
Definition: Directional2DEnvironment.cpp:927
Generated by   doxygen 1.8.17