HOG2
WeightedMap2DEnvironment.cpp
Go to the documentation of this file.
1 #include "WeightedMap2DEnvironment.h"
2 
3 using namespace GraphAbstractionConstants;
4 
10 WeightedMap2DEnvironment::WeightedMap2DEnvironment(MapAbstraction *mapAbs)
11 :AbsMapEnvironment(mapAbs)
12 {
13  // initialize all edge counters to 0
14  //Graph* g = ma->GetAbstractGraph(0);
15  //edge_iterator ei = g->getEdgeIter();
16 
17  oi = new BaseMapOccupancyInterface(map);
18  diffWeight = 1;
19  oldProportion = 0.7;
20 
21  useWindow = false;
22  noWeighting = false;
23 
24  queryOldProportion = 0.9;
25  updateOnQuery = false;
26 
27  updateSurrounding = false;
28  surroundingProportion = 0.9;
29  //for (edge* e = g->edgeIterNext(ei); e; e = g->edgeIterNext(ei))
30  //{
31  // // Reset the counters
32  // e->SetLabelL(kForwardCount, 0);
33  // e->SetLabelL(kBackwardCount, 0);
34  //}
35 
36  usePerceptron = false;
37  learningRate = 0.1;
38 
39 }
40 
41 WeightedMap2DEnvironment::WeightedMap2DEnvironment(AbsMapEnvironment *ame)
42 :AbsMapEnvironment(ame->GetMapAbstraction()->Clone(ame->GetMapAbstraction()->GetMap()->Clone()))
43 {
44  oi = ame->GetOccupancyInfo();
45  diffWeight = 1;
46  oldProportion = 0.7;
47  useWindow = false;
48  noWeighting = false;
49 
50  queryOldProportion = 0.9;
51  updateOnQuery = false;
52 
53  updateSurrounding = false;
54  surroundingProportion = 0.9;
55 
56  usePerceptron = false;
57  learningRate = 0.1;
58 
59 }
60 
64 WeightedMap2DEnvironment::~WeightedMap2DEnvironment()
65 {
66  angleLookup.clear();
67 }
68 
77 void WeightedMap2DEnvironment::ApplyAction(xyLoc &s, tDirection dir) const
78 {
79  // this isn't called
80 
81  //std::cout<<"Weighted update action\n";
82  //Vector2D angle;
83  xyLoc old = s;
84  switch (dir)
85  {
86  case kN: s.y-=1; /*angle.Set(0,-1);*/ break;
87  case kS: s.y+=1; /*angle.Set(0,1);*/break;
88  case kE: s.x+=1; /*angle.Set(1,0);*/ break;
89  case kW: s.x-=1; /*angle.Set(-1,0);*/break;
90  case kNW: s.y-=1; s.x-=1; /*angle.Set(-1,-1);*/ break;
91  case kSW: s.y+=1; s.x-=1; /*angle.Set(-1,1);*/break;
92  case kNE: s.y-=1; s.x+=1; /*angle.Set(1,-1);*/ break;
93  case kSE: s.y+=1; s.x+=1; /*angle.Set(1,1);*/ break;
94  default: break;
95  }
96 
97 
98 
99  // May need to do different check as well oi->CanMove for example
100  if (map->CanStep(s.x, s.y, old.x, old.y))// && !(oi->GetStateOccupied(s)))(should be done in unitSim)
101  {
102 // // Update the edge weight (mark the edge)
103 // node* from = ma->GetNodeFromMap(old.x, old.y);
104 // node* to = ma->GetNodeFromMap(s.x, s.y);
105 // edge* e = ma->GetAbstractGraph(0)->FindEdge(from->GetNum(),to->GetNum());
106 //
107 // if (e->getFrom() == from->GetNum())
108 // e->SetLabelL(kForwardCount, e->GetLabelL(kForwardCount) + 1);
109 // else
110 // e->SetLabelL(kBackwardCount, e->GetLabelL(kBackwardCount)+1);
111 
112  // Update the angle
113 
114 /* double oldProportion = 0.8;
115  double newProportion = 1 - oldProportion;
116 
117  // Update angle on new location
118  AngleUtil::AngleSearchNode sn(s,GetStateHash(s));
119  if (angleLookup.find(sn) == angleLookup.end())
120  {
121  angleLookup[sn] = angle;
122  }
123  else
124  {
125  Vector2D oldAngle = angleLookup[sn];
126  angleLookup[sn] = oldProportion * oldAngle + newProportion * angle;
127  }
128 
129  // Update angle on old location
130  AngleUtil::AngleSearchNode sn2(old,GetStateHash(old));
131  if (angleLookup.find(sn2) == angleLookup.end())
132  {
133  angleLookup[sn2] = angle;
134  }
135  else
136  {
137  Vector2D oldAngle = angleLookup[sn2];
138  angleLookup[sn2] = oldProportion * oldAngle + newProportion * angle;
139  }
140 
141  //UnitSimulation will deal with all occupancy interface 'stuff'
142  //oi->SetStateOccupied(s, false);
143  //oi->SetStateOccupied(old, true);*/
144 
145  return;
146  }
147  s = old;
148 }
149 
150 void WeightedMap2DEnvironment::UpdateAngle(const xyLoc &old, const xyLoc &s, double oldProp, double t)
151 {
152  if (old == s)
153  return;
154  tDirection dir = GetAction(old, s);
155  Vector2D angle;
156 
157  switch (dir)
158  {
159  case kN: angle.Set(0,-1); break;
160  case kS: angle.Set(0,1);break;
161  case kE: angle.Set(1,0); break;
162  case kW: angle.Set(-1,0);break;
163  case kNW: angle.Set(-1,-1); break;
164  case kSW: angle.Set(-1,1);break;
165  case kNE: angle.Set(1,-1); break;
166  case kSE: angle.Set(1,1); break;
167  default: break;
168  }
169  angle.Normalize(); //<-- otherwise diagonal ones are messed up
170  double newProportion = 1 - oldProp;
171 
172  if (updateSurrounding)
173  {
174  //Update new location
175  AngleUtil::AngleSearchNode sn(s,GetStateHash(s));
176  if (angleLookup.find(sn) == angleLookup.end())
177  {
178  if (usePerceptron)
179  {
180  Vector2D v = learningRate * angle;
181  v.SetUpdateTime(t);
182  angleLookup[sn] = learningRate * angle;
183 
184  }
185  else
186  {
187  angle.Normalize();
188  angle.SetUpdateTime(t);
189  angleLookup[sn] = angle;
190  }
191  }
192  else
193  {
194  if (usePerceptron)
195  {
196  Vector2D oldAngle = angleLookup[sn];
197 
198  float x = oldAngle.x - learningRate * (oldAngle.x - angle.x);
199  float y = oldAngle.y - learningRate * (oldAngle.y - angle.y);
200 
201  Vector2D newAngle(x,y);
202 // std::cout<<sqrt(x*x + y*y)<<std::endl;
203  if (sqrt(x*x + y*y) > 1)
204  newAngle.Normalize();
205 
206  newAngle.SetUpdateTime(t);
207 
208  angleLookup[sn] = newAngle;
209  }
210  else
211  {
212  Vector2D oldAngle = angleLookup[sn];
213 
214  Vector2D storeme = oldProp * oldAngle + newProportion * angle;
215  storeme.Normalize();
216  storeme.SetUpdateTime(t);
217  angleLookup[sn] = storeme;
218  }
219  }
220 
221  //Update all neighbours of new location
222  std::vector<xyLoc> neighbours;
223  GetSuccessors(s, neighbours);
224 
225  //oldProp = surroundingProportion;
226  //newProportion = 1 - oldProp;
227 
228  for (unsigned int i=0; i<neighbours.size(); i++)
229  {
230  AngleUtil::AngleSearchNode searchNode(neighbours[i],GetStateHash(neighbours[i]));
231  if (angleLookup.find(searchNode) == angleLookup.end())
232  {
233  if (usePerceptron)
234  {
235  Vector2D v = surroundingProportion * angle;
236  v.SetUpdateTime(t);
237  angleLookup[searchNode] = v;
238  }
239  else
240  {
241  angle.Normalize();
242  angle.SetUpdateTime(t);
243  angleLookup[searchNode] = angle;
244  }
245  }
246  else
247  {
248  if (usePerceptron)
249  {
250  Vector2D oldAngle = angleLookup[searchNode];
251 
252  float x = oldAngle.x - surroundingProportion * (oldAngle.x - angle.x);
253  float y = oldAngle.y - surroundingProportion * (oldAngle.y - angle.y);
254 
255  Vector2D newAngle(x,y);
256 // std::cout<<sqrt(x*x + y*y)<<std::endl;
257  if (sqrt(x*x + y*y) > 1)
258  newAngle.Normalize();
259  newAngle.SetUpdateTime(t);
260  angleLookup[searchNode] = newAngle;
261  }
262  else
263  {
264  Vector2D oldAngle = angleLookup[searchNode];
265  Vector2D storeme = oldProp * oldAngle + newProportion * angle;
266  storeme.Normalize();
267  storeme.SetUpdateTime(t);
268  angleLookup[searchNode] = storeme;
269  }
270  }
271  }
272  } // end if (updateSurrounding)
273  else
274  {
275  // Update angle on new location
276  AngleUtil::AngleSearchNode searchNode(s,GetStateHash(s));
277  if (angleLookup.find(searchNode) == angleLookup.end())
278  {
279  if (usePerceptron)
280  {
281  Vector2D v = learningRate * angle;
282  v.SetUpdateTime(t);
283  angleLookup[searchNode] = v;
284  }
285  else
286  {
287  angle.Normalize();
288  angle.SetUpdateTime(t);
289  angleLookup[searchNode] = angle;
290  }
291  }
292  else
293  {
294  if (usePerceptron)
295  {
296  Vector2D oldAngle = angleLookup[searchNode];
297 
298  float x = oldAngle.x - learningRate * (oldAngle.x - angle.x);
299  float y = oldAngle.y - learningRate * (oldAngle.y - angle.y);
300 
301  Vector2D newAngle(x,y);
302 // std::cout<<sqrt(x*x + y*y)<<std::endl;
303  if (sqrt(x*x + y*y) > 1)
304  newAngle.Normalize();
305 
306  newAngle.SetUpdateTime(t);
307  angleLookup[searchNode] = newAngle;
308  }
309  else
310  {
311  Vector2D oldAngle = angleLookup[searchNode];
312  Vector2D storeme = oldProp * oldAngle + newProportion * angle;
313  storeme.Normalize();
314  storeme.SetUpdateTime(t);
315  angleLookup[searchNode] = storeme;
316  }
317  }
318 
319  // Update angle on old location
320  AngleUtil::AngleSearchNode searchNode2(old,GetStateHash(old));
321  if (angleLookup.find(searchNode2) == angleLookup.end())
322  {
323  if (usePerceptron)
324  {
325  Vector2D v = learningRate * angle;
326  v.SetUpdateTime(t);
327  angleLookup[searchNode2] = v; // this was sn before, I changed it to sn2, but it could be wrong
328  }
329  else
330  {
331  angle.Normalize();
332  angle.SetUpdateTime(t);
333  angleLookup[searchNode2] = angle; // this was sn before, I changed it to sn2, but it could be wrong
334  }
335  }
336  else
337  {
338  if (usePerceptron)
339  {
340  Vector2D oldAngle = angleLookup[searchNode];
341 
342  float x = oldAngle.x - learningRate * (oldAngle.x - angle.x);
343  float y = oldAngle.y - learningRate * (oldAngle.y - angle.y);
344 
345  Vector2D newAngle(x,y);
346 // std::cout<<sqrt(x*x + y*y)<<std::endl;
347  if (sqrt(x*x + y*y) > 1)
348  newAngle.Normalize();
349 
350  newAngle.SetUpdateTime(t);
351  angleLookup[searchNode] = newAngle;
352  }
353  else
354  {
355  Vector2D oldAngle = angleLookup[searchNode];
356  Vector2D storeme = oldProp * oldAngle + newProportion * angle;
357  storeme.Normalize();
358  storeme.SetUpdateTime(t);
359  angleLookup[searchNode] = storeme;
360  }
361  }
362  }
363  return;
364 }
365 
366 
367 void WeightedMap2DEnvironment::UpdateAngle(const xyLoc &old, const xyLoc &s, double t)
368 {
369  UpdateAngle(old,s,oldProportion,t);
370 }
371 
379 double WeightedMap2DEnvironment::GCost(const xyLoc &l1, const xyLoc &l2) const
380 {
381  // Update angles
382 // if (updateOnQuery)
383 // {
384 // UpdateAngle(l1,l2,queryOldProportion);
385 // }
386 
387 
388  //std::cou<t<"l1 "<<l1<<" l2 "<<l2<<std::endl;
389  double hCost = HCost(l1, l2);
390 
391  if (noWeighting)
392  return hCost;
393 
394  // No weighting if we're using the window and we're outside the window
395  if (useWindow && ( (HCost(windowCenter,l1) > windowSize) ||
396  (HCost(windowCenter, l2) > windowSize)))
397  {
398  return hCost;
399  }
400 
401  if (fgreater(hCost, ROOT_TWO))
402  return DBL_MAX;
403 
404  tDirection dir = GetAction(l1,l2);
405 
406  Vector2D angle = GetAngleFromDirection(dir);
407 
408  AngleUtil::AngleSearchNode sn(l1,GetStateHash(l1));
409 
410  double weight1 = 0; double weight2 = 0;
411  auto loc = angleLookup.find(sn);
412  if (loc != angleLookup.end())
413  {
414  Vector2D old = loc->second;//angleLookup[sn];
415 
416  //double returnme = h + ( ((old.x-angle.x)*(old.x-angle.x)+(old.y-angle.y)*(old.y-angle.y)));
417 
418  weight1 = diffWeight * ((1 - ((old.x * angle.x)+(old.y * angle.y)))/2);
419 /* if (old == angle)
420  {
421  return h;
422  }
423  else
424  {
425  return 10 * h;
426  }*/
427  }
428 
429  AngleUtil::AngleSearchNode sn2(l2,GetStateHash(l2));
430  auto loc2 = angleLookup.find(sn);
431  if (loc2 != angleLookup.end())
432  {
433  Vector2D old = loc2->second;//angleLookup[sn2];
434  weight2 = diffWeight * ((1 - ((old.x * angle.x)+(old.y * angle.y)))/2);
435  }
436 
437  return hCost + (0.5 * weight1 + 0.5 * weight2);
438 
439 /* if (l1 == l2)
440  return 0;
441  // Get the edge between the two locations
442  Graph* g = ma->GetAbstractGraph(0);
443  node* from = ma->GetNodeFromMap(l1.x, l1.y);
444  node* to = ma->GetNodeFromMap(l2.x, l2.y);
445  edge* e = g->FindEdge(from->GetNum(),to->GetNum());
446 
447  double weight = 0;
448 
449  // add weight -- 1 / (# units that have passed in the other direction)
450  if (e->getFrom() == from->GetNum())
451  {
452  if (e->GetLabelL(kBackwardCount) > e->GetLabelL(kForwardCount))
453  {
454  weight = 1.0-(1.0 / (e->GetLabelL(kBackwardCount) - e->GetLabelL(kForwardCount)));
455  }
456  else
457  weight = 0;
458 
459  }
460  else
461  {
462  if (e->GetLabelL(kBackwardCount) < e->GetLabelL(kForwardCount))
463  {
464  weight = 1.0 - (1.0 / (e->GetLabelL(kForwardCount) - e->GetLabelL(kBackwardCount)));
465  }
466  else
467  weight = 0;
468  }
469  return h + 100*weight;*/
470 
471 }
472 
473 void WeightedMap2DEnvironment::OpenGLDraw() const
474 {
475 // return;
476  // Draw the map
477  //AbsMapEnvironment::OpenGLDraw();
478 
479  // Draw direction for all nodes
480 
481  Graph *g = ma->GetAbstractGraph(0);
482  node_iterator ni = g->getNodeIter();
483 
484  xyLoc s;
485 
486  if (1)
487  for (node *n = g->nodeIterNext(ni); n; n = g->nodeIterNext(ni))
488  {
489 
490  s.x = n->GetLabelL(kFirstData);
491  s.y = n->GetLabelL(kFirstData+1);
492 
493  AngleUtil::AngleSearchNode sn(s,GetStateHash(s));
494 
495  if (angleLookup.find(sn) != angleLookup.end())
496  {
497  const Vector2D old = (*angleLookup.find(sn)).second;
498  //const Vector2D old = angleLookup[sn];
499 
500  // Reduce length to 40%
501  GLdouble xx, yy, zz, rad;
502  //glColor3f(1.0, 0,0);
503  glColor3f(0,0,0);
504  map->GetOpenGLCoord(s.x, s.y, xx, yy, zz, rad);
505  //glLineWidth(2);
506  glBegin(GL_LINE_STRIP);
507  glVertex3f(xx-old.x*rad, yy-old.y*rad, zz-rad/2);
508 
509  //glColor3f(0.0, 0, 0);
510  glVertex3f(xx+old.x*rad, yy+old.y*rad, zz-rad/2);
511  glEnd();
512 
513  double percOffset=0.5;
514  double lengthOffset=0.5*rad;
515  glBegin(GL_LINE_STRIP);
516  glVertex3f(xx+percOffset*old.x*rad-old.y*lengthOffset,
517  yy+percOffset*old.y*rad+old.x*lengthOffset, zz-rad/2);
518  glVertex3f(xx+old.x*rad, yy+old.y*rad, zz-rad/2);
519 
520  glVertex3f(xx+percOffset*old.x*rad+old.y*lengthOffset,
521  yy+percOffset*old.y*rad-old.x*lengthOffset, zz-rad/2);
522  glEnd();
523 
524  }
525 
526  }
527 
528 
529  // Draw all edges
530 // Graph* g = ma->GetAbstractGraph(0);
531 //
532 // edge_iterator ei = g->getEdgeIter();
533 // for (edge *e = g->edgeIterNext(ei); e; e = g->edgeIterNext(ei))
534 // {
535 // DrawEdge(window, e);
536 // }
537 //
538 }
539 
540 void WeightedMap2DEnvironment::DrawEdge(const edge* e) const
541 {
542  Graph* g = ma->GetAbstractGraph(0);
543  node* from = g->GetNode(e->getFrom());
544  node* to = g->GetNode(e->getTo());
545 
546  GLdouble xx, yy, zz, rad;
547  if (e->GetLabelL(kBackwardCount) == e->GetLabelL(kForwardCount))
548  {
549  // whole edge should be same colour - white
550 
551  glColor3f(1.0, 1.0, 1.0);
552 
553  map->GetOpenGLCoord((int)from->GetLabelL(kFirstData), (int)from->GetLabelL(kFirstData+1), xx, yy, zz, rad);
554  glBegin(GL_LINE_STRIP);
555  glVertex3f(xx, yy, zz-rad/2);
556 
557  map->GetOpenGLCoord((int)to->GetLabelL(kFirstData), (int)to->GetLabelL(kFirstData+1), xx, yy, zz, rad);
558  glVertex3f(xx, yy, zz-rad/2);
559  glEnd();
560  }
561  else if (e->GetLabelL(kForwardCount) > e->GetLabelL(kBackwardCount))
562  {
563  // white on "from" side, blue on "to" side
564  glColor3f(1.0, 1.0, 1.0);
565 
566  map->GetOpenGLCoord((int)from->GetLabelL(kFirstData), (int)from->GetLabelL(kFirstData+1), xx, yy, zz, rad);
567  glBegin(GL_LINE_STRIP);
568  glVertex3f(xx, yy, zz-rad/2);
569 
570  glColor3f(0,0,1.0);
571  map->GetOpenGLCoord((int)to->GetLabelL(kFirstData), (int)to->GetLabelL(kFirstData+1), xx, yy, zz, rad);
572  glVertex3f(xx, yy, zz-rad/2);
573  glEnd();
574  }
575  else
576  {
577  // blue on "from" side, white on "to" side
578  glColor3f(0,0,1.0);
579  map->GetOpenGLCoord((int)from->GetLabelL(kFirstData), (int)from->GetLabelL(kFirstData+1), xx, yy, zz, rad);
580  glBegin(GL_LINE_STRIP);
581  glVertex3f(xx, yy, zz-rad/2);
582 
583 
584  glColor3f(1.0, 1.0, 1.0);
585  map->GetOpenGLCoord((int)to->GetLabelL(kFirstData), (int)to->GetLabelL(kFirstData+1), xx, yy, zz, rad);
586  glVertex3f(xx, yy, zz-rad/2);
587  glEnd();
588  }
589 }
590 
591 void WeightedMap2DEnvironment::OpenGLDraw(const xyLoc& initial, const tDirection &dir) const
592 {
593  // Figure out which edge this corresponds to
594  // Figure out which direction is preferred
595  // if none, then keep one colour
596 
597  xyLoc s = initial;
598 
599  switch (dir)
600  {
601  case kN: s.y-=1; break;
602  case kS: s.y+=1; break;
603  case kE: s.x+=1; break;
604  case kW: s.x-=1; break;
605  case kNW: s.y-=1; s.x-=1; break;
606  case kSW: s.y+=1; s.x-=1; break;
607  case kNE: s.y-=1; s.x+=1; break;
608  case kSE: s.y+=1; s.x+=1; break;
609  default: break;
610  }
611 
612  Graph* g = ma->GetAbstractGraph(0);
613  node* from = ma->GetNodeFromMap(initial.x, initial.y);
614  node* to = ma->GetNodeFromMap(s.x, s.y);
615  edge* e = g->FindEdge(from->GetNum(),to->GetNum());
616 
617  DrawEdge(e);
618 
619 }
620 
621 void WeightedMap2DEnvironment::SetAngle(xyLoc &l, Vector2D angle)
622 {
623  AngleUtil::AngleSearchNode sn(l,GetStateHash(l));
624  angleLookup[sn]=angle;
625 }
626 
627 Vector2D WeightedMap2DEnvironment::GetAngle(xyLoc &l)
628 {
629  AngleUtil::AngleSearchNode sn(l,GetStateHash(l));
630  if (angleLookup.find(sn) != angleLookup.end())
631  return angleLookup[sn];
632  else
633  {
634  Vector2D angle(0,0);
635  return angle;
636  }
637 }
638 
639 double WeightedMap2DEnvironment::ComputeArrowMetric(bool timed, double time, bool DoNormalize, double maxtime)
640 {
641  Graph* g = ma->GetAbstractGraph(0);
642  node_iterator ni = g->getNodeIter();
643 
644  // Create a vector of directions so we can loop through them
645  std::vector<tDirection> directions;
646  directions.push_back(kN);
647  directions.push_back(kNE);
648  directions.push_back(kE);
649  directions.push_back(kSE);
650  directions.push_back(kS);
651  directions.push_back(kSW);
652  directions.push_back(kW);
653  directions.push_back(kNW);
654 
655  int numDotProducts = 0;
656  double totalDotProducts = 0;
657 
658  for (node *n=g->nodeIterNext(ni); n; n=g->nodeIterNext(ni))
659  {
660 
661 
662  xyLoc current;
663  current.x = n->GetLabelL(kFirstData);
664  current.y = n->GetLabelL(kFirstData+1);
665 
666  Vector2D nodeVec = GetAngle(current);
667 
668  if (!timed || (time-nodeVec.GetUpdateTime() < maxtime))
669  {
670  if (nodeVec.x == 0 && nodeVec.y == 0) // check if there's no angle stored here
671  continue;
672 
673  //if (sqrt(nodeVec.x *nodeVec.x + nodeVec.y*nodeVec.y) < 0.5)
674  // continue;
675 
676  double bestDotProd = -2;
677  tDirection bestDir = kStay;
678 
679  for (unsigned int i=0; i<directions.size(); i++)
680  {
681  xyLoc nextThisDir;
682  GetNextState(current, directions[i], nextThisDir);
683 
684  if (!GetMap()->CanStep(current.x, current.y, nextThisDir.x, nextThisDir.y))
685  continue;
686 
687  Vector2D dirVec = GetAngleFromDirection(directions[i]);
688 
689  double dotProd = (nodeVec.x * dirVec.x) + (nodeVec.y * dirVec.y);
690 
691  if (dotProd > bestDotProd)
692  {
693  bestDotProd = dotProd;
694  bestDir = directions[i];
695  }
696  }
697 
698  if (bestDotProd == -2) continue;
699 
700  // Now that we know the closest direction, find out if there's a node there
701  xyLoc next;
702  GetNextState(current, bestDir, next);
703 
704  if (!(next==current))
705  {
706  numDotProducts++;
707  Vector2D nextNodeVec = GetAngle(next);
708  if (DoNormalize)
709  {
710  nodeVec.Normalize();
711  nextNodeVec.Normalize();
712  }
713  //double dotprod = (nodeVec.x * nextNodeVec.x) + (nodeVec.y * nextNodeVec.y);
714  //totalDotProducts += dotprod;
715  Vector2D difference((nodeVec.x + nextNodeVec.x)/2, (nodeVec.y + nextNodeVec.y)/2);
716  double addme = sqrt(difference.x * difference.x + difference.y * difference.y);
717  totalDotProducts += addme;
718  }
719  }
720  }
721  //std::cout<<"Timed: "<<timed<<" time: "<<time<<" norm? "<<DoNormalize<<" return "<<(totalDotProducts / (double)(numDotProducts))<<std::endl;
722  return (totalDotProducts / (double)(numDotProducts));
723 }
724 
725 Vector2D WeightedMap2DEnvironment::GetAngleFromDirection(tDirection dir) const
726 {
727  Vector2D angle;
728 
729  switch (dir)
730  {
731  case kN: angle.Set(0,-1); break;
732  case kS: angle.Set(0,1);break;
733  case kE: angle.Set(1,0); break;
734  case kW: angle.Set(-1,0);break;
735  case kNW: angle.Set(-1,-1); break;
736  case kSW: angle.Set(-1,1);break;
737  case kNE: angle.Set(1,-1); break;
738  case kSE: angle.Set(1,1); break;
739  default: break;
740  }
741 
742  angle.Normalize();
743 
744  return angle;
745 }
WeightedMap2DEnvironment::useWindow
bool useWindow
Definition: WeightedMap2DEnvironment.h:181
GraphAbstractionConstants
Definition: GraphAbstraction.h:22
WeightedMap2DEnvironment.h
A 2D map environment with edge costs weighted according to the number of times a unit has passed over...
WeightedMap2DEnvironment::GetAngleFromDirection
Vector2D GetAngleFromDirection(tDirection dir) const
Definition: WeightedMap2DEnvironment.cpp:725
Vector2D
Definition: WeightedMap2DEnvironment.h:40
Vector2D::GetUpdateTime
double GetUpdateTime()
Definition: WeightedMap2DEnvironment.h:47
edge::getFrom
unsigned int getFrom() const
Definition: Graph.h:146
edge::getTo
unsigned int getTo() const
Definition: Graph.h:147
kSE
@ kSE
Definition: Map2DEnvironment.h:79
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
Vector2D::x
float x
Definition: WeightedMap2DEnvironment.h:78
WeightedMap2DEnvironment::GCost
virtual double GCost(const xyLoc &node1, const xyLoc &node2) const
Definition: WeightedMap2DEnvironment.cpp:379
BaseMapOccupancyInterface
Definition: Map2DEnvironment.h:82
xyLoc::y
uint16_t y
Definition: Map2DEnvironment.h:42
kW
@ kW
Definition: Map2DEnvironment.h:78
WeightedMap2DEnvironment::windowSize
double windowSize
Definition: WeightedMap2DEnvironment.h:182
WeightedMap2DEnvironment::~WeightedMap2DEnvironment
virtual ~WeightedMap2DEnvironment()
Destructor for the WeightedMap2DEnvironment.
Definition: WeightedMap2DEnvironment.cpp:64
Graph::nodeIterNext
node * nodeIterNext(node_iterator &) const
Definition: Graph.cpp:303
WeightedMap2DEnvironment::ComputeArrowMetric
double ComputeArrowMetric(bool timed=false, double time=0, bool DoNormalize=false, double maxtime=0)
Definition: WeightedMap2DEnvironment.cpp:639
xyLoc
Definition: Map2DEnvironment.h:37
WeightedMap2DEnvironment::GetAngle
Vector2D GetAngle(xyLoc &l)
Definition: WeightedMap2DEnvironment.cpp:627
Graph
A generic Graph class.
Definition: Graph.h:66
WeightedMap2DEnvironment::OpenGLDraw
void OpenGLDraw() const
Definition: WeightedMap2DEnvironment.cpp:473
Graph::GetNode
node * GetNode(unsigned long num)
Definition: Graph.cpp:152
xyLoc::x
uint16_t x
Definition: Map2DEnvironment.h:41
node_iterator
std::vector< node * >::const_iterator node_iterator
Definition: Graph.h:33
WeightedMap2DEnvironment::ApplyAction
void ApplyAction(xyLoc &s, tDirection dir) const
ApplyAction is called when an action is applied.
Definition: WeightedMap2DEnvironment.cpp:77
AngleUtil::AngleSearchNode
Definition: WeightedMap2DEnvironment.h:94
WeightedMap2DEnvironment::SetAngle
void SetAngle(xyLoc &l, Vector2D angle)
Definition: WeightedMap2DEnvironment.cpp:621
edge::GetLabelL
long GetLabelL(unsigned int index) const
Definition: Graph.h:138
kStay
@ kStay
Definition: Map2DEnvironment.h:79
WeightedMap2DEnvironment::noWeighting
bool noWeighting
Definition: WeightedMap2DEnvironment.h:184
loc
Definition: MapGenerators.cpp:296
WeightedMap2DEnvironment::DrawEdge
void DrawEdge(const edge *e) const
Definition: WeightedMap2DEnvironment.cpp:540
kE
@ kE
Definition: Map2DEnvironment.h:78
GraphAbstractionConstants::kFirstData
@ kFirstData
Definition: GraphAbstraction.h:34
WeightedMap2DEnvironment::windowCenter
xyLoc windowCenter
Definition: WeightedMap2DEnvironment.h:180
ROOT_TWO
static const double ROOT_TWO
Definition: GLUtil.h:61
tDirection
tDirection
Definition: Map2DEnvironment.h:77
WeightedMap2DEnvironment::queryOldProportion
double queryOldProportion
Definition: WeightedMap2DEnvironment.h:187
WeightedMap2DEnvironment::oldProportion
double oldProportion
Definition: WeightedMap2DEnvironment.h:177
WeightedMap2DEnvironment::diffWeight
double diffWeight
Definition: WeightedMap2DEnvironment.h:176
WeightedMap2DEnvironment::surroundingProportion
double surroundingProportion
Definition: WeightedMap2DEnvironment.h:190
fgreater
bool fgreater(double a, double b)
Definition: FPUtil.h:29
Graph::getNodeIter
node_iterator getNodeIter() const
Definition: Graph.cpp:298
Vector2D::Set
void Set(float _x, float _y)
Definition: WeightedMap2DEnvironment.h:44
Vector2D::y
float y
Definition: WeightedMap2DEnvironment.h:78
kN
@ kN
Definition: Map2DEnvironment.h:78
WeightedMap2DEnvironment::angleLookup
AngleLookupTable angleLookup
Definition: WeightedMap2DEnvironment.h:172
WeightedMap2DEnvironment::WeightedMap2DEnvironment
WeightedMap2DEnvironment(MapAbstraction *ma)
Constructor for the WeightedMap2DEnvironment.
Definition: WeightedMap2DEnvironment.cpp:10
Vector2D::SetUpdateTime
void SetUpdateTime(double t)
Definition: WeightedMap2DEnvironment.h:46
kForwardCount
@ kForwardCount
Definition: WeightedMap2DEnvironment.h:115
kS
@ kS
Definition: Map2DEnvironment.h:78
node::GetNum
unsigned int GetNum() const
Definition: Graph.h:176
WeightedMap2DEnvironment::learningRate
double learningRate
Definition: WeightedMap2DEnvironment.h:193
WeightedMap2DEnvironment::UpdateAngle
void UpdateAngle(const xyLoc &old, const xyLoc &s, double t)
Definition: WeightedMap2DEnvironment.cpp:367
Vector2D::Normalize
void Normalize()
Definition: WeightedMap2DEnvironment.h:81
WeightedMap2DEnvironment::usePerceptron
bool usePerceptron
Definition: WeightedMap2DEnvironment.h:192
WeightedMap2DEnvironment::oi
BaseMapOccupancyInterface * oi
Definition: WeightedMap2DEnvironment.h:168
kNE
@ kNE
Definition: Map2DEnvironment.h:78
node::GetLabelL
long GetLabelL(unsigned int index) const
Definition: Graph.h:220
WeightedMap2DEnvironment::updateSurrounding
bool updateSurrounding
Definition: WeightedMap2DEnvironment.h:189
kSW
@ kSW
Definition: Map2DEnvironment.h:79
kBackwardCount
@ kBackwardCount
Definition: WeightedMap2DEnvironment.h:116
node
Nodes to be stored within a Graph.
Definition: Graph.h:170
kNW
@ kNW
Definition: Map2DEnvironment.h:78
edge
Edge class for connections between node in a Graph.
Definition: Graph.h:129
WeightedMap2DEnvironment::updateOnQuery
bool updateOnQuery
Definition: WeightedMap2DEnvironment.h:186
Generated by   doxygen 1.8.17